From 838e9fc76943eabd78064a8f18effd3dcd0fd1bf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 10 Mar 2015 15:13:22 -0700 Subject: [PATCH 001/258] Refactoring for multiplatform support Moved the NuttX specific board files to makefiles/nuttx and added a makfiles/linux directory with sample config and board files. Created a makefiles/toolchain_native.mk file for building for Linux with the native system compiler. GCC or clang can be used by setting a flag in the file. The Linux build creates an archive file and will build the tasks as threads. Other code changes are required to support both task based and thread based builds. The NuttX source should not be required for the Linux build. The target OS (NuttX or Linux) is selected by commenting out the desired line in setup.mk Signed-off-by: Mark Charlebois --- Makefile | 143 +------- makefiles/README.txt | 31 +- makefiles/firmware.mk | 192 +---------- makefiles/firmware_linux.mk | 56 ++++ makefiles/firmware_nuttx.mk | 160 +++++++++ makefiles/library.mk | 2 +- makefiles/linux.mk | 38 +++ makefiles/linux/board_linux.mk | 11 + makefiles/linux/config_linux_default.mk | 64 ++++ makefiles/linux_elf.mk | 69 ++++ makefiles/nuttx.mk | 2 + makefiles/{ => nuttx}/board_aerocore.mk | 0 makefiles/{ => nuttx}/board_px4fmu-v1.mk | 0 makefiles/{ => nuttx}/board_px4fmu-v2.mk | 0 makefiles/{ => nuttx}/board_px4io-v1.mk | 0 makefiles/{ => nuttx}/board_px4io-v2.mk | 0 .../{ => nuttx}/config_aerocore_default.mk | 0 .../{ => nuttx}/config_px4fmu-v1_default.mk | 0 .../{ => nuttx}/config_px4fmu-v2_default.mk | 0 .../config_px4fmu-v2_multiplatform.mk | 0 .../{ => nuttx}/config_px4fmu-v2_test.mk | 0 .../{ => nuttx}/config_px4io-v1_default.mk | 0 .../{ => nuttx}/config_px4io-v2_default.mk | 0 makefiles/nuttx_romfs.mk | 219 +++++++++++++ makefiles/setup.mk | 3 + makefiles/toolchain_gnu-arm-eabi.mk | 2 +- makefiles/toolchain_native.mk | 306 ++++++++++++++++++ 27 files changed, 979 insertions(+), 319 deletions(-) create mode 100644 makefiles/firmware_linux.mk create mode 100644 makefiles/firmware_nuttx.mk create mode 100644 makefiles/linux.mk create mode 100644 makefiles/linux/board_linux.mk create mode 100644 makefiles/linux/config_linux_default.mk create mode 100644 makefiles/linux_elf.mk rename makefiles/{ => nuttx}/board_aerocore.mk (100%) rename makefiles/{ => nuttx}/board_px4fmu-v1.mk (100%) rename makefiles/{ => nuttx}/board_px4fmu-v2.mk (100%) rename makefiles/{ => nuttx}/board_px4io-v1.mk (100%) rename makefiles/{ => nuttx}/board_px4io-v2.mk (100%) rename makefiles/{ => nuttx}/config_aerocore_default.mk (100%) rename makefiles/{ => nuttx}/config_px4fmu-v1_default.mk (100%) rename makefiles/{ => nuttx}/config_px4fmu-v2_default.mk (100%) rename makefiles/{ => nuttx}/config_px4fmu-v2_multiplatform.mk (100%) rename makefiles/{ => nuttx}/config_px4fmu-v2_test.mk (100%) rename makefiles/{ => nuttx}/config_px4io-v1_default.mk (100%) rename makefiles/{ => nuttx}/config_px4io-v2_default.mk (100%) create mode 100644 makefiles/nuttx_romfs.mk create mode 100644 makefiles/toolchain_native.mk diff --git a/Makefile b/Makefile index 201187e021..65eabf9832 100644 --- a/Makefile +++ b/Makefile @@ -53,13 +53,13 @@ export GIT_DESC # # Canned firmware configurations that we (know how to) build. # -KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/config_*.mk)))) CONFIGS ?= $(KNOWN_CONFIGS) # # Boards that we (know how to) build NuttX export kits for. # -KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/board_*.mk)))) BOARDS ?= $(KNOWN_BOARDS) # @@ -97,146 +97,31 @@ upload: endif endif -# -# Built products -# -DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) -STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) - -all: $(DESIRED_FIRMWARES) - -# -# Copy FIRMWARES into the image directory. -# -# XXX copying the .bin files is a hack to work around the PX4IO uploader -# not supporting .px4 files, and it should be deprecated onced that -# is taken care of. -# -$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 - @$(ECHO) %% Copying $@ - $(Q) $(COPY) $< $@ - $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) - -# -# Generate FIRMWARES. -# -.PHONY: $(FIRMWARES) -$(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) -$(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ -$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules - @$(ECHO) %%%% - @$(ECHO) %%%% Building $(config) in $(work_dir) - @$(ECHO) %%%% - $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ - -f $(PX4_MK_DIR)firmware.mk \ - CONFIG=$(config) \ - WORK_DIR=$(work_dir) \ - $(FIRMWARE_GOAL) - -# -# Make FMU firmwares depend on the corresponding IO firmware. -# -# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency -# and forces the _default config in all cases. There has to be a better way to do this... -# -FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) -define FMU_DEP -$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 -endef -FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) -$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) - -# -# Build the NuttX export archives. -# -# Note that there are no explicit dependencies extended from these -# archives. If NuttX is updated, the user is expected to rebuild the -# archives/build area manually. Likewise, when the 'archives' target is -# invoked, all archives are always rebuilt. -# -# XXX Should support fetching/unpacking from a separate directory to permit -# downloads of the prebuilt archives as well... -# -NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) -.PHONY: archives -archives: checksubmodules $(NUTTX_ARCHIVES) - -# We cannot build these parallel; note that we also force -j1 for the -# sub-make invocations. -ifneq ($(filter archives,$(MAKECMDGOALS)),) -.NOTPARALLEL: +ifeq ($(PX4_TARGET_OS),nuttx) +include $(PX4_BASE)makefiles/firmware_nuttx.mk endif - -J?=1 - -$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) -$(ARCHIVE_DIR)%.export: configuration = nsh -$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) - @$(ECHO) %% Configuring NuttX for $(board) - $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) - $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) - @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export - $(Q) $(MKDIR) -p $(dir $@) - $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ - $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) - -# -# The user can run the NuttX 'menuconfig' tool for a single board configuration with -# make BOARDS= menuconfig -# -ifeq ($(MAKECMDGOALS),menuconfig) -ifneq ($(words $(BOARDS)),1) -$(error BOARDS must specify exactly one board for the menuconfig goal) +ifeq ($(PX4_TARGET_OS),linux) +include $(PX4_BASE)makefiles/firmware_linux.mk endif -BOARD = $(BOARDS) -menuconfig: $(NUTTX_SRC) - @$(ECHO) %% Configuring NuttX for $(BOARD) - $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) - $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) - @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig - @$(ECHO) %% Saving configuration file - $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig -else -menuconfig: - @$(ECHO) "" - @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" - @$(ECHO) "" -endif - -$(NUTTX_SRC): checksubmodules - -$(UAVCAN_DIR): - $(Q) (./Tools/check_submodules.sh) - -.PHONY: checksubmodules -checksubmodules: - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) - -.PHONY: updatesubmodules -updatesubmodules: - $(Q) (git submodule init) - $(Q) (git submodule update) MSG_DIR = $(PX4_BASE)msg UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics -MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages +MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/$(PX4_TARGET_OS)/px4_messages MULTIPLATFORM_PREFIX = px4_ TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src +ifeq ($(PX4_TARGET_OS),nuttx) +OS_DEPS = checksubmodules +else +OS_DEPS = +endif + .PHONY: generateuorbtopicheaders -generateuorbtopicheaders: checksubmodules +generateuorbtopicheaders: $(OS_DEPS) @$(ECHO) "Generating uORB topic headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ diff --git a/makefiles/README.txt b/makefiles/README.txt index 8b84e4c40d..36edf6a47b 100644 --- a/makefiles/README.txt +++ b/makefiles/README.txt @@ -7,8 +7,9 @@ with Makefile in the parent directory. ../Makefile - Top-level makefile for the PX4 build system. This makefile supports - building NuttX archives, as well as supervising the building of all + Top-level makefile for the PX4 build system. + This makefile supports building NuttX archives for the NuttX based + configurations, as well as supervising the building of all of the defined PX4 firmware configurations. Try 'make help' in the parent directory for documentation. @@ -25,6 +26,14 @@ firmware.mk source tree. When used in this mode, at least BOARD, MODULES and CONFIG_FILE must be set. +firmware_nuttx.mk + + Called by firmware.mk to build NuttX based firmware. + +firmware_linux.mk + + Called by firmware.mk to build Linux (non-ROS) based firmware. + module.mk Called by firmware.mk to build individual modules. @@ -34,22 +43,28 @@ module.mk nuttx.mk - Called by ../Makefile to build or download the NuttX archives. + Called by ../Makefile to build or download the NuttX archives if + PX4_TARGET_OS is set to "nuttx". + +linux.mk + + Called by ../Makefile to set Linux specific parameters if + PX4_TARGET_OS is set to "linux". upload.mk Called by ../Makefile to upload files to a target board. Can be used - by external build systems as well. + by external build systems as well. (NuttX targets only) setup.mk - Provides common path and tool definitions. Implements host system-specific - compatibility hacks. + Provides common path and tool definitions. Implements host + system-specific compatibility hacks. Sets PX4_TARGET_OS. board_.mk - Board-specific configuration for . Typically sets CONFIG_ARCH - and then includes the toolchain definition for the board. + Board-specific configuration for . Typically sets + CONFIG_ARCH and then includes the toolchain definition for the board. config__.mk diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 4c10de931c..814bf8b8d6 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -132,7 +132,7 @@ include $(MK_DIR)/setup.mk ifneq ($(CONFIG_FILE),) CONFIG := $(subst config_,,$(basename $(notdir $(CONFIG_FILE)))) else -CONFIG_FILE := $(wildcard $(PX4_MK_DIR)/config_$(CONFIG).mk) +CONFIG_FILE := $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/config_$(CONFIG).mk) endif ifeq ($(CONFIG),) $(error Missing configuration name or file (specify with CONFIG=)) @@ -150,7 +150,7 @@ $(info % CONFIG = $(CONFIG)) ifeq ($(BOARD),) BOARD := $(firstword $(subst _, ,$(CONFIG))) endif -BOARD_FILE := $(wildcard $(PX4_MK_DIR)/board_$(BOARD).mk) +BOARD_FILE := $(wildcard $(PX4_MK_DIR)/$(PX4_TARGET_OS)/board_$(BOARD).mk) ifeq ($(BOARD_FILE),) $(error Config $(CONFIG) references board $(BOARD), but no board definition file found) endif @@ -186,10 +186,10 @@ EXTRA_CLEANS = INCLUDE_DIRS += $(PX4_MODULE_SRC)drivers/boards/$(BOARD) ################################################################################ -# NuttX libraries and paths +# OS specific libraries and paths ################################################################################ -include $(PX4_MK_DIR)/nuttx.mk +include $(PX4_MK_DIR)/$(PX4_TARGET_OS).mk ################################################################################ # Modules @@ -311,130 +311,6 @@ $(LIBRARY_CLEANS): LIBRARY_MK=$(mkfile) \ clean -################################################################################ -# ROMFS generation -################################################################################ - -ifneq ($(ROMFS_ROOT),) -ifeq ($(wildcard $(ROMFS_ROOT)),) -$(error ROMFS_ROOT specifies a directory that does not exist) -endif - -# -# Note that there is no support for more than one root directory or constructing -# a root from several templates. That would be a nice feature. -# - -# Add dependencies on anything in the ROMFS root directory -ROMFS_FILES += $(wildcard \ - $(ROMFS_ROOT)/* \ - $(ROMFS_ROOT)/*/* \ - $(ROMFS_ROOT)/*/*/* \ - $(ROMFS_ROOT)/*/*/*/* \ - $(ROMFS_ROOT)/*/*/*/*/* \ - $(ROMFS_ROOT)/*/*/*/*/*/*) -ifeq ($(ROMFS_FILES),) -$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files) -endif -ROMFS_DEPS += $(ROMFS_FILES) - -# Extra files that may be copied into the ROMFS /extras directory -# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional -ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES)) -ROMFS_DEPS += $(ROMFS_EXTRA_FILES) - -ROMFS_IMG = romfs.img -ROMFS_SCRATCH = romfs_scratch -ROMFS_CSRC = $(ROMFS_IMG:.img=.c) -ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) -LIBS += $(ROMFS_OBJ) -LINK_DEPS += $(ROMFS_OBJ) - -# Remove all comments from startup and mixer files -ROMFS_PRUNER = $(PX4_BASE)/Tools/px_romfs_pruner.py - -# Turn the ROMFS image into an object file -$(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) - $(call BIN_TO_OBJ,$<,$@,romfs_img) - -# Generate the ROMFS image from the root -$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) - @$(ECHO) "ROMFS: $@" - $(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol" - -# Construct the ROMFS scratch root from the canonical root -$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) - $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) - $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) -# delete all files in ROMFS_SCRATCH which start with a . or end with a ~ - $(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~ -ifneq ($(ROMFS_EXTRA_FILES),) - $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras - $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras -endif - $(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH) - -EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) - -endif - -################################################################################ -# Builtin command list generation -################################################################################ - -# -# Builtin commands can be generated by the configuration, in which case they -# must refer to commands that already exist, or indirectly generated by modules -# when they are built. -# -# The configuration supplies builtin command information in the BUILTIN_COMMANDS -# variable. Applications make empty files in $(WORK_DIR)/builtin_commands whose -# filename contains the same information. -# -# In each case, the command information consists of four fields separated with a -# period. These fields are the command's name, its thread priority, its stack size -# and the name of the function to call when starting the thread. -# -BUILTIN_CSRC = $(WORK_DIR)builtin_commands.c - -# command definitions from modules (may be empty at Makefile parsing time...) -MODULE_COMMANDS = $(subst COMMAND.,,$(notdir $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*))) - -# We must have at least one pre-defined builtin command in order to generate -# any of this. -# -ifneq ($(BUILTIN_COMMANDS),) - -# (BUILTIN_PROTO,,) -define BUILTIN_PROTO - $(ECHO) 'extern int $(word 4,$1)(int argc, char *argv[]);' >> $2; -endef - -# (BUILTIN_DEF,,) -define BUILTIN_DEF - $(ECHO) ' {"$(word 1,$1)", $(word 2,$1), $(word 3,$1), $(word 4,$1)},' >> $2; -endef - -# Don't generate until modules have updated their command files -$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(MODULE_MKFILES) $(BUILTIN_COMMAND_FILES) - @$(ECHO) "CMDS: $@" - $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ - $(Q) $(ECHO) '#include ' >> $@ - $(Q) $(ECHO) '#include ' >> $@ - $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) - $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) - $(Q) $(ECHO) 'const struct builtin_s g_builtins[] = {' >> $@ - $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) - $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) - $(Q) $(ECHO) ' {NULL, 0, 0, NULL}' >> $@ - $(Q) $(ECHO) '};' >> $@ - $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS) $(MODULE_COMMANDS));' >> $@ - -SRCS += $(BUILTIN_CSRC) - -EXTRA_CLEANS += $(BUILTIN_CSRC) - -endif ################################################################################ # Default SRCS generation @@ -453,20 +329,9 @@ SRCS += $(EMPTY_SRC) endif ################################################################################ -# Build rules +# Generic Build rules ################################################################################ -# -# What we're going to build. -# -PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 -PRODUCT_BIN = $(WORK_DIR)firmware.bin -PRODUCT_ELF = $(WORK_DIR)firmware.elf -PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml - -.PHONY: firmware -firmware: $(PRODUCT_BUNDLE) - # # Object files we will generate from sources # @@ -488,49 +353,16 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(call ASSEMBLE,$<,$@) # -# Built product rules +# Include the OS specific build rules +# The rules must define the "firmware" make target # -$(PRODUCT_BUNDLE): $(PRODUCT_BIN) - @$(ECHO) %% Generating $@ -ifdef GEN_PARAM_XML - python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml - $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ - --git_identity $(PX4_BASE) \ - --parameter_xml $(PRODUCT_PARAMXML) \ - --image $< > $@ -else - $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ - --git_identity $(PX4_BASE) \ - --image $< > $@ +ifeq ($(PX4_TARGET_OS),nuttx) +include $(MK_DIR)/nuttx_romfs.mk +endif +ifeq ($(PX4_TARGET_OS),linux) +include $(MK_DIR)/linux_elf.mk endif - -$(PRODUCT_BIN): $(PRODUCT_ELF) - $(call SYM_TO_BIN,$<,$@) - -$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) - $(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS)) - -# -# Utility rules -# - -.PHONY: upload -upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) - $(Q) $(MAKE) -f $(PX4_MK_DIR)/upload.mk \ - METHOD=serial \ - CONFIG=$(CONFIG) \ - BOARD=$(BOARD) \ - BUNDLE=$(PRODUCT_BUNDLE) \ - BIN=$(PRODUCT_BIN) - -.PHONY: clean -clean: $(MODULE_CLEANS) - @$(ECHO) %% cleaning - $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_ELF) - $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) - $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR) - # # DEP_INCLUDES is defined by the toolchain include in terms of $(OBJS) diff --git a/makefiles/firmware_linux.mk b/makefiles/firmware_linux.mk new file mode 100644 index 0000000000..6acec4e733 --- /dev/null +++ b/makefiles/firmware_linux.mk @@ -0,0 +1,56 @@ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Built products +# +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) + +all: $(FIRMWARES) + +# +# Generate FIRMWARES. +# +.PHONY: $(FIRMWARES) +$(BUILD_DIR)%.build/firmware.a: config = $(patsubst $(BUILD_DIR)%.build/firmware.a,%,$@) +$(BUILD_DIR)%.build/firmware.a: work_dir = $(BUILD_DIR)$(config).build/ +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: generateuorbtopicheaders + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ + -f $(PX4_MK_DIR)firmware.mk \ + CONFIG=$(config) \ + WORK_DIR=$(work_dir) \ + $(FIRMWARE_GOAL) + + diff --git a/makefiles/firmware_nuttx.mk b/makefiles/firmware_nuttx.mk new file mode 100644 index 0000000000..fb97ca4217 --- /dev/null +++ b/makefiles/firmware_nuttx.mk @@ -0,0 +1,160 @@ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Built products +# + +DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) +STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) + +all: $(DESIRED_FIRMWARES) + +# +# Copy FIRMWARES into the image directory. +# +# XXX copying the .bin files is a hack to work around the PX4IO uploader +# not supporting .px4 files, and it should be deprecated onced that +# is taken care of. +# +$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 + @$(ECHO) %% Copying $@ + $(Q) $(COPY) $< $@ + $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) + +# +# Generate FIRMWARES. +# +.PHONY: $(FIRMWARES) +$(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) +$(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ + -f $(PX4_MK_DIR)firmware.mk \ + CONFIG=$(config) \ + WORK_DIR=$(work_dir) \ + $(FIRMWARE_GOAL) + +# +# Make FMU firmwares depend on the corresponding IO firmware. +# +# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency +# and forces the _default config in all cases. There has to be a better way to do this... +# +FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) +define FMU_DEP +$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 +endef +FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) +$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) + +# +# Build the NuttX export archives. +# +# Note that there are no explicit dependencies extended from these +# archives. If NuttX is updated, the user is expected to rebuild the +# archives/build area manually. Likewise, when the 'archives' target is +# invoked, all archives are always rebuilt. +# +# XXX Should support fetching/unpacking from a separate directory to permit +# downloads of the prebuilt archives as well... +# +NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) +.PHONY: archives +archives: checksubmodules $(NUTTX_ARCHIVES) + +# We cannot build these parallel; note that we also force -j1 for the +# sub-make invocations. +ifneq ($(filter archives,$(MAKECMDGOALS)),) +.NOTPARALLEL: +endif + +J?=1 + +$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) +$(ARCHIVE_DIR)%.export: configuration = nsh +$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(board) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) + @$(ECHO) %% Exporting NuttX for $(board) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MKDIR) -p $(dir $@) + $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ + $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) + +# +# The user can run the NuttX 'menuconfig' tool for a single board configuration with +# make BOARDS= menuconfig +# +ifeq ($(MAKECMDGOALS),menuconfig) +ifneq ($(words $(BOARDS)),1) +$(error BOARDS must specify exactly one board for the menuconfig goal) +endif +BOARD = $(BOARDS) +menuconfig: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(BOARD) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) + @$(ECHO) %% Running menuconfig for $(BOARD) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + @$(ECHO) %% Saving configuration file + $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig +else +menuconfig: + @$(ECHO) "" + @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" + @$(ECHO) "" +endif + +$(NUTTX_SRC): checksubmodules + +$(UAVCAN_DIR): + $(Q) (./Tools/check_submodules.sh) + +.PHONY: checksubmodules +checksubmodules: + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) + +.PHONY: updatesubmodules +updatesubmodules: + $(Q) (git submodule init) + $(Q) (git submodule update) + diff --git a/makefiles/library.mk b/makefiles/library.mk index 28a421fe0e..e20fcce941 100644 --- a/makefiles/library.mk +++ b/makefiles/library.mk @@ -93,7 +93,7 @@ $(info %% LIBRARY_MK = $(LIBRARY_MK)) # # Get the board/toolchain config # -include $(PX4_MK_DIR)/board_$(BOARD).mk +include $(PX4_MK_DIR)/$(PX4_TARGET_OS)/board_$(BOARD).mk # # Get the library's config diff --git a/makefiles/linux.mk b/makefiles/linux.mk new file mode 100644 index 0000000000..b424521a4b --- /dev/null +++ b/makefiles/linux.mk @@ -0,0 +1,38 @@ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Rules and definitions related to handling the Linux specific impl when +# building firmware. +# + +MODULES += platforms/linux/px4_layer + diff --git a/makefiles/linux/board_linux.mk b/makefiles/linux/board_linux.mk new file mode 100644 index 0000000000..ffdfd036a5 --- /dev/null +++ b/makefiles/linux/board_linux.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the Linux port of PX4 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = NATIVE +CONFIG_BOARD = FOO + +include $(PX4_MK_DIR)/toolchain_native.mk diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk new file mode 100644 index 0000000000..57721c0db4 --- /dev/null +++ b/makefiles/linux/config_linux_default.mk @@ -0,0 +1,64 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common + +# +# Board support modules +# +#MODULES += drivers/dxl +#MODULES += drivers/i2c_pwm +#MODULES += drivers/lemonrx +#MODULES += drivers/mpu9x50 +#MODULES += drivers/um7 +MODULES += drivers/device +MODULES += modules/sensors + +# +# System commands +# +#MODULES += systemcmds/boardinfo + +# +# General system control +# +#MODULES += modules/mavlink +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf + +# +# Vehicle Control +# +MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB +MODULES += modules/dataman + +# +# Libraries +# +MODULES += lib/mathlib +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion +#MODULES += lib/utils + +# +# Linux port +# +MODULES += platforms/linux/px4_layer +MODULES += platforms/linux/publisher + diff --git a/makefiles/linux_elf.mk b/makefiles/linux_elf.mk new file mode 100644 index 0000000000..18578a55c1 --- /dev/null +++ b/makefiles/linux_elf.mk @@ -0,0 +1,69 @@ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Makefile for PX4 Linux based firmware images. +# + +################################################################################ +# Build rules +################################################################################ + +# +# What we're going to build. +# +PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a + +.PHONY: firmware +firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)mainapp + +# +# Built product rules +# + +$(PRODUCT_SHARED_LIB): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) + $(call LINK_A,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS)) + +MAIN = $(PX4_BASE)/src/platforms/linux/main.cpp +$(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) + $(PX4_BASE)/Tools/linux_apps.py > apps.h + $(call LINK,$@, -I. $(MAIN) $(PRODUCT_SHARED_LIB)) + +# +# Utility rules +# + +.PHONY: clean +clean: $(MODULE_CLEANS) + @$(ECHO) %% cleaning + $(Q) $(REMOVE) $(PRODUCT_ELF) + $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) + diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index bf07441408..a151e7d917 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -34,6 +34,8 @@ # building firmware. # +#MODULES += platforms/nuttx/px4_layer + # # Check that the NuttX archive for the selected board is available. # diff --git a/makefiles/board_aerocore.mk b/makefiles/nuttx/board_aerocore.mk similarity index 100% rename from makefiles/board_aerocore.mk rename to makefiles/nuttx/board_aerocore.mk diff --git a/makefiles/board_px4fmu-v1.mk b/makefiles/nuttx/board_px4fmu-v1.mk similarity index 100% rename from makefiles/board_px4fmu-v1.mk rename to makefiles/nuttx/board_px4fmu-v1.mk diff --git a/makefiles/board_px4fmu-v2.mk b/makefiles/nuttx/board_px4fmu-v2.mk similarity index 100% rename from makefiles/board_px4fmu-v2.mk rename to makefiles/nuttx/board_px4fmu-v2.mk diff --git a/makefiles/board_px4io-v1.mk b/makefiles/nuttx/board_px4io-v1.mk similarity index 100% rename from makefiles/board_px4io-v1.mk rename to makefiles/nuttx/board_px4io-v1.mk diff --git a/makefiles/board_px4io-v2.mk b/makefiles/nuttx/board_px4io-v2.mk similarity index 100% rename from makefiles/board_px4io-v2.mk rename to makefiles/nuttx/board_px4io-v2.mk diff --git a/makefiles/config_aerocore_default.mk b/makefiles/nuttx/config_aerocore_default.mk similarity index 100% rename from makefiles/config_aerocore_default.mk rename to makefiles/nuttx/config_aerocore_default.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/nuttx/config_px4fmu-v1_default.mk similarity index 100% rename from makefiles/config_px4fmu-v1_default.mk rename to makefiles/nuttx/config_px4fmu-v1_default.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk similarity index 100% rename from makefiles/config_px4fmu-v2_default.mk rename to makefiles/nuttx/config_px4fmu-v2_default.mk diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/nuttx/config_px4fmu-v2_multiplatform.mk similarity index 100% rename from makefiles/config_px4fmu-v2_multiplatform.mk rename to makefiles/nuttx/config_px4fmu-v2_multiplatform.mk diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/nuttx/config_px4fmu-v2_test.mk similarity index 100% rename from makefiles/config_px4fmu-v2_test.mk rename to makefiles/nuttx/config_px4fmu-v2_test.mk diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/nuttx/config_px4io-v1_default.mk similarity index 100% rename from makefiles/config_px4io-v1_default.mk rename to makefiles/nuttx/config_px4io-v1_default.mk diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/nuttx/config_px4io-v2_default.mk similarity index 100% rename from makefiles/config_px4io-v2_default.mk rename to makefiles/nuttx/config_px4io-v2_default.mk diff --git a/makefiles/nuttx_romfs.mk b/makefiles/nuttx_romfs.mk new file mode 100644 index 0000000000..ca6fefe29e --- /dev/null +++ b/makefiles/nuttx_romfs.mk @@ -0,0 +1,219 @@ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Makefile for PX4 Nuttx based firmware images. +# + +################################################################################ +# ROMFS generation +################################################################################ + +ifneq ($(ROMFS_ROOT),) +ifeq ($(wildcard $(ROMFS_ROOT)),) +$(error ROMFS_ROOT specifies a directory that does not exist) +endif + +# +# Note that there is no support for more than one root directory or constructing +# a root from several templates. That would be a nice feature. +# + +# Add dependencies on anything in the ROMFS root directory +ROMFS_FILES += $(wildcard \ + $(ROMFS_ROOT)/* \ + $(ROMFS_ROOT)/*/* \ + $(ROMFS_ROOT)/*/*/* \ + $(ROMFS_ROOT)/*/*/*/* \ + $(ROMFS_ROOT)/*/*/*/*/* \ + $(ROMFS_ROOT)/*/*/*/*/*/*) +ifeq ($(ROMFS_FILES),) +$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files) +endif +ROMFS_DEPS += $(ROMFS_FILES) + +# Extra files that may be copied into the ROMFS /extras directory +# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional +ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES)) +ROMFS_DEPS += $(ROMFS_EXTRA_FILES) + +ROMFS_IMG = romfs.img +ROMFS_SCRATCH = romfs_scratch +ROMFS_CSRC = $(ROMFS_IMG:.img=.c) +ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) +LIBS += $(ROMFS_OBJ) +LINK_DEPS += $(ROMFS_OBJ) + +# Remove all comments from startup and mixer files +ROMFS_PRUNER = $(PX4_BASE)/Tools/px_romfs_pruner.py + +# Turn the ROMFS image into an object file +$(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) + $(call BIN_TO_OBJ,$<,$@,romfs_img) + +# Generate the ROMFS image from the root +$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) + @$(ECHO) "ROMFS: $@" + $(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol" + +# Construct the ROMFS scratch root from the canonical root +$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) + $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) +# delete all files in ROMFS_SCRATCH which start with a . or end with a ~ + $(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~ +ifneq ($(ROMFS_EXTRA_FILES),) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras + $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras +endif + $(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH) + +EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) + +endif + +################################################################################ +# Builtin command list generation +################################################################################ + +# +# Builtin commands can be generated by the configuration, in which case they +# must refer to commands that already exist, or indirectly generated by modules +# when they are built. +# +# The configuration supplies builtin command information in the BUILTIN_COMMANDS +# variable. Applications make empty files in $(WORK_DIR)/builtin_commands whose +# filename contains the same information. +# +# In each case, the command information consists of four fields separated with a +# period. These fields are the command's name, its thread priority, its stack size +# and the name of the function to call when starting the thread. +# +BUILTIN_CSRC = $(WORK_DIR)builtin_commands.c + +# command definitions from modules (may be empty at Makefile parsing time...) +MODULE_COMMANDS = $(subst COMMAND.,,$(notdir $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*))) + +# We must have at least one pre-defined builtin command in order to generate +# any of this. +# +ifneq ($(BUILTIN_COMMANDS),) + +# (BUILTIN_PROTO,,) +define BUILTIN_PROTO + $(ECHO) 'extern int $(word 4,$1)(int argc, char *argv[]);' >> $2; +endef + +# (BUILTIN_DEF,,) +define BUILTIN_DEF + $(ECHO) ' {"$(word 1,$1)", $(word 2,$1), $(word 3,$1), $(word 4,$1)},' >> $2; +endef + +# Don't generate until modules have updated their command files +$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(MODULE_MKFILES) $(BUILTIN_COMMAND_FILES) + @$(ECHO) "CMDS: $@" + $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ + $(Q) $(ECHO) '#include ' >> $@ + $(Q) $(ECHO) '#include ' >> $@ + $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) + $(Q) $(ECHO) 'const struct builtin_s g_builtins[] = {' >> $@ + $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) + $(Q) $(ECHO) ' {NULL, 0, 0, NULL}' >> $@ + $(Q) $(ECHO) '};' >> $@ + $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS) $(MODULE_COMMANDS));' >> $@ + +SRCS += $(BUILTIN_CSRC) + +EXTRA_CLEANS += $(BUILTIN_CSRC) + +endif + +################################################################################ +# Build rules +################################################################################ + +# +# What we're going to build. +# +PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 +PRODUCT_BIN = $(WORK_DIR)firmware.bin +PRODUCT_ELF = $(WORK_DIR)firmware.elf +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml + +.PHONY: firmware +firmware: $(PRODUCT_BUNDLE) + +# +# Built product rules +# + +$(PRODUCT_BUNDLE): $(PRODUCT_BIN) + @$(ECHO) %% Generating $@ +ifdef GEN_PARAM_XML + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --parameter_xml $(PRODUCT_PARAMXML) \ + --image $< > $@ +else + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --image $< > $@ +endif + +$(PRODUCT_BIN): $(PRODUCT_ELF) + $(call SYM_TO_BIN,$<,$@) + +$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) + $(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS)) + +# +# Utility rules +# + +.PHONY: upload +upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) + $(Q) $(MAKE) -f $(PX4_MK_DIR)/upload.mk \ + METHOD=serial \ + CONFIG=$(CONFIG) \ + BOARD=$(BOARD) \ + BUNDLE=$(PRODUCT_BUNDLE) \ + BIN=$(PRODUCT_BIN) + +.PHONY: clean +clean: $(MODULE_CLEANS) + @$(ECHO) %% cleaning + $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_ELF) + $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) + $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR) + diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 351b9f1b71..1c6e5c3a2e 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,6 +33,9 @@ # Path and tool setup # +export PX4_TARGET_OS = nuttx +#export PX4_TARGET_OS = linux + # # Some useful paths. # diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 3b9fefb3ef..7cf72bdad8 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -107,7 +107,7 @@ endif ifeq ($(CONFIG_BOARD),) $(error Board config does not define CONFIG_BOARD) endif -ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) +ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) -D__PX4_NUTTX # optimisation flags # diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk new file mode 100644 index 0000000000..37e5dbed51 --- /dev/null +++ b/makefiles/toolchain_native.mk @@ -0,0 +1,306 @@ +# +# Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Definitions for a native GCC toolchain +# + +#$(info TOOLCHAIN native) + +# Toolchain commands. Normally only used inside this file. +# + +# Set to 1 for GCC-4.8.2 and to 0 for Clang-3.5 (Ubuntu 14.04) +USE_GCC=0 + +ifeq ($(USE_GCC),1) +# GCC Options: +CC = gcc +CXX = g++ +CPP = gcc -E + +# GCC Version +DEV_VER_SUPPORTED = 4.8.2 + +else +# Clang options +CC = clang-3.5 +CXX = clang++-3.5 +CPP = clang-3.5 -E + +# Clang GCC reported version +DEV_VER_SUPPORTED = 4.2.1 +endif + +LD = ld +AR = ar rcs +NM = nm +OBJCOPY = objcopy +OBJDUMP = objdump + +# Check if the right version of the toolchain is available +# +DEV_VER_FOUND = $(shell $(CC) -dumpversion) + +ifeq (,$(findstring $(DEV_VER_FOUND), $(DEV_VER_SUPPORTED))) +$(error Unsupported version of $(CC), found: $(DEV_VER_FOUND) instead of one in: $(DEV_VER_SUPPORTED)) +endif + + +# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup + +MAXOPTIMIZATION ?= -O3 + +# Enabling stack checks if OS was build with them +# + +# Set the board flags +# +ifeq ($(CONFIG_BOARD),) +$(error Board config does not define CONFIG_BOARD) +endif +ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ + -D__PX4_LINUX \ + -Dnoreturn_function= \ + -I/usr/include/eigen3 \ + -I$(PX4_BASE)/src/platforms/linux/include \ + -Wno-error=shadow + +# optimisation flags +# +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -g3 \ + -fno-strict-aliasing \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +# Language-specific flags +# +ARCHCFLAGS = -std=gnu99 -g +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++11 -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g + +# Generic warnings +# +# Disabled +# -Wshadow - Breaks for the libeigen package headers +# -Wframe-larger-than=1024 - Only needed for embedded + +ARCHWARNINGS = -Wall \ + -Wextra \ + -Werror \ + -Wfloat-equal \ + -Wpointer-arith \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter \ + -Werror=format-security \ + -Werror=array-bounds \ + -Wfatal-errors \ + -Werror=unused-variable \ + -Werror=reorder \ + -Werror=uninitialized \ + -Werror=init-self + +# Add compiler specific options +ifeq ($(USE_GCC),1) +ARCHDEFINES += -Wno-error=logical-op +ARCHWARNINGS += -Wdouble-promotion \ + -Wlogical-op \ + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=double-promotion +ARCHOPTIMIZATION += -fno-strength-reduce +endif + +# -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+ +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +# C-specific warnings +# +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wmissing-prototypes \ + -Wnested-externs + +# Add compiler specific options +ifeq ($(USE_GCC),1) +ARCHCWARNINGS += -Wold-style-declaration \ + -Wmissing-parameter-type +endif + +# C++-specific warnings +# +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-missing-field-initializers + +# pull in *just* libm from the toolchain ... this is grody +LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) +#EXTRA_LIBS += $(LIBM) +EXTRA_LIBS += -pthread -lm + +# Flags we pass to the C compiler +# +CFLAGS = $(ARCHCFLAGS) \ + $(ARCHCWARNINGS) \ + $(ARCHOPTIMIZATION) \ + $(ARCHCPUFLAGS) \ + $(ARCHINCLUDES) \ + $(INSTRUMENTATIONDEFINES) \ + $(ARCHDEFINES) \ + $(EXTRADEFINES) \ + $(EXTRACFLAGS) \ + -fno-common \ + $(addprefix -I,$(INCLUDE_DIRS)) + +# Flags we pass to the C++ compiler +# +CXXFLAGS = $(ARCHCXXFLAGS) \ + $(ARCHWARNINGSXX) \ + $(ARCHOPTIMIZATION) \ + $(ARCHCPUFLAGS) \ + $(ARCHXXINCLUDES) \ + $(INSTRUMENTATIONDEFINES) \ + $(ARCHDEFINES) \ + -DCONFIG_WCHAR_BUILTIN \ + $(EXTRADEFINES) \ + $(EXTRACXXFLAGS) \ + -Wno-effc++ \ + $(addprefix -I,$(INCLUDE_DIRS)) + +ifeq ($(USE_GCC),0) +CXXFLAGS += -Wno-deprecated-register \ + -Wno-tautological-constant-out-of-range-compare \ + -Wno-unused-private-field \ + -Wno-unused-const-variable +endif + +# Flags we pass to the assembler +# +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ + $(EXTRADEFINES) \ + $(EXTRAAFLAGS) + +# Flags we pass to the linker +# +LDFLAGS += $(EXTRALDFLAGS) \ + $(addprefix -L,$(LIB_DIRS)) + +# Compiler support library +# +LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) + +# Files that the final link depends on +# +#LINK_DEPS += $(LDSCRIPT) +LINK_DEPS += + +# Files to include to get automated dependencies +# +DEP_INCLUDES = $(subst .o,.d,$(OBJS)) + +# Compile C source $1 to object $2 +# as a side-effect, generate a dependency file +# +define COMPILE + @$(ECHO) "CC: $1" + @$(MKDIR) -p $(dir $2) + $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 +endef + +# Compile C++ source $1 to $2 +# as a side-effect, generate a dependency file +# +define COMPILEXX + @$(ECHO) "CXX: $1" + @$(MKDIR) -p $(dir $2) + @echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 +endef + +# Assemble $1 into $2 +# +define ASSEMBLE + @$(ECHO) "AS: $1" + @$(MKDIR) -p $(dir $2) + $(Q) $(CC) -c $(AFLAGS) $(abspath $1) -o $2 +endef + +# Produce partially-linked $1 from files in $2 +# +define PRELINK + @$(ECHO) "PRELINK: $1" + @$(MKDIR) -p $(dir $1) + $(Q) $(LD) -Ur -o $1 $2 + +endef +# $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + +# Update the archive $1 with the files in $2 +# +define ARCHIVE + @$(ECHO) "AR: $2" + @$(MKDIR) -p $(dir $1) + $(Q) $(AR) $1 $2 +endef + +# Link the objects in $2 into the shared library $1 +# +define LINK_A + @$(ECHO) "LINK_A: $1" + @$(MKDIR) -p $(dir $1) + echo "$(Q) $(AR) $1 $2" + $(Q) $(AR) $1 $2 +endef + +# Link the objects in $2 into the shared library $1 +# +define LINK_SO + @$(ECHO) "LINK_SO: $1" + @$(MKDIR) -p $(dir $1) + echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)" + $(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc +endef + +# Link the objects in $2 into the application $1 +# +define LINK + @$(ECHO) "LINK: $1" + @$(MKDIR) -p $(dir $1) + echo "$(Q) $(CXX) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS)" + $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) +endef + From 520459062d567fe3f1725b602be37771bebaed20 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 10 Mar 2015 17:36:20 -0700 Subject: [PATCH 002/258] Initial Linux support including execution shell Uncomment the following line in setup.mk and comment out the line above to enable the Linux build. export PX4_TARGET_OS = linux The build uses the clang compiler by default. The final bundled executable is mainapp located in: Build/linux_default.build/mainapp When you run mainapp it will provide a list of the built-in apps. You can type in the commands to run such as: hello_main start Because the Linux build is threaded and does not support tasks or processes, it cannot call errx, exit() _exit(), etc. It also requires unique scoped variables to test if a thread is running or if an application should exit. The px4::AppMgr class was added in px4_app.h for this purpose. The hello sample app demonstrates how this is used. Signed-off-by: Mark Charlebois --- Tools/linux_apps.py | 84 ++++++++ makefiles/linux/config_linux_default.mk | 35 ++-- makefiles/module.mk | 1 + makefiles/setup.mk | 4 +- src/platforms/linux/hello/hello_example.cpp | 61 ++++++ src/platforms/linux/hello/hello_example.h | 53 +++++ src/platforms/linux/hello/hello_main.cpp | 55 ++++++ .../linux/hello/hello_start_linux.cpp | 100 ++++++++++ src/platforms/linux/hello/module.mk | 43 ++++ .../linux/include/arch/board/board.h | 0 src/platforms/linux/include/board_config.h | 1 + src/platforms/linux/main.cpp | 98 +++++++++ src/platforms/linux/px4_layer/module.mk | 42 ++++ .../linux/px4_layer/px4_linux_impl.cpp | 51 +++++ .../linux/px4_layer/px4_linux_tasks.c | 186 ++++++++++++++++++ src/platforms/nuttx/px4_layer/module.mk | 40 ++++ .../nuttx/px4_layer/px4_nuttx_tasks.c | 118 +++++++++++ src/platforms/px4_app.h | 83 ++++++++ src/platforms/px4_includes.h | 34 +++- src/platforms/px4_middleware.h | 6 +- src/platforms/px4_posix.h | 76 +++++++ src/platforms/px4_tasks.h | 82 ++++++++ 22 files changed, 1229 insertions(+), 24 deletions(-) create mode 100755 Tools/linux_apps.py create mode 100644 src/platforms/linux/hello/hello_example.cpp create mode 100644 src/platforms/linux/hello/hello_example.h create mode 100644 src/platforms/linux/hello/hello_main.cpp create mode 100644 src/platforms/linux/hello/hello_start_linux.cpp create mode 100644 src/platforms/linux/hello/module.mk create mode 100644 src/platforms/linux/include/arch/board/board.h create mode 100644 src/platforms/linux/include/board_config.h create mode 100644 src/platforms/linux/main.cpp create mode 100644 src/platforms/linux/px4_layer/module.mk create mode 100644 src/platforms/linux/px4_layer/px4_linux_impl.cpp create mode 100644 src/platforms/linux/px4_layer/px4_linux_tasks.c create mode 100644 src/platforms/nuttx/px4_layer/module.mk create mode 100644 src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c create mode 100644 src/platforms/px4_app.h create mode 100644 src/platforms/px4_posix.h create mode 100644 src/platforms/px4_tasks.h diff --git a/Tools/linux_apps.py b/Tools/linux_apps.py new file mode 100755 index 0000000000..9a380efd01 --- /dev/null +++ b/Tools/linux_apps.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2015 Mark Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +import glob +#builtins = glob.glob("../Build/linux_default.build/builtin_commands/COMMAND*") +builtins = glob.glob("builtin_commands/COMMAND*") + +apps = [] +for f in builtins: + apps.append(f.split(".")[-1]) + +print +print """ +#include +#include +using namespace std; + +extern "C" { +""" +for app in apps: + print "extern int "+app+"(int argc, char *argv[]);" + +print """ +static int list_builtins(int argc, char *argv[]); +} + + +static map app_map(void); + +static map app_map(void) +{ + map apps; +""" +for app in apps: + print '\tapps["'+app+'"] = '+app+';' + +print '\tapps["list_builtins"] = list_builtins;' +print """ + return apps; +} + +map apps = app_map(); + +static int list_builtins(int argc, char *argv[]) +{ + cout << "Builtin Commands:" << endl; + for (map::iterator it=apps.begin(); it!=apps.end(); ++it) + cout << '\t' << it->first << endl; + + return 0; +} +""" + diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 57721c0db4..fc5d7639c7 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -5,18 +5,13 @@ # # Use the configuration's ROMFS. # -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +#ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common # # Board support modules # -#MODULES += drivers/dxl -#MODULES += drivers/i2c_pwm -#MODULES += drivers/lemonrx -#MODULES += drivers/mpu9x50 -#MODULES += drivers/um7 -MODULES += drivers/device -MODULES += modules/sensors +#MODULES += drivers/device +#MODULES += modules/sensors # # System commands @@ -27,38 +22,38 @@ MODULES += modules/sensors # General system control # #MODULES += modules/mavlink -MODULES += modules/mavlink +#MODULES += modules/mavlink # # Estimation modules (EKF/ SO3 / other filters) # -MODULES += modules/attitude_estimator_ekf +#MODULES += modules/attitude_estimator_ekf # # Vehicle Control # -MODULES += modules/mc_att_control +#MODULES += modules/mc_att_control # # Library modules # -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/uORB -MODULES += modules/dataman +#MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +#MODULES += modules/uORB +#MODULES += modules/dataman # # Libraries # -MODULES += lib/mathlib -MODULES += lib/geo -MODULES += lib/geo_lookup -MODULES += lib/conversion +#MODULES += lib/mathlib +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion #MODULES += lib/utils # # Linux port # MODULES += platforms/linux/px4_layer -MODULES += platforms/linux/publisher +MODULES += platforms/linux/hello diff --git a/makefiles/module.mk b/makefiles/module.mk index 9c1a828cc2..3c9ca35026 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -144,6 +144,7 @@ MODULE_ENTRYPOINT ?= $(MODULE_COMMAND)_main MODULE_STACKSIZE ?= CONFIG_PTHREAD_STACK_DEFAULT MODULE_PRIORITY ?= SCHED_PRIORITY_DEFAULT MODULE_COMMANDS += $(MODULE_COMMAND).$(MODULE_PRIORITY).$(MODULE_STACKSIZE).$(MODULE_ENTRYPOINT) +CXXFLAGS += -DPX4_MAIN=$(MODULE_COMMAND)_app_main endif ifneq ($(MODULE_COMMANDS),) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 1c6e5c3a2e..fd4cbb1a77 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,8 +33,8 @@ # Path and tool setup # -export PX4_TARGET_OS = nuttx -#export PX4_TARGET_OS = linux +#export PX4_TARGET_OS = nuttx +export PX4_TARGET_OS = linux # # Some useful paths. diff --git a/src/platforms/linux/hello/hello_example.cpp b/src/platforms/linux/hello/hello_example.cpp new file mode 100644 index 0000000000..7881b73d6e --- /dev/null +++ b/src/platforms/linux/hello/hello_example.cpp @@ -0,0 +1,61 @@ + +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "hello_example.h" +#include +#include + +px4::AppMgr HelloExample::mgr; + +int HelloExample::main() +{ + mgr.setRunning(true); + + int i=0; + while (!mgr.exitRequested() && i<5) { + sleep(2); + + printf(" Doing work...\n"); + ++i; + } + + return 0; +} diff --git a/src/platforms/linux/hello/hello_example.h b/src/platforms/linux/hello/hello_example.h new file mode 100644 index 0000000000..b736f60bcb --- /dev/null +++ b/src/platforms/linux/hello/hello_example.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include + +class HelloExample { +public: + HelloExample() {}; + + ~HelloExample() {}; + + int main(); + + static px4::AppMgr mgr; /* Manage requests to terminate app */ +}; diff --git a/src/platforms/linux/hello/hello_main.cpp b/src/platforms/linux/hello/hello_main.cpp new file mode 100644 index 0000000000..69e8c21ec0 --- /dev/null +++ b/src/platforms/linux/hello/hello_main.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include "hello_example.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "hello"); + + printf("hello\n"); + HelloExample hello; + hello.main(); + + printf("goodbye\n"); + return 0; +} diff --git a/src/platforms/linux/hello/hello_start_linux.cpp b/src/platforms/linux/hello/hello_start_linux.cpp new file mode 100644 index 0000000000..0275662610 --- /dev/null +++ b/src/platforms/linux/hello/hello_start_linux.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "hello_example.h" +#include +#include +#include +#include +#include + +#define SCHED_DEFAULT SCHED_FIFO +#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int hello_main(int argc, char *argv[]); +int hello_main(int argc, char *argv[]) +{ + + if (argc < 2) { + printf("usage: hello {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (HelloExample::mgr.isRunning()) { + printf("already running\n"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("hello", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + HelloExample::mgr.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (HelloExample::mgr.isRunning()) { + printf("is running\n"); + + } else { + printf("not started\n"); + } + + return 0; + } + + printf("usage: hello {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/linux/hello/module.mk b/src/platforms/linux/hello/module.mk new file mode 100644 index 0000000000..730f9189e7 --- /dev/null +++ b/src/platforms/linux/hello/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = hello + +SRCS = hello_main.cpp \ + hello_start_linux.cpp \ + hello_example.cpp + diff --git a/src/platforms/linux/include/arch/board/board.h b/src/platforms/linux/include/arch/board/board.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/platforms/linux/include/board_config.h b/src/platforms/linux/include/board_config.h new file mode 100644 index 0000000000..7bd465647c --- /dev/null +++ b/src/platforms/linux/include/board_config.h @@ -0,0 +1 @@ +#define UDID_START 0x1FFF7A10 diff --git a/src/platforms/linux/main.cpp b/src/platforms/linux/main.cpp new file mode 100644 index 0000000000..a0b2104150 --- /dev/null +++ b/src/platforms/linux/main.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ +/** + * @file main.cpp + * Basic shell to execute builtin "apps" + * + * @author Mark Charlebois + */ + +#include +#include +#include +#include + +using namespace std; + +typedef int (*px4_main_t)(int argc, char *argv[]); + +#include "apps.h" + +// FIXME - the code below only passes 1 arg for now +void run_cmd(const string &command, const string &apparg); +void run_cmd(const string &command, const string &apparg) { + const char *arg[3]; + + if (apps.find(command) != apps.end()) { + arg[0] = (char *)command.c_str(); + arg[1] = (char *)apparg.c_str(); + arg[2] = (char *)0; + apps[command](2,(char **)arg); + } + else + { + cout << "Invalid command" << endl; + } +} + +int main(int argc, char **argv) +{ + string mystr; + string command; + string apparg; + + // Execute a command list of provided + if (argc == 2) { + ifstream infile(argv[1]); + + //vector tokens; + + for (string line; getline(infile, line, '\n'); ) { + stringstream(line) >> command >> apparg; + cout << "Command " << command << ", apparg " << apparg << endl; + run_cmd(command, apparg); + } + } + + while(1) { + run_cmd("list_builtins", ""); + + cout << "Enter a command and its args:" << endl; + getline (cin,mystr); + stringstream(mystr) >> command >> apparg; + run_cmd(command, apparg); + mystr = ""; + command = ""; + apparg = ""; + } +} diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk new file mode 100644 index 0000000000..da247b8dd7 --- /dev/null +++ b/src/platforms/linux/px4_layer/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# NuttX / uORB adapter library +# + +SRCS = \ + px4_linux_impl.cpp \ + px4_linux_tasks.c + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp new file mode 100644 index 0000000000..c5ce678094 --- /dev/null +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_linux_impl.cpp + * + * PX4 Middleware Wrapper Linux Implementation + */ + +#include +#include + +namespace px4 +{ + +void init(int argc, char *argv[], const char *app_name) +{ + printf("App name: %s\n", app_name); +} + +} diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c new file mode 100644 index 0000000000..a00d39b5bf --- /dev/null +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Author: @author Mark Charlebois + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_linux_tasks.c + * Implementation of existing task API for Linux + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +//#include + +#include + +#define MAX_CMD_LEN 100 + +#define PX4_MAX_TASKS 100 +static pthread_t taskmap[PX4_MAX_TASKS] = {}; + +typedef struct +{ + px4_main_t entry; + int argc; + char *argv[]; + // strings are allocated after the +} pthdata_t; + +void entry_adapter ( void *ptr ); +void entry_adapter ( void *ptr ) +{ + pthdata_t *data; + data = (pthdata_t *) ptr; + + data->entry(data->argc, data->argv); + free(ptr); + pthread_exit(0); +} + +void +px4_systemreset(bool to_bootloader) +{ + printf("Called px4_system_reset\n"); +} + +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +{ + int rv = 0; + int argc = 0; + int i; + unsigned int len = 0; + unsigned long offset; + unsigned long structsize; + char * p = (char *)argv; + + px4_task_t task; + + // Calculate argc + while (p != (char *)0) { + p = argv[argc]; + if (p == (char *)0) + break; + ++argc; + len += strlen(p)+1; + } + structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); + pthdata_t *taskdata; + + // not safe to pass stack data to the thread creation + taskdata = (pthdata_t *)malloc(structsize+len); + offset = ((unsigned long)taskdata)+structsize; + + taskdata->entry = entry; + taskdata->argc = argc; + + for (i=0; iargv[i] = (char *)offset; + strcpy((char *)offset, argv[i]); + offset+=strlen(argv[i])+1; + } + // Must add NULL at end of argv + taskdata->argv[argc] = (char *)0; + + //printf("Called px4_task_spawn_cmd\n"); + // FIXME - add handling for scheduler and priority + rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata); + //printf("pthread_create task=%d rv=%d\n",(int)task, rv); + + for (i=0; i=PX4_MAX_TASKS) { + return -ENOSPC; + } + return i; +} + +int px4_task_delete(px4_task_t id) +{ + int rv = 0; + int i; + pthread_t pid; + //printf("Called px4_task_delete\n"); + + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) { + return -EINVAL; + } + // If current thread then exit, otherwise cancel + if (pthread_self() == pid) { + pthread_exit(0); + } else { + rv = pthread_cancel(pid); + } + + taskmap[id] = 0; + + return rv; +} + +void px4_killall(void) +{ + //printf("Called px4_killall\n"); + for (int i=0; i + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_nuttx_tasks.c + * Implementation of existing task API for NuttX + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +// Didn't seem right to include up_internal.h, so direct extern instead. +extern void up_systemreset(void) noreturn_function; + +void +px4_systemreset(bool to_bootloader) +{ + if (to_bootloader) { + stm32_pwr_enablebkp(); + + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xb007b007; + } + up_systemreset(); + + /* lock up here */ + while(true); +} + +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); + +void px4_killall() +{ +// printf("Sending SIGUSR1 to all processes now\n"); + + /* iterate through all tasks and send kill signal */ + sched_foreach(kill_task, NULL); +} + +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) +{ + kill(tcb->pid, SIGUSR1); +} + +int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) +{ + int pid; + + sched_lock(); + + /* create the task */ + pid = task_create(name, priority, stack_size, entry, argv); + + if (pid > 0) { + + /* configure the scheduler */ + struct sched_param param; + + param.sched_priority = priority; + sched_setscheduler(pid, scheduler, ¶m); + + /* XXX do any other private task accounting here before the task starts */ + } + + sched_unlock(); + + return pid; +} + +int px4_task_delete(int pid) +{ + return task_delete(pid); +} diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h new file mode 100644 index 0000000000..1314397b5d --- /dev/null +++ b/src/platforms/px4_app.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#pragma once + +namespace px4 { + +class AppMgr { +public: + ~AppMgr() {} + +#if defined(__PX4_ROS) + AppMgr() {} + + bool exitRequested() { return !ros::ok(); } + void requestExit() { ros::shutdown(); } +#else + AppMgr() : _exitRequested(false), _isRunning(false) {} + + bool exitRequested() { return _exitRequested; } + void requestExit() { _exitRequested = true; } + + bool isRunning() { return _isRunning; } + void setRunning(bool running) { _isRunning = running; } + +protected: + bool _exitRequested; + bool _isRunning; +#endif +private: + AppMgr(const AppMgr&); + const AppMgr& operator=(const AppMgr&); +}; +} + +// Task/process based build +#if defined(__PX4_ROS) || defined(__PX4_NUTTX) + +#define PX4_MAIN main + +// Thread based build +#else + +// The name passed must be globally unique +// set PX4_APPMAIN in module.mk +// EXTRADEFINES += -DPX4_MAIN=foo_app +#ifdef PX4_MAIN + +extern int PX4_MAIN(int argc, char *argv[]); +#endif + +#endif + diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 0e98783fda..8af93b2b2a 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -70,7 +70,7 @@ #include #endif -#else +#elif defined(__PX4_NUTTX) /* * Building for NuttX */ @@ -102,4 +102,36 @@ #include #include +#elif defined(__PX4_LINUX) +#include +#include +#include + +#define ASSERT(x) assert(x) + +#ifdef __cplusplus +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif +#include +#include +#include +#else +#define "No target platform defined" #endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index 735d34234a..d86ddd6645 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -54,12 +54,16 @@ __EXPORT uint64_t get_time_micros(); * Returns true if the app/task should continue to run */ inline bool ok() { return ros::ok(); } -#else +#elif defined(__PX4_NUTTX) extern bool task_should_exit; /** * Returns true if the app/task should continue to run */ __EXPORT inline bool ok() { return !task_should_exit; } +#else +/** + * Linux needs to have globally unique checks for thread/task status + */ #endif class Rate diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h new file mode 100644 index 0000000000..d08c8d2380 --- /dev/null +++ b/src/platforms/px4_posix.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_posix.h + * + * Includes POSIX-like functions for virtual character devices + */ + +#pragma once + +#include +#include +#include + + +#define PX4_F_RDONLY 1 +#define PX4_F_WRONLY 2 + +__BEGIN_DECLS + +extern int px4_errno; + +#ifndef __PX4_NUTTX +typedef short pollevent_t; +#endif + +typedef struct { + /* This part of the struct is POSIX-like */ + int fd; /* The descriptor being polled */ + pollevent_t events; /* The input event flags */ + pollevent_t revents; /* The output event flags */ + + /* Required for PX4 compatability */ + sem_t *sem; /* Pointer to semaphore used to post output event */ + void *priv; /* For use by drivers */ +} px4_pollfd_struct_t; + +__EXPORT int px4_open(const char *path, int flags); +__EXPORT int px4_close(int fd); +__EXPORT ssize_t px4_read(int fd, void *buffer, size_t buflen); +__EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); +__EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); +__EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); + +__END_DECLS diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h new file mode 100644 index 0000000000..ac75126c44 --- /dev/null +++ b/src/platforms/px4_tasks.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * Author: Mark Charlebois 2015 + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_tasks.h + * Preserve existing task API call signature with OS abstraction + */ + +#pragma once + +#include + +#ifdef __PX4_ROS +#error "PX4 tasks not supported in ROS" +#elif defined(__PX4_NUTTX) +typedef int px4_task_t; +#elif defined(__PX4_LINUX) +#include +typedef pthread_t px4_task_t; + +typedef struct { + int argc; + char **argv; +} px4_task_args_t; +#else +#error "No target OS defined" +#endif + +typedef int (*px4_main_t)(int argc, char *argv[]); + +__BEGIN_DECLS + +/** Reboots the board */ +__EXPORT void px4_systemreset(bool to_bootloader) noreturn_function; + +/** Sends SIGUSR1 to all processes */ +__EXPORT void px4_killall(void); + +/** Starts a task and performs any specific accounting, scheduler setup, etc. */ +__EXPORT px4_task_t px4_task_spawn_cmd(const char *name, + int priority, + int scheduler, + int stack_size, + px4_main_t entry, + char * const argv[]); + +__EXPORT int px4_task_delete(px4_task_t pid); + +__END_DECLS + From 51a71d54c6fd55c81c92b75fbd59c3801e010600 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 10:56:06 -0700 Subject: [PATCH 003/258] checksubmodules target needed for Linux build Moved checksubmodules target back to Makefile. NuttX download still done for Linux as it would require too much surgery to remove it. Signed-off-by: Mark Charlebois --- Makefile | 15 +++++++++------ makefiles/firmware_nuttx.mk | 9 --------- 2 files changed, 9 insertions(+), 15 deletions(-) diff --git a/Makefile b/Makefile index 65eabf9832..a10e5f2b33 100644 --- a/Makefile +++ b/Makefile @@ -114,14 +114,17 @@ TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src -ifeq ($(PX4_TARGET_OS),nuttx) -OS_DEPS = checksubmodules -else -OS_DEPS = -endif +.PHONY: checksubmodules +checksubmodules: + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) + +.PHONY: updatesubmodules +updatesubmodules: + $(Q) (git submodule init) + $(Q) (git submodule update) .PHONY: generateuorbtopicheaders -generateuorbtopicheaders: $(OS_DEPS) +generateuorbtopicheaders: checksubmodules @$(ECHO) "Generating uORB topic headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH):$(PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ diff --git a/makefiles/firmware_nuttx.mk b/makefiles/firmware_nuttx.mk index fb97ca4217..77bb2bace0 100644 --- a/makefiles/firmware_nuttx.mk +++ b/makefiles/firmware_nuttx.mk @@ -149,12 +149,3 @@ $(NUTTX_SRC): checksubmodules $(UAVCAN_DIR): $(Q) (./Tools/check_submodules.sh) -.PHONY: checksubmodules -checksubmodules: - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) - -.PHONY: updatesubmodules -updatesubmodules: - $(Q) (git submodule init) - $(Q) (git submodule update) - From ddb32742ebd299e83e268c7ffa99c808bdaf4f8a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 11:10:53 -0700 Subject: [PATCH 004/258] Use OS independent API for task creation/deletion Calls to task_delete and task_spawn_cmd are now px4_task_delete and px4_task_spawn_cmd respectively. The px4_tasks.h header was added to the affected files and incusions of nuttx/config.h were removed. Signed-off-by: Mark Charlebois --- src/drivers/gps/gps.cpp | 6 +++--- src/drivers/hil/hil.cpp | 6 +++--- src/drivers/hott/hott_sensors/hott_sensors.cpp | 4 ++-- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 4 ++-- src/drivers/md25/md25_main.cpp | 4 ++-- src/drivers/mkblctrl/mkblctrl.cpp | 6 +++--- src/drivers/px4fmu/fmu.cpp | 6 +++--- src/drivers/px4io/px4io.cpp | 6 +++--- src/drivers/roboclaw/roboclaw_main.cpp | 4 ++-- src/examples/publisher/publisher_start_nuttx.cpp | 13 +++++++------ src/examples/rover_steering_control/main.cpp | 6 +++--- src/examples/subscriber/subscriber_start_nuttx.cpp | 13 +++++++------ .../attitude_estimator_ekf_main.cpp | 6 +++--- .../attitude_estimator_so3_main.cpp | 6 +++--- src/modules/bottle_drop/bottle_drop.cpp | 6 +++--- src/modules/commander/commander.cpp | 4 ++-- .../ekf_att_pos_estimator_main.cpp | 6 +++--- .../fixedwing_backside/fixedwing_backside_main.cpp | 4 ++-- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++--- src/modules/land_detector/land_detector_main.cpp | 5 +++-- src/modules/mavlink/mavlink_main.cpp | 6 +++--- src/modules/mc_att_control/mc_att_control_main.cpp | 6 +++--- .../mc_att_control_start_nuttx.cpp | 3 ++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 +++-- .../mc_pos_control_start_nuttx.cpp | 3 ++- src/modules/navigator/navigator_main.cpp | 6 +++--- src/modules/segway/segway_main.cpp | 4 ++-- src/modules/sensors/sensors.cpp | 6 +++--- src/modules/uORB/uORB.cpp | 4 ++-- src/modules/uavcan/uavcan_main.cpp | 6 +++--- .../vtol_att_control/vtol_att_control_main.cpp | 6 +++--- 32 files changed, 94 insertions(+), 88 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 714c80ded2..b727c55404 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -213,7 +213,7 @@ GPS::~GPS() /* well, kill it anyway, though this will probably crash */ if (_task != -1) - task_delete(_task); + px4_task_delete(_task); g_dev = nullptr; @@ -229,7 +229,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_spawn_cmd("gps", SCHED_DEFAULT, + _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 961ec47246..a82dec8ac7 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -48,7 +48,7 @@ * driver. Use instead the normal FMU or IO driver. */ -#include +#include #include #include @@ -188,7 +188,7 @@ HIL::~HIL() /* if we have given up, kill it */ if (--i == 0) { - task_delete(_task); + px4_task_delete(_task); break; } @@ -228,7 +228,7 @@ HIL::init() // gpio_reset(); /* start the HIL interface task */ - _task = task_spawn_cmd("fmuhil", + _task = px4_task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1200, diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 4ac7e4bdfb..e5bbc41d13 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -42,7 +42,7 @@ */ #include -#include +#include #include #include #include @@ -209,7 +209,7 @@ hott_sensors_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd(daemon_name, + deamon_task = px4_task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 43df0cb8cc..841f3ed1a8 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -44,7 +44,7 @@ */ #include -#include +#include #include #include #include @@ -235,7 +235,7 @@ hott_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd(daemon_name, + deamon_task = px4_task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index d25d16fa15..7bcc1c9d68 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -44,7 +44,7 @@ * */ -#include +#include #include #include #include @@ -110,7 +110,7 @@ int md25_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("md25", + deamon_task = px4_task_spawn_cmd("md25", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3b8c4ee777..bf017103cf 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -41,7 +41,7 @@ * */ -#include +#include #include #include @@ -258,7 +258,7 @@ MK::~MK() /* if we have given up, kill it */ if (--i == 0) { - task_delete(_task); + px4_task_delete(_task); break; } @@ -302,7 +302,7 @@ MK::init(unsigned motors) } /* start the IO interface task */ - _task = task_spawn_cmd("mkblctrl", + _task = px4_task_spawn_cmd("mkblctrl", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 1500, diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 92afc7cd74..f36a07b04a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -37,7 +37,7 @@ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. */ -#include +#include #include #include @@ -302,7 +302,7 @@ PX4FMU::~PX4FMU() /* if we have given up, kill it */ if (--i == 0) { - task_delete(_task); + px4_task_delete(_task); break; } @@ -341,7 +341,7 @@ PX4FMU::init() gpio_reset(); /* start the IO interface task */ - _task = task_spawn_cmd("fmuservo", + _task = px4_task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1600, diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 33125699f5..9fe69113ba 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -38,7 +38,7 @@ * PX4IO is connected via I2C or DMA enabled high-speed UART. */ -#include +#include #include #include @@ -550,7 +550,7 @@ PX4IO::~PX4IO() /* well, kill it anyway, though this will probably crash */ if (_task != -1) - task_delete(_task); + px4_task_delete(_task); if (_interface != nullptr) delete _interface; @@ -841,7 +841,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_spawn_cmd("px4io", + _task = px4_task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1800, diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index ea7178076b..198e2f0c15 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -43,7 +43,7 @@ * */ -#include +#include #include #include #include @@ -105,7 +105,7 @@ int roboclaw_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("roboclaw", + deamon_task = px4_task_spawn_cmd("roboclaw", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index 4ff2cebfbc..2f12e298b2 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -36,6 +36,7 @@ * * @author Thomas Gubler */ +#include #include #include #include @@ -68,12 +69,12 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + daemon_task = px4_task_spawn_cmd("publisher", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); exit(0); } diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 41df96775c..6fc4af4f4b 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -39,7 +39,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include @@ -408,7 +408,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int rover_steering_control_main(int argc, char *argv[]) { @@ -425,7 +425,7 @@ int rover_steering_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("rover_steering_control", + deamon_task = px4_task_spawn_cmd("rover_steering_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 757e8ec521..091a15d2aa 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -36,6 +36,7 @@ * * @author Thomas Gubler */ +#include #include #include #include @@ -68,12 +69,12 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + daemon_task = px4_task_spawn_cmd("subscriber", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); exit(0); } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index b2a3572a7d..87a1b48762 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -41,7 +41,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include @@ -114,7 +114,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int attitude_estimator_ekf_main(int argc, char *argv[]) { @@ -131,7 +131,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", + attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 7700, diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 82bcbc66ff..448f1c3913 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -50,7 +50,7 @@ * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 */ -#include +#include #include #include #include @@ -132,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { @@ -149,7 +149,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", + attitude_estimator_so3_task = px4_task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index b267209fe3..3681bdb515 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -40,7 +40,7 @@ * @author Julian Oes */ -#include +#include #include #include #include @@ -210,7 +210,7 @@ BottleDrop::~BottleDrop() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_main_task); + px4_task_delete(_main_task); break; } } while (_main_task != -1); @@ -225,7 +225,7 @@ BottleDrop::start() ASSERT(_main_task == -1); /* start the task */ - _main_task = task_spawn_cmd("bottle_drop", + _main_task = px4_task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, 1500, diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb1ed7f5df..bc118538b2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -42,7 +42,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include @@ -275,7 +275,7 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", + daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 3400, diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e0b4cb2a0d..db817e2bce 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -43,7 +43,7 @@ #include "AttitudePositionEstimatorEKF.h" #include "estimator_22states.h" -#include +#include #include #include #include @@ -281,7 +281,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_estimator_task); + px4_task_delete(_estimator_task); break; } } while (_estimator_task != -1); @@ -1035,7 +1035,7 @@ int AttitudePositionEstimatorEKF::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", + _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 7500, diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index bd14863247..4c0a7d36d3 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -39,7 +39,7 @@ * Fixedwing backside controller using control library */ -#include +#include #include #include #include @@ -111,7 +111,7 @@ int fixedwing_backside_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_backside", + deamon_task = px4_task_spawn_cmd("fixedwing_backside", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0a8d07fed9..43124db895 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -41,7 +41,7 @@ * */ -#include +#include #include #include #include @@ -424,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -1117,7 +1117,7 @@ FixedwingAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("fw_att_control", + _control_task = px4_task_spawn_cmd("fw_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 34265d6a4e..76093a5f93 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -51,7 +51,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include @@ -542,7 +542,7 @@ FixedwingPositionControl::~FixedwingPositionControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -1621,7 +1621,7 @@ FixedwingPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("fw_pos_control_l1", + _control_task = px4_task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index b4b7c33a20..e0402419ff 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -38,6 +38,7 @@ * @author Johan Jansen */ +#include //usleep #include //usleep #include #include @@ -95,7 +96,7 @@ static void land_detector_stop() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_landDetectorTaskID); + px4_task_delete(_landDetectorTaskID); break; } } while (land_detector_task->isRunning()); @@ -136,7 +137,7 @@ static int land_detector_start(const char *mode) } //Start new thread task - _landDetectorTaskID = task_spawn_cmd("land_detector", + _landDetectorTaskID = px4_task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1000, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ba049bac40..4956bbb9db 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -39,7 +39,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include @@ -242,7 +242,7 @@ Mavlink::~Mavlink() /* if we have given up, kill it */ if (++i > 50) { //TODO store main task handle in Mavlink instance to allow killing task - //task_delete(_mavlink_task); + //px4_task_delete(_mavlink_task); break; } } while (_task_running); @@ -1618,7 +1618,7 @@ Mavlink::start(int argc, char *argv[]) // between the starting task and the spawned // task - start_helper() only returns // when the started task exits. - task_spawn_cmd(buf, + px4_task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2400, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3a0dfe4c3b..8898433c84 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,7 +53,7 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include +#include #include #include #include @@ -404,7 +404,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -905,7 +905,7 @@ MulticopterAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("mc_att_control", + _control_task = px4_task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index dec1e1745d..6d16e0155d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -36,6 +36,7 @@ * * @author Thomas Gubler */ +#include #include #include #include @@ -68,7 +69,7 @@ int mc_att_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("mc_att_control_m", + daemon_task = px4_task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1900, diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5191a4de3b..867521cc27 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -50,6 +50,7 @@ */ #include +#include #include #include #include @@ -384,7 +385,7 @@ MulticopterPositionControl::~MulticopterPositionControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -1423,7 +1424,7 @@ MulticopterPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("mc_pos_control", + _control_task = px4_task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 0db67d83ff..bd67366cf6 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -36,6 +36,7 @@ * * @author Thomas Gubler */ +#include #include #include #include @@ -68,7 +69,7 @@ int mc_pos_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("mc_pos_control_m", + daemon_task = px4_task_spawn_cmd("mc_pos_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9691e26a8d..dd07892654 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -43,7 +43,7 @@ * @author Thomas Gubler */ -#include +#include #include #include @@ -174,7 +174,7 @@ Navigator::~Navigator() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_navigator_task); + px4_task_delete(_navigator_task); break; } } while (_navigator_task != -1); @@ -515,7 +515,7 @@ Navigator::start() ASSERT(_navigator_task == -1); /* start the task */ - _navigator_task = task_spawn_cmd("navigator", + _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, 1800, diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index b36d56d6d7..7a4c4bb6f6 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -39,7 +39,7 @@ * Segway controller using control library */ -#include +#include #include #include #include @@ -106,7 +106,7 @@ int segway_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = task_spawn_cmd("segway", + deamon_task = px4_task_spawn_cmd("segway", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fc8627c15..7519153805 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -46,7 +46,7 @@ * @author Anton Babushkin */ -#include +#include #include #include @@ -640,7 +640,7 @@ Sensors::~Sensors() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_sensors_task); + px4_task_delete(_sensors_task); break; } } while (_sensors_task != -1); @@ -2235,7 +2235,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_spawn_cmd("sensors_task", + _sensors_task = px4_task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b4f81d4293..0effddb2d4 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -36,7 +36,7 @@ * A lightweight object broker. */ -#include +#include #include @@ -965,7 +965,7 @@ latency_test(orb_id_t T, bool print) /* test pub / sub latency */ - int pubsub_task = task_spawn_cmd("uorb_latency", + int pubsub_task = px4_task_spawn_cmd("uorb_latency", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 2d5abf9591..21adcea8a4 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include +#include #include #include @@ -109,7 +109,7 @@ UavcanNode::~UavcanNode() /* if we have given up, kill it */ if (--i == 0) { - task_delete(_task); + px4_task_delete(_task); break; } @@ -194,7 +194,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, + _instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index defcff8e4a..20af0978a8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -44,7 +44,7 @@ * */ -#include +#include #include #include #include @@ -298,7 +298,7 @@ VtolAttitudeControl::~VtolAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -859,7 +859,7 @@ VtolAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("vtol_att_control", + _control_task = px4_task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, From bf429188b436e2b00cc614f20db4930b0c2497d4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 11:52:18 -0700 Subject: [PATCH 005/258] Reverted: Use OS independent API for task creation/deletion Keep existing API use in code. Bind the use of the OS independent implementation in the systemlib layer. Signed-off-by: Mark Charlebois --- src/drivers/gps/gps.cpp | 6 +++--- src/drivers/hil/hil.cpp | 6 +++--- src/drivers/hott/hott_sensors/hott_sensors.cpp | 4 ++-- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 4 ++-- src/drivers/md25/md25_main.cpp | 4 ++-- src/drivers/mkblctrl/mkblctrl.cpp | 6 +++--- src/drivers/px4fmu/fmu.cpp | 6 +++--- src/drivers/px4io/px4io.cpp | 6 +++--- src/drivers/roboclaw/roboclaw_main.cpp | 4 ++-- src/examples/publisher/publisher_start_nuttx.cpp | 3 +-- src/examples/rover_steering_control/main.cpp | 6 +++--- src/examples/subscriber/subscriber_start_nuttx.cpp | 3 +-- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 6 +++--- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 6 +++--- src/modules/bottle_drop/bottle_drop.cpp | 6 +++--- src/modules/commander/commander.cpp | 4 ++-- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++--- src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 4 ++-- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++--- src/modules/land_detector/land_detector_main.cpp | 5 ++--- src/modules/mavlink/mavlink_main.cpp | 6 +++--- src/modules/mc_att_control/mc_att_control_main.cpp | 6 +++--- .../mc_att_control_start_nuttx.cpp | 3 +-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 ++--- .../mc_pos_control_start_nuttx.cpp | 3 +-- src/modules/navigator/navigator_main.cpp | 6 +++--- src/modules/segway/segway_main.cpp | 4 ++-- src/modules/sensors/sensors.cpp | 6 +++--- src/modules/uORB/uORB.cpp | 4 ++-- src/modules/uavcan/uavcan_main.cpp | 6 +++--- src/modules/vtol_att_control/vtol_att_control_main.cpp | 6 +++--- 32 files changed, 78 insertions(+), 84 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index b727c55404..714c80ded2 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -213,7 +213,7 @@ GPS::~GPS() /* well, kill it anyway, though this will probably crash */ if (_task != -1) - px4_task_delete(_task); + task_delete(_task); g_dev = nullptr; @@ -229,7 +229,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, + _task = task_spawn_cmd("gps", SCHED_DEFAULT, SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index a82dec8ac7..961ec47246 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -48,7 +48,7 @@ * driver. Use instead the normal FMU or IO driver. */ -#include +#include #include #include @@ -188,7 +188,7 @@ HIL::~HIL() /* if we have given up, kill it */ if (--i == 0) { - px4_task_delete(_task); + task_delete(_task); break; } @@ -228,7 +228,7 @@ HIL::init() // gpio_reset(); /* start the HIL interface task */ - _task = px4_task_spawn_cmd("fmuhil", + _task = task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1200, diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index e5bbc41d13..4ac7e4bdfb 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -42,7 +42,7 @@ */ #include -#include +#include #include #include #include @@ -209,7 +209,7 @@ hott_sensors_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = px4_task_spawn_cmd(daemon_name, + deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 841f3ed1a8..43df0cb8cc 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -44,7 +44,7 @@ */ #include -#include +#include #include #include #include @@ -235,7 +235,7 @@ hott_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = px4_task_spawn_cmd(daemon_name, + deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 7bcc1c9d68..d25d16fa15 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -44,7 +44,7 @@ * */ -#include +#include #include #include #include @@ -110,7 +110,7 @@ int md25_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = px4_task_spawn_cmd("md25", + deamon_task = task_spawn_cmd("md25", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index bf017103cf..3b8c4ee777 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -41,7 +41,7 @@ * */ -#include +#include #include #include @@ -258,7 +258,7 @@ MK::~MK() /* if we have given up, kill it */ if (--i == 0) { - px4_task_delete(_task); + task_delete(_task); break; } @@ -302,7 +302,7 @@ MK::init(unsigned motors) } /* start the IO interface task */ - _task = px4_task_spawn_cmd("mkblctrl", + _task = task_spawn_cmd("mkblctrl", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 1500, diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f36a07b04a..92afc7cd74 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -37,7 +37,7 @@ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. */ -#include +#include #include #include @@ -302,7 +302,7 @@ PX4FMU::~PX4FMU() /* if we have given up, kill it */ if (--i == 0) { - px4_task_delete(_task); + task_delete(_task); break; } @@ -341,7 +341,7 @@ PX4FMU::init() gpio_reset(); /* start the IO interface task */ - _task = px4_task_spawn_cmd("fmuservo", + _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1600, diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9fe69113ba..33125699f5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -38,7 +38,7 @@ * PX4IO is connected via I2C or DMA enabled high-speed UART. */ -#include +#include #include #include @@ -550,7 +550,7 @@ PX4IO::~PX4IO() /* well, kill it anyway, though this will probably crash */ if (_task != -1) - px4_task_delete(_task); + task_delete(_task); if (_interface != nullptr) delete _interface; @@ -841,7 +841,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = px4_task_spawn_cmd("px4io", + _task = task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1800, diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 198e2f0c15..ea7178076b 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -43,7 +43,7 @@ * */ -#include +#include #include #include #include @@ -105,7 +105,7 @@ int roboclaw_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = px4_task_spawn_cmd("roboclaw", + deamon_task = task_spawn_cmd("roboclaw", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index 2f12e298b2..aaf28fdb7f 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -36,7 +36,6 @@ * * @author Thomas Gubler */ -#include #include #include #include @@ -69,7 +68,7 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = px4_task_spawn_cmd("publisher", + daemon_task = task_spawn_cmd("publisher", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 6fc4af4f4b..41df96775c 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -39,7 +39,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include @@ -408,7 +408,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_task_spawn_cmd(). + * to task_spawn_cmd(). */ int rover_steering_control_main(int argc, char *argv[]) { @@ -425,7 +425,7 @@ int rover_steering_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = px4_task_spawn_cmd("rover_steering_control", + deamon_task = task_spawn_cmd("rover_steering_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 091a15d2aa..91543d5d23 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -36,7 +36,6 @@ * * @author Thomas Gubler */ -#include #include #include #include @@ -69,7 +68,7 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = px4_task_spawn_cmd("subscriber", + daemon_task = task_spawn_cmd("subscriber", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 87a1b48762..b2a3572a7d 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -41,7 +41,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include @@ -114,7 +114,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_task_spawn_cmd(). + * to task_spawn_cmd(). */ int attitude_estimator_ekf_main(int argc, char *argv[]) { @@ -131,7 +131,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf", + attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 7700, diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 448f1c3913..82bcbc66ff 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -50,7 +50,7 @@ * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 */ -#include +#include #include #include #include @@ -132,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_task_spawn_cmd(). + * to task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { @@ -149,7 +149,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_task = px4_task_spawn_cmd("attitude_estimator_so3", + attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 3681bdb515..b267209fe3 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -40,7 +40,7 @@ * @author Julian Oes */ -#include +#include #include #include #include @@ -210,7 +210,7 @@ BottleDrop::~BottleDrop() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_main_task); + task_delete(_main_task); break; } } while (_main_task != -1); @@ -225,7 +225,7 @@ BottleDrop::start() ASSERT(_main_task == -1); /* start the task */ - _main_task = px4_task_spawn_cmd("bottle_drop", + _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, 1500, diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc118538b2..bb1ed7f5df 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -42,7 +42,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include @@ -275,7 +275,7 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = px4_task_spawn_cmd("commander", + daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 3400, diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index db817e2bce..e0b4cb2a0d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -43,7 +43,7 @@ #include "AttitudePositionEstimatorEKF.h" #include "estimator_22states.h" -#include +#include #include #include #include @@ -281,7 +281,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_estimator_task); + task_delete(_estimator_task); break; } } while (_estimator_task != -1); @@ -1035,7 +1035,7 @@ int AttitudePositionEstimatorEKF::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 7500, diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4c0a7d36d3..bd14863247 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -39,7 +39,7 @@ * Fixedwing backside controller using control library */ -#include +#include #include #include #include @@ -111,7 +111,7 @@ int fixedwing_backside_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = px4_task_spawn_cmd("fixedwing_backside", + deamon_task = task_spawn_cmd("fixedwing_backside", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 43124db895..0a8d07fed9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -41,7 +41,7 @@ * */ -#include +#include #include #include #include @@ -424,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_control_task); + task_delete(_control_task); break; } } while (_control_task != -1); @@ -1117,7 +1117,7 @@ FixedwingAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = px4_task_spawn_cmd("fw_att_control", + _control_task = task_spawn_cmd("fw_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 76093a5f93..34265d6a4e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -51,7 +51,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include @@ -542,7 +542,7 @@ FixedwingPositionControl::~FixedwingPositionControl() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_control_task); + task_delete(_control_task); break; } } while (_control_task != -1); @@ -1621,7 +1621,7 @@ FixedwingPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = px4_task_spawn_cmd("fw_pos_control_l1", + _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index e0402419ff..b4b7c33a20 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -38,7 +38,6 @@ * @author Johan Jansen */ -#include //usleep #include //usleep #include #include @@ -96,7 +95,7 @@ static void land_detector_stop() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_landDetectorTaskID); + task_delete(_landDetectorTaskID); break; } } while (land_detector_task->isRunning()); @@ -137,7 +136,7 @@ static int land_detector_start(const char *mode) } //Start new thread task - _landDetectorTaskID = px4_task_spawn_cmd("land_detector", + _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1000, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4956bbb9db..ba049bac40 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -39,7 +39,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include @@ -242,7 +242,7 @@ Mavlink::~Mavlink() /* if we have given up, kill it */ if (++i > 50) { //TODO store main task handle in Mavlink instance to allow killing task - //px4_task_delete(_mavlink_task); + //task_delete(_mavlink_task); break; } } while (_task_running); @@ -1618,7 +1618,7 @@ Mavlink::start(int argc, char *argv[]) // between the starting task and the spawned // task - start_helper() only returns // when the started task exits. - px4_task_spawn_cmd(buf, + task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2400, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8898433c84..3a0dfe4c3b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,7 +53,7 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include +#include #include #include #include @@ -404,7 +404,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_control_task); + task_delete(_control_task); break; } } while (_control_task != -1); @@ -905,7 +905,7 @@ MulticopterAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = px4_task_spawn_cmd("mc_att_control", + _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index 6d16e0155d..dec1e1745d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -36,7 +36,6 @@ * * @author Thomas Gubler */ -#include #include #include #include @@ -69,7 +68,7 @@ int mc_att_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = px4_task_spawn_cmd("mc_att_control_m", + daemon_task = task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1900, diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 867521cc27..5191a4de3b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -50,7 +50,6 @@ */ #include -#include #include #include #include @@ -385,7 +384,7 @@ MulticopterPositionControl::~MulticopterPositionControl() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_control_task); + task_delete(_control_task); break; } } while (_control_task != -1); @@ -1424,7 +1423,7 @@ MulticopterPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = px4_task_spawn_cmd("mc_pos_control", + _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index bd67366cf6..0db67d83ff 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -36,7 +36,6 @@ * * @author Thomas Gubler */ -#include #include #include #include @@ -69,7 +68,7 @@ int mc_pos_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = px4_task_spawn_cmd("mc_pos_control_m", + daemon_task = task_spawn_cmd("mc_pos_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd07892654..9691e26a8d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -43,7 +43,7 @@ * @author Thomas Gubler */ -#include +#include #include #include @@ -174,7 +174,7 @@ Navigator::~Navigator() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_navigator_task); + task_delete(_navigator_task); break; } } while (_navigator_task != -1); @@ -515,7 +515,7 @@ Navigator::start() ASSERT(_navigator_task == -1); /* start the task */ - _navigator_task = px4_task_spawn_cmd("navigator", + _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, 1800, diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 7a4c4bb6f6..b36d56d6d7 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -39,7 +39,7 @@ * Segway controller using control library */ -#include +#include #include #include #include @@ -106,7 +106,7 @@ int segway_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = px4_task_spawn_cmd("segway", + deamon_task = task_spawn_cmd("segway", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7519153805..3fc8627c15 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -46,7 +46,7 @@ * @author Anton Babushkin */ -#include +#include #include #include @@ -640,7 +640,7 @@ Sensors::~Sensors() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_sensors_task); + task_delete(_sensors_task); break; } } while (_sensors_task != -1); @@ -2235,7 +2235,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = px4_task_spawn_cmd("sensors_task", + _sensors_task = task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 0effddb2d4..b4f81d4293 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -36,7 +36,7 @@ * A lightweight object broker. */ -#include +#include #include @@ -965,7 +965,7 @@ latency_test(orb_id_t T, bool print) /* test pub / sub latency */ - int pubsub_task = px4_task_spawn_cmd("uorb_latency", + int pubsub_task = task_spawn_cmd("uorb_latency", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 21adcea8a4..2d5abf9591 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include +#include #include #include @@ -109,7 +109,7 @@ UavcanNode::~UavcanNode() /* if we have given up, kill it */ if (--i == 0) { - px4_task_delete(_task); + task_delete(_task); break; } @@ -194,7 +194,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 20af0978a8..defcff8e4a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -44,7 +44,7 @@ * */ -#include +#include #include #include #include @@ -298,7 +298,7 @@ VtolAttitudeControl::~VtolAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - px4_task_delete(_control_task); + task_delete(_control_task); break; } } while (_control_task != -1); @@ -859,7 +859,7 @@ VtolAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = px4_task_spawn_cmd("vtol_att_control", + _control_task = task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, From 82a7fd5115a340c48d9b09cb3bc421e441c1f4a4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 12:07:41 -0700 Subject: [PATCH 006/258] Code uses px4_config.h instead of nuttx/config.h Use OS independent header file for config info. Signed-off-by: Mark Charlebois --- src/platforms/px4_config.h | 45 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 src/platforms/px4_config.h diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h new file mode 100644 index 0000000000..7984432536 --- /dev/null +++ b/src/platforms/px4_config.h @@ -0,0 +1,45 @@ + +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_config.h + * Preserve abiility to load config information that is used in subsequent + * includes or code + */ + +#pragma once + +#if defined(__PX4_NUTTX) +#include +#endif From 9758112e311b1c2746a8d0e85465179a0084f96b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 12:16:44 -0700 Subject: [PATCH 007/258] Use px4_config.h instead of nuttx/config.h Modified code to use OS independent header file for config settings. Signed-off-by: Mark Charlebois --- src/drivers/airspeed/airspeed.cpp | 2 +- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/ardrone_interface/ardrone_interface.c | 2 +- src/drivers/ardrone_interface/ardrone_motor_control.c | 2 +- src/drivers/batt_smbus/batt_smbus.cpp | 2 +- src/drivers/blinkm/blinkm.cpp | 2 +- src/drivers/bma180/bma180.cpp | 2 +- src/drivers/boards/aerocore/aerocore_init.c | 2 +- src/drivers/boards/aerocore/aerocore_led.c | 2 +- src/drivers/boards/aerocore/aerocore_spi.c | 2 +- src/drivers/boards/aerocore/board_config.h | 2 +- src/drivers/boards/px4fmu-v1/board_config.h | 2 +- src/drivers/boards/px4fmu-v1/px4fmu_can.c | 2 +- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 2 +- src/drivers/boards/px4fmu-v1/px4fmu_led.c | 2 +- src/drivers/boards/px4fmu-v1/px4fmu_spi.c | 2 +- src/drivers/boards/px4fmu-v1/px4fmu_usb.c | 2 +- src/drivers/boards/px4fmu-v2/board_config.h | 2 +- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 2 +- src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 2 +- src/drivers/boards/px4fmu-v2/px4fmu_can.c | 2 +- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 2 +- src/drivers/boards/px4fmu-v2/px4fmu_usb.c | 2 +- src/drivers/boards/px4io-v1/board_config.h | 2 +- src/drivers/boards/px4io-v1/px4io_init.c | 2 +- src/drivers/boards/px4io-v2/board_config.h | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 +- src/drivers/device/device.h | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/gimbal/gimbal.cpp | 2 +- src/drivers/gps/gps.cpp | 2 +- src/drivers/hil/hil.cpp | 2 +- src/drivers/hmc5883/hmc5883.cpp | 2 +- src/drivers/hmc5883/hmc5883_i2c.cpp | 2 +- src/drivers/hmc5883/hmc5883_spi.cpp | 2 +- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/led/led.cpp | 2 +- src/drivers/ll40ls/ll40ls.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/mb12xx/mb12xx.cpp | 2 +- src/drivers/md25/md25_main.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/mkblctrl/mkblctrl_params.c | 2 +- src/drivers/mpu6000/mpu6000.cpp | 2 +- src/drivers/ms5611/ms5611.cpp | 2 +- src/drivers/ms5611/ms5611_i2c.cpp | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 2 +- src/drivers/oreoled/oreoled.cpp | 2 +- src/drivers/pca8574/pca8574.cpp | 2 +- src/drivers/pca9685/pca9685.cpp | 2 +- src/drivers/pwm_input/pwm_input.cpp | 2 +- src/drivers/px4flow/px4flow.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 2 +- src/drivers/px4io/px4io_i2c.cpp | 2 +- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/px4io/px4io_uploader.cpp | 2 +- src/drivers/rgbled/rgbled.cpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 2 +- src/drivers/stm32/adc/adc.cpp | 2 +- src/drivers/stm32/drv_hrt.c | 2 +- src/drivers/stm32/drv_pwm_servo.c | 2 +- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 +- src/drivers/trone/trone.cpp | 2 +- src/examples/fixedwing_control/main.c | 2 +- .../flow_position_estimator/flow_position_estimator_main.c | 2 +- src/examples/hwtest/hwtest.c | 2 +- src/examples/matlab_csv_serial/matlab_csv_serial.c | 2 +- src/examples/px4_daemon_app/px4_daemon_app.c | 2 +- src/examples/px4_mavlink_debug/px4_mavlink_debug.c | 2 +- src/examples/px4_simple_app/px4_simple_app.c | 2 +- src/examples/rover_steering_control/main.cpp | 2 +- src/lib/geo/geo.c | 2 +- src/lib/launchdetection/launchdetection_params.c | 2 +- src/lib/mathlib/CMSIS/Include/arm_math.h | 2 +- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 2 +- src/modules/bottle_drop/bottle_drop.cpp | 2 +- src/modules/bottle_drop/bottle_drop_params.c | 2 +- src/modules/commander/commander.cpp | 2 +- src/modules/commander/commander_params.c | 2 +- src/modules/dataman/dataman.c | 2 +- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- .../ekf_att_pos_estimator/ekf_att_pos_estimator_params.c | 2 +- src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/fw_att_control/fw_att_control_params.c | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- src/modules/fw_pos_control_l1/landingslope.cpp | 2 +- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 +- src/modules/mavlink/mavlink.c | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- src/modules/navigator/datalinkloss_params.c | 2 +- src/modules/navigator/geofence.cpp | 2 +- src/modules/navigator/geofence_params.c | 2 +- src/modules/navigator/gpsfailure_params.c | 2 +- src/modules/navigator/mission_params.c | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/navigator_params.c | 2 +- src/modules/navigator/rcloss_params.c | 2 +- src/modules/navigator/rtl_params.c | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- src/modules/px4iofirmware/adc.c | 2 +- src/modules/px4iofirmware/controls.c | 2 +- src/modules/px4iofirmware/dsm.c | 2 +- src/modules/px4iofirmware/mixer.cpp | 2 +- src/modules/px4iofirmware/px4io.c | 2 +- src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/safety.c | 2 +- src/modules/px4iofirmware/sbus.c | 2 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/segway/segway_main.cpp | 2 +- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/conversions.c | 2 +- src/modules/systemlib/cpuload.c | 2 +- src/modules/systemlib/err.c | 2 +- src/modules/systemlib/mcu_version.c | 2 +- src/modules/systemlib/mixer/mixer.cpp | 2 +- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/systemlib/mixer/mixer_group.cpp | 2 +- src/modules/systemlib/mixer/mixer_load.c | 2 +- src/modules/systemlib/mixer/mixer_load.h | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 2 +- src/modules/systemlib/mixer/mixer_simple.cpp | 2 +- src/modules/systemlib/otp.c | 2 +- src/modules/systemlib/ppm_decode.c | 2 +- src/modules/systemlib/rc_check.c | 2 +- src/modules/systemlib/system_params.c | 2 +- src/modules/systemlib/systemlib.c | 2 +- src/modules/systemlib/up_cxxinitialize.c | 2 +- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/uORB.cpp | 2 +- src/modules/uavcan/uavcan_main.cpp | 2 +- src/modules/uavcan/uavcan_main.hpp | 2 +- src/modules/uavcan/uavcan_params.c | 2 +- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c | 2 +- src/systemcmds/bl_update/bl_update.c | 2 +- src/systemcmds/config/config.c | 2 +- src/systemcmds/dumpfile/dumpfile.c | 2 +- src/systemcmds/esc_calib/esc_calib.c | 2 +- src/systemcmds/i2c/i2c.c | 2 +- src/systemcmds/motor_test/motor_test.c | 2 +- src/systemcmds/mtd/24xxxx_mtd.c | 2 +- src/systemcmds/mtd/mtd.c | 2 +- src/systemcmds/nshterm/nshterm.c | 2 +- src/systemcmds/param/param.c | 2 +- src/systemcmds/perf/perf.c | 2 +- src/systemcmds/pwm/pwm.c | 2 +- src/systemcmds/reboot/reboot.c | 2 +- src/systemcmds/reflect/reflect.c | 2 +- src/systemcmds/tests/test_adc.c | 2 +- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_dataman.c | 2 +- src/systemcmds/tests/test_float.c | 2 +- src/systemcmds/tests/test_gpio.c | 2 +- src/systemcmds/tests/test_hott_telemetry.c | 2 +- src/systemcmds/tests/test_hrt.c | 2 +- src/systemcmds/tests/test_int.c | 2 +- src/systemcmds/tests/test_jig_voltages.c | 2 +- src/systemcmds/tests/test_led.c | 2 +- src/systemcmds/tests/test_mixer.cpp | 2 +- src/systemcmds/tests/test_ppm_loopback.c | 2 +- src/systemcmds/tests/test_rc.c | 2 +- src/systemcmds/tests/test_sensors.c | 2 +- src/systemcmds/tests/test_servo.c | 2 +- src/systemcmds/tests/test_sleep.c | 2 +- src/systemcmds/tests/test_time.c | 2 +- src/systemcmds/tests/test_uart_baudchange.c | 2 +- src/systemcmds/tests/test_uart_console.c | 2 +- src/systemcmds/tests/test_uart_loopback.c | 2 +- src/systemcmds/tests/test_uart_send.c | 2 +- src/systemcmds/tests/tests.h | 2 +- src/systemcmds/tests/tests_main.c | 2 +- src/systemcmds/top/top.c | 2 +- src/systemcmds/usb_connected/usb_connected.c | 2 +- 185 files changed, 185 insertions(+), 185 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index d5935dfd46..7a6a58746c 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -38,7 +38,7 @@ * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ -#include +#include #include diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index cdaf244dd1..b440338cf9 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -38,7 +38,7 @@ * Generic driver for airspeed sensors connected via I2C. */ -#include +#include #include diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 53d98ba9a1..cfc80ace22 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -37,7 +37,7 @@ * Implementation of AR.Drone 1.0 / 2.0 motor control interface. */ -#include +#include #include #include #include diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 0f45b65366..46e1f86511 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -37,7 +37,7 @@ * Implementation of AR.Drone 1.0 / 2.0 motor control interface */ -#include +#include #include #include #include diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 6d1b3f6ca5..66f959a904 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -39,7 +39,7 @@ * @author Randy Mackay */ -#include +#include #include #include diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index d749a0d7b4..0b9dfd2051 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -93,7 +93,7 @@ * */ -#include +#include #include #include diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index e43a348052..f4e4622adc 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -36,7 +36,7 @@ * Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI. */ -#include +#include #include #include diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 1ce235da8f..3bdb0d4238 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -45,7 +45,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index 94a716029f..22b3a2c240 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -37,7 +37,7 @@ * AeroCore LED backend. */ -#include +#include #include diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index 0fa474a8c1..5a4dd15b26 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 19518d73d9..1ef2e917dd 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -43,7 +43,7 @@ * Included Files ****************************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 17fa9c542e..37bebcbfd8 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -43,7 +43,7 @@ * Included Files ****************************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c index 1e1f100406..104d6ff237 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_can.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 293021f8b2..85abe0671c 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -45,7 +45,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index ee53fc43d7..888e4f551b 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -37,7 +37,7 @@ * PX4FMU LED backend. */ -#include +#include #include diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index 16f94a9f23..8ef17c36ed 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c index 0fc8569aa4..69f6bd9b2a 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 6188e29ae1..86c026fb04 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -43,7 +43,7 @@ * Included Files ****************************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 9b25c574a8..c95c2a46e5 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -45,7 +45,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 3c05bfa46a..74f77b3ef6 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -37,7 +37,7 @@ * PX4FMU LED backend. */ -#include +#include #include diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c index f66c7cd79f..62f170957d 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_can.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 27f193513b..64b9926b8d 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c index f329e06ff9..e4cfc27e15 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c @@ -41,7 +41,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 1be4877bad..59c17431c7 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -43,7 +43,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 5a02a97164..019f011fa7 100644 --- a/src/drivers/boards/px4io-v1/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -45,7 +45,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 99baee41df..e9b17e7dc7 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -43,7 +43,7 @@ * Included Files ******************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index fd7eb33c3e..f08299e536 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -45,7 +45,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 4d4bed8358..cf7ec47fc5 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -43,7 +43,7 @@ /* * Includes here should only cover the needs of the framework definitions. */ -#include +#include #include #include diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 4c75143aca..03ceb1d5cf 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -38,7 +38,7 @@ * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ -#include +#include #include diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 1e27309d83..3e1767002b 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -40,7 +40,7 @@ * and output via the standardized control group #2 and a mixer. */ -#include +#include #include #include diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 714c80ded2..da92c851bc 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 961ec47246..398f053d70 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -48,7 +48,7 @@ * driver. Use instead the normal FMU or IO driver. */ -#include +#include #include #include diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 3a3848446d..639012dcf9 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -37,7 +37,7 @@ * Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI. */ -#include +#include #include diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index b13f1fca8f..880610bfd9 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -38,7 +38,7 @@ */ /* XXX trim includes */ -#include +#include #include #include diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index b88310539a..0eec3d8d18 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -38,7 +38,7 @@ */ /* XXX trim includes */ -#include +#include #include #include diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 4ac7e4bdfb..7bc3ca8353 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -42,7 +42,7 @@ */ #include -#include +#include #include #include #include diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 43df0cb8cc..df6fe950b9 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -44,7 +44,7 @@ */ #include -#include +#include #include #include #include diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 7687236408..2b3f4bf51b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -39,7 +39,7 @@ * also supported by this driver. */ -#include +#include #include #include diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 8f79fc3972..4d1d24a92d 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -37,7 +37,7 @@ * LED driver. */ -#include +#include #include #include diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8e2caf8a04..b3368406a1 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -38,7 +38,7 @@ * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. */ -#include +#include #include diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e594c92a11..0cfe735412 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -36,7 +36,7 @@ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. */ -#include +#include #include #include diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index d5ce5d0feb..1a4ea17cf1 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -39,7 +39,7 @@ * Driver for the Maxbotix sonar range finders connected via I2C. */ -#include +#include #include diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index d25d16fa15..afbd10c5cf 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -44,7 +44,7 @@ * */ -#include +#include #include #include #include diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 45684b5906..03e7846851 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -51,7 +51,7 @@ */ -#include +#include #include diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3b8c4ee777..5fc1dd6b21 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -41,7 +41,7 @@ * */ -#include +#include #include #include diff --git a/src/drivers/mkblctrl/mkblctrl_params.c b/src/drivers/mkblctrl/mkblctrl_params.c index 4296260f8b..8c3db720ad 100644 --- a/src/drivers/mkblctrl/mkblctrl_params.c +++ b/src/drivers/mkblctrl/mkblctrl_params.c @@ -40,7 +40,7 @@ * @author Marco Bauer */ -#include +#include #include /** diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b6642e2bbd..bf0e23da6b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -40,7 +40,7 @@ * @author Pat Hickey */ -#include +#include #include #include diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index ef94d03633..9d0e436eeb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -36,7 +36,7 @@ * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. */ -#include +#include #include #include diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index ca651409f9..30d5e48881 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -38,7 +38,7 @@ */ /* XXX trim includes */ -#include +#include #include #include diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 554a1d6596..0e68e9cb5d 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -38,7 +38,7 @@ */ /* XXX trim includes */ -#include +#include #include #include diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index b44c4b720b..b92e950010 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -39,7 +39,7 @@ * */ -#include +#include #include #include diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index 904ce18e8a..c697125a71 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -41,7 +41,7 @@ * @author Anton Babushkin */ -#include +#include #include diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index 6f69ce8a1f..c26e1b8fed 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -46,7 +46,7 @@ * @author Thomas Gubler */ -#include +#include #include diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 2d771266c0..c7d58579e8 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -38,7 +38,7 @@ * which in turn was based on drv_hrt.c */ -#include +#include #include #include diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 8ea7a61709..2adcf1ac9a 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -38,7 +38,7 @@ * Driver for the PX4FLOW module connected via I2C. */ -#include +#include #include diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 92afc7cd74..f0412584fa 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -37,7 +37,7 @@ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. */ -#include +#include #include #include diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 33125699f5..1be561dc6e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -38,7 +38,7 @@ * PX4IO is connected via I2C or DMA enabled high-speed UART. */ -#include +#include #include #include diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index c57ddf65b0..4cceb5cf29 100755 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -38,7 +38,7 @@ */ /* XXX trim includes */ -#include +#include #include #include diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 42d87f7e58..566d43fcb7 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -38,7 +38,7 @@ */ /* XXX trim includes */ -#include +#include #include #include diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 8f98ac92dc..027253905f 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -36,7 +36,7 @@ * Firmware uploader for PX4IO */ -#include +#include #include #include diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 1e38a766e1..999983467b 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -40,7 +40,7 @@ * @author Anton Babushkin */ -#include +#include #include diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index ea7178076b..e8462ef5f8 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -43,7 +43,7 @@ * */ -#include +#include #include #include #include diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 66641d6408..5c9978f1f0 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -39,7 +39,7 @@ * Driver for the Lightware SF0x laser rangefinder series */ -#include +#include #include #include diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index ca2c9fc731..9130c0c29a 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -40,7 +40,7 @@ * and so forth. It avoids the gross complexity of the NuttX ADC driver. */ -#include +#include #include #include diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 603c2eb9da..e37b750fe8 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -45,7 +45,7 @@ * claim the timer and then drive it directly. */ -#include +#include #include #include diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index dbb45a1380..77f5387749 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -40,7 +40,7 @@ * have output pins, does not require an interrupt. */ -#include +#include #include #include diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a18b54981f..ca4794bb16 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -86,7 +86,7 @@ * */ -#include +#include #include #include diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 6dad7b3b15..a3c8633725 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -38,7 +38,7 @@ * Driver for the TeraRanger One range finders connected via I2C. */ -#include +#include #include diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 1d590ae61c..168c2bfa92 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -42,7 +42,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index ab67bd2532..db21a9cb31 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -39,7 +39,7 @@ * Optical flow position estimator */ -#include +#include #include #include #include diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 8fd8870da7..298f13e842 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -43,7 +43,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 085ef1fed7..98d2849580 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -40,7 +40,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 6e99939d14..c1d3f8f0bf 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -43,7 +43,7 @@ #include #include -#include +#include #include #include diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c index aa1eb5ed8f..2c95a17330 100644 --- a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c @@ -37,7 +37,7 @@ * Debug application example for PX4 autopilot */ -#include +#include #include #include #include diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index e22c59fa22..a1876265da 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -37,7 +37,7 @@ * Minimal application example for PX4 autopilot */ -#include +#include #include #include #include diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 41df96775c..a64c169863 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -39,7 +39,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index f595467b36..08405dd655 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -43,7 +43,7 @@ */ #include -#include +#include #include #include #include diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index e3aa7ab2d2..5159f2fcb2 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -39,7 +39,7 @@ * @author Thomas Gubler */ -#include +#include #include diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h index 61d3a3b61f..fafa15029d 100644 --- a/src/lib/mathlib/CMSIS/Include/arm_math.h +++ b/src/lib/mathlib/CMSIS/Include/arm_math.h @@ -267,7 +267,7 @@ #define __CMSIS_GENERIC /* disable NVIC and Systick functions */ /* PX4 */ -#include +#include #ifdef CONFIG_ARCH_CORTEXM4 # define ARM_MATH_CM4 1 #endif diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index b2a3572a7d..30439fad58 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -41,7 +41,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 82bcbc66ff..11c302a8c8 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -50,7 +50,7 @@ * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 */ -#include +#include #include #include #include diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index b267209fe3..00e618609c 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -40,7 +40,7 @@ * @author Julian Oes */ -#include +#include #include #include #include diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index 51ebfb9a19..4088c9f4a4 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -38,7 +38,7 @@ * @author Dominik Juchli */ -#include +#include #include /** diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb1ed7f5df..48b5070440 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -42,7 +42,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6663525cc4..02010405d3 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -42,7 +42,7 @@ * @author Julian Oes */ -#include +#include #include PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 68bf120240..c3b4ab8d4d 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -40,7 +40,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e0b4cb2a0d..e1446bbe9b 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -43,7 +43,7 @@ #include "AttitudePositionEstimatorEKF.h" #include "estimator_22states.h" -#include +#include #include #include #include diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index f8cca6c0dd..f45700eada 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -39,7 +39,7 @@ * @author Lorenz Meier */ -#include +#include #include diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index bd14863247..447e229b1b 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -39,7 +39,7 @@ * Fixedwing backside controller using control library */ -#include +#include #include #include #include diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0a8d07fed9..70ac4b74ea 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -41,7 +41,7 @@ * */ -#include +#include #include #include #include diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6b248cbe2e..df93b3adf4 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -41,7 +41,7 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * @author Thomas Gubler */ -#include +#include #include diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 34265d6a4e..20c2bd46d2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -51,7 +51,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index c00d822327..5720d7b57c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -39,7 +39,7 @@ * @author Lorenz Meier */ -#include +#include #include /* diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 42e00da05b..744c5509f1 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -39,7 +39,7 @@ #include "landingslope.h" -#include +#include #include #include #include diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index fff506865a..3b98454b95 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -39,7 +39,7 @@ * @author Thomas Gubler */ -#include +#include #include /* diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 796d5cbf28..2963e80cb6 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -38,7 +38,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ba049bac40..ce80953e2d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -39,7 +39,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index faede15cb9..81c76ef3f0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -41,7 +41,7 @@ */ /* XXX trim includes */ -#include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3a0dfe4c3b..5f5443ab11 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,7 +53,7 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include +#include #include #include #include diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 9abc012cf2..5d7775bb23 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -39,7 +39,7 @@ * @author Thomas Gubler */ -#include +#include #include diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 1b66107347..70d93b953d 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index f3e7d4c84a..7c1d8a266a 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -39,7 +39,7 @@ * @author Thomas Gubler */ -#include +#include #include diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c index 98104af29e..ca2f540505 100644 --- a/src/modules/navigator/gpsfailure_params.c +++ b/src/modules/navigator/gpsfailure_params.c @@ -39,7 +39,7 @@ * @author Thomas Gubler */ -#include +#include #include diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 6310cf6de4..2c8af8aee0 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -39,7 +39,7 @@ * @author Julian Oes */ -#include +#include #include diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9691e26a8d..e5e7103ca4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -43,7 +43,7 @@ * @author Thomas Gubler */ -#include +#include #include #include diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ef4a8dc0c7..b4f948fff0 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -40,7 +40,7 @@ * @author Thomas Gubler */ -#include +#include #include diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index f48aabc808..3c9a6e40ef 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -39,7 +39,7 @@ * @author Thomas Gubler */ -#include +#include #include diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 7800a6f03f..2b3465eae6 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -39,7 +39,7 @@ * @author Julian Oes */ -#include +#include #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 437395b1f1..84ae34df2b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 2f5908ac5e..d3db26c233 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -36,7 +36,7 @@ * * Simple ADC support for PX4IO on STM32. */ -#include +#include #include #include diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index e04ffc9400..547e5f43ad 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -37,7 +37,7 @@ * R/C inputs and servo outputs. */ -#include +#include #include #include diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 6e57c9ec75..afde16ed39 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -39,7 +39,7 @@ * Decodes into the global PPM buffer and updates accordingly. */ -#include +#include #include #include diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6fa26d4fff..d4b95ab537 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -37,7 +37,7 @@ * Control channel input/output mixer and failsafe. */ -#include +#include #include #include diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 14ee9cb405..94f1fc11ef 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -36,7 +36,7 @@ * Top-level logic for the PX4IO module. */ -#include +#include #include #include // required for task_create diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 8ddf45a12d..0e024ee509 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -37,7 +37,7 @@ * General defines and structures for the PX4IO module firmware. */ -#include +#include #include #include diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index e7976446cd..4454d121d1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -37,7 +37,7 @@ * Implementation of the PX4IO register space. */ -#include +#include #include #include diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index ff2e4af6ea..06f8733a52 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -36,7 +36,7 @@ * Safety button logic. */ -#include +#include #include diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 14d8ccca2e..767741bc5c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -37,7 +37,7 @@ * Serial protocol decoder for the Futaba S.bus protocol. */ -#include +#include #include #include diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8193cf1814..d19c1942be 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -42,7 +42,7 @@ * @author Ban Siesta */ -#include +#include #include #include #include diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index b36d56d6d7..d633f2a420 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -39,7 +39,7 @@ * Segway controller using control library */ -#include +#include #include #include #include diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 18e47865b1..12d50809cf 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -41,7 +41,7 @@ * @author Thomas Gubler */ -#include +#include #include /** diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fc8627c15..86041165b3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -46,7 +46,7 @@ * @author Anton Babushkin */ -#include +#include #include #include diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c index 9105d83cbe..08eeefcaf2 100644 --- a/src/modules/systemlib/conversions.c +++ b/src/modules/systemlib/conversions.c @@ -37,7 +37,7 @@ * Implementation of commonly used conversions. */ -#include +#include #include #include "conversions.h" diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index b41896abd9..9540584a7b 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -41,7 +41,7 @@ * @author Lorenz Meier * @author Petri Tanskanen */ -#include +#include #include #include diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index a1a8e7ea8e..3e1bdd0b19 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -38,7 +38,7 @@ * the same names. */ -#include +#include #include #include diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 24f4e42076..515455f59e 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -42,7 +42,7 @@ #include "mcu_version.h" -#include +#include #ifdef CONFIG_ARCH_CHIP_STM32 #include diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 3ab41c5c58..6d5bcd4857 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -37,7 +37,7 @@ * Programmable multi-channel mixer library. */ -#include +#include #include #include diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 1190683015..cc3342a572 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -128,7 +128,7 @@ #ifndef _SYSTEMLIB_MIXER_MIXER_H #define _SYSTEMLIB_MIXER_MIXER_H value -#include +#include #include "drivers/drv_mixer.h" #include diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index ca5101e657..259e1e687c 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -37,7 +37,7 @@ * Mixer collection. */ -#include +#include #include #include diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index 0d629d6100..9401655e21 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -37,7 +37,7 @@ * Programmable multi-channel mixer library. */ -#include +#include #include #include #include diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h index 4b7091d5b2..3536d7e555 100644 --- a/src/modules/systemlib/mixer/mixer_load.h +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -40,7 +40,7 @@ #ifndef _SYSTEMLIB_MIXER_LOAD_H #define _SYSTEMLIB_MIXER_LOAD_H value -#include +#include __BEGIN_DECLS diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index e9a8a87ca8..5489e3924f 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,7 +36,7 @@ * * Multi-rotor mixers. */ -#include +#include #include #include #include diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index e48bda6918..693f23ede2 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -37,7 +37,7 @@ * Simple summing mixer. */ -#include +#include #include #include diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 0548a9f7db..8d3257f23a 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -43,7 +43,7 @@ * */ -#include +#include #include #include #include diff --git a/src/modules/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c index a5d2f738df..fe4ddde190 100644 --- a/src/modules/systemlib/ppm_decode.c +++ b/src/modules/systemlib/ppm_decode.c @@ -37,7 +37,7 @@ * PPM input decoder. */ -#include +#include #include #include diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index b35b333af2..c84cffa18f 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -37,7 +37,7 @@ * RC calibration check */ -#include +#include #include #include diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 17ce65d13c..454e5b5aa6 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -37,7 +37,7 @@ * System wide parameters */ -#include +#include #include /** diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 280c30023d..73df1051d0 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -37,7 +37,7 @@ * Implementation of commonly used low-level system-call like functions. */ -#include +#include #include #include #include diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index c78f295708..f3aca64c7d 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -38,7 +38,7 @@ * Included Files ************************************************************************************/ -#include +#include #include diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1a38b981e8..eb89b79585 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -41,7 +41,7 @@ * @defgroup topics List of all uORB topics. */ -#include +#include #include diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b4f81d4293..64ee78414d 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -36,7 +36,7 @@ * A lightweight object broker. */ -#include +#include #include diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 2d5abf9591..078decd92c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include +#include #include #include diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 96b3ec1a60..c6f895c557 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -33,7 +33,7 @@ #pragma once -#include +#include #include #include #include diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index e6ea8a8fb7..a74c92dc55 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -35,7 +35,7 @@ * @author Pavel Kirienko */ -#include +#include #include /** diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index defcff8e4a..4a1f5c1f1f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -44,7 +44,7 @@ * */ -#include +#include #include #include #include diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c index 542aef5aac..7aa96bb5c0 100644 --- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c +++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c @@ -37,7 +37,7 @@ * Implementation of existing task API for NuttX */ -#include +#include #include #include #include diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index 02d2d6f3cf..082865a866 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -37,7 +37,7 @@ * STM32F4 bootloader update tool. */ -#include +#include #include #include diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 78db198018..210b4caa1f 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -39,7 +39,7 @@ * config tool. Takes the device name as the first parameter. */ -#include +#include #include #include diff --git a/src/systemcmds/dumpfile/dumpfile.c b/src/systemcmds/dumpfile/dumpfile.c index c188143429..f0e5581ab7 100644 --- a/src/systemcmds/dumpfile/dumpfile.c +++ b/src/systemcmds/dumpfile/dumpfile.c @@ -39,7 +39,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index d74010553a..f04ca29ff9 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -38,7 +38,7 @@ * Tool for ESC calibration */ -#include +#include #include #include diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 34405c4969..2991456cf1 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -38,7 +38,7 @@ * i2c hacking tool */ -#include +#include #include #include diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 77dc2f0d50..b77730ef16 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -38,7 +38,7 @@ * Tool for drive testing */ -#include +#include #include #include diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index f85ed8e2d0..2aba9dbb69 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -50,7 +50,7 @@ * Included Files ************************************************************************************/ -#include +#include #include #include diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a925cdd40f..c1dbcad709 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -39,7 +39,7 @@ * @author Lorenz Meier */ -#include +#include #include #include diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 50547a5626..5690815322 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -39,7 +39,7 @@ * for diagnostics */ -#include +#include #include #include #include diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 6b855cf582..7355176554 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -39,7 +39,7 @@ * Parameter tool. */ -#include +#include #include #include diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index 4ab92dde6b..fb4e5030fc 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -36,7 +36,7 @@ ****************************************************************************/ -#include +#include #include #include #include diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 6bb9f235cb..d6b03eb880 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -37,7 +37,7 @@ * PWM servo output configuration and monitoring tool. */ -#include +#include #include #include diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index d46f96545e..ee9f2e0d8d 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -37,7 +37,7 @@ * Tool similar to UNIX reboot command */ -#include +#include #include #include #include diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c index 6bb53c71a5..fe5f0f0359 100644 --- a/src/systemcmds/reflect/reflect.c +++ b/src/systemcmds/reflect/reflect.c @@ -39,7 +39,7 @@ * @author Andrew Tridgell */ -#include +#include #include #include #include diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index ef12c9ad2e..7cb807d4c7 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -36,7 +36,7 @@ * Test for the analog to digital converter. */ -#include +#include #include #include diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 9a41b19b95..180c3f1032 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index c58e1a51ea..02fe5d963f 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 1b170f1bbe..55f466e734 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -37,7 +37,7 @@ * Floating point tests */ -#include +#include #include #include #include diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index 82c1225500..e14b4b4e6c 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c index ef6173c9ee..281e7e5029 100644 --- a/src/systemcmds/tests/test_hott_telemetry.c +++ b/src/systemcmds/tests/test_hott_telemetry.c @@ -44,7 +44,7 @@ ****************************************************************************/ #include -#include +#include #include #include diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index bc0aeefbae..40043c69be 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c index c59cee7b7c..95ee7652dd 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/systemcmds/tests/test_int.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index 554125302a..a04aacc3a7 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -35,7 +35,7 @@ * Included Files ****************************************************************************/ -#include +#include #include #include diff --git a/src/systemcmds/tests/test_led.c b/src/systemcmds/tests/test_led.c index a8ee678b5a..f56660b74a 100644 --- a/src/systemcmds/tests/test_led.c +++ b/src/systemcmds/tests/test_led.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 19aa059707..765ba3fad9 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -37,7 +37,7 @@ * Mixer load test */ -#include +#include #include diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index c802cdf0c6..7f1323f2b9 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 3a8144077c..fed11c8cb1 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index b6d660c836..8d27247f79 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -38,7 +38,7 @@ * @author Lorenz Meier */ -#include +#include #include diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index 9ffd83c65f..7dea9ac549 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/src/systemcmds/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c index 3097e68a44..e21763f43f 100644 --- a/src/systemcmds/tests/test_sleep.c +++ b/src/systemcmds/tests/test_sleep.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index 78e13a2420..48e084326b 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index cd2e561313..5ee2b6c5b5 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -37,7 +37,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c index f8582b52f8..e49fc1f94c 100644 --- a/src/systemcmds/tests/test_uart_console.c +++ b/src/systemcmds/tests/test_uart_console.c @@ -37,7 +37,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index 6076dbdeae..a929a6f94d 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index 70a9c719e1..ce63071ac2 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -37,7 +37,7 @@ * Included Files ****************************************************************************/ -#include +#include #include diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 9d13d50d77..552a0c2d6b 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -44,7 +44,7 @@ * Included Files ****************************************************************************/ -#include +#include /**************************************************************************** * Definitions diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 55d53503a8..04b5efe230 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -38,7 +38,7 @@ * @author Lorenz Meier */ -#include +#include #include diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 37e913040d..ca48a86a82 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -40,7 +40,7 @@ * @author Lorenz Meier */ -#include +#include #include #include #include diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c index 530fee3405..d938863b62 100644 --- a/src/systemcmds/usb_connected/usb_connected.c +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -39,7 +39,7 @@ * @author Andrew Tridgell */ -#include +#include #include #include #include From e3f152b5c136920cd032989df57051f45148f3f9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 13:00:28 -0700 Subject: [PATCH 008/258] Changed main_t to px4_main_t in systemlib.h Signed-off-by: Mark Charlebois --- src/modules/systemlib/systemlib.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 2f24215a9f..2196133f8d 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,6 +42,7 @@ #include #include #include +#include __BEGIN_DECLS @@ -63,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name, int priority, int scheduler, int stack_size, - main_t entry, + px4_main_t entry, char * const argv[]); enum MULT_PORTS { From 62f8bd667987893e41968f5526bfb37a3152c041 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 13:27:08 -0700 Subject: [PATCH 009/258] Linux build support for some libs The following libs can now be built under Linux: lib/mathlib lib/geo lib/geo_lookup The constants used for ROS are now shared with Linux in px4_defines.h Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 7 +- src/lib/geo/geo.c | 12 +- src/lib/mathlib/math/Limits.cpp | 2 +- src/platforms/linux/include/queue.h | 143 ++++++++++++++++++++++++ src/platforms/px4_defines.h | 86 ++++++++------ 5 files changed, 204 insertions(+), 46 deletions(-) create mode 100644 src/platforms/linux/include/queue.h diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index fc5d7639c7..172565b320 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -45,11 +45,10 @@ # # Libraries # -#MODULES += lib/mathlib -#MODULES += lib/geo -#MODULES += lib/geo_lookup +MODULES += lib/mathlib +MODULES += lib/geo +MODULES += lib/geo_lookup #MODULES += lib/conversion -#MODULES += lib/utils # # Linux port diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 08405dd655..30b9d9ec28 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -274,17 +274,17 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { - double lat_now_rad = lat_now / 180.0d * M_PI; - double lon_now_rad = lon_now / 180.0d * M_PI; - double lat_next_rad = lat_next / 180.0d * M_PI; - double lon_next_rad = lon_next / 180.0d * M_PI; + double lat_now_rad = lat_now / (double)180.0 * M_PI; + double lon_now_rad = lon_now / (double)180.0 * M_PI; + double lat_next_rad = lat_next / (double)180.0 * M_PI; + double lon_next_rad = lon_next / (double)180.0 * M_PI; double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); + double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a)); return CONSTANTS_RADIUS_OF_EARTH * c; } diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index 0ae545f1a5..dffa213e05 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -47,7 +47,7 @@ namespace math { -#ifndef CONFIG_ARCH_ARM +#if !defined(CONFIG_ARCH_ARM) && !defined(__PX4_LINUX) #define M_PI_F 3.14159265358979323846f #endif diff --git a/src/platforms/linux/include/queue.h b/src/platforms/linux/include/queue.h new file mode 100644 index 0000000000..c65f760a2d --- /dev/null +++ b/src/platforms/linux/include/queue.h @@ -0,0 +1,143 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +// Required for Linux +#define FAR +#ifndef NULL +#define NULL (void *)0 +#endif + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s +{ + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s +{ + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s +{ + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s +{ + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +// FIXME - hack for mavlink until kernel work queue can be removed +#define LPWORK 1 +typedef struct { + dq_entry_t dq; +} work_s; +inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2); +inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2) {} + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ + diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 996e56efb9..6cb7f3f42c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -65,41 +65,7 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) -#define OK 0 -#define ERROR -1 - -//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work -#define isfinite(_value) std::isfinite(_value) - -/* Useful constants. */ -#define M_E_F 2.7182818284590452354f -#define M_LOG2E_F 1.4426950408889634074f -#define M_LOG10E_F 0.43429448190325182765f -#define M_LN2_F _M_LN2_F -#define M_LN10_F 2.30258509299404568402f -#define M_PI_F 3.14159265358979323846f -#define M_TWOPI_F (M_PI_F * 2.0f) -#define M_PI_2_F 1.57079632679489661923f -#define M_PI_4_F 0.78539816339744830962f -#define M_3PI_4_F 2.3561944901923448370E0f -#define M_SQRTPI_F 1.77245385090551602792981f -#define M_1_PI_F 0.31830988618379067154f -#define M_2_PI_F 0.63661977236758134308f -#define M_2_SQRTPI_F 1.12837916709551257390f -#define M_DEG_TO_RAD_F 0.01745329251994f -#define M_RAD_TO_DEG_F 57.2957795130823f -#define M_SQRT2_F 1.41421356237309504880f -#define M_SQRT1_2_F 0.70710678118654752440f -#define M_LN2LO_F 1.9082149292705877000E-10f -#define M_LN2HI_F 6.9314718036912381649E-1f -#define M_SQRT3_F 1.73205080756887719000f -#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ -#define M_LOG2_E_F _M_LN2_F -#define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ -#define M_DEG_TO_RAD 0.01745329251994 -#define M_RAD_TO_DEG 57.2957795130823 - -#else +#elif defined(__PX4_NUTTX) || defined(__PX4_LINUX) /* * Building for NuttX */ @@ -118,6 +84,13 @@ typedef param_t px4_param_t; /* Get value of parameter by name */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt) +#else +#error "No target OS defined" +#endif + +/* NuttX Specific defines */ +#if defined(__PX4_NUTTX) + /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) #define isspace(c) \ @@ -125,6 +98,49 @@ typedef param_t px4_param_t; (c) == '\r' || (c) == '\f' || c== '\v') #endif +/* Linux Specific defines */ +#elif defined(__PX4_LINUX) + +// Flag is meaningless on Linux +#define O_BINARY 0 + +#endif + + +/* Defines for ROS and Linux */ +#if defined(__PX4_ROS) || defined(__PX4_LINUX) +#define OK 0 +#define ERROR -1 + +#include +/* Float defines of the standard double length constants */ +#define M_E_F (float)M_E +#define M_LOG2E_F (float)M_LOG2E +#define M_LOG10E_F (float)M_LOG10E +#define M_LN2_F (float)M_LN2 +#define M_LN10_F (float)M_LN10 +#define M_PI_F (float)M_PI +#define M_TWOPI_F (M_PI_F * 2.0f) +#define M_PI_2_F (float)M_PI_2 +#define M_PI_4_F (float)M_PI_4 +#define M_3PI_4_F (float)2.3561944901923448370E0f +#define M_SQRTPI_F (float)1.77245385090551602792981f +#define M_1_PI_F (float)M_1_PI +#define M_2_PI_F (float)M_2_PI +#define M_2_SQRTPI_F 1.12837916709551257390f +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f +#define M_SQRT2_F (float)M_SQRT2 +#define M_SQRT1_2_F (float)M_SQRT1_2 +#define M_LN2LO_F 1.9082149292705877000E-10f +#define M_LN2HI_F 6.9314718036912381649E-1f +#define M_SQRT3_F 1.73205080756887719000f +#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ +#define M_LOG2_E_F _M_LN2_F +#define M_INVLN2_F 1.4426950408889633870E0f/* 1 / log(2) */ +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 + #endif /* Defines for all platforms */ From 566c3ed3c36fae94fa6bcc26ecc28d4a0d01207a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 14:22:31 -0700 Subject: [PATCH 010/258] Linux: Autodetect clang-3.5 toolchain Use clang-3.5 if found, otherwise gcc Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 37e5dbed51..5691e87a9f 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -39,7 +39,13 @@ # # Set to 1 for GCC-4.8.2 and to 0 for Clang-3.5 (Ubuntu 14.04) + +HAVE_CLANG35=$(shell clang-3.6 --version) +ifeq ($(HAVE_CLANG35),) +USE_GCC=1 +else USE_GCC=0 +endif ifeq ($(USE_GCC),1) # GCC Options: From 4a4fcb5d75e254ba4d53c93842acf7e449d153ed Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 14:48:21 -0700 Subject: [PATCH 011/258] Linux: fixes for gcc Fixes to compile with gcc Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/px4_linux_impl.cpp | 1 + src/platforms/linux/px4_layer/px4_linux_tasks.c | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index c5ce678094..86c8b16cc5 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -37,6 +37,7 @@ * PX4 Middleware Wrapper Linux Implementation */ +#include #include #include diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index a00d39b5bf..0e069dd990 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -88,7 +88,7 @@ px4_systemreset(bool to_bootloader) px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) { - int rv = 0; + int rv; int argc = 0; int i; unsigned int len = 0; @@ -128,6 +128,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int //printf("Called px4_task_spawn_cmd\n"); // FIXME - add handling for scheduler and priority rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata); + + if (rv != 0) + { + return (rv < 0) ? rv : -rv; + } //printf("pthread_create task=%d rv=%d\n",(int)task, rv); for (i=0; i Date: Wed, 11 Mar 2015 15:06:38 -0700 Subject: [PATCH 012/258] Linux: added support for gcc 4.6 Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 5691e87a9f..bb791195eb 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -54,7 +54,7 @@ CXX = g++ CPP = gcc -E # GCC Version -DEV_VER_SUPPORTED = 4.8.2 +DEV_VER_SUPPORTED = 4.6 4.8.2 else # Clang options From 5084a61f0ed5ed0910d3067c927c2787ee69a574 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 16:45:19 -0700 Subject: [PATCH 013/258] Abstractions to compile systemlib for Linux and Nuttx Modified uint32_t casts of pointers to unsigned long for portability. It otherwise breaks on x86_64. Added _PX4_IOC to handle the conflice between _IOC on Linux and NuttX. Removed use of px4::ok() because it cannot be used in a thread based implementation. Changed to use px4::AppMgr which uses ok() on ROS. Removed up_cxxinitialize.c from Linux build. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 +- makefiles/setup.mk | 4 +- src/examples/publisher/publisher_example.cpp | 4 +- src/examples/publisher/publisher_example.h | 3 + .../subscriber/subscriber_example.cpp | 2 +- src/examples/subscriber/subscriber_example.h | 2 + .../mc_att_control.cpp | 2 +- .../mc_att_control.h | 2 + .../mc_pos_control.cpp | 2 +- .../mc_pos_control.h | 2 + src/modules/systemlib/cpuload.c | 6 +- src/modules/systemlib/mcu_version.c | 1 + src/modules/systemlib/module.mk | 5 +- src/modules/systemlib/otp.c | 4 +- src/modules/systemlib/otp.h | 39 ++++----- src/modules/systemlib/param/param.c | 5 +- src/modules/systemlib/perf_counter.c | 22 ++--- src/modules/systemlib/rc_check.c | 1 + src/modules/systemlib/systemlib.c | 67 ++------------- src/modules/systemlib/up_cxxinitialize.c | 8 +- src/platforms/linux/include/crc32.h | 83 +++++++++++++++++++ src/platforms/px4_app.h | 2 - src/platforms/px4_config.h | 2 + src/platforms/px4_defines.h | 10 +++ src/platforms/px4_nodehandle.h | 10 ++- 25 files changed, 174 insertions(+), 116 deletions(-) create mode 100644 src/platforms/linux/include/crc32.h diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 172565b320..f0575db3f4 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -37,7 +37,7 @@ # # Library modules # -#MODULES += modules/systemlib +MODULES += modules/systemlib #MODULES += modules/systemlib/mixer #MODULES += modules/uORB #MODULES += modules/dataman diff --git a/makefiles/setup.mk b/makefiles/setup.mk index fd4cbb1a77..1c6e5c3a2e 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,8 +33,8 @@ # Path and tool setup # -#export PX4_TARGET_OS = nuttx -export PX4_TARGET_OS = linux +export PX4_TARGET_OS = nuttx +#export PX4_TARGET_OS = linux # # Some useful paths. diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index e7ab9eb5af..4fddd072e1 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -44,7 +44,7 @@ using namespace px4; PublisherExample::PublisherExample() : - _n(), + _n(mgr), _rc_channels_pub(_n.advertise()), _v_att_pub(_n.advertise()), _parameter_update_pub(_n.advertise()) @@ -55,7 +55,7 @@ int PublisherExample::main() { px4::Rate loop_rate(10); - while (px4::ok()) { + while (!mgr.exitRequested()) { loop_rate.sleep(); _n.spinOnce(); diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 0a66d3edca..8b0d806bd0 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -39,6 +39,7 @@ */ #pragma once #include +#include class PublisherExample { @@ -48,6 +49,8 @@ public: ~PublisherExample() {}; int main(); + + static px4::AppMgr mgr; protected: px4::NodeHandle _n; px4::Publisher *_rc_channels_pub; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 5a23dc80b8..91fb131f32 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -49,7 +49,7 @@ void rc_channels_callback_function(const px4_rc_channels &msg) } SubscriberExample::SubscriberExample() : - _n(), + _n(_mgr), _p_sub_interv("SUB_INTERV", PARAM_SUB_INTERV_DEFAULT), _p_test_float("SUB_TESTF", PARAM_SUB_TESTF_DEFAULT) { diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 9eb5dd2a0a..8003525dd5 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -51,12 +51,14 @@ public: ~SubscriberExample() {}; void spin() {_n.spin();} + protected: px4::NodeHandle _n; px4::ParameterInt _p_sub_interv; px4::ParameterFloat _p_test_float; px4::Subscriber *_sub_rc_chan; + AppMgr _mgr; void rc_channels_callback(const px4_rc_channels &msg); void vehicle_attitude_callback(const px4_vehicle_attitude &msg); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index b5a551109f..34859071f8 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -71,7 +71,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(nullptr), _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), - _n(), + _n(_mgr), /* parameters */ _params_handles({ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index 1242c7fe37..50f06d6967 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -94,6 +94,8 @@ private: px4::NodeHandle _n; + px4::AppMgr _mgr; + struct { px4::ParameterFloat roll_p; px4::ParameterFloat roll_rate_p; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index d135eecfbb..1fd86fd509 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -65,7 +65,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _local_pos_sp_msg(), _global_vel_sp_msg(), - _n(), + _n(_mgr), /* parameters */ _params_handles({ diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 54c6de155c..3c6f28050e 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -107,6 +107,8 @@ protected: px4::NodeHandle _n; + px4::AppMgr _mgr; + struct { px4::ParameterFloat thr_min; px4::ParameterFloat thr_max; diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 9540584a7b..4f58ac71a7 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -42,15 +42,15 @@ * @author Petri Tanskanen */ #include -#include +//#include #include #include #include -#include +//#include -#include +//#include #include diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 515455f59e..1f66971c15 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -43,6 +43,7 @@ #include "mcu_version.h" #include +#include #ifdef CONFIG_ARCH_CHIP_STM32 #include diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f2499bbb13..5e35f2c2bc 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -43,7 +43,6 @@ SRCS = err.c \ conversions.c \ cpuload.c \ getopt_long.c \ - up_cxxinitialize.c \ pid/pid.c \ systemlib.c \ airspeed.c \ @@ -57,6 +56,10 @@ SRCS = err.c \ circuit_breaker_params.c \ mcu_version.c +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS += up_cxxinitialize.c +endif + MAXOPTIMIZATION = -Os EXTRACFLAGS = -Wno-sign-compare diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 8d3257f23a..cb8ffbd3c8 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -170,7 +170,7 @@ void F_lock(void) } // flash write word. -int F_write_word(uint32_t Address, uint32_t Data) +int F_write_word(unsigned long Address, uint32_t Data) { unsigned char octet[4] = {0, 0, 0, 0}; @@ -185,7 +185,7 @@ int F_write_word(uint32_t Address, uint32_t Data) } // flash write byte -int F_write_byte(uint32_t Address, uint8_t Data) +int F_write_byte(unsigned long Address, uint8_t Data) { volatile int status = F_COMPLETE; diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index 273b064f0e..3ea0f80bd3 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -54,6 +54,7 @@ __BEGIN_DECLS #include +#include #include // possible flash statuses @@ -64,30 +65,30 @@ __BEGIN_DECLS #define F_COMPLETE 5 typedef struct { - volatile uint32_t accesscontrol; // 0x00 - volatile uint32_t key; // 0x04 - volatile uint32_t optionkey; // 0x08 - volatile uint32_t status; // 0x0C - volatile uint32_t control; // 0x10 - volatile uint32_t optioncontrol; //0x14 + volatile unsigned long accesscontrol; // 0x00 + volatile unsigned long key; // 0x04 + volatile unsigned long optionkey; // 0x08 + volatile unsigned long status; // 0x0C + volatile unsigned long control; // 0x10 + volatile unsigned long optioncontrol; //0x14 } flash_registers; -#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address +#define PERIPH_BASE ((unsigned long)0x40000000) //Peripheral base address #define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000) #define F_R_BASE (AHB1PERIPH_BASE + 0x3C00) #define FLASH ((flash_registers *) F_R_BASE) -#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit -#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit -#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit -#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF) -#define F_PSIZE_WORD ((uint32_t)0x00000200) -#define F_PSIZE_BYTE ((uint32_t)0x00000000) -#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register -#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit. +#define F_BSY ((unsigned long)0x00010000) //FLASH Busy flag bit +#define F_OPERR ((unsigned long)0x00000002) //FLASH operation Error flag bit +#define F_WRPERR ((unsigned long)0x00000010) //FLASH Write protected error flag bit +#define CR_PSIZE_MASK ((unsigned long)0xFFFFFCFF) +#define F_PSIZE_WORD ((unsigned long)0x00000200) +#define F_PSIZE_BYTE ((unsigned long)0x00000000) +#define F_CR_PG ((unsigned long)0x00000001) // a bit in the F_CR register +#define F_CR_LOCK ((unsigned long)0x80000000) // also another bit. -#define F_KEY1 ((uint32_t)0x45670123) -#define F_KEY2 ((uint32_t)0xCDEF89AB) +#define F_KEY1 ((unsigned long)0x45670123) +#define F_KEY2 ((unsigned long)0xCDEF89AB) #define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) @@ -143,8 +144,8 @@ __EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signat __EXPORT int lock_otp(void); -__EXPORT int F_write_byte(uint32_t Address, uint8_t Data); -__EXPORT int F_write_word(uint32_t Address, uint32_t Data); +__EXPORT int F_write_byte(unsigned long Address, uint8_t Data); +__EXPORT int F_write_word(unsigned long Address, uint32_t Data); __END_DECLS diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 8dea6e6cc0..dfbe582c55 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -41,7 +41,8 @@ * and background parameter saving. */ -#include +//#include +#include #include #include #include @@ -646,7 +647,7 @@ param_save_default(void) const char *filename = param_get_default_file(); /* write parameters to temp file */ - fd = open(filename, O_WRONLY | O_CREAT); + fd = open(filename, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("failed to open param file: %s", filename); diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 950577f003..909979eca3 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -386,7 +386,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_COUNT: dprintf(fd, "%s: %llu events\n", handle->name, - ((struct perf_ctr_count *)handle)->event_count); + (unsigned long long)((struct perf_ctr_count *)handle)->event_count); break; case PC_ELAPSED: { @@ -395,12 +395,12 @@ perf_print_counter_fd(int fd, perf_counter_t handle) dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", handle->name, - pce->event_count, - pce->event_overruns, - pce->time_total, - pce->time_total / pce->event_count, - pce->time_least, - pce->time_most, + (unsigned long long)pce->event_count, + (unsigned long long)pce->event_overruns, + (unsigned long long)pce->time_total, + (unsigned long long)pce->time_total / pce->event_count, + (unsigned long long)pce->time_least, + (unsigned long long)pce->time_most, (double)(1e6f * rms)); break; } @@ -411,10 +411,10 @@ perf_print_counter_fd(int fd, perf_counter_t handle) dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", handle->name, - pci->event_count, - (pci->time_last - pci->time_first) / pci->event_count, - pci->time_least, - pci->time_most, + (unsigned long long)pci->event_count, + (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, + (unsigned long long)pci->time_least, + (unsigned long long)pci->time_most, (double)(1e6f * rms)); break; } diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index c84cffa18f..f576736ab1 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -40,6 +40,7 @@ #include #include +#include #include #include diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 73df1051d0..b86e107737 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -37,78 +37,21 @@ * Implementation of commonly used low-level system-call like functions. */ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - +#include #include "systemlib.h" -// Didn't seem right to include up_internal.h, so direct extern instead. -extern void up_systemreset(void) noreturn_function; - void systemreset(bool to_bootloader) { - if (to_bootloader) { - stm32_pwr_enablebkp(); - - /* XXX wow, this is evil - write a magic number into backup register zero */ - *(uint32_t *)0x40002850 = 0xb007b007; - } - - up_systemreset(); - - /* lock up here */ - while (true); + px4_systemreset(to_bootloader); } -static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); - void killall() { -// printf("Sending SIGUSR1 to all processes now\n"); - - /* iterate through all tasks and send kill signal */ - sched_foreach(kill_task, NULL); + px4_killall(); } -static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) { - kill(tcb->pid, SIGUSR1); -} - -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) -{ - int pid; - - sched_lock(); - - /* create the task */ - pid = task_create(name, priority, stack_size, entry, argv); - - if (pid > 0) { - - /* configure the scheduler */ - struct sched_param param; - - param.sched_priority = priority; - sched_setscheduler(pid, scheduler, ¶m); - - /* XXX do any other private task accounting here before the task starts */ - } - - sched_unlock(); - - return pid; + return px4_task_spawn_cmd(name, scheduler, priority, stack_size, entry, argv); } diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index f3aca64c7d..41707360e4 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -39,10 +39,11 @@ ************************************************************************************/ #include +#include -#include +//#include -#include +//#include //#include //#include "chip.h" @@ -122,7 +123,8 @@ extern uint32_t _etext; * ***************************************************************************/ -__EXPORT void up_cxxinitialize(void) +__EXPORT void up_cxxinitialize(void); +void up_cxxinitialize(void) { initializer_t *initp; diff --git a/src/platforms/linux/include/crc32.h b/src/platforms/linux/include/crc32.h new file mode 100644 index 0000000000..bf828e3e0e --- /dev/null +++ b/src/platforms/linux/include/crc32.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * include/crc.h + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +#ifndef __INCLUDE_CRC32_H +#define __INCLUDE_CRC32_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: crc32part + * + * Description: + * Continue CRC calculation on a part of the buffer. + * + ****************************************************************************/ + +EXTERN uint32_t crc32part(const uint8_t *src, size_t len, + uint32_t crc32val); + +/**************************************************************************** + * Name: crc32 + * + * Description: + * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' + * + ****************************************************************************/ + +EXTERN uint32_t crc32(const uint8_t *src, size_t len); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_CRC32_H */ diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h index 1314397b5d..2c9765ae29 100644 --- a/src/platforms/px4_app.h +++ b/src/platforms/px4_app.h @@ -66,8 +66,6 @@ private: // Task/process based build #if defined(__PX4_ROS) || defined(__PX4_NUTTX) -#define PX4_MAIN main - // Thread based build #else diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 7984432536..83bcb8882d 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -42,4 +42,6 @@ #if defined(__PX4_NUTTX) #include +#elif defined (__PX4_LINUX) +#define CONFIG_NFILE_STREAMS 1 #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6cb7f3f42c..ab41625d5e 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -98,12 +98,22 @@ typedef param_t px4_param_t; (c) == '\r' || (c) == '\f' || c== '\v') #endif +#define _PX4_IOC(x,y) _IOC(x,y) + /* Linux Specific defines */ #elif defined(__PX4_LINUX) // Flag is meaningless on Linux #define O_BINARY 0 +// NuttX _IOC is equivalent to Linux _IO +#define _PX4_IOC(x,y) _IO(x,y) + +/* FIXME - Used to satisfy build */ +#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) + +#define getreg32(a) (*(volatile uint32_t *)(a)) + #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 83a3e422dd..c45ce5cc7f 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -42,6 +42,7 @@ #include "px4_subscriber.h" #include "px4_publisher.h" #include "px4_middleware.h" +#include "px4_app.h" #if defined(__PX4_ROS) /* includes when building for ros */ @@ -141,10 +142,11 @@ protected: class __EXPORT NodeHandle { public: - NodeHandle() : + NodeHandle(AppMgr &a) : _subs(), _pubs(), - _sub_min_interval(nullptr) + _sub_min_interval(nullptr), + _mgr(a) {} ~NodeHandle() @@ -262,7 +264,7 @@ public: */ void spin() { - while (ok()) { + while (!_mgr.exitRequested()) { const int timeout_ms = 100; /* Only continue in the loop if the nodehandle has subscriptions */ @@ -287,6 +289,8 @@ protected: SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ + AppMgr &_mgr; + /** * Check if this is the smallest interval so far and update _sub_min_interval */ From 35e00f0ba1cc4a0156782d48151c26c502788116 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 16:59:19 -0700 Subject: [PATCH 014/258] Added missing _PX4_IOC substitutions Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 +- src/include/mavlink/mavlink_log.h | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index f0575db3f4..a501e67193 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -38,7 +38,7 @@ # Library modules # MODULES += modules/systemlib -#MODULES += modules/systemlib/mixer +MODULES += modules/systemlib/mixer #MODULES += modules/uORB #MODULES += modules/dataman diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 4962d029d5..4ba2f98ad5 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -44,6 +44,7 @@ /* * IOCTL interface for sending log messages. */ +#include #include /** @@ -56,9 +57,9 @@ */ #define MAVLINK_LOG_MAXLEN 50 -#define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1) -#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2) -#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3) +#define MAVLINK_IOC_SEND_TEXT_INFO _PX4_IOC(0x1100, 1) +#define MAVLINK_IOC_SEND_TEXT_CRITICAL _PX4_IOC(0x1100, 2) +#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _PX4_IOC(0x1100, 3) #ifdef __cplusplus extern "C" { From 672ba1a43b210c872f404ab03ac3e8cd5d2dd052 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 20:29:11 -0700 Subject: [PATCH 015/258] Linux: support gcc-4.6 and c++0x GCC 4.6 is too old for -std=c++11 but it supports -std=c++0x Signed-off-by: Mark Charlebois --- makefiles/setup.mk | 4 ++-- makefiles/toolchain_native.mk | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 1c6e5c3a2e..fd4cbb1a77 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,8 +33,8 @@ # Path and tool setup # -export PX4_TARGET_OS = nuttx -#export PX4_TARGET_OS = linux +#export PX4_TARGET_OS = nuttx +export PX4_TARGET_OS = linux # # Some useful paths. diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index bb791195eb..91fafc6efd 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -114,7 +114,7 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # Language-specific flags # ARCHCFLAGS = -std=gnu99 -g -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++11 -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g # Generic warnings # From a68f025ff97c26ff8ed84c8e937bd7355379c15e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 21:32:39 -0700 Subject: [PATCH 016/258] Added support for other clang on Ubuntu 12.04 A Ubuntu 12.04 compatible version of clang-3.4.1 can be downloaded from http://llvm.org/releases/3.4.1/clang+llvm-3.4.1-x86_64-unknown-ubuntu12.04.tar.xz Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 91fafc6efd..3d796659a8 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -1,5 +1,8 @@ # -# Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-2014 PX4 Development Team. All rights reuint32_tserved. +# +# 2005 Modified for clang and GCC on Linux: +# Author: Mark Charlebois # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -40,11 +43,23 @@ # Set to 1 for GCC-4.8.2 and to 0 for Clang-3.5 (Ubuntu 14.04) -HAVE_CLANG35=$(shell clang-3.6 --version) -ifeq ($(HAVE_CLANG35),) +HAVE_CLANG35=$(shell clang-3.5 -dumpversion) + +# Clang will report 4.2.1 as GCC version +HAVE_CLANG=$(shell clang -dumpversion) + USE_GCC=1 -else +#If using ubuntu 14.04 and packaged clang 4.2.1 +ifeq ($(HAVE_CLANG35),3.5) USE_GCC=0 +CLANGVER=-3.5 +else + +#If using ubuntu 12.04 and downloaded clang 3.4.1 +ifeq ($(HAVE_CLANG),4.2.1) +USE_GCC=0 +CLANGVER= +endif endif ifeq ($(USE_GCC),1) @@ -58,9 +73,9 @@ DEV_VER_SUPPORTED = 4.6 4.8.2 else # Clang options -CC = clang-3.5 -CXX = clang++-3.5 -CPP = clang-3.5 -E +CC = clang$(CLANGVER) +CXX = clang++$(CLANGVER) +CPP = clang$(CLANGVER) -E # Clang GCC reported version DEV_VER_SUPPORTED = 4.2.1 From aeb65702a2389588be392849ff26f273526e2ea7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 11 Mar 2015 22:11:27 -0700 Subject: [PATCH 017/258] Fixed support for clang 3.5 "clang-3.5 -dumpversion" returns 4.2.1 as the GCC version. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 2 +- src/modules/uORB/uORB.cpp | 117 ++++++++++++++++++---------------- 2 files changed, 63 insertions(+), 56 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 3d796659a8..a2bd5a4bc7 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -50,7 +50,7 @@ HAVE_CLANG=$(shell clang -dumpversion) USE_GCC=1 #If using ubuntu 14.04 and packaged clang 4.2.1 -ifeq ($(HAVE_CLANG35),3.5) +ifeq ($(HAVE_CLANG35),4.2.1) USE_GCC=0 CLANGVER=-3.5 else diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 64ee78414d..b027453702 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -36,7 +36,11 @@ * A lightweight object broker. */ -#include +#include +#include +#include +#include +#include #include @@ -46,16 +50,12 @@ #include #include #include -#include #include #include #include #include #include -#include -#include -#include #include #include @@ -123,17 +123,17 @@ public: ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); - virtual int open(struct file *filp); - virtual int close(struct file *filp); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int open(device::px4_dev_handle_t *handlep); + virtual int close(device::px4_dev_handle_t *handlep); + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual ssize_t write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); protected: - virtual pollevent_t poll_state(struct file *filp); - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + virtual pollevent_t poll_state(device::px4_dev_handle_t *handlep); + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); private: struct SubscriberData { @@ -152,8 +152,8 @@ private: pid_t _publisher; /**< if nonzero, current publisher */ const int _priority; /**< priority of topic */ - SubscriberData *filp_to_sd(struct file *filp) { - SubscriberData *sd = (SubscriberData *)(filp->f_priv); + SubscriberData *handlep_to_sd(device::px4_dev_handle_t *handlep) { + SubscriberData *sd = (SubscriberData *)(handlep->priv); return sd; } @@ -199,12 +199,12 @@ ORBDevNode::~ORBDevNode() } int -ORBDevNode::open(struct file *filp) +ORBDevNode::open(device::px4_dev_handle_t *handlep) { int ret; /* is this a publisher? */ - if (filp->f_oflags == O_WRONLY) { + if (handlep->flags == PX4_F_WRONLY) { /* become the publisher if we can */ lock(); @@ -221,7 +221,7 @@ ORBDevNode::open(struct file *filp) /* now complete the open */ if (ret == OK) { - ret = CDev::open(filp); + ret = CDev::open(handlep); /* open failed - not the publisher anymore */ if (ret != OK) @@ -232,7 +232,7 @@ ORBDevNode::open(struct file *filp) } /* is this a new subscriber? */ - if (filp->f_oflags == O_RDONLY) { + if (handlep->flags == PX4_F_RDONLY) { /* allocate subscriber data */ SubscriberData *sd = new SubscriberData; @@ -248,9 +248,9 @@ ORBDevNode::open(struct file *filp) /* set priority */ sd->priority = _priority; - filp->f_priv = (void *)sd; + handlep->priv = (void *)sd; - ret = CDev::open(filp); + ret = CDev::open(handlep); if (ret != OK) delete sd; @@ -263,14 +263,14 @@ ORBDevNode::open(struct file *filp) } int -ORBDevNode::close(struct file *filp) +ORBDevNode::close(device::px4_dev_handle_t *handlep) { /* is this the publisher closing? */ if (getpid() == _publisher) { _publisher = 0; } else { - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = handlep_to_sd(handlep); if (sd != nullptr) { hrt_cancel(&sd->update_call); @@ -278,13 +278,13 @@ ORBDevNode::close(struct file *filp) } } - return CDev::close(filp); + return CDev::close(handlep); } ssize_t -ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) +ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) { - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + SubscriberData *sd = (SubscriberData *)handlep_to_sd(handlep); /* if the object has not been written yet, return zero */ if (_data == nullptr) @@ -321,7 +321,7 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) } ssize_t -ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) +ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen) { /* * Writes are legal from interrupt context as long as the @@ -330,7 +330,7 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) * Writes outside interrupt context will allocate the object * if it has not yet been allocated. * - * Note that filp will usually be NULL. + * Note that handlep will usually be NULL. */ if (nullptr == _data) { if (!up_interrupt_context()) { @@ -369,9 +369,9 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) } int -ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) +ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) { - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = handlep_to_sd(handlep); switch (cmd) { case ORBIOCLASTUPDATE: @@ -396,7 +396,7 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); + return CDev::ioctl(handlep, cmd, arg); } } @@ -427,9 +427,9 @@ ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *d } pollevent_t -ORBDevNode::poll_state(struct file *filp) +ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) { - SubscriberData *sd = filp_to_sd(filp); + SubscriberData *sd = handlep_to_sd(handlep); /* * If the topic appears updated to the subscriber, say so. @@ -441,9 +441,9 @@ ORBDevNode::poll_state(struct file *filp) } void -ORBDevNode::poll_notify_one(struct pollfd *fds, pollevent_t events) +ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - SubscriberData *sd = filp_to_sd((struct file *)fds->priv); + SubscriberData *sd = handlep_to_sd((device::px4_dev_handle_t *)fds->priv); /* * If the topic looks updated to the subscriber, go ahead and notify them. @@ -560,7 +560,7 @@ public: ORBDevMaster(Flavor f); ~ORBDevMaster(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); private: Flavor _flavor; }; @@ -580,7 +580,7 @@ ORBDevMaster::~ORBDevMaster() } int -ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) +ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) { int ret; @@ -632,6 +632,7 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) devpath = strdup(nodepath); if (devpath == nullptr) { + // FIXME - looks like we leaked memory here for objname return -ENOMEM; } @@ -641,6 +642,8 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) /* if we didn't get a device, that's bad */ if (node == nullptr) { unlock(); + + // FIXME - looks like we leaked memory here for devpath and objname return -ENOMEM; } @@ -670,7 +673,7 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); + return CDev::ioctl(handlep, cmd, arg); } } @@ -745,7 +748,7 @@ int pubsublatency_main(void) float latency_integral = 0.0f; /* wakeup source(s) */ - struct pollfd fds[3]; + px4_pollfd_struct_t fds[3]; int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); @@ -772,7 +775,7 @@ int pubsublatency_main(void) for (unsigned i = 0; i < maxruns; i++) { /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(orb_test), test_multi_sub, &t); timingsgroup = 0; @@ -965,11 +968,11 @@ latency_test(orb_id_t T, bool print) /* test pub / sub latency */ - int pubsub_task = task_spawn_cmd("uorb_latency", + int pubsub_task = px4_task_spawn_cmd("uorb_latency", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, - (main_t)&pubsublatency_main, + (px4_main_t)&pubsublatency_main, nullptr); /* give the test task some data */ @@ -1092,13 +1095,13 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri const struct orb_advertdata adv = { meta, instance, priority }; /* open the control device */ - fd = open(TOPIC_MASTER_DEVICE_PATH, 0); + fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); if (fd < 0) goto out; /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); + ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); /* it's OK if it already exists */ if ((OK != ret) && (EEXIST == errno)) { @@ -1108,7 +1111,7 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri out: if (fd >= 0) - close(fd); + px4_close(fd); return ret; } @@ -1145,6 +1148,9 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* * Generate the path to the node and try to open it. */ + + // FIXME - if *instance is uninitialized, why is this being called? Seems risky and + // its definiately a waste. This is the case in muli-topic test. ret = node_mkpath(path, f, meta, instance); if (ret != OK) { @@ -1153,12 +1159,13 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve } /* open the path as either the advertiser or the subscriber */ - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); /* if we want to advertise and the node existed, we have to re-try again */ if ((fd >= 0) && (instance != nullptr) && (advertiser)) { /* close the fd, we want a new one */ - close(fd); + px4_close(fd); + /* the node_advertise call will automatically go for the next free entry */ fd = -1; } @@ -1181,7 +1188,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* on success, try the open again */ if (ret == OK) { - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); } } @@ -1214,8 +1221,8 @@ orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *inst return ERROR; /* get the advertiser handle and close the node */ - result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - close(fd); + result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + px4_close(fd); if (result == ERROR) return ERROR; @@ -1243,7 +1250,7 @@ orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) int orb_unsubscribe(int handle) { - return close(handle); + return px4_close(handle); } int @@ -1257,7 +1264,7 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { int ret; - ret = read(handle, buffer, meta->o_size); + ret = px4_read(handle, (char *)buffer, meta->o_size); if (ret < 0) return ERROR; @@ -1273,24 +1280,24 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer) int orb_check(int handle, bool *updated) { - return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } int orb_stat(int handle, uint64_t *time) { - return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); + return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } int orb_priority(int handle, int *priority) { - return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); + return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } int orb_set_interval(int handle, unsigned interval) { - return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); } From a25ae71f990aaf1aca999941ece3551f7b8ce269 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 12 Mar 2015 12:07:09 -0700 Subject: [PATCH 018/258] Linux: support ld.gold The -Ur option is not supported in gold Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index a2bd5a4bc7..db40689acc 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -81,7 +81,7 @@ CPP = clang$(CLANGVER) -E DEV_VER_SUPPORTED = 4.2.1 endif -LD = ld +LD = ld.gold AR = ar rcs NM = nm OBJCOPY = objcopy @@ -282,10 +282,11 @@ endef # Produce partially-linked $1 from files in $2 # +#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -o $1 $2 + $(Q) $(LD) -r -o $1 $2 endef # $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 From 0553a68a1a1a367cfa14d0e5db3b3663b30ae3b3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 12 Mar 2015 12:13:34 -0700 Subject: [PATCH 019/258] Fixed usage string for hello app hello should have been hello_main Signed-off-by: Mark Charlebois --- src/platforms/linux/hello/hello_start_linux.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/linux/hello/hello_start_linux.cpp b/src/platforms/linux/hello/hello_start_linux.cpp index 0275662610..a7d21fa59c 100644 --- a/src/platforms/linux/hello/hello_start_linux.cpp +++ b/src/platforms/linux/hello/hello_start_linux.cpp @@ -95,6 +95,6 @@ int hello_main(int argc, char *argv[]) return 0; } - printf("usage: hello {start|stop|status}\n"); + printf("usage: hello_main {start|stop|status}\n"); return 1; } From f3596e555b1a906e75365f526652edd7f09e49bb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 12 Mar 2015 15:26:10 -0700 Subject: [PATCH 020/258] Added Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 3 +- src/drivers/device/device.h | 531 +-------------- src/drivers/device/device_nuttx.h | 560 +++++++++++++++ src/drivers/device/module.mk | 9 +- src/drivers/device/vcdev.cpp | 643 ++++++++++++++++++ src/drivers/device/vdevice.cpp | 107 +++ src/drivers/device/vdevice.h | 496 ++++++++++++++ src/platforms/linux/vcdev_test/module.mk | 43 ++ .../linux/vcdev_test/vcdevtest_example.cpp | 150 ++++ .../linux/vcdev_test/vcdevtest_example.h | 58 ++ .../linux/vcdev_test/vcdevtest_main.cpp | 55 ++ .../vcdev_test/vcdevtest_start_linux.cpp | 100 +++ src/platforms/px4_posix.h | 7 + 13 files changed, 2235 insertions(+), 527 deletions(-) create mode 100644 src/drivers/device/device_nuttx.h create mode 100644 src/drivers/device/vcdev.cpp create mode 100644 src/drivers/device/vdevice.cpp create mode 100644 src/drivers/device/vdevice.h create mode 100644 src/platforms/linux/vcdev_test/module.mk create mode 100644 src/platforms/linux/vcdev_test/vcdevtest_example.cpp create mode 100644 src/platforms/linux/vcdev_test/vcdevtest_example.h create mode 100644 src/platforms/linux/vcdev_test/vcdevtest_main.cpp create mode 100644 src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index a501e67193..56567e48f4 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -10,7 +10,7 @@ # # Board support modules # -#MODULES += drivers/device +MODULES += drivers/device #MODULES += modules/sensors # @@ -55,4 +55,5 @@ MODULES += lib/geo_lookup # MODULES += platforms/linux/px4_layer MODULES += platforms/linux/hello +MODULES += platforms/linux/vcdev_test diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index cf7ec47fc5..98851edc83 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -31,530 +31,11 @@ * ****************************************************************************/ -/** - * @file device.h - * - * Definitions for the generic base classes in the device framework. - */ +#pragma once -#ifndef _DEVICE_DEVICE_H -#define _DEVICE_DEVICE_H +#ifdef __PX4_NUTTX +#include "device_nuttx.h" +#elif defined (__PX4_LINUX) +#include "vdevice.h" +#endif -/* - * Includes here should only cover the needs of the framework definitions. - */ -#include - -#include -#include -#include -#include -#include - -#include - -/** - * Namespace encapsulating all device framework classes, functions and data. - */ -namespace device __EXPORT -{ - -/** - * Fundamental base class for all device drivers. - * - * This class handles the basic "being a driver" things, including - * interrupt registration and dispatch. - */ -class __EXPORT Device -{ -public: - /** - * Destructor. - * - * Public so that anonymous devices can be destroyed. - */ - virtual ~Device(); - - /** - * Interrupt handler. - */ - virtual void interrupt(void *ctx); /**< interrupt handler */ - - /* - * Direct access methods. - */ - - /** - * Initialise the driver and make it ready for use. - * - * @return OK if the driver initialized OK, negative errno otherwise; - */ - virtual int init(); - - /** - * Read directly from the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param offset The device address at which to start reading - * @param data The buffer into which the read values should be placed. - * @param count The number of items to read. - * @return The number of items read on success, negative errno otherwise. - */ - virtual int read(unsigned address, void *data, unsigned count); - - /** - * Write directly to the device. - * - * The actual size of each unit quantity is device-specific. - * - * @param address The device address at which to start writing. - * @param data The buffer from which values should be read. - * @param count The number of items to write. - * @return The number of items written on success, negative errno otherwise. - */ - virtual int write(unsigned address, void *data, unsigned count); - - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform. - * @param arg An argument to the operation. - * @return Negative errno on error, OK or positive value on success. - */ - virtual int ioctl(unsigned operation, unsigned &arg); - - /* - device bus types for DEVID - */ - enum DeviceBusType { - DeviceBusType_UNKNOWN = 0, - DeviceBusType_I2C = 1, - DeviceBusType_SPI = 2, - DeviceBusType_UAVCAN = 3, - }; - - /* - broken out device elements. The bitfields are used to keep - the overall value small enough to fit in a float accurately, - which makes it possible to transport over the MAVLink - parameter protocol without loss of information. - */ - struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; - - union DeviceId { - struct DeviceStructure devid_s; - uint32_t devid; - }; - -protected: - const char *_name; /**< driver name */ - bool _debug_enabled; /**< if true, debug messages are printed */ - union DeviceId _device_id; /**< device identifier information */ - - /** - * Constructor - * - * @param name Driver name - * @param irq Interrupt assigned to the device. - */ - Device(const char *name, - int irq = 0); - - /** - * Enable the device interrupt - */ - void interrupt_enable(); - - /** - * Disable the device interrupt - */ - void interrupt_disable(); - - /** - * Take the driver lock. - * - * Each driver instance has its own lock/semaphore. - * - * Note that we must loop as the wait may be interrupted by a signal. - */ - void lock() { - do {} while (sem_wait(&_lock) != 0); - } - - /** - * Release the driver lock. - */ - void unlock() { - sem_post(&_lock); - } - - /** - * Log a message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void log(const char *fmt, ...); - - /** - * Print a debug message. - * - * The message is prefixed with the driver name, and followed - * by a newline. - */ - void debug(const char *fmt, ...); - -private: - int _irq; - bool _irq_attached; - sem_t _lock; - - /** disable copy construction for this and all subclasses */ - Device(const Device &); - - /** disable assignment for this and all subclasses */ - Device &operator = (const Device &); - - /** - * Register ourselves as a handler for an interrupt - * - * @param irq The interrupt to claim - * @return OK if the interrupt was registered - */ - int dev_register_interrupt(int irq); - - /** - * Unregister ourselves as a handler for any interrupt - */ - void dev_unregister_interrupt(); - - /** - * Interrupt dispatcher - * - * @param irq The interrupt that has been triggered. - * @param context Pointer to the interrupted context. - */ - static void dev_interrupt(int irq, void *context); - -}; - -/** - * Abstract class for any character device - */ -class __EXPORT CDev : public Device -{ -public: - /** - * Constructor - * - * @param name Driver name - * @param devname Device node name - * @param irq Interrupt assigned to the device - */ - CDev(const char *name, const char *devname, int irq = 0); - - /** - * Destructor - */ - virtual ~CDev(); - - virtual int init(); - - /** - * Handle an open of the device. - * - * This function is called for every open of the device. The default - * implementation maintains _open_count and always returns OK. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the open is allowed, -errno otherwise. - */ - virtual int open(struct file *filp); - - /** - * Handle a close of the device. - * - * This function is called for every close of the device. The default - * implementation maintains _open_count and returns OK as long as it is not zero. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the close was successful, -errno otherwise. - */ - virtual int close(struct file *filp); - - /** - * Perform a read from the device. - * - * The default implementation returns -ENOSYS. - * - * @param filp Pointer to the NuttX file structure. - * @param buffer Pointer to the buffer into which data should be placed. - * @param buflen The number of bytes to be read. - * @return The number of bytes read or -errno otherwise. - */ - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - - /** - * Perform a write to the device. - * - * The default implementation returns -ENOSYS. - * - * @param filp Pointer to the NuttX file structure. - * @param buffer Pointer to the buffer from which data should be read. - * @param buflen The number of bytes to be written. - * @return The number of bytes written or -errno otherwise. - */ - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - - /** - * Perform a logical seek operation on the device. - * - * The default implementation returns -ENOSYS. - * - * @param filp Pointer to the NuttX file structure. - * @param offset The new file position relative to whence. - * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. - * @return The previous offset, or -errno otherwise. - */ - virtual off_t seek(struct file *filp, off_t offset, int whence); - - /** - * Perform an ioctl operation on the device. - * - * The default implementation handles DIOC_GETPRIV, and otherwise - * returns -ENOTTY. Subclasses should call the default implementation - * for any command they do not handle themselves. - * - * @param filp Pointer to the NuttX file structure. - * @param cmd The ioctl command value. - * @param arg The ioctl argument value. - * @return OK on success, or -errno otherwise. - */ - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Perform a poll setup/teardown operation. - * - * This is handled internally and should not normally be overridden. - * - * @param filp Pointer to the NuttX file structure. - * @param fds Poll descriptor being waited on. - * @param arg True if this is establishing a request, false if - * it is being torn down. - * @return OK on success, or -errno otherwise. - */ - virtual int poll(struct file *filp, struct pollfd *fds, bool setup); - - /** - * Test whether the device is currently open. - * - * This can be used to avoid tearing down a device that is still active. - * Note - not virtual, cannot be overridden by a subclass. - * - * @return True if the device is currently open. - */ - bool is_open() { return _open_count > 0; } - -protected: - /** - * Pointer to the default cdev file operations table; useful for - * registering clone devices etc. - */ - static const struct file_operations fops; - - /** - * Check the current state of the device for poll events from the - * perspective of the file. - * - * This function is called by the default poll() implementation when - * a poll is set up to determine whether the poll should return immediately. - * - * The default implementation returns no events. - * - * @param filp The file that's interested. - * @return The current set of poll events. - */ - virtual pollevent_t poll_state(struct file *filp); - - /** - * Report new poll events. - * - * This function should be called anytime the state of the device changes - * in a fashion that might be interesting to a poll waiter. - * - * @param events The new event(s) being announced. - */ - virtual void poll_notify(pollevent_t events); - - /** - * Internal implementation of poll_notify. - * - * @param fds A poll waiter to notify. - * @param events The event(s) to send to the waiter. - */ - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); - - /** - * Notification of the first open. - * - * This function is called when the device open count transitions from zero - * to one. The driver lock is held for the duration of the call. - * - * The default implementation returns OK. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the open should proceed, -errno otherwise. - */ - virtual int open_first(struct file *filp); - - /** - * Notification of the last close. - * - * This function is called when the device open count transitions from - * one to zero. The driver lock is held for the duration of the call. - * - * The default implementation returns OK. - * - * @param filp Pointer to the NuttX file structure. - * @return OK if the open should return OK, -errno otherwise. - */ - virtual int close_last(struct file *filp); - - /** - * Register a class device name, automatically adding device - * class instance suffix if need be. - * - * @param class_devname Device class name - * @return class_instamce Class instance created, or -errno on failure - */ - virtual int register_class_devname(const char *class_devname); - - /** - * Register a class device name, automatically adding device - * class instance suffix if need be. - * - * @param class_devname Device class name - * @param class_instance Device class instance from register_class_devname() - * @return OK on success, -errno otherwise - */ - virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); - - /** - * Get the device name. - * - * @return the file system string of the device handle - */ - const char* get_devname() { return _devname; } - - bool _pub_blocked; /**< true if publishing should be blocked */ - -private: - static const unsigned _max_pollwaiters = 8; - - const char *_devname; /**< device node name */ - bool _registered; /**< true if device name was registered */ - unsigned _open_count; /**< number of successful opens */ - - struct pollfd *_pollset[_max_pollwaiters]; - - /** - * Store a pollwaiter in a slot where we can find it later. - * - * Expands the pollset as required. Must be called with the driver locked. - * - * @return OK, or -errno on error. - */ - int store_poll_waiter(struct pollfd *fds); - - /** - * Remove a poll waiter. - * - * @return OK, or -errno on error. - */ - int remove_poll_waiter(struct pollfd *fds); - - /* do not allow copying this class */ - CDev(const CDev&); - CDev operator=(const CDev&); -}; - -/** - * Abstract class for character device accessed via PIO - */ -class __EXPORT PIO : public CDev -{ -public: - /** - * Constructor - * - * @param name Driver name - * @param devname Device node name - * @param base Base address of the device PIO area - * @param irq Interrupt assigned to the device (or zero if none) - */ - PIO(const char *name, - const char *devname, - uint32_t base, - int irq = 0); - virtual ~PIO(); - - virtual int init(); - -protected: - - /** - * Read a register - * - * @param offset Register offset in bytes from the base address. - */ - uint32_t reg(uint32_t offset) { - return *(volatile uint32_t *)(_base + offset); - } - - /** - * Write a register - * - * @param offset Register offset in bytes from the base address. - * @param value Value to write. - */ - void reg(uint32_t offset, uint32_t value) { - *(volatile uint32_t *)(_base + offset) = value; - } - - /** - * Modify a register - * - * Note that there is a risk of a race during the read/modify/write cycle - * that must be taken care of by the caller. - * - * @param offset Register offset in bytes from the base address. - * @param clearbits Bits to clear in the register - * @param setbits Bits to set in the register - */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { - uint32_t val = reg(offset); - val &= ~clearbits; - val |= setbits; - reg(offset, val); - } - -private: - uint32_t _base; -}; - -} // namespace device - -// class instance for primary driver of each class -enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 -}; - -#endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h new file mode 100644 index 0000000000..87e5e2abe0 --- /dev/null +++ b/src/drivers/device/device_nuttx.h @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file device_nuttx.h + * + * Definitions for the generic base classes in the device framework. + */ + +#ifndef _DEVICE_DEVICE_H +#define _DEVICE_DEVICE_H + +/* + * Includes here should only cover the needs of the framework definitions. + */ +#include + +#include +#include +#include +#include +#include + +/** + * Namespace encapsulating all device framework classes, functions and data. + */ +namespace device __EXPORT +{ + +/** + * Fundamental base class for all device drivers. + * + * This class handles the basic "being a driver" things, including + * interrupt registration and dispatch. + */ +class __EXPORT Device +{ +public: + /** + * Destructor. + * + * Public so that anonymous devices can be destroyed. + */ + virtual ~Device(); + + /** + * Interrupt handler. + */ + virtual void interrupt(void *ctx); /**< interrupt handler */ + + /* + * Direct access methods. + */ + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialized OK, negative errno otherwise; + */ + virtual int init(); + +#if 0 + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + virtual int read(unsigned address, void *data, unsigned count); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + virtual int write(unsigned address, void *data, unsigned count); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int ioctl(unsigned operation, unsigned &arg); +#endif + + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, + }; + + /* + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. + */ + struct DeviceStructure { + enum DeviceBusType bus_type:3; + uint8_t bus:5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; + +protected: + const char *_name; /**< driver name */ + bool _debug_enabled; /**< if true, debug messages are printed */ + union DeviceId _device_id; /**< device identifier information */ + + /** + * Constructor + * + * @param name Driver name + * @param irq Interrupt assigned to the device. + */ + Device(const char *name, + int irq = 0); + + /** + * Enable the device interrupt + */ + void interrupt_enable(); + + /** + * Disable the device interrupt + */ + void interrupt_disable(); + + /** + * Take the driver lock. + * + * Each driver instance has its own lock/semaphore. + * + * Note that we must loop as the wait may be interrupted by a signal. + */ + void lock() { + do {} while (sem_wait(&_lock) != 0); + } + + /** + * Release the driver lock. + */ + void unlock() { + sem_post(&_lock); + } + + /** + * Log a message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void log(const char *fmt, ...); + + /** + * Print a debug message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void debug(const char *fmt, ...); + +private: + int _irq; + bool _irq_attached; + sem_t _lock; + + /** disable copy construction for this and all subclasses */ + Device(const Device &); + + /** disable assignment for this and all subclasses */ + Device &operator = (const Device &); + + /** + * Register ourselves as a handler for an interrupt + * + * @param irq The interrupt to claim + * @return OK if the interrupt was registered + */ + int dev_register_interrupt(int irq); + + /** + * Unregister ourselves as a handler for any interrupt + */ + void dev_unregister_interrupt(); + + /** + * Interrupt dispatcher + * + * @param irq The interrupt that has been triggered. + * @param context Pointer to the interrupted context. + */ + static void dev_interrupt(int irq, void *context); + +}; + +/** + * Abstract class for any character device + */ +class __EXPORT CDev : public Device +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param irq Interrupt assigned to the device + */ + CDev(const char *name, const char *devname, int irq = 0); + + /** + * Destructor + */ + virtual ~CDev(); + + virtual int init(); + + /** + * Handle an open of the device. + * + * This function is called for every open of the device. The default + * implementation maintains _open_count and always returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open is allowed, -errno otherwise. + */ + virtual int open(struct file *filp); + + /** + * Handle a close of the device. + * + * This function is called for every close of the device. The default + * implementation maintains _open_count and returns OK as long as it is not zero. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the close was successful, -errno otherwise. + */ + virtual int close(struct file *filp); + + /** + * Perform a read from the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer into which data should be placed. + * @param buflen The number of bytes to be read. + * @return The number of bytes read or -errno otherwise. + */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + /** + * Perform a write to the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer from which data should be read. + * @param buflen The number of bytes to be written. + * @return The number of bytes written or -errno otherwise. + */ + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + + /** + * Perform a logical seek operation on the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param offset The new file position relative to whence. + * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. + * @return The previous offset, or -errno otherwise. + */ + virtual off_t seek(struct file *filp, off_t offset, int whence); + + /** + * Perform an ioctl operation on the device. + * + * The default implementation handles DIOC_GETPRIV, and otherwise + * returns -ENOTTY. Subclasses should call the default implementation + * for any command they do not handle themselves. + * + * @param filp Pointer to the NuttX file structure. + * @param cmd The ioctl command value. + * @param arg The ioctl argument value. + * @return OK on success, or -errno otherwise. + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Perform a poll setup/teardown operation. + * + * This is handled internally and should not normally be overridden. + * + * @param filp Pointer to the NuttX file structure. + * @param fds Poll descriptor being waited on. + * @param arg True if this is establishing a request, false if + * it is being torn down. + * @return OK on success, or -errno otherwise. + */ + virtual int poll(struct file *filp, struct pollfd *fds, bool setup); + + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * Note - not virtual, cannot be overridden by a subclass. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } + +protected: + /** + * Pointer to the default cdev file operations table; useful for + * registering clone devices etc. + */ + static const struct file_operations fops; + + /** + * Check the current state of the device for poll events from the + * perspective of the file. + * + * This function is called by the default poll() implementation when + * a poll is set up to determine whether the poll should return immediately. + * + * The default implementation returns no events. + * + * @param filp The file that's interested. + * @return The current set of poll events. + */ + virtual pollevent_t poll_state(struct file *filp); + + /** + * Report new poll events. + * + * This function should be called anytime the state of the device changes + * in a fashion that might be interesting to a poll waiter. + * + * @param events The new event(s) being announced. + */ + virtual void poll_notify(pollevent_t events); + + /** + * Internal implementation of poll_notify. + * + * @param fds A poll waiter to notify. + * @param events The event(s) to send to the waiter. + */ + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + + /** + * Notification of the first open. + * + * This function is called when the device open count transitions from zero + * to one. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(struct file *filp); + + /** + * Notification of the last close. + * + * This function is called when the device open count transitions from + * one to zero. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should return OK, -errno otherwise. + */ + virtual int close_last(struct file *filp); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + + bool _pub_blocked; /**< true if publishing should be blocked */ + +private: + static const unsigned _max_pollwaiters = 8; + + const char *_devname; /**< device node name */ + bool _registered; /**< true if device name was registered */ + unsigned _open_count; /**< number of successful opens */ + + struct pollfd *_pollset[_max_pollwaiters]; + + /** + * Store a pollwaiter in a slot where we can find it later. + * + * Expands the pollset as required. Must be called with the driver locked. + * + * @return OK, or -errno on error. + */ + int store_poll_waiter(struct pollfd *fds); + + /** + * Remove a poll waiter. + * + * @return OK, or -errno on error. + */ + int remove_poll_waiter(struct pollfd *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); +}; + +/** + * Abstract class for character device accessed via PIO + */ +class __EXPORT PIO : public CDev +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param base Base address of the device PIO area + * @param irq Interrupt assigned to the device (or zero if none) + */ + PIO(const char *name, + const char *devname, + uint32_t base, + int irq = 0); + virtual ~PIO(); + + virtual int init(); + +protected: + + /** + * Read a register + * + * @param offset Register offset in bytes from the base address. + */ + unsigned long reg(uint32_t offset) { + return *(volatile uint32_t *)(_base + offset); + } + + /** + * Write a register + * + * @param offset Register offset in bytes from the base address. + * @param value Value to write. + */ + void reg(uint32_t offset, uint32_t value) { + *(volatile uint32_t *)(_base + offset) = value; + } + + /** + * Modify a register + * + * Note that there is a risk of a race during the read/modify/write cycle + * that must be taken care of by the caller. + * + * @param offset Register offset in bytes from the base address. + * @param clearbits Bits to clear in the register + * @param setbits Bits to set in the register + */ + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + uint32_t val = reg(offset); + val &= ~clearbits; + val |= setbits; + reg(offset, val); + } + +private: + unsigned long _base; +}; + +} // namespace device + +// class instance for primary driver of each class +enum CLASS_DEVICE { + CLASS_DEVICE_PRIMARY=0, + CLASS_DEVICE_SECONDARY=1, + CLASS_DEVICE_TERTIARY=2 +}; + +#endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 8c716d9cdb..216fae1ab3 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -35,8 +35,15 @@ # Build the device driver framework. # -SRCS = cdev.cpp \ +#ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = \ device.cpp \ + cdev.cpp \ i2c.cpp \ pio.cpp \ spi.cpp +#endif +#ifeq ($(PX4_TARGET_OS),linux) +SRCS = vcdev.cpp \ + vdevice.cpp +#endif diff --git a/src/drivers/device/vcdev.cpp b/src/drivers/device/vcdev.cpp new file mode 100644 index 0000000000..174094547c --- /dev/null +++ b/src/drivers/device/vcdev.cpp @@ -0,0 +1,643 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file cdev.cpp + * + * Character device base class. + */ + +//#include "px4_platform.h" +//#include "px4_device.h" +#include "px4_posix.h" +#include "device.h" +#include "drivers/drv_device.h" + +#include +#include +#include + +namespace device +{ + +int px4_errno; + + +struct px4_dev_t { + char *name; + void *cdev; + + px4_dev_t(const char *n, void *c) : cdev(c) { + name = strdup(n); + } + + ~px4_dev_t() { free(name); } + +private: + px4_dev_t() {} +}; + +#define PX4_MAX_DEV 100 +static px4_dev_t *devmap[PX4_MAX_DEV]; + +/* + * The standard NuttX operation dispatch table can't call C++ member functions + * directly, so we have to bounce them through this dispatch table. + */ + +CDev::CDev(const char *name, + const char *devname) : + // base class + Device(name), + // public + // protected + _pub_blocked(false), + // private + _devname(devname), + _registered(false), + _open_count(0) +{ + for (unsigned i = 0; i < _max_pollwaiters; i++) + _pollset[i] = nullptr; +} + +CDev::~CDev() +{ + if (_registered) + unregister_driver(_devname); +} + +int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + + int class_instance = 0; + int ret = -ENOSPC; + + while (class_instance < 4) { + char name[32]; + snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); + ret = register_driver(name, (void *)this); + class_instance++; + } + + if (class_instance == 4) { + return ret; + } + return class_instance; +} + +int +CDev::register_driver(const char *name, void *data) +{ + int ret = -ENOSPC; + + if (name == NULL || data == NULL) + return -EINVAL; + + // Make sure the device does not already exist + // FIXME - convert this to a map for efficiency + for (int i=0;iname,name) == 0)) { + return -EEXIST; + } + } + for (int i=0;iname) == 0)) { + delete devmap[i]; + devmap[i] = NULL; + log("Unregistered DEV %s", name); + ret = PX4_OK; + break; + } + } + return ret; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + for (int i=0;iname,name) == 0) { + delete devmap[i]; + devmap[i] = NULL; + return PX4_OK; + } + } + return -EINVAL; +} + +int +CDev::init() +{ + // base class init first + int ret = Device::init(); + + if (ret != PX4_OK) + goto out; + + // now register the driver + if (_devname != nullptr) { + ret = register_driver(_devname, (void *)this); + + if (ret != PX4_OK) + goto out; + + _registered = true; + } + +out: + return ret; +} + +/* + * Default implementations of the character device interface + */ +int +CDev::open(px4_dev_handle_t *handlep) +{ + int ret = PX4_OK; + + debug("CDev::open"); + lock(); + /* increment the open count */ + _open_count++; + + if (_open_count == 1) { + + /* first-open callback may decline the open */ + ret = open_first(handlep); + + if (ret != PX4_OK) + _open_count--; + } + + unlock(); + + return ret; +} + +int +CDev::open_first(px4_dev_handle_t *handlep) +{ + debug("CDev::open_first"); + return PX4_OK; +} + +int +CDev::close(px4_dev_handle_t *handlep) +{ + debug("CDev::close"); + int ret = PX4_OK; + + lock(); + + if (_open_count > 0) { + /* decrement the open count */ + _open_count--; + + /* callback cannot decline the close */ + if (_open_count == 0) + ret = close_last(handlep); + + } else { + ret = -EBADF; + } + + unlock(); + + return ret; +} + +int +CDev::close_last(px4_dev_handle_t *handlep) +{ + debug("CDev::close_last"); + return PX4_OK; +} + +ssize_t +CDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + debug("CDev::read"); + return -ENOSYS; +} + +ssize_t +CDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +{ + debug("CDev::write"); + return -ENOSYS; +} + +off_t +CDev::seek(px4_dev_handle_t *handlep, off_t offset, int whence) +{ + return -ENOSYS; +} + +int +CDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + debug("CDev::ioctl"); + switch (cmd) { + + /* fetch a pointer to the driver's private data */ + case PX4_DIOC_GETPRIV: + *(void **)(uintptr_t)arg = (void *)this; + return PX4_OK; + break; + case PX4_DEVIOCSPUBBLOCK: + _pub_blocked = (arg != 0); + return PX4_OK; + break; + case PX4_DEVIOCGPUBBLOCK: + return _pub_blocked; + break; + } + +#if 0 + /* try the superclass. The different ioctl() function form + * means we need to copy arg */ + unsigned arg2 = arg; + int ret = Device::ioctl(cmd, arg2); + if (ret != -ENODEV) + return ret; +#endif + return -ENOTTY; +} + +int +CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) +{ + int ret = PX4_OK; + log("CDev::Poll %s", setup ? "setup" : "teardown"); + + /* + * Lock against pollnotify() (and possibly other callers) + */ + lock(); + + if (setup) { + /* + * Save the file pointer in the pollfd for the subclass' + * benefit. + */ + fds->priv = (void *)handlep; + log("CDev::poll: fds->priv = %p", handlep); + + /* + * Handle setup requests. + */ + ret = store_poll_waiter(fds); + + if (ret == PX4_OK) { + + /* + * Check to see whether we should send a poll notification + * immediately. + */ + fds->revents |= fds->events & poll_state(handlep); + + /* yes? post the notification */ + if (fds->revents != 0) + sem_post(fds->sem); + } + + } else { + /* + * Handle a teardown request. + */ + ret = remove_poll_waiter(fds); + } + + unlock(); + + return ret; +} + +void +CDev::poll_notify(pollevent_t events) +{ + log("CDev::poll_notify"); + + /* lock against poll() as well as other wakeups */ + lock(); + //irqstate_t state = irqsave(); + + for (unsigned i = 0; i < _max_pollwaiters; i++) + if (nullptr != _pollset[i]) + poll_notify_one(_pollset[i], events); + + unlock(); + //irqrestore(state); +} + +void +CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +{ + log("CDev::poll_notify_one"); + int value; + sem_getvalue(fds->sem, &value); + + /* update the reported event set */ + fds->revents |= fds->events & events; + + /* if the state is now interesting, wake the waiter if it's still asleep */ + /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ + if ((fds->revents != 0) && (value <= 0)) + sem_post(fds->sem); +} + +pollevent_t +CDev::poll_state(px4_dev_handle_t *handlep) +{ + /* by default, no poll events to report */ + return 0; +} + +int +CDev::store_poll_waiter(px4_pollfd_struct_t *fds) +{ + /* + * Look for a free slot. + */ + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (nullptr == _pollset[i]) { + + /* save the pollfd */ + _pollset[i] = fds; + + return PX4_OK; + } + } + + return -ENOMEM; +} + +int +CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) +{ + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (fds == _pollset[i]) { + + _pollset[i] = nullptr; + return PX4_OK; + + } + } + + puts("poll: bad fd state"); + return -EINVAL; +} + +static CDev *get_dev(const char *path) +{ + int i=0; + for (; iname, path) == 0)) { + return (CDev *)(devmap[i]->cdev); + } + } + return NULL; +} + + +#define PX4_MAX_FD 100 +static px4_dev_handle_t *filemap[PX4_MAX_FD] = {}; + +} // namespace device + +extern "C" { + +int px4_errno; + +int +px4_open(const char *path, int flags) +{ + device::CDev *dev = device::get_dev(path); + int ret = 0; + int i; + + if (!dev) { + ret = -EINVAL; + } + else { + for (i=0; iopen(device::filemap[i]); + } + else { + ret = -ENOENT; + } + } + if (ret < 0) { + px4_errno = -ret; + return -1; + } + printf("px4_open fd = %d\n", device::filemap[i]->fd); + return device::filemap[i]->fd; +} + +int +px4_close(int fd) +{ + int ret; + printf("px4_close fd = %d\n", fd); + if (fd < PX4_MAX_FD && fd >= 0) { + ret = ((device::CDev *)(device::filemap[fd]->cdev))->close(device::filemap[fd]); + device::filemap[fd] = NULL; + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +ssize_t +px4_read(int fd, void *buffer, size_t buflen) +{ + int ret; + printf("px4_read fd = %d\n", fd); + if (fd < PX4_MAX_FD && fd >= 0) + ret = ((device::CDev *)(device::filemap[fd]->cdev))->read(device::filemap[fd], (char *)buffer, buflen); + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +ssize_t +px4_write(int fd, const void *buffer, size_t buflen) +{ + int ret = PX4_ERROR; + printf("px4_write fd = %d\n", fd); + if (fd < PX4_MAX_FD && fd >= 0) + ret = ((device::CDev *)(device::filemap[fd]->cdev))->write(device::filemap[fd], (const char *)buffer, buflen); + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +int +px4_ioctl(int fd, int cmd, unsigned long arg) +{ + int ret = PX4_ERROR; + if (fd < PX4_MAX_FD && fd >= 0) + ret = ((device::CDev *)(device::filemap[fd]->cdev))->ioctl(device::filemap[fd], cmd, arg); + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + else { + px4_errno = -EINVAL; + } + return ret; +} + +int +px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) +{ + sem_t sem; + int count = 0; + int ret; + unsigned int i; + struct timespec ts; + + printf("Called px4_poll %d\n", timeout); + sem_init(&sem, 0, 0); + + // For each fd + for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) + { + + // TODO - get waiting FD events + + printf("Called setup CDev->poll %d\n", fds[i].fd); + ret = ((device::CDev *)(device::filemap[fds[i].fd]->cdev))->poll(device::filemap[fds[i].fd], &fds[i], true); + + if (ret < 0) + break; + } + } + + if (ret >= 0) + { + if (timeout >= 0) + { + ts.tv_sec = timeout/1000; + ts.tv_nsec = (timeout % 1000)*1000; + //log("sem_wait for %d\n", timeout); + sem_timedwait(&sem, &ts); + //log("sem_wait finished\n"); + } + else + { + printf("Blocking sem_wait\n"); + sem_wait(&sem); + printf("Blocking sem_wait finished\n"); + } + + // For each fd + for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) + { + printf("Called CDev->poll %d\n", fds[i].fd); + ret = ((device::CDev *)(device::filemap[fds[i].fd]->cdev))->poll(device::filemap[fds[i].fd], &fds[i], false); + + if (ret < 0) + break; + } + } + } + sem_destroy(&sem); + + return count; +} + +} + diff --git a/src/drivers/device/vdevice.cpp b/src/drivers/device/vdevice.cpp new file mode 100644 index 0000000000..7098339649 --- /dev/null +++ b/src/drivers/device/vdevice.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file device.cpp + * + * Fundamental driver base class for the virtual device framework. + */ + +#include "device.h" + +#include +#include +#include + +namespace device +{ + +Device::Device(const char *name) : + // public + // protected + _name(name), + _debug_enabled(true) +{ + sem_init(&_lock, 0, 1); + + /* setup a default device ID. When bus_type is UNKNOWN the + other fields are invalid */ + _device_id.devid = 0; + _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; + _device_id.devid_s.bus = 0; + _device_id.devid_s.address = 0; + _device_id.devid_s.devtype = 0; +} + +Device::~Device() +{ + sem_destroy(&_lock); +} + +int +Device::init() +{ + int ret = OK; + + return ret; +} + +void +Device::log(const char *fmt, ...) +{ + va_list ap; + + printf("[%s] ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +} + +void +Device::debug(const char *fmt, ...) +{ + va_list ap; + + if (_debug_enabled) { + printf("<%s> ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); + } +} + +} // namespace device diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdevice.h new file mode 100644 index 0000000000..b9d742118e --- /dev/null +++ b/src/drivers/device/vdevice.h @@ -0,0 +1,496 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file vdevice.h + * + * Definitions for the generic base classes in the virtual device framework. + */ + +#pragma once + +/* + * Includes here should only cover the needs of the framework definitions. + */ +#include + +#include +#include +#include +#include +#include +#include + +/** + * Namespace encapsulating all device framework classes, functions and data. + */ +namespace device __EXPORT +{ + +#define PX4_F_RDONLY 1 +#define PX4_F_WRONLY 2 + +struct px4_dev_handle_t { + int fd; + int flags; + void *priv; + void *cdev; + + px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), cdev(NULL) {} + px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), cdev(c) {} +}; + +/** + * Fundamental base class for all device drivers. + * + * This class handles the basic "being a driver" things, including + * interrupt registration and dispatch. + */ +class __EXPORT Device +{ +public: + /** + * Destructor. + * + * Public so that anonymous devices can be destroyed. + */ + virtual ~Device(); + + /* + * Direct access methods. + */ + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialized OK, negative errno otherwise; + */ + virtual int init(); + + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, + }; + + /* + broken out device elements. The bitfields are used to keep + the overall value small enough to fit in a float accurately, + which makes it possible to transport over the MAVLink + parameter protocol without loss of information. + */ + struct DeviceStructure { + enum DeviceBusType bus_type:3; + uint8_t bus:5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; + + union DeviceId { + struct DeviceStructure devid_s; + uint32_t devid; + }; + +protected: + const char *_name; /**< driver name */ + bool _debug_enabled; /**< if true, debug messages are printed */ + union DeviceId _device_id; /**< device identifier information */ + + /** + * Constructor + * + * @param name Driver name + * @param irq Interrupt assigned to the device. + */ + Device(const char *name); + + /** + * Take the driver lock. + * + * Each driver instance has its own lock/semaphore. + * + * Note that we must loop as the wait may be interrupted by a signal. + */ + void lock() { + log("lock"); + do {} while (sem_wait(&_lock) != 0); + } + + /** + * Release the driver lock. + */ + void unlock() { + log("unlock"); + sem_post(&_lock); + } + + /** + * Log a message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void log(const char *fmt, ...); + + /** + * Print a debug message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void debug(const char *fmt, ...); + +private: + sem_t _lock; + + /** disable copy construction for this and all subclasses */ + Device(const Device &); + + /** disable assignment for this and all subclasses */ + Device &operator = (const Device &); +}; + +/** + * Abstract class for any character device + */ +class __EXPORT CDev : public Device +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + */ + CDev(const char *name, const char *devname); + + /** + * Destructor + */ + virtual ~CDev(); + + virtual int init(); + + /** + * Perform a poll setup/teardown operation. Based on NuttX implementation. + * + * This is handled internally and should not normally be overridden. + * + * @param handlep Pointer to the internal file structure. + * @param fds Poll descriptor being waited on. + * @param setup True if this is establishing a request, false if + * it is being torn down. + * @return OK on success, or -errno otherwise. + */ + virtual int poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup); + + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * Note - not virtual, cannot be overridden by a subclass. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } + + /** + * Handle an open of the device. + * + * This function is called for every open of the device. The default + * implementation maintains _open_count and always returns OK. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the open is allowed, -errno otherwise. + */ + virtual int open(px4_dev_handle_t *handlep); + + /** + * Handle a close of the device. + * + * This function is called for every close of the device. The default + * implementation maintains _open_count and returns OK as long as it is not zero. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the close was successful, -errno otherwise. + */ + virtual int close(px4_dev_handle_t *handlep); + + /** + * Perform a read from the device. + * + * The default implementation returns -ENOSYS. + * + * @param handlep Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer into which data should be placed. + * @param buflen The number of bytes to be read. + * @return The number of bytes read or -errno otherwise. + */ + virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); + + /** + * Perform a write to the device. + * + * The default implementation returns -ENOSYS. + * + * @param handlep Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer from which data should be read. + * @param buflen The number of bytes to be written. + * @return The number of bytes written or -errno otherwise. + */ + virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); + + /** + * Perform a logical seek operation on the device. + * + * The default implementation returns -ENOSYS. + * + * @param handlep Pointer to the NuttX file structure. + * @param offset The new file position relative to whence. + * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. + * @return The previous offset, or -errno otherwise. + */ + virtual off_t seek(px4_dev_handle_t *handlep, off_t offset, int whence); + + /** + * Perform an ioctl operation on the device. + * + * The default implementation handles DIOC_GETPRIV, and otherwise + * returns -ENOTTY. Subclasses should call the default implementation + * for any command they do not handle themselves. + * + * @param handlep Pointer to the NuttX file structure. + * @param cmd The ioctl command value. + * @param arg The ioctl argument value. + * @return OK on success, or -errno otherwise. + */ + virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + +protected: + + int register_driver(const char *name, void *data); + int unregister_driver(const char *name); + + /** + * Check the current state of the device for poll events from the + * perspective of the file. + * + * This function is called by the default poll() implementation when + * a poll is set up to determine whether the poll should return immediately. + * + * The default implementation returns no events. + * + * @param handlep The file that's interested. + * @return The current set of poll events. + */ + virtual pollevent_t poll_state(px4_dev_handle_t *handlep); + + /** + * Report new poll events. + * + * This function should be called anytime the state of the device changes + * in a fashion that might be interesting to a poll waiter. + * + * @param events The new event(s) being announced. + */ + virtual void poll_notify(pollevent_t events); + + /** + * Internal implementation of poll_notify. + * + * @param fds A poll waiter to notify. + * @param events The event(s) to send to the waiter. + */ + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + + /** + * Notification of the first open. + * + * This function is called when the device open count transitions from zero + * to one. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(px4_dev_handle_t *handlep); + + /** + * Notification of the last close. + * + * This function is called when the device open count transitions from + * one to zero. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param handlep Pointer to the NuttX file structure. + * @return OK if the open should return OK, -errno otherwise. + */ + virtual int close_last(px4_dev_handle_t *handlep); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + + bool _pub_blocked; /**< true if publishing should be blocked */ + +private: + static const unsigned _max_pollwaiters = 8; + + const char *_devname; /**< device node name */ + bool _registered; /**< true if device name was registered */ + unsigned _open_count; /**< number of successful opens */ + + px4_pollfd_struct_t *_pollset[_max_pollwaiters]; + + /** + * Store a pollwaiter in a slot where we can find it later. + * + * Expands the pollset as required. Must be called with the driver locked. + * + * @return OK, or -errno on error. + */ + int store_poll_waiter(px4_pollfd_struct_t *fds); + + /** + * Remove a poll waiter. + * + * @return OK, or -errno on error. + */ + int remove_poll_waiter(px4_pollfd_struct_t *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); +}; + +/** + * Abstract class for character device accessed via PIO + */ +class __EXPORT PIO : public CDev +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param base Base address of the device PIO area + * @param irq Interrupt assigned to the device (or zero if none) + */ + PIO(const char *name, + const char *devname, + unsigned long base + ); + virtual ~PIO(); + + virtual int init(); + +protected: + + /** + * Read a register + * + * @param offset Register offset in bytes from the base address. + */ + uint32_t reg(uint32_t offset) { + return *(volatile uint32_t *)(_base + offset); + } + + /** + * Write a register + * + * @param offset Register offset in bytes from the base address. + * @param value Value to write. + */ + void reg(uint32_t offset, uint32_t value) { + *(volatile uint32_t *)(_base + offset) = value; + } + + /** + * Modify a register + * + * Note that there is a risk of a race during the read/modify/write cycle + * that must be taken care of by the caller. + * + * @param offset Register offset in bytes from the base address. + * @param clearbits Bits to clear in the register + * @param setbits Bits to set in the register + */ + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + uint32_t val = reg(offset); + val &= ~clearbits; + val |= setbits; + reg(offset, val); + } + +private: + unsigned long _base; +}; + +} // namespace device + +// class instance for primary driver of each class +enum CLASS_DEVICE { + CLASS_DEVICE_PRIMARY=0, + CLASS_DEVICE_SECONDARY=1, + CLASS_DEVICE_TERTIARY=2 +}; + diff --git a/src/platforms/linux/vcdev_test/module.mk b/src/platforms/linux/vcdev_test/module.mk new file mode 100644 index 0000000000..72cde9e2e3 --- /dev/null +++ b/src/platforms/linux/vcdev_test/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = vcdevtest + +SRCS = vcdevtest_main.cpp \ + vcdevtest_start_linux.cpp \ + vcdevtest_example.cpp + diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp new file mode 100644 index 0000000000..dadd9a2913 --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp @@ -0,0 +1,150 @@ + +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file vcdevtest_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "vcdevtest_example.h" +#include "drivers/device/device.h" +#include +#include + +px4::AppMgr VCDevExample::mgr; + +using namespace device; + +#define TESTDEV "/dev/vdevex" + +class VCDevNode : public CDev { +public: + VCDevNode() : CDev("vcdevtest", TESTDEV) {}; + + ~VCDevNode() {} +}; + +VCDevExample::~VCDevExample() { + if (_node) { + delete _node; + _node = 0; + } +} + +int test_pub_block(int fd, unsigned long blocked) +{ + int ret = px4_ioctl(fd, PX4_DEVIOCSPUBBLOCK, blocked); + if (ret < 0) { + printf("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); + return -px4_errno; + } + + ret = px4_ioctl(fd, PX4_DEVIOCGPUBBLOCK, 0); + if (ret < 0) { + printf("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); + return -px4_errno; + } + printf("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + + return 0; +} + +int VCDevExample::main() +{ + mgr.setRunning(true); + + _node = new VCDevNode(); + + if (_node == 0) { + printf("Failed to allocate VCDevNode\n"); + return -ENOMEM; + } + + if (_node->init() != PX4_OK) { + printf("Failed to init VCDevNode\n"); + return 1; + } + + int fd = px4_open(TESTDEV, PX4_F_RDONLY); + + if (fd < 0) { + printf("Open failed %d %d", fd, px4_errno); + return -px4_errno; + } + + void *p = 0; + int ret = px4_ioctl(fd, PX4_DIOC_GETPRIV, (unsigned long)&p); + if (ret < 0) { + printf("ioctl PX4_DIOC_GETPRIV failed %d %d", ret, px4_errno); + return -px4_errno; + } + printf("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); + + ret = test_pub_block(fd, 1); + if (ret < 0) + return ret; + ret = test_pub_block(fd, 0); + if (ret < 0) + return ret; + + int i=0; + px4_pollfd_struct_t fds[1]; + + while (!mgr.exitRequested() && i<5) { + sleep(2); + + printf(" polling...\n"); + fds[0].fd = fd; + fds[0].events = POLLIN; + fds[0].revents = 0; + + ret = px4_poll(fds, 1, 500); + if (ret < 0) { + printf("poll failed %d %d\n", ret, px4_errno); + px4_close(fd); + } + else if (ret == 0) + printf(" Nothing to read\n"); + else { + printf(" %d to read\n", ret); + } + printf(" Doing work...\n"); + ++i; + } + px4_close(fd); + + return 0; +} diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.h b/src/platforms/linux/vcdev_test/vcdevtest_example.h new file mode 100644 index 0000000000..ed00fd5cb9 --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file vcdevtest_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include + +class VCDevNode; + +class VCDevExample { +public: + VCDevExample() : _node(0) {}; + + ~VCDevExample(); + + int main(); + + static px4::AppMgr mgr; /* Manage requests to terminate app */ + +private: + VCDevNode *_node; +}; diff --git a/src/platforms/linux/vcdev_test/vcdevtest_main.cpp b/src/platforms/linux/vcdev_test/vcdevtest_main.cpp new file mode 100644 index 0000000000..6d8d31a4c1 --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_main.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file vcdevtest_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include "vcdevtest_example.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "vcdevtest"); + + printf("vcdevtest\n"); + VCDevExample vcdevtest; + vcdevtest.main(); + + printf("goodbye\n"); + return 0; +} diff --git a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp new file mode 100644 index 0000000000..2f37fb05de --- /dev/null +++ b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file vcdevtest_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "vcdevtest_example.h" +#include +#include +#include +#include +#include + +#define SCHED_DEFAULT SCHED_FIFO +#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int vcdevtest_main(int argc, char *argv[]); +int vcdevtest_main(int argc, char *argv[]) +{ + + if (argc < 2) { + printf("usage: vcdevtest {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (VCDevExample::mgr.isRunning()) { + printf("already running\n"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("vcdevtest", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + VCDevExample::mgr.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (VCDevExample::mgr.isRunning()) { + printf("is running\n"); + + } else { + printf("not started\n"); + } + + return 0; + } + + printf("usage: vcdevtest_main {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index d08c8d2380..88b8f4e80c 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -47,6 +47,13 @@ #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 +#define PX4_DIOC_GETPRIV 1 +#define PX4_DEVIOCSPUBBLOCK 2 +#define PX4_DEVIOCGPUBBLOCK 3 + +#define PX4_ERROR (-1) +#define PX4_OK 0 + __BEGIN_DECLS extern int px4_errno; From 4016ad2ff52163c78db42662d18f02312db333e6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 12 Mar 2015 16:21:40 -0700 Subject: [PATCH 021/258] Revert uORB to previous version Signed-off-by: Mark Charlebois --- src/modules/uORB/uORB.cpp | 117 ++++++++++++++++++-------------------- 1 file changed, 55 insertions(+), 62 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b027453702..64ee78414d 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -36,11 +36,7 @@ * A lightweight object broker. */ -#include -#include -#include -#include -#include +#include #include @@ -50,12 +46,16 @@ #include #include #include +#include #include #include #include #include #include +#include +#include +#include #include #include @@ -123,17 +123,17 @@ public: ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); - virtual int open(device::px4_dev_handle_t *handlep); - virtual int close(device::px4_dev_handle_t *handlep); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual ssize_t write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int open(struct file *filp); + virtual int close(struct file *filp); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); protected: - virtual pollevent_t poll_state(device::px4_dev_handle_t *handlep); - virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + virtual pollevent_t poll_state(struct file *filp); + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); private: struct SubscriberData { @@ -152,8 +152,8 @@ private: pid_t _publisher; /**< if nonzero, current publisher */ const int _priority; /**< priority of topic */ - SubscriberData *handlep_to_sd(device::px4_dev_handle_t *handlep) { - SubscriberData *sd = (SubscriberData *)(handlep->priv); + SubscriberData *filp_to_sd(struct file *filp) { + SubscriberData *sd = (SubscriberData *)(filp->f_priv); return sd; } @@ -199,12 +199,12 @@ ORBDevNode::~ORBDevNode() } int -ORBDevNode::open(device::px4_dev_handle_t *handlep) +ORBDevNode::open(struct file *filp) { int ret; /* is this a publisher? */ - if (handlep->flags == PX4_F_WRONLY) { + if (filp->f_oflags == O_WRONLY) { /* become the publisher if we can */ lock(); @@ -221,7 +221,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) /* now complete the open */ if (ret == OK) { - ret = CDev::open(handlep); + ret = CDev::open(filp); /* open failed - not the publisher anymore */ if (ret != OK) @@ -232,7 +232,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) } /* is this a new subscriber? */ - if (handlep->flags == PX4_F_RDONLY) { + if (filp->f_oflags == O_RDONLY) { /* allocate subscriber data */ SubscriberData *sd = new SubscriberData; @@ -248,9 +248,9 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) /* set priority */ sd->priority = _priority; - handlep->priv = (void *)sd; + filp->f_priv = (void *)sd; - ret = CDev::open(handlep); + ret = CDev::open(filp); if (ret != OK) delete sd; @@ -263,14 +263,14 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) } int -ORBDevNode::close(device::px4_dev_handle_t *handlep) +ORBDevNode::close(struct file *filp) { /* is this the publisher closing? */ if (getpid() == _publisher) { _publisher = 0; } else { - SubscriberData *sd = handlep_to_sd(handlep); + SubscriberData *sd = filp_to_sd(filp); if (sd != nullptr) { hrt_cancel(&sd->update_call); @@ -278,13 +278,13 @@ ORBDevNode::close(device::px4_dev_handle_t *handlep) } } - return CDev::close(handlep); + return CDev::close(filp); } ssize_t -ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) { - SubscriberData *sd = (SubscriberData *)handlep_to_sd(handlep); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); /* if the object has not been written yet, return zero */ if (_data == nullptr) @@ -321,7 +321,7 @@ ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) } ssize_t -ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) { /* * Writes are legal from interrupt context as long as the @@ -330,7 +330,7 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t * Writes outside interrupt context will allocate the object * if it has not yet been allocated. * - * Note that handlep will usually be NULL. + * Note that filp will usually be NULL. */ if (nullptr == _data) { if (!up_interrupt_context()) { @@ -369,9 +369,9 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t } int -ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) { - SubscriberData *sd = handlep_to_sd(handlep); + SubscriberData *sd = filp_to_sd(filp); switch (cmd) { case ORBIOCLASTUPDATE: @@ -396,7 +396,7 @@ ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return CDev::ioctl(handlep, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } @@ -427,9 +427,9 @@ ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *d } pollevent_t -ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) +ORBDevNode::poll_state(struct file *filp) { - SubscriberData *sd = handlep_to_sd(handlep); + SubscriberData *sd = filp_to_sd(filp); /* * If the topic appears updated to the subscriber, say so. @@ -441,9 +441,9 @@ ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) } void -ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +ORBDevNode::poll_notify_one(struct pollfd *fds, pollevent_t events) { - SubscriberData *sd = handlep_to_sd((device::px4_dev_handle_t *)fds->priv); + SubscriberData *sd = filp_to_sd((struct file *)fds->priv); /* * If the topic looks updated to the subscriber, go ahead and notify them. @@ -560,7 +560,7 @@ public: ORBDevMaster(Flavor f); ~ORBDevMaster(); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: Flavor _flavor; }; @@ -580,7 +580,7 @@ ORBDevMaster::~ORBDevMaster() } int -ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret; @@ -632,7 +632,6 @@ ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar devpath = strdup(nodepath); if (devpath == nullptr) { - // FIXME - looks like we leaked memory here for objname return -ENOMEM; } @@ -642,8 +641,6 @@ ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar /* if we didn't get a device, that's bad */ if (node == nullptr) { unlock(); - - // FIXME - looks like we leaked memory here for devpath and objname return -ENOMEM; } @@ -673,7 +670,7 @@ ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar default: /* give it to the superclass */ - return CDev::ioctl(handlep, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } @@ -748,7 +745,7 @@ int pubsublatency_main(void) float latency_integral = 0.0f; /* wakeup source(s) */ - px4_pollfd_struct_t fds[3]; + struct pollfd fds[3]; int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); @@ -775,7 +772,7 @@ int pubsublatency_main(void) for (unsigned i = 0; i < maxruns; i++) { /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(orb_test), test_multi_sub, &t); timingsgroup = 0; @@ -968,11 +965,11 @@ latency_test(orb_id_t T, bool print) /* test pub / sub latency */ - int pubsub_task = px4_task_spawn_cmd("uorb_latency", + int pubsub_task = task_spawn_cmd("uorb_latency", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, - (px4_main_t)&pubsublatency_main, + (main_t)&pubsublatency_main, nullptr); /* give the test task some data */ @@ -1095,13 +1092,13 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri const struct orb_advertdata adv = { meta, instance, priority }; /* open the control device */ - fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); + fd = open(TOPIC_MASTER_DEVICE_PATH, 0); if (fd < 0) goto out; /* advertise the object */ - ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); /* it's OK if it already exists */ if ((OK != ret) && (EEXIST == errno)) { @@ -1111,7 +1108,7 @@ node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int pri out: if (fd >= 0) - px4_close(fd); + close(fd); return ret; } @@ -1148,9 +1145,6 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* * Generate the path to the node and try to open it. */ - - // FIXME - if *instance is uninitialized, why is this being called? Seems risky and - // its definiately a waste. This is the case in muli-topic test. ret = node_mkpath(path, f, meta, instance); if (ret != OK) { @@ -1159,13 +1153,12 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve } /* open the path as either the advertiser or the subscriber */ - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); /* if we want to advertise and the node existed, we have to re-try again */ if ((fd >= 0) && (instance != nullptr) && (advertiser)) { /* close the fd, we want a new one */ - px4_close(fd); - + close(fd); /* the node_advertise call will automatically go for the next free entry */ fd = -1; } @@ -1188,7 +1181,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* on success, try the open again */ if (ret == OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); } } @@ -1221,8 +1214,8 @@ orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *inst return ERROR; /* get the advertiser handle and close the node */ - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - px4_close(fd); + result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + close(fd); if (result == ERROR) return ERROR; @@ -1250,7 +1243,7 @@ orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) int orb_unsubscribe(int handle) { - return px4_close(handle); + return close(handle); } int @@ -1264,7 +1257,7 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { int ret; - ret = px4_read(handle, (char *)buffer, meta->o_size); + ret = read(handle, buffer, meta->o_size); if (ret < 0) return ERROR; @@ -1280,24 +1273,24 @@ orb_copy(const struct orb_metadata *meta, int handle, void *buffer) int orb_check(int handle, bool *updated) { - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); } int orb_stat(int handle, uint64_t *time) { - return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); + return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } int orb_priority(int handle, int *priority) { - return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); + return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } int orb_set_interval(int handle, unsigned interval) { - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); } From 97a72563b800e27aadebe1bf28a5217fa3144bc9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 12 Mar 2015 16:29:17 -0700 Subject: [PATCH 022/258] Fixes to compile again for NuttX Signed-off-by: Mark Charlebois --- src/drivers/device/device_nuttx.h | 8 ++++---- src/drivers/device/module.mk | 8 ++++---- src/platforms/px4_posix.h | 1 + 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index 87e5e2abe0..3d5737cee4 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -51,6 +51,8 @@ #include #include +#include + /** * Namespace encapsulating all device framework classes, functions and data. */ @@ -89,7 +91,6 @@ public: */ virtual int init(); -#if 0 /** * Read directly from the device. * @@ -122,7 +123,6 @@ public: * @return Negative errno on error, OK or positive value on success. */ virtual int ioctl(unsigned operation, unsigned &arg); -#endif /* device bus types for DEVID @@ -513,7 +513,7 @@ protected: * * @param offset Register offset in bytes from the base address. */ - unsigned long reg(uint32_t offset) { + uint32_t reg(uint32_t offset) { return *(volatile uint32_t *)(_base + offset); } @@ -545,7 +545,7 @@ protected: } private: - unsigned long _base; + uint32_t _base; }; } // namespace device diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 216fae1ab3..522639dc17 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -35,15 +35,15 @@ # Build the device driver framework. # -#ifeq ($(PX4_TARGET_OS),nuttx) +ifeq ($(PX4_TARGET_OS),nuttx) SRCS = \ device.cpp \ cdev.cpp \ i2c.cpp \ pio.cpp \ spi.cpp -#endif -#ifeq ($(PX4_TARGET_OS),linux) +endif +ifeq ($(PX4_TARGET_OS),linux) SRCS = vcdev.cpp \ vdevice.cpp -#endif +endif diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 88b8f4e80c..5694d4e865 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include From 9718404d7f61e501e720f83cc3c6163a443addd9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 11:49:32 -0700 Subject: [PATCH 023/258] Added new file for virt dev posix-like functions and test case Moved posix-like functions to vcdev_posix.cpp and updated the copyright notice. Added test case to make sure poll unblocks when a write occurs. Signed-off-by: Mark Charlebois --- src/drivers/device/module.mk | 3 +- src/drivers/device/vcdev.cpp | 213 ++----------- src/drivers/device/vcdev_posix.cpp | 281 ++++++++++++++++++ src/drivers/device/vdevice.cpp | 2 +- src/drivers/device/vdevice.h | 6 +- .../linux/vcdev_test/vcdevtest_example.cpp | 45 ++- .../vcdev_test/vcdevtest_start_linux.cpp | 5 - src/platforms/px4_tasks.h | 6 + 8 files changed, 355 insertions(+), 206 deletions(-) create mode 100644 src/drivers/device/vcdev_posix.cpp diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 522639dc17..5475e01e41 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -45,5 +45,6 @@ SRCS = \ endif ifeq ($(PX4_TARGET_OS),linux) SRCS = vcdev.cpp \ - vdevice.cpp + vdevice.cpp \ + vcdev_posix.cpp endif diff --git a/src/drivers/device/vcdev.cpp b/src/drivers/device/vcdev.cpp index 174094547c..edebe3b416 100644 --- a/src/drivers/device/vcdev.cpp +++ b/src/drivers/device/vcdev.cpp @@ -32,13 +32,11 @@ ****************************************************************************/ /** - * @file cdev.cpp + * @file vcdev.cpp * - * Character device base class. + * Virtual character device base class. */ -//#include "px4_platform.h" -//#include "px4_device.h" #include "px4_posix.h" #include "device.h" #include "drivers/drv_device.h" @@ -138,7 +136,7 @@ CDev::register_driver(const char *name, void *data) for (int i=0;iname) == 0)) { delete devmap[i]; devmap[i] = NULL; - log("Unregistered DEV %s", name); + debug("Unregistered DEV %s", name); ret = PX4_OK; break; } @@ -325,7 +323,7 @@ int CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) { int ret = PX4_OK; - log("CDev::Poll %s", setup ? "setup" : "teardown"); + debug("CDev::Poll %s", setup ? "setup" : "teardown"); /* * Lock against pollnotify() (and possibly other callers) @@ -338,7 +336,7 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * benefit. */ fds->priv = (void *)handlep; - log("CDev::poll: fds->priv = %p", handlep); + debug("CDev::poll: fds->priv = %p", handlep); /* * Handle setup requests. @@ -373,30 +371,33 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) void CDev::poll_notify(pollevent_t events) { - log("CDev::poll_notify"); + debug("CDev::poll_notify events = %0x", events); /* lock against poll() as well as other wakeups */ lock(); - //irqstate_t state = irqsave(); for (unsigned i = 0; i < _max_pollwaiters; i++) if (nullptr != _pollset[i]) poll_notify_one(_pollset[i], events); + for (unsigned i = 0; i < _max_pollwaiters; i++) + if (nullptr != _pollset[i]) + debug(" CHECK fds=%p %0x %0x",_pollset[i], _pollset[i]->revents, _pollset[i]->events); unlock(); - //irqrestore(state); } void CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - log("CDev::poll_notify_one"); + debug("CDev::poll_notify_one"); int value; sem_getvalue(fds->sem, &value); /* update the reported event set */ fds->revents |= fds->events & events; + debug(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value); + /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ if ((fds->revents != 0) && (value <= 0)) @@ -406,6 +407,7 @@ CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) pollevent_t CDev::poll_state(px4_dev_handle_t *handlep) { + debug("CDev::poll_notify"); /* by default, no poll events to report */ return 0; } @@ -416,6 +418,7 @@ CDev::store_poll_waiter(px4_pollfd_struct_t *fds) /* * Look for a free slot. */ + debug("CDev::store_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -432,6 +435,7 @@ CDev::store_poll_waiter(px4_pollfd_struct_t *fds) int CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { + debug("CDev::remove_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -445,7 +449,7 @@ CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) return -EINVAL; } -static CDev *get_dev(const char *path) +CDev *CDev::getDev(const char *path) { int i=0; for (; iopen(device::filemap[i]); - } - else { - ret = -ENOENT; - } - } - if (ret < 0) { - px4_errno = -ret; - return -1; - } - printf("px4_open fd = %d\n", device::filemap[i]->fd); - return device::filemap[i]->fd; -} - -int -px4_close(int fd) -{ - int ret; - printf("px4_close fd = %d\n", fd); - if (fd < PX4_MAX_FD && fd >= 0) { - ret = ((device::CDev *)(device::filemap[fd]->cdev))->close(device::filemap[fd]); - device::filemap[fd] = NULL; - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - return ret; -} - -ssize_t -px4_read(int fd, void *buffer, size_t buflen) -{ - int ret; - printf("px4_read fd = %d\n", fd); - if (fd < PX4_MAX_FD && fd >= 0) - ret = ((device::CDev *)(device::filemap[fd]->cdev))->read(device::filemap[fd], (char *)buffer, buflen); - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - return ret; -} - -ssize_t -px4_write(int fd, const void *buffer, size_t buflen) -{ - int ret = PX4_ERROR; - printf("px4_write fd = %d\n", fd); - if (fd < PX4_MAX_FD && fd >= 0) - ret = ((device::CDev *)(device::filemap[fd]->cdev))->write(device::filemap[fd], (const char *)buffer, buflen); - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - return ret; -} - -int -px4_ioctl(int fd, int cmd, unsigned long arg) -{ - int ret = PX4_ERROR; - if (fd < PX4_MAX_FD && fd >= 0) - ret = ((device::CDev *)(device::filemap[fd]->cdev))->ioctl(device::filemap[fd], cmd, arg); - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - else { - px4_errno = -EINVAL; - } - return ret; -} - -int -px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) -{ - sem_t sem; - int count = 0; - int ret; - unsigned int i; - struct timespec ts; - - printf("Called px4_poll %d\n", timeout); - sem_init(&sem, 0, 0); - - // For each fd - for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) - { - - // TODO - get waiting FD events - - printf("Called setup CDev->poll %d\n", fds[i].fd); - ret = ((device::CDev *)(device::filemap[fds[i].fd]->cdev))->poll(device::filemap[fds[i].fd], &fds[i], true); - - if (ret < 0) - break; - } - } - - if (ret >= 0) - { - if (timeout >= 0) - { - ts.tv_sec = timeout/1000; - ts.tv_nsec = (timeout % 1000)*1000; - //log("sem_wait for %d\n", timeout); - sem_timedwait(&sem, &ts); - //log("sem_wait finished\n"); - } - else - { - printf("Blocking sem_wait\n"); - sem_wait(&sem); - printf("Blocking sem_wait finished\n"); - } - - // For each fd - for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) - { - printf("Called CDev->poll %d\n", fds[i].fd); - ret = ((device::CDev *)(device::filemap[fds[i].fd]->cdev))->poll(device::filemap[fds[i].fd], &fds[i], false); - - if (ret < 0) - break; - } - } - } - sem_destroy(&sem); - - return count; -} - -} - diff --git a/src/drivers/device/vcdev_posix.cpp b/src/drivers/device/vcdev_posix.cpp new file mode 100644 index 0000000000..6df88a2f66 --- /dev/null +++ b/src/drivers/device/vcdev_posix.cpp @@ -0,0 +1,281 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file vcdev_posix.cpp + * + * POSIX-like API for virtual character device + */ + +#include "px4_posix.h" +#include "device.h" + +#include +#include +#include +#include +#include + +#define DEBUG(...) +//#define DEBUG(...) printf(__VA_ARGS__) + +using namespace device; + +extern "C" { + +struct timerData { + sem_t &sem; + struct timespec &ts; + + timerData(sem_t &s, struct timespec &t) : sem(s), ts(t) {} + ~timerData() {} +}; + +static void *timer_handler(void *data) +{ + struct timerData *td = (struct timerData *)data; + + if (td->ts.tv_sec) { + sleep(td->ts.tv_sec); + } + usleep(td->ts.tv_nsec/1000); + sem_post(&(td->sem)); + + printf("Timer expired\n"); + return 0; +} + +#define PX4_MAX_FD 100 +static px4_dev_handle_t *filemap[PX4_MAX_FD] = {}; + +int px4_errno; + +int +px4_open(const char *path, int flags) +{ + CDev *dev = CDev::getDev(path); + int ret = 0; + int i; + + if (!dev) { + ret = -EINVAL; + } + else { + for (i=0; iopen(filemap[i]); + } + else { + ret = -ENOENT; + } + } + if (ret < 0) { + px4_errno = -ret; + return -1; + } + DEBUG("px4_open fd = %d", filemap[i]->fd); + return filemap[i]->fd; +} + +int +px4_close(int fd) +{ + int ret; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_close fd = %d\n", fd); + ret = dev->close(filemap[fd]); + filemap[fd] = NULL; + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +ssize_t +px4_read(int fd, void *buffer, size_t buflen) +{ + int ret; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_read fd = %d\n", fd); + ret = dev->read(filemap[fd], (char *)buffer, buflen); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +ssize_t +px4_write(int fd, const void *buffer, size_t buflen) +{ + int ret = PX4_ERROR; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_write fd = %d\n", fd); + ret = dev->write(filemap[fd], (const char *)buffer, buflen); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + return ret; +} + +int +px4_ioctl(int fd, int cmd, unsigned long arg) +{ + int ret = PX4_ERROR; + if (fd < PX4_MAX_FD && fd >= 0) { + CDev *dev = (CDev *)(filemap[fd]->cdev); + DEBUG("px4_ioctl fd = %d\n", fd); + ret = dev->ioctl(filemap[fd], cmd, arg); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + else { + px4_errno = -EINVAL; + } + return ret; +} + +int +px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) +{ + sem_t sem; + int count = 0; + int ret; + unsigned int i; + struct timespec ts; + + printf("Called px4_poll timeout = %d\n", timeout); + sem_init(&sem, 0, 0); + + // For each fd + for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) + { + CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; + DEBUG("px4_poll: CDev->poll(setup) %d\n", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + + if (ret < 0) + break; + DEBUG("xxxx fds=%p fds[%d].revents = %d\n", fds, i, fds[i].revents); + } + } + + if (ret >= 0) + { + if (timeout >= 0) + { + pthread_t pt; + void *res; + + ts.tv_sec = timeout/1000; + ts.tv_nsec = (timeout % 1000)*1000; + + // Create a timer to unblock + struct timerData td(sem, ts); + int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td); + if (rv != 0) { + count = -1; + goto cleanup; + } + sem_wait(&sem); + + // Make sure timer thread is killed before sem goes + // out of scope + (void)pthread_cancel(pt); + (void)pthread_join(pt, &res); + } + else + { + sem_wait(&sem); + } + + // For each fd + for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) + { + DEBUG("zzzz fds=%p fds[%d].revents = %d\n",fds, i, fds[i].revents); + + CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; + DEBUG("px4_poll: CDev->poll(teardown) %d\n", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], false); + + if (ret < 0) + break; + + DEBUG("yyyy fds=%p fds[%d].revents = %d\n", fds, i, fds[i].revents); + if (fds[i].revents) + count += 1; + } + } + } + +cleanup: + sem_destroy(&sem); + + return count; +} + +} + diff --git a/src/drivers/device/vdevice.cpp b/src/drivers/device/vdevice.cpp index 7098339649..487249aed5 100644 --- a/src/drivers/device/vdevice.cpp +++ b/src/drivers/device/vdevice.cpp @@ -50,7 +50,7 @@ Device::Device(const char *name) : // public // protected _name(name), - _debug_enabled(true) + _debug_enabled(false) { sem_init(&_lock, 0, 1); diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdevice.h index b9d742118e..f35ac9fc19 100644 --- a/src/drivers/device/vdevice.h +++ b/src/drivers/device/vdevice.h @@ -146,7 +146,7 @@ protected: * Note that we must loop as the wait may be interrupted by a signal. */ void lock() { - log("lock"); + debug("lock"); do {} while (sem_wait(&_lock) != 0); } @@ -154,7 +154,7 @@ protected: * Release the driver lock. */ void unlock() { - log("unlock"); + debug("unlock"); sem_post(&_lock); } @@ -300,6 +300,8 @@ public: */ virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + static CDev *getDev(const char *path); + protected: int register_driver(const char *name, void *data); diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp index dadd9a2913..5650b4640f 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp @@ -39,6 +39,7 @@ * @author Mark Charlebois */ +#include #include "vcdevtest_example.h" #include "drivers/device/device.h" #include @@ -50,13 +51,45 @@ using namespace device; #define TESTDEV "/dev/vdevex" +int writer_main(int argc, char *argv[]) +{ + char buf[1] = { '1' }; + + int fd = px4_open(TESTDEV, PX4_F_RDONLY); + if (fd < 0) { + printf("--- Open failed %d %d", fd, px4_errno); + return -px4_errno; + } + + // Wait for 3 seconds + printf("--- Sleeping for 3 sec\n"); + int ret = sleep(3); + if (ret < 0) { + printf("--- usleep failed %d %d\n", ret, errno); + return ret; + } + + printf("--- writing to fd\n"); + return px4_write(fd, buf, 1); +} + class VCDevNode : public CDev { public: VCDevNode() : CDev("vcdevtest", TESTDEV) {}; ~VCDevNode() {} + + virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); }; +ssize_t VCDevNode::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +{ + // ignore what was written, but let pollers know something was written + poll_notify(POLLIN); + + return buflen; +} + VCDevExample::~VCDevExample() { if (_node) { delete _node; @@ -123,6 +156,14 @@ int VCDevExample::main() int i=0; px4_pollfd_struct_t fds[1]; + // Start a task that will write something in 3 seconds + (void)px4_task_spawn_cmd("writer", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 2000, + writer_main, + (char* const*)NULL); + while (!mgr.exitRequested() && i<5) { sleep(2); @@ -131,7 +172,9 @@ int VCDevExample::main() fds[0].events = POLLIN; fds[0].revents = 0; - ret = px4_poll(fds, 1, 500); + printf(" Calling Poll\n"); + ret = px4_poll(fds, 1, 1000); + printf(" Done poll\n"); if (ret < 0) { printf("poll failed %d %d\n", ret, px4_errno); px4_close(fd); diff --git a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp index 2f37fb05de..c006136114 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp +++ b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp @@ -42,11 +42,6 @@ #include #include #include -#include - -#define SCHED_DEFAULT SCHED_FIFO -#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) -#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) static int daemon_task; /* Handle of deamon task / thread */ diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index ac75126c44..18a6ced79e 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -48,6 +48,12 @@ typedef int px4_task_t; #elif defined(__PX4_LINUX) #include +#include + +#define SCHED_DEFAULT SCHED_FIFO +#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) + typedef pthread_t px4_task_t; typedef struct { From 5259f1c861d09a94601f5c21cf09d726b10c87f7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 16:19:07 -0700 Subject: [PATCH 024/258] Linux: add queue functions from NuttX and HRT stubs The High Resilution Timer functions are stubbed out for now. Certain queue functions are required to compile uORB so adding the queue.c from NuttX. Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/drv_hrt.c | 162 ++++++++++++++++++++++ src/platforms/linux/px4_layer/lib_crc32.c | 126 +++++++++++++++++ src/platforms/linux/px4_layer/queue.c | 102 ++++++++++++++ 3 files changed, 390 insertions(+) create mode 100644 src/platforms/linux/px4_layer/drv_hrt.c create mode 100644 src/platforms/linux/px4_layer/lib_crc32.c create mode 100644 src/platforms/linux/px4_layer/queue.c diff --git a/src/platforms/linux/px4_layer/drv_hrt.c b/src/platforms/linux/px4_layer/drv_hrt.c new file mode 100644 index 0000000000..d501236d57 --- /dev/null +++ b/src/platforms/linux/px4_layer/drv_hrt.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file drv_hrt.h + * + * High-resolution timer with callouts and timekeeping. + */ + +#include + +static hrt_abstime dummy_timer = 0; + +/* latency histogram */ +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* + * Get absolute time. + */ +hrt_abstime hrt_absolute_time(void) +{ + return dummy_timer; +} + +/* + * Convert a timespec to absolute time. + */ +hrt_abstime ts_to_abstime(struct timespec *ts) +{ + return dummy_timer; +} + +/* + * Convert absolute time to a timespec. + */ +void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); + +/* + * Compute the delta between a timestamp taken in the past + * and now. + * + * This function is safe to use even if the timestamp is updated + * by an interrupt during execution. + */ +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) +{ + return dummy_timer; +} + +/* + * Store the absolute time in an interrupt-safe fashion. + * + * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. + */ +hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) +{ + return dummy_timer; +} + +/* + * Call callout(arg) after delay has elapsed. + * + * If callout is NULL, this can be used to implement a timeout by testing the call + * with hrt_called(). + */ +void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ +} + +/* + * Call callout(arg) at absolute time calltime. + */ +void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ +} + +/* + * Call callout(arg) after delay, and then after every interval. + * + * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may + * jitter but should not drift. + */ +void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ +} + +/* + * If this returns true, the entry has been invoked and removed from the callout list, + * or it has never been entered. + * + * Always returns false for repeating callouts. + */ +bool hrt_called(struct hrt_call *entry) +{ + return true; +} + +/* + * Remove the entry from the callout list. + */ +void hrt_cancel(struct hrt_call *entry) +{ +} + +/* + * initialise a hrt_call structure + */ +void hrt_call_init(struct hrt_call *entry) +{ +} + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ +} + +/* + * Initialise the HRT. + */ +void hrt_init(void) +{ +} + diff --git a/src/platforms/linux/px4_layer/lib_crc32.c b/src/platforms/linux/px4_layer/lib_crc32.c new file mode 100644 index 0000000000..4ba6fbf6df --- /dev/null +++ b/src/platforms/linux/px4_layer/lib_crc32.c @@ -0,0 +1,126 @@ +/************************************************************************************************ + * libc/misc/lib_crc32.c + * + * This file is a part of NuttX: + * + * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. + * + * The logic in this file was developed by Gary S. Brown: + * + * COPYRIGHT (C) 1986 Gary S. Brown. You may use this program, or code or tables + * extracted from it, as desired without restriction. + * + * First, the polynomial itself and its table of feedback terms. The polynomial is: + * + * X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0 + * + * Note that we take it "backwards" and put the highest-order term in the lowest-order bit. + * The X^32 term is "implied"; the LSB is the X^31 term, etc. The X^0 term (usually shown + * as "+1") results in the MSB being 1 + * + * Note that the usual hardware shift register implementation, which is what we're using + * (we're merely optimizing it by doing eight-bit chunks at a time) shifts bits into the + * lowest-order term. In our implementation, that means shifting towards the right. Why + * do we do it this way? Because the calculated CRC must be transmitted in order from + * highest-order term to lowest-order term. UARTs transmit characters in order from LSB + * to MSB. By storing the CRC this way we hand it to the UART in the order low-byte to + * high-byte; the UART sends each low-bit to hight-bit; and the result is transmission bit + * by bit from highest- to lowest-order term without requiring any bit shuffling on our + * part. Reception works similarly + * + * The feedback terms table consists of 256, 32-bit entries. Notes + * + * - The table can be generated at runtime if desired; code to do so is shown later. It + * might not be obvious, but the feedback terms simply represent the results of eight + * shift/xor operations for all combinations of data and CRC register values + * + * - The values must be right-shifted by eight bits by the updcrc logic; the shift must + * be u_(bring in zeroes). On some hardware you could probably optimize the shift in + * assembler by using byte-swap instructions polynomial $edb88320 + ************************************************************************************************/ + +/************************************************************************************************ + * Included Files + ************************************************************************************************/ + +#include +#include +#include + +// Needed for Linux +#define FAR + +/************************************************************************************************ + * Private Data + ************************************************************************************************/ + +static const uint32_t crc32_tab[] = +{ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; + +/************************************************************************************************ + * Public Functions + ************************************************************************************************/ +/************************************************************************************************ + * Name: crc32part + * + * Description: + * Continue CRC calculation on a part of the buffer. + * + ************************************************************************************************/ + +uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) +{ + size_t i; + + for (i = 0; i < len; i++) + { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } + return crc32val; +} + +/************************************************************************************************ + * Name: crc32 + * + * Description: + * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' + * + ************************************************************************************************/ + +uint32_t crc32(FAR const uint8_t *src, size_t len) +{ + return crc32part(src, len, 0); +} diff --git a/src/platforms/linux/px4_layer/queue.c b/src/platforms/linux/px4_layer/queue.c new file mode 100644 index 0000000000..2543782b87 --- /dev/null +++ b/src/platforms/linux/px4_layer/queue.c @@ -0,0 +1,102 @@ +/************************************************************************ + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + * Modified for use in Linux by Mark Charlebois + * + ************************************************************************/ + +// FIXME - need px4_queue +#include +#include + +__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue); +void sq_rem(sq_entry_t *node, sq_queue_t *queue) +{ + if (queue->head && node) + { + if (node == queue->head) + { + queue->head = node->flink; + if (node == queue->tail) + { + queue->tail = NULL; + } + } + else + { + sq_entry_t *prev; + for(prev = (sq_entry_t*)queue->head; + prev && prev->flink != node; + prev = prev->flink); + + if (prev) + { + sq_remafter(prev, queue); + } + } + } +} + +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue) +{ + sq_entry_t *ret = node->flink; + if (queue->head && ret) + { + if (queue->tail == ret) + { + queue->tail = node; + node->flink = NULL; + } + else + { + node->flink = ret->flink; + } + + ret->flink = NULL; + } + + return ret; +} + +__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); +void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = queue->head; + if (!queue->head) + { + queue->tail = node; + } + queue->head = node; +} + + From 938751993d2c5539c5cc6904e84bb62ed8145357 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 16:39:41 -0700 Subject: [PATCH 025/258] Changed AppMgr to AppState The previous name implied some kind of daemon. AppState is aggregated state of an application's running state and interfaces to request app termination, and check app state. Signed-off-by: Mark Charlebois --- src/platforms/linux/hello/hello_example.cpp | 6 ++-- src/platforms/linux/hello/hello_example.h | 2 +- .../linux/hello/hello_start_linux.cpp | 6 ++-- .../linux/vcdev_test/vcdevtest_example.cpp | 19 +++++++----- .../linux/vcdev_test/vcdevtest_example.h | 2 +- .../vcdev_test/vcdevtest_start_linux.cpp | 6 ++-- src/platforms/px4_app.h | 30 ++++++++++++------- src/platforms/px4_nodehandle.h | 8 ++--- 8 files changed, 45 insertions(+), 34 deletions(-) diff --git a/src/platforms/linux/hello/hello_example.cpp b/src/platforms/linux/hello/hello_example.cpp index 7881b73d6e..a30aeb57bc 100644 --- a/src/platforms/linux/hello/hello_example.cpp +++ b/src/platforms/linux/hello/hello_example.cpp @@ -43,14 +43,14 @@ #include #include -px4::AppMgr HelloExample::mgr; +px4::AppState HelloExample::appState; int HelloExample::main() { - mgr.setRunning(true); + appState.setRunning(true); int i=0; - while (!mgr.exitRequested() && i<5) { + while (!appState.exitRequested() && i<5) { sleep(2); printf(" Doing work...\n"); diff --git a/src/platforms/linux/hello/hello_example.h b/src/platforms/linux/hello/hello_example.h index b736f60bcb..a4ae517056 100644 --- a/src/platforms/linux/hello/hello_example.h +++ b/src/platforms/linux/hello/hello_example.h @@ -49,5 +49,5 @@ public: int main(); - static px4::AppMgr mgr; /* Manage requests to terminate app */ + static px4::AppState appState; /* track requests to terminate app */ }; diff --git a/src/platforms/linux/hello/hello_start_linux.cpp b/src/platforms/linux/hello/hello_start_linux.cpp index a7d21fa59c..240c5d845e 100644 --- a/src/platforms/linux/hello/hello_start_linux.cpp +++ b/src/platforms/linux/hello/hello_start_linux.cpp @@ -63,7 +63,7 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (HelloExample::mgr.isRunning()) { + if (HelloExample::appState.isRunning()) { printf("already running\n"); /* this is not an error */ return 0; @@ -80,12 +80,12 @@ int hello_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - HelloExample::mgr.requestExit(); + HelloExample::appState.requestExit(); return 0; } if (!strcmp(argv[1], "status")) { - if (HelloExample::mgr.isRunning()) { + if (HelloExample::appState.isRunning()) { printf("is running\n"); } else { diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp index 5650b4640f..39c804ada4 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp @@ -45,7 +45,7 @@ #include #include -px4::AppMgr VCDevExample::mgr; +px4::AppState VCDevExample::appState; using namespace device; @@ -117,7 +117,7 @@ int test_pub_block(int fd, unsigned long blocked) int VCDevExample::main() { - mgr.setRunning(true); + appState.setRunning(true); _node = new VCDevNode(); @@ -164,7 +164,7 @@ int VCDevExample::main() writer_main, (char* const*)NULL); - while (!mgr.exitRequested() && i<5) { + while (!appState.exitRequested() && i<3) { sleep(2); printf(" polling...\n"); @@ -179,12 +179,15 @@ int VCDevExample::main() printf("poll failed %d %d\n", ret, px4_errno); px4_close(fd); } - else if (ret == 0) - printf(" Nothing to read\n"); - else { - printf(" %d to read\n", ret); + else if (i > 0 && ret == 0) + printf(" Nothing to read - PASS\n"); + else if (i == 0) { + if (ret == 1) + printf(" %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); + else + printf(" %d to read - FAIL\n", ret); + } - printf(" Doing work...\n"); ++i; } px4_close(fd); diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.h b/src/platforms/linux/vcdev_test/vcdevtest_example.h index ed00fd5cb9..4898210dff 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_example.h +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.h @@ -51,7 +51,7 @@ public: int main(); - static px4::AppMgr mgr; /* Manage requests to terminate app */ + static px4::AppState appState; /* track requests to terminate app */ private: VCDevNode *_node; diff --git a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp index c006136114..9adb650850 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp +++ b/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp @@ -58,7 +58,7 @@ int vcdevtest_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (VCDevExample::mgr.isRunning()) { + if (VCDevExample::appState.isRunning()) { printf("already running\n"); /* this is not an error */ return 0; @@ -75,12 +75,12 @@ int vcdevtest_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - VCDevExample::mgr.requestExit(); + VCDevExample::appState.requestExit(); return 0; } if (!strcmp(argv[1], "status")) { - if (VCDevExample::mgr.isRunning()) { + if (VCDevExample::appState.isRunning()) { printf("is running\n"); } else { diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h index 2c9765ae29..9aa285e228 100644 --- a/src/platforms/px4_app.h +++ b/src/platforms/px4_app.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,21 +31,28 @@ * ****************************************************************************/ +/** + * @file px4_app.h + * + * PX4 app template classes, functions and defines. Apps need to call their + * main function PX4_MAIN. + */ + #pragma once namespace px4 { -class AppMgr { +class AppState { public: - ~AppMgr() {} + ~AppState() {} #if defined(__PX4_ROS) - AppMgr() {} + AppState() {} bool exitRequested() { return !ros::ok(); } void requestExit() { ros::shutdown(); } #else - AppMgr() : _exitRequested(false), _isRunning(false) {} + AppState() : _exitRequested(false), _isRunning(false) {} bool exitRequested() { return _exitRequested; } void requestExit() { _exitRequested = true; } @@ -58,22 +65,23 @@ protected: bool _isRunning; #endif private: - AppMgr(const AppMgr&); - const AppMgr& operator=(const AppMgr&); + AppState(const AppState&); + const AppState& operator=(const AppState&); }; } +// PX4_MAIN is defined if module.mk sets MODULE_COMMAND +// For ROS and NuttX it is "main" and for Linux it is +// $(MODULE_COMMAND)_app_main since some apps already +// define $(MODULE_COMMAND)_main + // Task/process based build #if defined(__PX4_ROS) || defined(__PX4_NUTTX) // Thread based build #else -// The name passed must be globally unique -// set PX4_APPMAIN in module.mk -// EXTRADEFINES += -DPX4_MAIN=foo_app #ifdef PX4_MAIN - extern int PX4_MAIN(int argc, char *argv[]); #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index c45ce5cc7f..e4a483de65 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -142,11 +142,11 @@ protected: class __EXPORT NodeHandle { public: - NodeHandle(AppMgr &a) : + NodeHandle(AppState &a) : _subs(), _pubs(), _sub_min_interval(nullptr), - _mgr(a) + _appState(a) {} ~NodeHandle() @@ -264,7 +264,7 @@ public: */ void spin() { - while (!_mgr.exitRequested()) { + while (!_appState.exitRequested()) { const int timeout_ms = 100; /* Only continue in the loop if the nodehandle has subscriptions */ @@ -289,7 +289,7 @@ protected: SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ - AppMgr &_mgr; + AppState &_appState; /** * Check if this is the smallest interval so far and update _sub_min_interval From 7b0783a0701cdbd5b74c9e11fe9b45b1e5672a4c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 16:42:04 -0700 Subject: [PATCH 026/258] Added MuORB based on virtual CDev implementation uORB module now compiles and runs for Linux using the virtual CDev implementation. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 +- src/drivers/device/vcdev.cpp | 3 - src/drivers/device/vcdev_posix.cpp | 60 +- src/drivers/drv_orb_dev.h | 3 +- src/modules/systemlib/param/param.c | 2 +- src/modules/uORB/MuORB.cpp | 1330 +++++++++++++++++ src/modules/uORB/module.mk | 8 +- src/platforms/linux/px4_layer/module.mk | 5 +- .../linux/px4_layer/px4_linux_impl.cpp | 42 + src/platforms/px4_posix.h | 3 + 10 files changed, 1415 insertions(+), 43 deletions(-) create mode 100644 src/modules/uORB/MuORB.cpp diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 56567e48f4..25821922d1 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -39,7 +39,7 @@ MODULES += drivers/device # MODULES += modules/systemlib MODULES += modules/systemlib/mixer -#MODULES += modules/uORB +MODULES += modules/uORB #MODULES += modules/dataman # diff --git a/src/drivers/device/vcdev.cpp b/src/drivers/device/vcdev.cpp index edebe3b416..da1ff05764 100644 --- a/src/drivers/device/vcdev.cpp +++ b/src/drivers/device/vcdev.cpp @@ -379,9 +379,6 @@ CDev::poll_notify(pollevent_t events) for (unsigned i = 0; i < _max_pollwaiters; i++) if (nullptr != _pollset[i]) poll_notify_one(_pollset[i], events); - for (unsigned i = 0; i < _max_pollwaiters; i++) - if (nullptr != _pollset[i]) - debug(" CHECK fds=%p %0x %0x",_pollset[i], _pollset[i]->revents, _pollset[i]->events); unlock(); } diff --git a/src/drivers/device/vcdev_posix.cpp b/src/drivers/device/vcdev_posix.cpp index 6df88a2f66..fab308ef07 100644 --- a/src/drivers/device/vcdev_posix.cpp +++ b/src/drivers/device/vcdev_posix.cpp @@ -46,9 +46,6 @@ #include #include -#define DEBUG(...) -//#define DEBUG(...) printf(__VA_ARGS__) - using namespace device; extern "C" { @@ -71,7 +68,7 @@ static void *timer_handler(void *data) usleep(td->ts.tv_nsec/1000); sem_post(&(td->sem)); - printf("Timer expired\n"); + PX4_DEBUG("timer_handler: Timer expired\n"); return 0; } @@ -80,8 +77,12 @@ static px4_dev_handle_t *filemap[PX4_MAX_FD] = {}; int px4_errno; -int -px4_open(const char *path, int flags) +inline bool valid_fd(int fd) +{ + return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); +} + +int px4_open(const char *path, int flags) { CDev *dev = CDev::getDev(path); int ret = 0; @@ -108,17 +109,16 @@ px4_open(const char *path, int flags) px4_errno = -ret; return -1; } - DEBUG("px4_open fd = %d", filemap[i]->fd); + PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); return filemap[i]->fd; } -int -px4_close(int fd) +int px4_close(int fd) { int ret; - if (fd < PX4_MAX_FD && fd >= 0) { + if (valid_fd(fd)) { CDev *dev = (CDev *)(filemap[fd]->cdev); - DEBUG("px4_close fd = %d\n", fd); + PX4_DEBUG("px4_close fd = %d\n", fd); ret = dev->close(filemap[fd]); filemap[fd] = NULL; } @@ -131,13 +131,12 @@ px4_close(int fd) return ret; } -ssize_t -px4_read(int fd, void *buffer, size_t buflen) +ssize_t px4_read(int fd, void *buffer, size_t buflen) { int ret; - if (fd < PX4_MAX_FD && fd >= 0) { + if (valid_fd(fd)) { CDev *dev = (CDev *)(filemap[fd]->cdev); - DEBUG("px4_read fd = %d\n", fd); + PX4_DEBUG("px4_read fd = %d\n", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); } else { @@ -149,13 +148,12 @@ px4_read(int fd, void *buffer, size_t buflen) return ret; } -ssize_t -px4_write(int fd, const void *buffer, size_t buflen) +ssize_t px4_write(int fd, const void *buffer, size_t buflen) { int ret = PX4_ERROR; - if (fd < PX4_MAX_FD && fd >= 0) { + if (valid_fd(fd)) { CDev *dev = (CDev *)(filemap[fd]->cdev); - DEBUG("px4_write fd = %d\n", fd); + PX4_DEBUG("px4_write fd = %d\n", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); } else { @@ -167,13 +165,12 @@ px4_write(int fd, const void *buffer, size_t buflen) return ret; } -int -px4_ioctl(int fd, int cmd, unsigned long arg) +int px4_ioctl(int fd, int cmd, unsigned long arg) { int ret = PX4_ERROR; - if (fd < PX4_MAX_FD && fd >= 0) { + if (valid_fd(fd)) { CDev *dev = (CDev *)(filemap[fd]->cdev); - DEBUG("px4_ioctl fd = %d\n", fd); + PX4_DEBUG("px4_ioctl fd = %d\n", fd); ret = dev->ioctl(filemap[fd], cmd, arg); } else { @@ -188,8 +185,7 @@ px4_ioctl(int fd, int cmd, unsigned long arg) return ret; } -int -px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) +int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) { sem_t sem; int count = 0; @@ -197,7 +193,7 @@ px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) unsigned int i; struct timespec ts; - printf("Called px4_poll timeout = %d\n", timeout); + PX4_DEBUG("Called px4_poll timeout = %d\n", timeout); sem_init(&sem, 0, 0); // For each fd @@ -208,15 +204,14 @@ px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) fds[i].priv = NULL; // If fd is valid - if (fds[i].fd >= 0 && fds[i].fd < PX4_MAX_FD) + if (valid_fd(fds[i].fd)) { CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; - DEBUG("px4_poll: CDev->poll(setup) %d\n", fds[i].fd); + PX4_DEBUG("px4_poll: CDev->poll(setup) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) break; - DEBUG("xxxx fds=%p fds[%d].revents = %d\n", fds, i, fds[i].revents); } } @@ -253,18 +248,15 @@ px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) for (i=0; i= 0 && fds[i].fd < PX4_MAX_FD) + if (valid_fd(fds[i].fd)) { - DEBUG("zzzz fds=%p fds[%d].revents = %d\n",fds, i, fds[i].revents); - CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; - DEBUG("px4_poll: CDev->poll(teardown) %d\n", fds[i].fd); + PX4_DEBUG("px4_poll: CDev->poll(teardown) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) break; - DEBUG("yyyy fds=%p fds[%d].revents = %d\n", fds, i, fds[i].revents); if (fds[i].revents) count += 1; } diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index c1db6b534b..37e9bdb7cf 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -40,6 +40,7 @@ * uORB published object driver. */ +#include #include #include #include @@ -61,7 +62,7 @@ #define ORB_MAXNAME 32 #define _ORBIOCBASE (0x2600) -#define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n)) +#define _ORBIOC(_n) (_PX4_IOC(_ORBIOCBASE, _n)) /* * IOCTLs for the uORB control device diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index dfbe582c55..8cc1e6e278 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -71,7 +71,7 @@ /** * Array of static parameter info. */ -#ifdef _UNIT_TEST +#if defined(_UNIT_TEST) || defined(__PX4_LINUX) extern struct param_info_s param_array[]; extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp new file mode 100644 index 0000000000..ffa51bae82 --- /dev/null +++ b/src/modules/uORB/MuORB.cpp @@ -0,0 +1,1330 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file uORB.cpp + * A lightweight object broker. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#include "uORB.h" + +/** + * Utility functions. + */ +namespace +{ + +/* internal use only */ +static const unsigned orb_maxpath = 64; + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +enum Flavor { + PUBSUB, + PARAM +}; + +struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; +}; + +int +node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) +{ + unsigned len; + + unsigned index = 0; + + if (instance != nullptr) { + index = *instance; + } + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); + + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } + + return PX4_OK; +} + +} + +/** + * Per-object device instance. + */ +class ORBDevNode : public device::CDev +{ +public: + ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); + ~ORBDevNode(); + + virtual int open(device::px4_dev_handle_t *handlep); + virtual int close(device::px4_dev_handle_t *handlep); + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual ssize_t write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + +protected: + virtual pollevent_t poll_state(device::px4_dev_handle_t *handlep); + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + +private: + struct SubscriberData { + unsigned generation; /**< last generation the subscriber has seen */ + unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ + void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ + bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ + }; + + const struct orb_metadata *_meta; /**< object metadata information */ + uint8_t *_data; /**< allocated object buffer */ + hrt_abstime _last_update; /**< time the object was last updated */ + volatile unsigned _generation; /**< object generation count */ + pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ + + SubscriberData *handlep_to_sd(device::px4_dev_handle_t *handlep); + + /** + * Perform a deferred update for a rate-limited subscriber. + */ + void update_deferred(); + + /** + * Bridge from hrt_call to update_deferred + * + * void *arg ORBDevNode pointer for which the deferred update is performed. + */ + static void update_deferred_trampoline(void *arg); + + /** + * Check whether a topic appears updated to a subscriber. + * + * @param sd The subscriber for whom to check. + * @return True if the topic should appear updated to the subscriber + */ + bool appears_updated(SubscriberData *sd); +}; + +ORBDevNode::SubscriberData *ORBDevNode::handlep_to_sd(device::px4_dev_handle_t *handlep) +{ + PX4_DEBUG("ORBDevNode::handlep_to_sd %p\n", handlep); + ORBDevNode::SubscriberData *sd; + if (handlep) { + sd = (ORBDevNode::SubscriberData *)(handlep->priv); + PX4_DEBUG(" sd = %p \n", sd); + } + else { + PX4_DEBUG("*** ERROR ORBDevNode::handlep_to_sd(0)\n"); + sd = 0; + } + return sd; +} + +ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : + CDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0), + _priority(priority) +{ + // enable debug() calls + _debug_enabled = true; +} + +ORBDevNode::~ORBDevNode() +{ + if (_data != nullptr) + delete[] _data; + +} + +int +ORBDevNode::open(device::px4_dev_handle_t *handlep) +{ + int ret; + + /* is this a publisher? */ + if (handlep->flags == PX4_F_WRONLY) { + + /* become the publisher if we can */ + lock(); + + if (_publisher == 0) { + _publisher = getpid(); + ret = PX4_OK; + + } else { + ret = -EBUSY; + } + + unlock(); + + /* now complete the open */ + if (ret == PX4_OK) { + ret = CDev::open(handlep); + + /* open failed - not the publisher anymore */ + if (ret != PX4_OK) + _publisher = 0; + } + + return ret; + } + + /* is this a new subscriber? */ + if (handlep->flags == PX4_F_RDONLY) { + + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; + + if (nullptr == sd) + return -ENOMEM; + + memset(sd, 0, sizeof(*sd)); + + /* default to no pending update */ + sd->generation = _generation; + + /* set priority */ + sd->priority = _priority; + + handlep->priv = (void *)sd; + + ret = CDev::open(handlep); + + if (ret != PX4_OK) { + PX4_DEBUG("ERROR: CDev::open failed\n"); + delete sd; + } + + PX4_DEBUG("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", handlep->fd, handlep->flags, handlep->priv, handlep->cdev); + return ret; + } + + /* can only be pub or sub, not both */ + return -EINVAL; +} + +int +ORBDevNode::close(device::px4_dev_handle_t *handlep) +{ + PX4_DEBUG("ORBDevNode::close fd = %d\n", handlep->fd); + /* is this the publisher closing? */ + if (getpid() == _publisher) { + _publisher = 0; + + } else { + SubscriberData *sd = handlep_to_sd(handlep); + + if (sd != nullptr) { + PX4_DEBUG(" delete handlep->priv %p", handlep->priv); + hrt_cancel(&sd->update_call); + delete sd; + } + } + + return CDev::close(handlep); +} + +ssize_t +ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + PX4_DEBUG("ORBDevNode::read fd = %d\n", handlep->fd); + SubscriberData *sd = (SubscriberData *)handlep_to_sd(handlep); + + /* if the object has not been written yet, return zero */ + if (_data == nullptr) + return 0; + + /* if the caller's buffer is the wrong size, that's an error */ + if (buflen != _meta->o_size) + return -EIO; + + /* + * Perform an atomic copy & state update + */ + // FIXME - This used to disable interrupts + //lock(); + + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) + memcpy(buffer, _data, _meta->o_size); + + /* track the last generation that the file has seen */ + sd->generation = _generation; + + /* set priority */ + sd->priority = _priority; + + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; + + //unlock(); + + return _meta->o_size; +} + +ssize_t +ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +{ + PX4_DEBUG("ORBDevNode::write handlep = %p (null is normal)\n", handlep); + /* + * Writes are legal from interrupt context as long as the + * object has already been initialised from thread context. + * + * Writes outside interrupt context will allocate the object + * if it has not yet been allocated. + * + * Note that handlep will usually be NULL. + */ + if (nullptr == _data) { + lock(); + + /* re-check size */ + if (nullptr == _data) + _data = new uint8_t[_meta->o_size]; + + unlock(); + + /* failed or could not allocate */ + if (nullptr == _data) + return -ENOMEM; + } + + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) + return -EIO; + + /* Perform an atomic copy. */ + // FIXME - make sure lock is what we want here + lock(); + memcpy(_data, buffer, _meta->o_size); + unlock(); + + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; + + /* notify any poll waiters */ + poll_notify(POLLIN); + + return _meta->o_size; +} + +int +ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + PX4_DEBUG("ORBDevNode::ioctl fd = %d cmd = %d\n", handlep->fd, cmd); + SubscriberData *sd = handlep_to_sd(handlep); + + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return PX4_OK; + + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return PX4_OK; + + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return PX4_OK; + + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return PX4_OK; + + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return PX4_OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(handlep, cmd, arg); + } +} + +ssize_t +ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +{ + PX4_DEBUG("ORBDevNode::publish meta = %p\n", meta); + ORBDevNode *devnode = (ORBDevNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (devnode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = devnode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return PX4_OK; +} + +pollevent_t +ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) +{ + PX4_DEBUG("ORBDevNode::poll_state fd = %d\n", handlep->fd); + SubscriberData *sd = handlep_to_sd(handlep); + + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) + return POLLIN; + + return 0; +} + +void +ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +{ + PX4_DEBUG("ORBDevNode::poll_notify_one fds = %p fds->priv = %p\n", fds, fds->priv); + SubscriberData *sd = handlep_to_sd((device::px4_dev_handle_t *)fds->priv); + + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) + CDev::poll_notify_one(fds, events); +} + +bool +ORBDevNode::appears_updated(SubscriberData *sd) +{ + PX4_DEBUG("ORBDevNode::appears_updated sd = %p\n", sd); + /* assume it doesn't look updated */ + bool ret = false; + + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + + /* + * If the subscriber's generation count matches the update generation + * count, there has been no update from their perspective; if they + * don't match then we might have a visible update. + */ + while (sd->generation != _generation) { + + /* + * Handle non-rate-limited subscribers. + */ + if (sd->update_interval == 0) { + ret = true; + break; + } + + /* + * If we have previously told the subscriber that there is data, + * and they have not yet collected it, continue to tell them + * that there has been an update. This mimics the non-rate-limited + * behaviour where checking / polling continues to report an update + * until the topic is read. + */ + if (sd->update_reported) { + ret = true; + break; + } + + /* + * If the interval timer is still running, the topic should not + * appear updated, even though at this point we know that it has. + * We have previously been through here, so the subscriber + * must have collected the update we reported, otherwise + * update_reported would still be true. + */ + if (!hrt_called(&sd->update_call)) + break; + + /* + * Make sure that we don't consider the topic to be updated again + * until the interval has passed once more by restarting the interval + * timer and thereby re-scheduling a poll notification at that time. + */ + hrt_call_after(&sd->update_call, + sd->update_interval, + &ORBDevNode::update_deferred_trampoline, + (void *)this); + + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; + + break; + } + +out: + /* consider it updated */ + return ret; +} + +void +ORBDevNode::update_deferred() +{ + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); +} + +void +ORBDevNode::update_deferred_trampoline(void *arg) +{ + ORBDevNode *node = (ORBDevNode *)arg; + + node->update_deferred(); +} + +/** + * Master control device for ObjDev. + * + * Used primarily to create new objects via the ORBIOCCREATE + * ioctl. + */ +class ORBDevMaster : public device::CDev +{ +public: + ORBDevMaster(Flavor f); + ~ORBDevMaster(); + + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); +private: + Flavor _flavor; +}; + +ORBDevMaster::ORBDevMaster(Flavor f) : + CDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) +{ + // enable debug() calls + _debug_enabled = true; + +} + +ORBDevMaster::~ORBDevMaster() +{ +} + +int +ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + int ret; + + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; + const char *objname; + const char *devpath; + char nodepath[orb_maxpath]; + ORBDevNode *node; + + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + + /* construct a path to the node - this also checks the node name */ + ret = node_mkpath(nodepath, _flavor, meta, adv->instance); + + if (ret != PX4_OK) { + return ret; + } + + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + if (devpath == nullptr) { + // FIXME - looks like we leaked memory here for objname + return -ENOMEM; + } + + /* construct the new node */ + node = new ORBDevNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + + // FIXME - looks like we leaked memory here for devpath and objname + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != PX4_OK) { + delete node; + free((void *)objname); + free((void *)devpath); + } + + group_tries++; + + } while (ret != PX4_OK && (group_tries < max_group_tries)); + + if (group_tries > max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + + return ret; + } + + default: + /* give it to the superclass */ + return CDev::ioctl(handlep, cmd, arg); + } +} + + +/** + * Local functions in support of the shell command. + */ + +namespace +{ + +ORBDevMaster *g_dev; +bool pubsubtest_passed = false; +bool pubsubtest_print = false; +int pubsubtest_res = PX4_OK; + +struct orb_test { + int val; + hrt_abstime time; +}; + +ORB_DEFINE(orb_test, struct orb_test); +ORB_DEFINE(orb_multitest, struct orb_test); + +struct orb_test_medium { + int val; + hrt_abstime time; + char junk[64]; +}; + +ORB_DEFINE(orb_test_medium, struct orb_test_medium); + +struct orb_test_large { + int val; + hrt_abstime time; + char junk[512]; +}; + +ORB_DEFINE(orb_test_large, struct orb_test_large); + +int +test_fail(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "FAIL: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return ERROR; +} + +int +test_note(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "note: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return PX4_OK; +} + +int pubsublatency_main(void) +{ + /* poll on test topic and output latency */ + float latency_integral = 0.0f; + + /* wakeup source(s) */ + px4_pollfd_struct_t fds[3]; + + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); + int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); + int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); + + struct orb_test_large t; + + /* clear all ready flags */ + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + + fds[0].fd = test_multi_sub; + fds[0].events = POLLIN; + fds[1].fd = test_multi_sub_medium; + fds[1].events = POLLIN; + fds[2].fd = test_multi_sub_large; + fds[2].events = POLLIN; + + const unsigned maxruns = 1000; + unsigned timingsgroup = 0; + + unsigned *timings = new unsigned[maxruns]; + + for (unsigned i = 0; i < maxruns; i++) { + /* wait for up to 500ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + timingsgroup = 0; + } else if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + timingsgroup = 1; + } else if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + timingsgroup = 2; + } + + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + hrt_abstime elt = hrt_elapsed_time(&t.time); + latency_integral += elt; + timings[i] = elt; + } + + orb_unsubscribe(test_multi_sub); + orb_unsubscribe(test_multi_sub_medium); + orb_unsubscribe(test_multi_sub_large); + + if (pubsubtest_print) { + char fname[32]; + sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); + FILE *f = fopen(fname, "w"); + if (f == NULL) { + warnx("Error opening file!\n"); + return ERROR; + } + + for (unsigned i = 0; i < maxruns; i++) { + fprintf(f, "%u\n", timings[i]); + } + + fclose(f); + } + + delete[] timings; + + warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); + + pubsubtest_passed = true; + + if (static_cast(latency_integral / maxruns) > 30.0f) { + pubsubtest_res = ERROR; + } else { + pubsubtest_res = PX4_OK; + } + + return pubsubtest_res; +} + +template int latency_test(orb_id_t T, bool print); + +int +test() +{ + struct orb_test t, u; + int pfd, sfd; + bool updated; + + t.val = 0; + pfd = orb_advertise(ORB_ID(orb_test), &t); + + if (pfd < 0) + return test_fail("advertise failed: %d", errno); + + test_note("publish handle 0x%08x", pfd); + sfd = orb_subscribe(ORB_ID(orb_test)); + + if (sfd < 0) + return test_fail("subscribe failed: %d", errno); + + test_note("subscribe fd %d", sfd); + u.val = 1; + + if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(1) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + + if (PX4_OK != orb_check(sfd, &updated)) + return test_fail("check(1) failed"); + + if (updated) + return test_fail("spurious updated flag"); + + t.val = 2; + test_note("try publish"); + + if (PX4_OK != orb_publish(ORB_ID(orb_test), pfd, &t)) + return test_fail("publish failed"); + + if (PX4_OK != orb_check(sfd, &updated)) + return test_fail("check(2) failed"); + + if (!updated) + return test_fail("missing updated flag"); + + if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(2) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); + + orb_unsubscribe(sfd); + close(pfd); + + /* this routine tests the multi-topic support */ + test_note("try multi-topic support"); + + int instance0; + int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); + + test_note("advertised"); + + int instance1; + int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); + + if (instance0 != 0) + return test_fail("mult. id0: %d", instance0); + + if (instance1 != 1) + return test_fail("mult. id1: %d", instance1); + + t.val = 103; + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 fail"); + + test_note("published"); + + t.val = 203; + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) + return test_fail("mult. pub1 fail"); + + /* subscribe to both topics and ensure valid data is received */ + int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) + return test_fail("sub #0 copy failed: %d", errno); + + if (u.val != 103) + return test_fail("sub #0 val. mismatch: %d", u.val); + + int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) + return test_fail("sub #1 copy failed: %d", errno); + + if (u.val != 203) + return test_fail("sub #1 val. mismatch: %d", u.val); + + /* test priorities */ + int prio; + if (PX4_OK != orb_priority(sfd0, &prio)) + return test_fail("prio #0"); + + if (prio != ORB_PRIO_MAX) + return test_fail("prio: %d", prio); + + if (PX4_OK != orb_priority(sfd1, &prio)) + return test_fail("prio #1"); + + if (prio != ORB_PRIO_MIN) + return test_fail("prio: %d", prio); + + if (PX4_OK != latency_test(ORB_ID(orb_test), false)) + return test_fail("latency test failed"); + + return test_note("PASS"); +} + +template int +latency_test(orb_id_t T, bool print) +{ + S t; + t.val = 308; + t.time = hrt_absolute_time(); + + int pfd0 = orb_advertise(T, &t); + + pubsubtest_print = print; + + pubsubtest_passed = false; + + /* test pub / sub latency */ + + int pubsub_task = px4_task_spawn_cmd("uorb_latency", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&pubsublatency_main, + nullptr); + + /* give the test task some data */ + while (!pubsubtest_passed) { + t.val = 308; + t.time = hrt_absolute_time(); + if (PX4_OK != orb_publish(T, pfd0, &t)) + return test_fail("mult. pub0 timing fail"); + + /* simulate >800 Hz system operation */ + usleep(1000); + } + + close(pfd0); + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + + return pubsubtest_res; +} + +int +info() +{ + return PX4_OK; +} + + +} // namespace + +/* + * uORB server 'main'. + */ +extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } + +int +uorb_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + warnx("already loaded"); + /* user wanted to start uorb, its already running, no error */ + return 0; + } + + /* create the driver */ + g_dev = new ORBDevMaster(PUBSUB); + + if (g_dev == nullptr) { + warnx("driver alloc failed"); + return -ENOMEM; + } + + if (PX4_OK != g_dev->init()) { + warnx("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + return PX4_OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + return test(); + + /* + * Test the latency. + */ + if (!strcmp(argv[1], "latency_test")) { + + if (argc > 2 && !strcmp(argv[2], "medium")) { + return latency_test(ORB_ID(orb_test_medium), true); + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return latency_test(ORB_ID(orb_test_large), true); + } else { + return latency_test(ORB_ID(orb_test), true); + } + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) + return info(); + + warnx("unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); + return -EINVAL; +} + +/* + * Library functions. + */ +namespace +{ + +/** + * Advertise a node; don't consider it an error if the node has + * already been advertised. + * + * @todo verify that the existing node is the same as the one + * we tried to advertise. + */ +int +node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) +{ + int fd = -1; + int ret = ERROR; + + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; + + /* open the control device */ + fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); + + if (fd < 0) + goto out; + + /* advertise the object */ + ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); + + /* it's PX4_OK if it already exists */ + if ((PX4_OK != ret) && (EEXIST == errno)) { + ret = PX4_OK; + } + +out: + + if (fd >= 0) + px4_close(fd); + + return ret; +} + +/** + * Common implementation for orb_advertise and orb_subscribe. + * + * Handles creation of the object and the initial publication for + * advertisers. + */ +int +node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) +{ + char path[orb_maxpath]; + int fd, ret; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return ERROR; + } + + /* + * Advertiser must publish an initial value. + */ + if (advertiser && (data == nullptr)) { + errno = EINVAL; + return ERROR; + } + + /* + * Generate the path to the node and try to open it. + */ + + // FIXME - if *instance is uninitialized, why is this being called? Seems risky and + // its definiately a waste. This is the case in muli-topic test. + ret = node_mkpath(path, f, meta, instance); + + if (ret != PX4_OK) { + errno = -ret; + return ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + px4_close(fd); + + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + + /* try to create the node */ + ret = node_advertise(meta, instance, priority); + + if (ret == PX4_OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = node_mkpath(path, f, meta, instance); + + if (ret != PX4_OK) { + errno = -ret; + return ERROR; + } + } + + /* on success, try the open again */ + if (ret == PX4_OK) { + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + } + } + + if (fd < 0) { + errno = EIO; + return ERROR; + } + + /* everything has been PX4_OK, we can return the handle now */ + return fd; +} + +} // namespace + +orb_advert_t +orb_advertise(const struct orb_metadata *meta, const void *data) +{ + PX4_DEBUG("orb_advertise meta = %p\n", meta); + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); +} + +orb_advert_t +orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +{ + int result, fd; + orb_advert_t advertiser; + + PX4_DEBUG("orb_advertise_multi meta = %p\n", meta); + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true, instance, priority); + if (fd == ERROR) { + PX4_DEBUG(" node_open as advertiser failed.\n"); + return ERROR; + } + + PX4_DEBUG(" node_open as advertiser passed. fd = %d, instance = %p %d\n", fd, instance, instance != 0 ? *instance : 0); + + /* get the advertiser handle and close the node */ + result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + px4_close(fd); + if (result == ERROR) { + PX4_DEBUG(" px4_ioctl ORBIOCGADVERTISER failed. fd = %d\n", fd); + return ERROR; + } + + /* the advertiser must perform an initial publish to initialise the object */ + result = orb_publish(meta, advertiser, data); + if (result == ERROR) { + PX4_DEBUG(" orb_publish failed\n"); + return ERROR; + } + + return advertiser; +} + +int +orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(PUBSUB, meta, nullptr, false); +} + +int +orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); +} + +int +orb_unsubscribe(int fd) +{ + return px4_close(fd); +} + +int +orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ + return ORBDevNode::publish(meta, handle, data); +} + +int +orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = px4_read(handle, buffer, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return PX4_OK; +} + +int +orb_check(int handle, bool *updated) +{ + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int +orb_stat(int handle, uint64_t *time) +{ + return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); +} + +int +orb_priority(int handle, int *priority) +{ + return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); +} + +int +orb_set_interval(int handle, unsigned interval) +{ + return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 71ad09130c..82fb9a2c65 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -39,8 +39,12 @@ MODULE_COMMAND = uorb MODULE_STACKSIZE = 2048 -SRCS = uORB.cpp \ - objects_common.cpp \ +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = uORB.cpp +else +SRCS = MuORB.cpp +endif +SRCS += objects_common.cpp \ Publication.cpp \ Subscription.cpp diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk index da247b8dd7..16634eb416 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/linux/px4_layer/module.mk @@ -37,6 +37,9 @@ SRCS = \ px4_linux_impl.cpp \ - px4_linux_tasks.c + px4_linux_tasks.c \ + lib_crc32.c \ + drv_hrt.c \ + queue.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index 86c8b16cc5..e94d51f288 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -40,12 +40,54 @@ #include #include #include +#include "systemlib/param/param.h" + +__BEGIN_DECLS + +// FIXME - This needs to be properly initialized +struct param_info_s param_array[256]; +struct param_info_s *param_info_base; +struct param_info_s *param_info_limit; + +__END_DECLS namespace px4 { void init(int argc, char *argv[], const char *app_name) { + struct param_info_s test_1 = { + "TEST_1", + PARAM_TYPE_INT32 + }; + test_1.val.i = 2; + + struct param_info_s test_2 = { + "TEST_2", + PARAM_TYPE_INT32 + }; + test_2.val.i = 4; + + struct param_info_s rc_x = { + "RC_X", + PARAM_TYPE_INT32 + }; + rc_x.val.i = 8; + + struct param_info_s rc2_x = { + "RC2_X", + PARAM_TYPE_INT32 + }; + rc2_x.val.i = 16; + + param_array[0] = test_1; + param_array[1] = test_2; + param_array[2] = rc_x; + param_array[3] = rc2_x; + param_info_base = (struct param_info_s *) ¶m_array[0]; + param_info_limit = (struct param_info_s *) ¶m_array[4]; // needs to point at the end of the data, + // therefore number of params + 1 + printf("App name: %s\n", app_name); } diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 5694d4e865..0e732df4b8 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -55,6 +55,9 @@ #define PX4_ERROR (-1) #define PX4_OK 0 +//#define PX4_DEBUG(...) +#define PX4_DEBUG(...) printf(__VA_ARGS__) + __BEGIN_DECLS extern int px4_errno; From fd7863270e77a90fd309642b1707eb7424c0decb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 16:58:24 -0700 Subject: [PATCH 027/258] Nuttx: fixed missing changes from AppMgr to AppState Signed-off-by: Mark Charlebois --- src/examples/publisher/publisher_example.cpp | 4 +- src/examples/publisher/publisher_example.h | 2 +- .../subscriber/subscriber_example.cpp | 2 +- src/examples/subscriber/subscriber_example.h | 2 +- src/modules/dataman/dataman.c | 42 +++++++++++++------ .../mc_att_control.cpp | 2 +- .../mc_att_control.h | 2 +- .../mc_pos_control.cpp | 2 +- .../mc_pos_control.h | 2 +- 9 files changed, 38 insertions(+), 22 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 4fddd072e1..7ce1533125 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -44,7 +44,7 @@ using namespace px4; PublisherExample::PublisherExample() : - _n(mgr), + _n(appState), _rc_channels_pub(_n.advertise()), _v_att_pub(_n.advertise()), _parameter_update_pub(_n.advertise()) @@ -55,7 +55,7 @@ int PublisherExample::main() { px4::Rate loop_rate(10); - while (!mgr.exitRequested()) { + while (!appState.exitRequested()) { loop_rate.sleep(); _n.spinOnce(); diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 8b0d806bd0..2841320734 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -50,7 +50,7 @@ public: int main(); - static px4::AppMgr mgr; + static px4::AppState appState; protected: px4::NodeHandle _n; px4::Publisher *_rc_channels_pub; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 91fb131f32..ae58f634ce 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -49,7 +49,7 @@ void rc_channels_callback_function(const px4_rc_channels &msg) } SubscriberExample::SubscriberExample() : - _n(_mgr), + _n(_appState), _p_sub_interv("SUB_INTERV", PARAM_SUB_INTERV_DEFAULT), _p_test_float("SUB_TESTF", PARAM_SUB_TESTF_DEFAULT) { diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 8003525dd5..4fdd236366 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -58,7 +58,7 @@ protected: px4::ParameterFloat _p_test_float; px4::Subscriber *_sub_rc_chan; - AppMgr _mgr; + AppState _appState; void rc_channels_callback(const px4_rc_channels &msg); void vehicle_attitude_callback(const px4_vehicle_attitude &msg); diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index c3b4ab8d4d..9b64b8baa1 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -40,7 +40,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include @@ -129,7 +129,12 @@ static sem_t g_sys_state_mutex; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; +// FIXME - need a configurable path that is not OS specific +#ifdef __PX4_NUTTX static const char *k_data_manager_device_path = "/fs/microsd/dataman"; +#else +static const char *k_data_manager_device_path = "/tmp/dataman"; +#endif /* The data manager work queues */ @@ -668,7 +673,7 @@ task_main(int argc, char *argv[]) } /* Open or create the data manager file */ - g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); + g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, 0x0777); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); @@ -827,31 +832,40 @@ stop(void) static void usage(void) { - errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}"); + warnx("usage: dataman {start|stop|status|poweronrestart|inflightrestart}"); } int dataman_main(int argc, char *argv[]) { - if (argc < 2) + if (argc < 2) { usage(); + return -1; + } if (!strcmp(argv[1], "start")) { - if (g_fd >= 0) - errx(1, "already running"); + if (g_fd >= 0) { + warnx("dataman already running"); + return -1; + } start(); - if (g_fd < 0) - errx(1, "start failed"); + if (g_fd < 0) { + warnx("dataman start failed"); + return -1; + } - exit(0); + return 0; } /* Worker thread should be running for all other commands */ - if (g_fd < 0) - errx(1, "not running"); + if (g_fd < 0) { + warnx("dataman worker thread not running"); + usage(); + return -1; + } if (!strcmp(argv[1], "stop")) stop(); @@ -861,8 +875,10 @@ dataman_main(int argc, char *argv[]) dm_restart(DM_INIT_REASON_POWER_ON); else if (!strcmp(argv[1], "inflightrestart")) dm_restart(DM_INIT_REASON_IN_FLIGHT); - else + else { usage(); + return -1; + } - exit(1); + return 1; } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 34859071f8..e478a92e9e 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -71,7 +71,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(nullptr), _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), - _n(_mgr), + _n(_appState), /* parameters */ _params_handles({ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index 50f06d6967..aab41f97ba 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -94,7 +94,7 @@ private: px4::NodeHandle _n; - px4::AppMgr _mgr; + px4::AppState _appState; struct { px4::ParameterFloat roll_p; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 1fd86fd509..72ee214292 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -65,7 +65,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _local_pos_sp_msg(), _global_vel_sp_msg(), - _n(_mgr), + _n(_appState), /* parameters */ _params_handles({ diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 3c6f28050e..4f786c4298 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -107,7 +107,7 @@ protected: px4::NodeHandle _n; - px4::AppMgr _mgr; + px4::AppState _appState; struct { px4::ParameterFloat thr_min; From 2abfb7a5beb2bcfef3a9b93ce756397e353f0f93 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 17:53:39 -0700 Subject: [PATCH 028/258] Linux: added queue files for dataman support The dataman module now works under linux using /tmp/dataman as the file path. Two files from NuttX were added to the Linux impl for single linked queue handling. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 +- src/modules/dataman/dataman.c | 4 +- src/platforms/linux/px4_layer/module.mk | 4 +- src/platforms/linux/px4_layer/sq_addlast.c | 72 +++++++++++++++++++ src/platforms/linux/px4_layer/sq_remfirst.c | 76 +++++++++++++++++++++ 5 files changed, 155 insertions(+), 3 deletions(-) create mode 100644 src/platforms/linux/px4_layer/sq_addlast.c create mode 100644 src/platforms/linux/px4_layer/sq_remfirst.c diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 25821922d1..e4382cfce5 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -40,7 +40,7 @@ MODULES += drivers/device MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB -#MODULES += modules/dataman +MODULES += modules/dataman # # Libraries diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 9b64b8baa1..c125716342 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -40,7 +40,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include @@ -48,6 +48,8 @@ #include #include #include +#include +#include #include "dataman.h" #include diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk index 16634eb416..2dd89b0216 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/linux/px4_layer/module.mk @@ -40,6 +40,8 @@ SRCS = \ px4_linux_tasks.c \ lib_crc32.c \ drv_hrt.c \ - queue.c + queue.c \ + sq_addlast.c \ + sq_remfirst.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/linux/px4_layer/sq_addlast.c b/src/platforms/linux/px4_layer/sq_addlast.c new file mode 100644 index 0000000000..faa07efb5c --- /dev/null +++ b/src/platforms/linux/px4_layer/sq_addlast.c @@ -0,0 +1,72 @@ +/************************************************************ + * libc/queue/sq_addlast.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addlast + * + * Description: + * The sq_addlast function places the 'node' at the tail of + * the 'queue' + ************************************************************/ + +void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = NULL; + if (!queue->head) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/linux/px4_layer/sq_remfirst.c b/src/platforms/linux/px4_layer/sq_remfirst.c new file mode 100644 index 0000000000..f81c18dc2e --- /dev/null +++ b/src/platforms/linux/px4_layer/sq_remfirst.c @@ -0,0 +1,76 @@ +/************************************************************ + * libc/queue/sq_remfirst.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_remfirst + * + * Description: + * sq_remfirst function removes the first entry from + * 'queue' + * + ************************************************************/ + +FAR sq_entry_t *sq_remfirst(sq_queue_t *queue) +{ + FAR sq_entry_t *ret = queue->head; + + if (ret) + { + queue->head = ret->flink; + if (!queue->head) + { + queue->tail = NULL; + } + + ret->flink = NULL; + } + + return ret; +} + From d013ac092769f7975e540a24c5756002251248a4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 23:36:46 -0700 Subject: [PATCH 029/258] Support for building more modules with Linux Added more queue support to linux/px4_layer. Use virt char devices for ms5611, and mavlink. Added more HRT functionality. uORB latency test now fails. Likely due to bad HRT impl for Linux. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 8 +- makefiles/toolchain_native.mk | 5 +- .../device/{cdev.cpp => cdev_nuttx.cpp} | 0 src/drivers/device/device.cpp | 159 +- src/drivers/device/device_nuttx.cpp | 257 ++ src/drivers/device/i2c.h | 6 +- src/drivers/device/i2c_linux.cpp | 257 ++ src/drivers/device/{i2c.cpp => i2c_nuttx.cpp} | 0 src/drivers/device/module.mk | 11 +- src/drivers/device/ringbuffer.h | 2 +- src/drivers/device/spi.h | 2 +- src/drivers/device/vdevice.h | 33 + src/drivers/drv_accel.h | 2 +- src/drivers/drv_airspeed.h | 2 +- src/drivers/drv_baro.h | 3 +- src/drivers/drv_device.h | 2 +- src/drivers/drv_gyro.h | 2 +- src/drivers/drv_mag.h | 2 +- src/drivers/drv_sensor.h | 3 +- src/drivers/ms5611/module.mk | 6 +- src/drivers/ms5611/ms5611.h | 6 +- src/drivers/ms5611/ms5611_i2c.cpp | 31 +- src/drivers/ms5611/ms5611_linux.cpp | 1215 +++++++++ .../ms5611/{ms5611.cpp => ms5611_nuttx.cpp} | 0 src/drivers/ms5611/ms5611_spi.cpp | 2 +- src/modules/mavlink/mavlink_ftp.h | 2 +- src/modules/mavlink/mavlink_ftp_linux.cpp | 861 ++++++ src/modules/mavlink/mavlink_main.h | 19 +- src/modules/mavlink/mavlink_main_linux.cpp | 1795 +++++++++++++ src/modules/mavlink/mavlink_main_linux.h | 417 +++ src/modules/mavlink/mavlink_main_nuttx.h | 415 +++ src/modules/mavlink/mavlink_receiver.cpp | 9 +- src/modules/mavlink/module.mk | 14 +- src/modules/sensors/module.mk | 8 +- src/modules/sensors/sensors_linux.cpp | 2308 +++++++++++++++++ .../{sensors.cpp => sensors_nuttx.cpp} | 0 src/platforms/linux/include/queue.h | 8 - src/platforms/linux/main.cpp | 59 +- src/platforms/linux/px4_layer/dq_addlast.c | 74 + src/platforms/linux/px4_layer/dq_remfirst.c | 82 + src/platforms/linux/px4_layer/drv_hrt.c | 197 +- src/platforms/linux/px4_layer/module.mk | 5 +- .../linux/px4_layer/px4_linux_impl.cpp | 21 + .../linux/px4_layer/px4_linux_tasks.c | 34 +- src/platforms/linux/px4_layer/sq_addafter.c | 71 + .../vdevice.cpp => platforms/px4_adc.h} | 91 +- src/platforms/px4_config.h | 6 + src/platforms/px4_defines.h | 3 + src/platforms/px4_i2c.h | 129 + src/platforms/px4_posix.h | 10 +- src/platforms/px4_spi.h | 35 + src/platforms/px4_tasks.h | 4 + src/platforms/px4_workqueue.h | 116 + 53 files changed, 8455 insertions(+), 354 deletions(-) rename src/drivers/device/{cdev.cpp => cdev_nuttx.cpp} (100%) create mode 100644 src/drivers/device/device_nuttx.cpp create mode 100644 src/drivers/device/i2c_linux.cpp rename src/drivers/device/{i2c.cpp => i2c_nuttx.cpp} (100%) create mode 100644 src/drivers/ms5611/ms5611_linux.cpp rename src/drivers/ms5611/{ms5611.cpp => ms5611_nuttx.cpp} (100%) create mode 100644 src/modules/mavlink/mavlink_ftp_linux.cpp create mode 100644 src/modules/mavlink/mavlink_main_linux.cpp create mode 100644 src/modules/mavlink/mavlink_main_linux.h create mode 100644 src/modules/mavlink/mavlink_main_nuttx.h create mode 100644 src/modules/sensors/sensors_linux.cpp rename src/modules/sensors/{sensors.cpp => sensors_nuttx.cpp} (100%) create mode 100644 src/platforms/linux/px4_layer/dq_addlast.c create mode 100644 src/platforms/linux/px4_layer/dq_remfirst.c create mode 100644 src/platforms/linux/px4_layer/sq_addafter.c rename src/{drivers/device/vdevice.cpp => platforms/px4_adc.h} (59%) create mode 100644 src/platforms/px4_i2c.h create mode 100644 src/platforms/px4_spi.h create mode 100644 src/platforms/px4_workqueue.h diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index e4382cfce5..8286c1c367 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -11,7 +11,8 @@ # Board support modules # MODULES += drivers/device -#MODULES += modules/sensors +MODULES += modules/sensors +MODULES += drivers/ms5611 # # System commands @@ -21,8 +22,7 @@ MODULES += drivers/device # # General system control # -#MODULES += modules/mavlink -#MODULES += modules/mavlink +MODULES += modules/mavlink # # Estimation modules (EKF/ SO3 / other filters) @@ -48,7 +48,7 @@ MODULES += modules/dataman MODULES += lib/mathlib MODULES += lib/geo MODULES += lib/geo_lookup -#MODULES += lib/conversion +MODULES += lib/conversion # # Linux port diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index db40689acc..a9598faa39 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -81,7 +81,8 @@ CPP = clang$(CLANGVER) -E DEV_VER_SUPPORTED = 4.2.1 endif -LD = ld.gold +#LD = ld.gold +LD = ld AR = ar rcs NM = nm OBJCOPY = objcopy @@ -286,7 +287,7 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -r -o $1 $2 + $(Q) $(LD) -Ur -o $1 $2 endef # $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev_nuttx.cpp similarity index 100% rename from src/drivers/device/cdev.cpp rename to src/drivers/device/cdev_nuttx.cpp diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index d46901683f..b4976250d4 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -34,64 +34,24 @@ /** * @file device.cpp * - * Fundamental driver base class for the device framework. + * Fundamental driver base class for the virtual device framework. */ #include "device.h" -#include +#include +#include #include #include -#include namespace device { -/** - * Interrupt dispatch table entry. - */ -struct irq_entry { - int irq; - Device *owner; -}; - -static const unsigned irq_nentries = 8; /**< size of the interrupt dispatch table */ -static irq_entry irq_entries[irq_nentries]; /**< interrupt dispatch table (XXX should be a vector) */ - -/** - * Register an interrupt to a specific device. - * - * @param irq The interrupt number to register. - * @param owner The device receiving the interrupt. - * @return OK if the interrupt was registered. - */ -static int register_interrupt(int irq, Device *owner); - -/** - * Unregister an interrupt. - * - * @param irq The previously-registered interrupt to be de-registered. - */ -static void unregister_interrupt(int irq); - -/** - * Handle an interrupt. - * - * @param irq The interrupt being invoked. - * @param context The interrupt register context. - * @return Always returns OK. - */ -static int interrupt(int irq, void *context); - -Device::Device(const char *name, - int irq) : +Device::Device(const char *name) : // public // protected _name(name), - _debug_enabled(false), - // private - _irq(irq), - _irq_attached(false) + _debug_enabled(false) { sem_init(&_lock, 0, 1); @@ -107,9 +67,6 @@ Device::Device(const char *name, Device::~Device() { sem_destroy(&_lock); - - if (_irq_attached) - unregister_interrupt(_irq); } int @@ -117,45 +74,9 @@ Device::init() { int ret = OK; - // If assigned an interrupt, connect it - if (_irq) { - /* ensure it's disabled */ - up_disable_irq(_irq); - - /* register */ - ret = register_interrupt(_irq, this); - - if (ret != OK) - goto out; - - _irq_attached = true; - } - -out: return ret; } -void -Device::interrupt_enable() -{ - if (_irq_attached) - up_enable_irq(_irq); -} - -void -Device::interrupt_disable() -{ - if (_irq_attached) - up_disable_irq(_irq); -} - -void -Device::interrupt(void *context) -{ - // default action is to disable the interrupt so we don't get called again - interrupt_disable(); -} - void Device::log(const char *fmt, ...) { @@ -184,74 +105,26 @@ Device::debug(const char *fmt, ...) } } -static int -register_interrupt(int irq, Device *owner) +int +Device::dev_read(unsigned offset, void *data, unsigned count) { - int ret = -ENOMEM; - - // look for a slot where we can register the interrupt - for (unsigned i = 0; i < irq_nentries; i++) { - if (irq_entries[i].irq == 0) { - - // great, we could put it here; try attaching it - ret = irq_attach(irq, &interrupt); - - if (ret == OK) { - irq_entries[i].irq = irq; - irq_entries[i].owner = owner; - } - - break; - } - } - - return ret; -} - -static void -unregister_interrupt(int irq) -{ - for (unsigned i = 0; i < irq_nentries; i++) { - if (irq_entries[i].irq == irq) { - irq_entries[i].irq = 0; - irq_entries[i].owner = nullptr; - } - } -} - -static int -interrupt(int irq, void *context) -{ - for (unsigned i = 0; i < irq_nentries; i++) { - if (irq_entries[i].irq == irq) { - irq_entries[i].owner->interrupt(context); - break; - } - } - - return OK; + return -ENODEV; } int -Device::read(unsigned offset, void *data, unsigned count) +Device::dev_write(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int -Device::write(unsigned offset, void *data, unsigned count) +Device::dev_ioctl(unsigned operation, unsigned &arg) { - return -ENODEV; -} - -int -Device::ioctl(unsigned operation, unsigned &arg) -{ - switch (operation) { - case DEVIOCGDEVICEID: - return (int)_device_id.devid; - } - return -ENODEV; + switch (operation) { + case PX4_DEVIOCGDEVICEID: + return (int)_device_id.devid; + } + return -ENODEV; } } // namespace device diff --git a/src/drivers/device/device_nuttx.cpp b/src/drivers/device/device_nuttx.cpp new file mode 100644 index 0000000000..d46901683f --- /dev/null +++ b/src/drivers/device/device_nuttx.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file device.cpp + * + * Fundamental driver base class for the device framework. + */ + +#include "device.h" + +#include +#include +#include +#include + +namespace device +{ + +/** + * Interrupt dispatch table entry. + */ +struct irq_entry { + int irq; + Device *owner; +}; + +static const unsigned irq_nentries = 8; /**< size of the interrupt dispatch table */ +static irq_entry irq_entries[irq_nentries]; /**< interrupt dispatch table (XXX should be a vector) */ + +/** + * Register an interrupt to a specific device. + * + * @param irq The interrupt number to register. + * @param owner The device receiving the interrupt. + * @return OK if the interrupt was registered. + */ +static int register_interrupt(int irq, Device *owner); + +/** + * Unregister an interrupt. + * + * @param irq The previously-registered interrupt to be de-registered. + */ +static void unregister_interrupt(int irq); + +/** + * Handle an interrupt. + * + * @param irq The interrupt being invoked. + * @param context The interrupt register context. + * @return Always returns OK. + */ +static int interrupt(int irq, void *context); + +Device::Device(const char *name, + int irq) : + // public + // protected + _name(name), + _debug_enabled(false), + // private + _irq(irq), + _irq_attached(false) +{ + sem_init(&_lock, 0, 1); + + /* setup a default device ID. When bus_type is UNKNOWN the + other fields are invalid */ + _device_id.devid = 0; + _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; + _device_id.devid_s.bus = 0; + _device_id.devid_s.address = 0; + _device_id.devid_s.devtype = 0; +} + +Device::~Device() +{ + sem_destroy(&_lock); + + if (_irq_attached) + unregister_interrupt(_irq); +} + +int +Device::init() +{ + int ret = OK; + + // If assigned an interrupt, connect it + if (_irq) { + /* ensure it's disabled */ + up_disable_irq(_irq); + + /* register */ + ret = register_interrupt(_irq, this); + + if (ret != OK) + goto out; + + _irq_attached = true; + } + +out: + return ret; +} + +void +Device::interrupt_enable() +{ + if (_irq_attached) + up_enable_irq(_irq); +} + +void +Device::interrupt_disable() +{ + if (_irq_attached) + up_disable_irq(_irq); +} + +void +Device::interrupt(void *context) +{ + // default action is to disable the interrupt so we don't get called again + interrupt_disable(); +} + +void +Device::log(const char *fmt, ...) +{ + va_list ap; + + printf("[%s] ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +} + +void +Device::debug(const char *fmt, ...) +{ + va_list ap; + + if (_debug_enabled) { + printf("<%s> ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); + } +} + +static int +register_interrupt(int irq, Device *owner) +{ + int ret = -ENOMEM; + + // look for a slot where we can register the interrupt + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == 0) { + + // great, we could put it here; try attaching it + ret = irq_attach(irq, &interrupt); + + if (ret == OK) { + irq_entries[i].irq = irq; + irq_entries[i].owner = owner; + } + + break; + } + } + + return ret; +} + +static void +unregister_interrupt(int irq) +{ + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == irq) { + irq_entries[i].irq = 0; + irq_entries[i].owner = nullptr; + } + } +} + +static int +interrupt(int irq, void *context) +{ + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == irq) { + irq_entries[i].owner->interrupt(context); + break; + } + } + + return OK; +} + +int +Device::read(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::write(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::ioctl(unsigned operation, unsigned &arg) +{ + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } + return -ENODEV; +} + +} // namespace device diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index cb13fed83c..97ab25672c 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -42,7 +42,7 @@ #include "device.h" -#include +#include namespace device __EXPORT { @@ -124,7 +124,7 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(i2c_msg_s *msgv, unsigned msgs); + int transfer(px4_i2c_msg_t *msgv, unsigned msgs); /** * Change the bus address. @@ -142,7 +142,7 @@ protected: private: uint16_t _address; uint32_t _frequency; - struct i2c_dev_s *_dev; + px4_i2c_dev_t *_dev; I2C(const device::I2C &); I2C operator=(const device::I2C &); diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp new file mode 100644 index 0000000000..4ab3ff1723 --- /dev/null +++ b/src/drivers/device/i2c_linux.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file i2c.cpp + * + * Base class for devices attached via the I2C bus. + * + * @todo Bus frequency changes; currently we do nothing with the value + * that is supplied. Should we just depend on the bus knowing? + */ + +#include "i2c.h" + +namespace device +{ + +unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 }; + +I2C::I2C(const char *name, + const char *devname, + int bus, + uint16_t address, + uint32_t frequency, + int irq) : + // base class + CDev(name, devname), + // public + // protected + _retries(0), + // private + _bus(bus), + _address(address), + _frequency(frequency), + _dev(nullptr) +{ + // fill in _device_id fields for a I2C device + _device_id.devid_s.bus_type = DeviceBusType_I2C; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; +} + +I2C::~I2C() +{ + if (_dev) { + px4_i2cuninitialize(_dev); + _dev = nullptr; + } +} + +int +I2C::set_bus_clock(unsigned bus, unsigned clock_hz) +{ + int index = bus - 1; + + if (index < 0 || index >= static_cast(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) { + return -EINVAL; + } + + if (_bus_clocks[index] > 0) { + // debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); + } + _bus_clocks[index] = clock_hz; + + return PX4_OK; +} + +int +I2C::init() +{ + int ret = PX4_OK; + unsigned bus_index; + + // attach to the i2c bus + _dev = px4_i2cinitialize(_bus); + + if (_dev == nullptr) { + debug("failed to init I2C"); + ret = -ENOENT; + goto out; + } + + // the above call fails for a non-existing bus index, + // so the index math here is safe. + bus_index = _bus - 1; + + // abort if the max frequency we allow (the frequency we ask) + // is smaller than the bus frequency + if (_bus_clocks[bus_index] > _frequency) { + (void)px4_i2cuninitialize(_dev); + _dev = nullptr; + log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", + _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); + ret = -EINVAL; + goto out; + } + + // set the bus frequency on the first access if it has + // not been set yet + if (_bus_clocks[bus_index] == 0) { + _bus_clocks[bus_index] = _frequency; + } + + // set frequency for this instance once to the bus speed + // the bus speed is the maximum supported by all devices on the bus, + // as we have to prioritize performance over compatibility. + // If a new device requires a lower clock speed, this has to be + // manually set via "fmu i2c " before starting any + // drivers. + // This is necessary as automatically lowering the bus speed + // for maximum compatibility could induce timing issues on + // critical sensors the adopter might be unaware of. + I2C_SETFREQUENCY(_dev, _bus_clocks[bus_index]); + + // call the probe function to check whether the device is present + ret = probe(); + + if (ret != PX4_OK) { + debug("probe failed"); + goto out; + } + + // do base class init, which will create device node, etc + ret = CDev::init(); + + if (ret != PX4_OK) { + debug("cdev init failed"); + goto out; + } + + // tell the world where we are + log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", + _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + +out: + if ((ret != PX4_OK) && (_dev != nullptr)) { + px4_i2cuninitialize(_dev); + _dev = nullptr; + } + return ret; +} + +int +I2C::probe() +{ + // Assume the device is too stupid to be discoverable. + return PX4_OK; +} + +int +I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + px4_i2c_msg_t msgv[2]; + unsigned msgs; + int ret; + unsigned retry_count = 0; + + do { + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + + msgs = 0; + + if (send_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = const_cast(send); + msgv[msgs].length = send_len; + msgs++; + } + + if (recv_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -EINVAL; + + ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + + /* success */ + if (ret == PX4_OK) + break; + + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) + px4_i2creset(_dev); + + } while (retry_count++ < _retries); + + return ret; + +} + +int +I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs) +{ + int ret; + unsigned retry_count = 0; + + /* force the device address into the message vector */ + for (unsigned i = 0; i < msgs; i++) + msgv[i].addr = _address; + + + do { + ret = I2C_TRANSFER(_dev, msgv, msgs); + + /* success */ + if (ret == PX4_OK) + break; + + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) + px4_i2creset(_dev); + + } while (retry_count++ < _retries); + + return ret; +} + +} // namespace device diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c_nuttx.cpp similarity index 100% rename from src/drivers/device/i2c.cpp rename to src/drivers/device/i2c_nuttx.cpp diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 5475e01e41..a3660d70a5 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -37,14 +37,15 @@ ifeq ($(PX4_TARGET_OS),nuttx) SRCS = \ - device.cpp \ - cdev.cpp \ - i2c.cpp \ + device_nuttx.cpp \ + cdev_nuttx.cpp \ + i2c_nuttx.cpp \ pio.cpp \ spi.cpp endif ifeq ($(PX4_TARGET_OS),linux) SRCS = vcdev.cpp \ - vdevice.cpp \ - vcdev_posix.cpp + device.cpp \ + vcdev_posix.cpp \ + i2c_linux.cpp endif diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index b26e2e7c83..35b38efd7d 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -503,7 +503,7 @@ RingBuffer::resize(unsigned new_size) void RingBuffer::print_info(const char *name) { - printf("%s %u/%u (%u/%u @ %p)\n", + printf("%s %u/%lu (%u/%u @ %p)\n", name, _num_items, _num_items * _item_size, diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 1d98376898..4822880144 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -42,7 +42,7 @@ #include "device.h" -#include +#include namespace device __EXPORT { diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdevice.h index f35ac9fc19..bf823f5b5d 100644 --- a/src/drivers/device/vdevice.h +++ b/src/drivers/device/vdevice.h @@ -97,6 +97,39 @@ public: */ virtual int init(); + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + int dev_read(unsigned address, void *data, unsigned count); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + int dev_write(unsigned address, void *data, unsigned count); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + int dev_ioctl(unsigned operation, unsigned &arg); + /* device bus types for DEVID */ diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index fccd446ad0..738b28a6e1 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -94,7 +94,7 @@ ORB_DECLARE(sensor_accel); */ #define _ACCELIOCBASE (0x2100) -#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) +#define _ACCELIOC(_n) (_PX4_IOC(_ACCELIOCBASE, _n)) /** set the accel internal sample rate to at least (arg) Hz */ diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index fddef36263..ff49a60664 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -59,7 +59,7 @@ */ #define _AIRSPEEDIOCBASE (0x7700) -#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n)) +#define __AIRSPEEDIOC(_n) (_PX4_IOC(_AIRSPEEDIOCBASE, _n)) #define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0) #define AIRSPEEDIOCGSCALE __AIRSPEEDIOC(1) diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 65fb3a4cf4..791e3c5cc1 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -40,6 +40,7 @@ #ifndef _DRV_BARO_H #define _DRV_BARO_H +#include #include #include @@ -71,7 +72,7 @@ ORB_DECLARE(sensor_baro); */ #define _BAROIOCBASE (0x2200) -#define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n)) +#define _BAROIOC(_n) (_PX4_IOC(_BAROIOCBASE, _n)) /** set corrected MSL pressure in pascals */ #define BAROIOCSMSLPRESSURE _BAROIOC(0) diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 19d55cef38..650b18bed5 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -51,7 +51,7 @@ */ #define _DEVICEIOCBASE (0x100) -#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n)) +#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n)) /** ask device to stop publishing */ #define DEVIOCSPUBBLOCK _DEVICEIOC(0) diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 122d204159..8e80413ac9 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -91,7 +91,7 @@ ORB_DECLARE(sensor_gyro); */ #define _GYROIOCBASE (0x2300) -#define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n)) +#define _GYROIOC(_n) (_PX4_IOC(_GYROIOCBASE, _n)) /** set the gyro internal sample rate to at least (arg) Hz */ #define GYROIOCSSAMPLERATE _GYROIOC(0) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 3c12216a62..e2f8493e38 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -91,7 +91,7 @@ ORB_DECLARE(sensor_mag); */ #define _MAGIOCBASE (0x2400) -#define _MAGIOC(_n) (_IOC(_MAGIOCBASE, _n)) +#define _MAGIOC(_n) (_PX4_IOC(_MAGIOCBASE, _n)) /** set the mag internal sample rate to at least (arg) Hz */ #define MAGIOCSSAMPLERATE _MAGIOC(0) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 467dace082..b1148ce76f 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -40,6 +40,7 @@ #ifndef _DRV_SENSOR_H #define _DRV_SENSOR_H +#include #include #include @@ -69,7 +70,7 @@ */ #define _SENSORIOCBASE (0x2000) -#define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n)) +#define _SENSORIOC(_n) (_PX4_IOC(_SENSORIOCBASE, _n)) /** * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index ee74058fc1..ba1ac7efcf 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -37,6 +37,10 @@ MODULE_COMMAND = ms5611 -SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = ms5611_nuttx.cpp ms5611_spi.cpp ms5611_i2c.cpp +else +SRCS = ms5611_linux.cpp ms5611_spi.cpp ms5611_i2c.cpp +endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index c946e38cb8..d129891bda 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 30d5e48881..733fafbd87 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -39,12 +39,13 @@ /* XXX trim includes */ #include +#include #include #include #include #include -#include +//#include #include #include @@ -70,8 +71,8 @@ public: virtual ~MS5611_I2C(); virtual int init(); - virtual int read(unsigned offset, void *data, unsigned count); - virtual int ioctl(unsigned operation, unsigned &arg); + virtual int dev_read(unsigned offset, void *data, unsigned count); + virtual int dev_ioctl(unsigned operation, unsigned &arg); protected: virtual int probe(); @@ -98,7 +99,7 @@ private: /** * Read the MS5611 PROM * - * @return OK if the PROM reads successfully. + * @return PX4_OK if the PROM reads successfully. */ int _read_prom(); @@ -128,7 +129,7 @@ MS5611_I2C::init() } int -MS5611_I2C::read(unsigned offset, void *data, unsigned count) +MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count) { union _cvt { uint8_t b[4]; @@ -139,7 +140,7 @@ MS5611_I2C::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ uint8_t cmd = 0; int ret = transfer(&cmd, 1, &buf[0], 3); - if (ret == OK) { + if (ret == PX4_OK) { /* fetch the raw value */ cvt->b[0] = buf[2]; cvt->b[1] = buf[1]; @@ -151,7 +152,7 @@ MS5611_I2C::read(unsigned offset, void *data, unsigned count) } int -MS5611_I2C::ioctl(unsigned operation, unsigned &arg) +MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg) { int ret; @@ -176,14 +177,14 @@ MS5611_I2C::probe() { _retries = 10; - if ((OK == _probe_address(MS5611_ADDRESS_1)) || - (OK == _probe_address(MS5611_ADDRESS_2))) { + if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) || + (PX4_OK == _probe_address(MS5611_ADDRESS_2))) { /* * Disable retries; we may enable them selectively in some cases, * but the device gets confused if we retry some of the commands. */ _retries = 0; - return OK; + return PX4_OK; } return -EIO; @@ -196,14 +197,14 @@ MS5611_I2C::_probe_address(uint8_t address) set_address(address); /* send reset command */ - if (OK != _reset()) + if (PX4_OK != _reset()) return -EIO; /* read PROM */ - if (OK != _read_prom()) + if (PX4_OK != _read_prom()) return -EIO; - return OK; + return PX4_OK; } @@ -254,7 +255,7 @@ MS5611_I2C::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) break; /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ @@ -264,5 +265,5 @@ MS5611_I2C::_read_prom() } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + return ms5611::crc4(&_prom.c[0]) ? PX4_OK : -EIO; } diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp new file mode 100644 index 0000000000..043f16407b --- /dev/null +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -0,0 +1,1215 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file ms5611.cpp + * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "ms5611.h" + +enum MS5611_BUS { + MS5611_BUS_ALL = 0, + MS5611_BUS_I2C_INTERNAL, + MS5611_BUS_I2C_EXTERNAL, + MS5611_BUS_SPI_INTERNAL, + MS5611_BUS_SPI_EXTERNAL +}; + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +//#ifndef CONFIG_SCHED_WORKQUEUE +//# error This requires CONFIG_SCHED_WORKQUEUE. +//#endif + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) + +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +/* + * MS5611 internal constants and data structures. + */ + +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" +#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" + +class MS5611 : public device::CDev +{ +public: + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); + ~MS5611(); + + virtual int init(); + + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + Device *_interface; + + ms5611::prom_s _prom; + + struct work_s _work; + unsigned _measure_ticks; + + RingBuffer *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + float _P; + float _T; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in Pa */ + + orb_advert_t _baro_topic; + int _orb_class_instance; + int _class_instance; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Initialize the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + virtual int measure(); + + /** + * Collect the result of the most recent measurement. + */ + virtual int collect(); +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); + +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : + CDev("MS5611", path), + _interface(interface), + _prom(prom_buf.s), + _measure_ticks(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _baro_topic(-1), + _orb_class_instance(-1), + _class_instance(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), + _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) +{ + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MS5611::~MS5611() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + if (_class_instance != -1) + unregister_class_devname(get_devname(), _class_instance); + + /* free any existing reports */ + if (_reports != nullptr) + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + + delete _interface; +} + +int +MS5611::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(baro_report)); + + if (_reports == nullptr) { + debug("can't get memory for reports"); + ret = -ENOMEM; + goto out; + } + + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); + + struct baro_report brp; + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + _reports->flush(); + + /* this do..while is goto without goto */ + do { + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + _reports->get(&brp); + + ret = OK; + + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + + + if (_baro_topic < 0) { + warnx("failed to create sensor_baro publication"); + } + + } while (0); + +out: + return ret; +} + +ssize_t +MS5611::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(brp)) { + ret += sizeof(*brp); + brp++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _measure_phase = 0; + _reports->flush(); + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(brp)) + ret = sizeof(*brp); + + } while (0); + + return ret; +} + +int +MS5611::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + if (!_reports->resize(arg)) { + return -ENOMEM; + } + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* + * Since we are initialized, we do not need to do anything, since the + * PROM is correctly read and the part does not need to be configured. + */ + return OK; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return CDev::ioctl(handlep, cmd, arg); +} + +void +MS5611::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); +} + +void +MS5611::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611::cycle_trampoline(void *arg) +{ + MS5611 *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void +MS5611::cycle() +{ + int ret; + unsigned dummy; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with a message for this. + */ + } else { + //log("collection error %d", ret); + } + /* issue a reset command to the sensor */ + _interface->dev_ioctl(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* issue a reset command to the sensor */ + _interface->dev_ioctl(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +int +MS5611::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + */ + ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); + if (OK != ret) + perf_count(_comms_errors); + + perf_end(_measure_perf); + + return ret; +} + +int +MS5611::collect() +{ + int ret; + uint32_t raw; + + perf_begin(_sample_perf); + + struct baro_report report; + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + ret = _interface->dev_read(0, (void *)&raw, 0); + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); + + /* temperature compensation */ + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; + + /* generate a new report */ + report.temperature = _TEMP / 100.0f; + report.pressure = P / 100.0f; /* convert to millibar */ + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us + */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* publish it */ + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +void +MS5611::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", (long long)_SENS); + printf("OFF: %lld\n", (long long)_OFF); + printf("P: %.3f\n", (double)_P); + printf("T: %.3f\n", (double)_T); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + + printf("factory_setup %u\n", _prom.factory_setup); + printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.serial_and_crc); +} + +/** + * Local functions in support of the shell command. + */ +namespace ms5611 +{ + +/* + list of supported bus configurations + */ +struct ms5611_bus_option { + enum MS5611_BUS busid; + const char *devpath; + MS5611_constructor interface_constructor; + uint8_t busnum; + MS5611 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, +#endif +#ifdef PX4_SPIDEV_BARO + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +bool start_bus(struct ms5611_bus_option &bus); +struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); +void start(enum MS5611_BUS busid); +void test(enum MS5611_BUS busid); +void reset(enum MS5611_BUS busid); +void info(); +void calibrate(unsigned altitude, enum MS5611_BUS busid); +void usage(); + +/** + * MS5611 crc4 cribbed from the datasheet + */ +bool +crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + + +/** + * Start the driver. + */ +bool +start_bus(struct ms5611_bus_option &bus) +{ + if (bus.dev != nullptr) { + warnx("bus option already started"); + return false; + } + + prom_u prom_buf; + device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new MS5611(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = open(bus.devpath, O_RDONLY); + + /* set the poll rate to default, starts automatic data collection */ + if (fd == -1) { + warnx("can't open baro device"); + return false; + } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + warnx("failed setting default poll rate"); + return false; + } + + close(fd); + return true; +} + + +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum MS5611_BUS busid) +{ + uint8_t i; + bool started = false; + + for (i=0; iprint_info(); + } + } +} + +/** + * Calculate actual MSL pressure given current altitude + */ +void +calibrate(unsigned altitude, enum MS5611_BUS busid) +{ + struct ms5611_bus_option &bus = find_bus(busid); + struct baro_report report; + float pressure; + float p1; + + int fd; + + fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); + + /* start the sensor polling at max */ + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) + errx(1, "failed to set poll rate"); + + /* average a few measurements */ + pressure = 0.0f; + + for (unsigned i = 0; i < 20; i++) { + struct pollfd fds; + int ret; + ssize_t sz; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 1000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "sensor read failed"); + + pressure += report.pressure; + } + + pressure /= 20; /* average */ + pressure /= 10; /* scale from millibar to kPa */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); + + p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); + + warnx("calculated MSL pressure %10.4fkPa", (double)p1); + + /* save as integer Pa */ + p1 *= 1000.0f; + + if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) + err(1, "BAROIOCSMSLPRESSURE"); + + close(fd); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("options:"); + warnx(" -X (external I2C bus)"); + warnx(" -I (intternal I2C bus)"); + warnx(" -S (external SPI bus)"); + warnx(" -s (internal SPI bus)"); +} + +} // namespace + +int +ms5611_main(int argc, char *argv[]) +{ + enum MS5611_BUS busid = MS5611_BUS_ALL; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "XISs")) != EOF) { + switch (ch) { + case 'X': + busid = MS5611_BUS_I2C_EXTERNAL; + break; + case 'I': + busid = MS5611_BUS_I2C_INTERNAL; + break; + case 'S': + busid = MS5611_BUS_SPI_EXTERNAL; + break; + case 's': + busid = MS5611_BUS_SPI_INTERNAL; + break; + default: + ms5611::usage(); + return 1; + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) + ms5611::start(busid); + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) + ms5611::test(busid); + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) + ms5611::reset(busid); + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) + ms5611::info(); + + /* + * Perform MSL pressure calibration given an altitude in metres + */ + if (!strcmp(verb, "calibrate")) { + if (argc < 2) + errx(1, "missing altitude"); + + long altitude = strtol(argv[optind+1], nullptr, 10); + + ms5611::calibrate(altitude, busid); + } + + warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); + return 1; +} diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp similarity index 100% rename from src/drivers/ms5611/ms5611.cpp rename to src/drivers/ms5611/ms5611_nuttx.cpp diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 0e68e9cb5d..7ec7718157 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +//#include #include #include diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 9693a92a97..eca1710174 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include "mavlink_messages.h" diff --git a/src/modules/mavlink/mavlink_ftp_linux.cpp b/src/modules/mavlink/mavlink_ftp_linux.cpp new file mode 100644 index 0000000000..1ff7dce735 --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp_linux.cpp @@ -0,0 +1,861 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/// @file mavlink_ftp.cpp +/// @author px4dev, Don Gagne + +#include +#include +#include +#include +#include +#include + +#include "mavlink_ftp.h" +#include "mavlink_tests/mavlink_ftp_test.h" + +// Uncomment the line below to get better debug output. Never commit with this left on. +//#define MAVLINK_FTP_DEBUG + +MavlinkFTP * +MavlinkFTP::get_server(void) +{ + static MavlinkFTP server; + return &server; +} + +MavlinkFTP::MavlinkFTP() : + _request_bufs{}, + _request_queue{}, + _request_queue_sem{}, + _utRcvMsgFunc{}, + _ftp_test{} +{ + // initialise the request freelist + dq_init(&_request_queue); + sem_init(&_request_queue_sem, 0, 1); + + // initialize session list + for (size_t i=0; imsgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + mavlink_msg_file_transfer_protocol_decode(msg, &req->message); + +#ifdef MAVLINK_FTP_UNIT_TEST + if (!_utRcvMsgFunc) { + warnx("Incorrectly written unit test\n"); + return; + } + // We use fake ids when unit testing + req->serverSystemId = MavlinkFtpTest::serverSystemId; + req->serverComponentId = MavlinkFtpTest::serverComponentId; + req->serverChannel = MavlinkFtpTest::serverChannel; +#else + // Not unit testing, use the real thing + req->serverSystemId = mavlink->get_system_id(); + req->serverComponentId = mavlink->get_component_id(); + req->serverChannel = mavlink->get_channel(); +#endif + + // This is the system id we want to target when sending + req->targetSystemId = msg->sysid; + + if (req->message.target_system == req->serverSystemId) { + req->mavlink = mavlink; +#ifdef MAVLINK_FTP_UNIT_TEST + // We are running in Unit Test mode. Don't queue, just call _worket directly. + _process_request(req); +#else + // We are running in normal mode. Queue the request to the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); +#endif + return; + } + } + + _return_request(req); +} + +/// @brief Queued static work queue routine to handle mavlink messages +void +MavlinkFTP::_worker_trampoline(void *arg) +{ + Request* req = reinterpret_cast(arg); + MavlinkFTP* server = MavlinkFTP::get_server(); + + // call the server worker with the work item + server->_process_request(req); +} + +/// @brief Processes an FTP message +void +MavlinkFTP::_process_request(Request *req) +{ + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + + ErrorCode errorCode = kErrNone; + + // basic sanity checks; must validate length before use + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; + goto out; + } + +#ifdef MAVLINK_FTP_DEBUG + printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); +#endif + + switch (payload->opcode) { + case kCmdNone: + break; + + case kCmdTerminateSession: + errorCode = _workTerminate(payload); + break; + + case kCmdResetSessions: + errorCode = _workReset(payload); + break; + + case kCmdListDirectory: + errorCode = _workList(payload); + break; + + case kCmdOpenFileRO: + errorCode = _workOpen(payload, O_RDONLY); + break; + + case kCmdCreateFile: + errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY); + break; + + case kCmdOpenFileWO: + errorCode = _workOpen(payload, O_CREAT | O_WRONLY); + break; + + case kCmdReadFile: + errorCode = _workRead(payload); + break; + + case kCmdWriteFile: + errorCode = _workWrite(payload); + break; + + case kCmdRemoveFile: + errorCode = _workRemoveFile(payload); + break; + + case kCmdRename: + errorCode = _workRename(payload); + break; + + case kCmdTruncateFile: + errorCode = _workTruncateFile(payload); + break; + + case kCmdCreateDirectory: + errorCode = _workCreateDirectory(payload); + break; + + case kCmdRemoveDirectory: + errorCode = _workRemoveDirectory(payload); + break; + + + case kCmdCalcFileCRC32: + errorCode = _workCalcFileCRC32(payload); + break; + + default: + errorCode = kErrUnknownCommand; + break; + } + +out: + // handle success vs. error + if (errorCode == kErrNone) { + payload->req_opcode = payload->opcode; + payload->opcode = kRspAck; +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: ack\n"); +#endif + } else { + int r_errno = errno; + warnx("FTP: nak %u", errorCode); + payload->req_opcode = payload->opcode; + payload->opcode = kRspNak; + payload->size = 1; + payload->data[0] = errorCode; + if (errorCode == kErrFailErrno) { + payload->size = 2; + payload->data[1] = r_errno; + } + } + + + // respond to the request + _reply(req); + + _return_request(req); +} + +/// @brief Sends the specified FTP reponse message out through mavlink +void +MavlinkFTP::_reply(Request *req) +{ + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + + payload->seqNumber = payload->seqNumber + 1; + + mavlink_message_t msg; + msg.checksum = 0; +#ifndef MAVLINK_FTP_UNIT_TEST + uint16_t len = +#endif + mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id + req->serverComponentId, // Sender component id + req->serverChannel, // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + req->targetSystemId, // Target system id + 0, // Target component id + (const uint8_t*)payload); // Payload to pack into message + + bool success = true; +#ifdef MAVLINK_FTP_UNIT_TEST + // Unit test hook is set, call that instead + _utRcvMsgFunc(&msg, _ftp_test); +#else + Mavlink *mavlink = req->mavlink; + + mavlink->lockMessageBufferMutex(); + success = mavlink->message_buffer_write(&msg, len); + mavlink->unlockMessageBufferMutex(); + +#endif + + if (!success) { + warnx("FTP TX ERR"); + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d", + req->serverSystemId, + req->serverComponentId, + req->serverChannel, + msg.checksum); + } +#endif +} + +/// @brief Responds to a List command +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(PayloadHeader* payload) +{ + char dirPath[kMaxDataLength]; + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); + + DIR *dp = opendir(dirPath); + + if (dp == nullptr) { + warnx("FTP: can't open path '%s'", dirPath); + return kErrFailErrno; + } + +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: list %s offset %d", dirPath, payload->offset); +#endif + + ErrorCode errorCode = kErrNone; + struct dirent entry, *result = nullptr; + unsigned offset = 0; + + // move to the requested offset + seekdir(dp, payload->offset); + + for (;;) { + // read the directory entry + if (readdir_r(dp, &entry, &result)) { + warnx("FTP: list %s readdir_r failure\n", dirPath); + errorCode = kErrFailErrno; + break; + } + + // no more entries? + if (result == nullptr) { + if (payload->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + break; + } + + uint32_t fileSize = 0; + char buf[256]; + char direntType; + + // Determine the directory entry type + switch (entry.d_type) { +#ifdef __PX4_NUTTX + case DTYPE_FILE: +#else + case DT_REG: +#endif + // For files we get the file size as well + direntType = kDirentFile; + snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); + struct stat st; + if (stat(buf, &st) == 0) { + fileSize = st.st_size; + } + break; +#ifdef __PX4_NUTTX + case DTYPE_DIRECTORY: +#else + case DT_DIR: +#endif + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // Don't bother sending these back + direntType = kDirentSkip; + } else { + direntType = kDirentDir; + } + break; + default: + // We only send back file and diretory entries, skip everything else + direntType = kDirentSkip; + } + + if (direntType == kDirentSkip) { + // Skip send only dirent identifier + buf[0] = '\0'; + } else if (direntType == kDirentFile) { + // Files send filename and file length + snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); + } else { + // Everything else just sends name + strncpy(buf, entry.d_name, sizeof(buf)); + buf[sizeof(buf)-1] = 0; + } + size_t nameLen = strlen(buf); + + // Do we have room for the name, the one char directory identifier and the null terminator? + if ((offset + nameLen + 2) > kMaxDataLength) { + break; + } + + // Move the data into the buffer + payload->data[offset++] = direntType; + strcpy((char *)&payload->data[offset], buf); +#ifdef MAVLINK_FTP_DEBUG + printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); +#endif + offset += nameLen + 1; + } + + closedir(dp); + payload->size = offset; + + return errorCode; +} + +/// @brief Responds to an Open command +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) +{ + int session_index = _find_unused_session(); + if (session_index < 0) { + warnx("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; + } + + char *filename = _data_as_cstring(payload); + + uint32_t fileSize = 0; + struct stat st; + if (stat(filename, &st) != 0) { + // fail only if requested open for read + if (oflag & O_RDONLY) + return kErrFailErrno; + else + st.st_size = 0; + } + fileSize = st.st_size; + + int fd = ::open(filename, oflag); + if (fd < 0) { + return kErrFailErrno; + } + _session_fds[session_index] = fd; + + payload->session = session_index; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; + + return kErrNone; +} + +/// @brief Responds to a Read command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(PayloadHeader* payload) +{ + int session_index = payload->session; + + if (!_valid_session(session_index)) { + return kErrInvalidSession; + } + + // Seek to the specified position +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", payload->offset); +#endif + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + warnx("seek fail"); + return kErrEOF; + } + + int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof + warnx("read fail %d", bytes_read); + return kErrFailErrno; + } + + payload->size = bytes_read; + + return kErrNone; +} + +/// @brief Responds to a Write command +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(PayloadHeader* payload) +{ + int session_index = payload->session; + + if (!_valid_session(session_index)) { + return kErrInvalidSession; + } + + // Seek to the specified position +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", payload->offset); +#endif + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + warnx("seek fail"); + return kErrFailErrno; + } + + int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size); + if (bytes_written < 0) { + // Negative return indicates error other than eof + warnx("write fail %d", bytes_written); + return kErrFailErrno; + } + + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = bytes_written; + + return kErrNone; +} + +/// @brief Responds to a RemoveFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveFile(PayloadHeader* payload) +{ + char file[kMaxDataLength]; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + + if (unlink(file) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a TruncateFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTruncateFile(PayloadHeader* payload) +{ + char file[kMaxDataLength]; + const char temp_file[] = "/fs/microsd/.trunc.tmp"; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + payload->size = 0; + + // emulate truncate(file, payload->offset) by + // copying to temp and overwrite with O_TRUNC flag. + + struct stat st; + if (stat(file, &st) != 0) { + return kErrFailErrno; + } + + if (!S_ISREG(st.st_mode)) { + errno = EISDIR; + return kErrFailErrno; + } + + // check perms allow us to write (not romfs) + if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) { + errno = EROFS; + return kErrFailErrno; + } + + if (payload->offset == (unsigned)st.st_size) { + // nothing to do + return kErrNone; + } + else if (payload->offset == 0) { + // 1: truncate all data + int fd = ::open(file, O_TRUNC | O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + ::close(fd); + return kErrNone; + } + else if (payload->offset > (unsigned)st.st_size) { + // 2: extend file + int fd = ::open(file, O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) { + ::close(fd); + return kErrFailErrno; + } + + bool ok = 1 == ::write(fd, "", 1); + ::close(fd); + + return (ok)? kErrNone : kErrFailErrno; + } + else { + // 3: truncate + if (_copy_file(file, temp_file, payload->offset) != 0) { + return kErrFailErrno; + } + if (_copy_file(temp_file, file, payload->offset) != 0) { + return kErrFailErrno; + } + if (::unlink(temp_file) != 0) { + return kErrFailErrno; + } + + return kErrNone; + } +} + +/// @brief Responds to a Terminate command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(PayloadHeader* payload) +{ + if (!_valid_session(payload->session)) { + return kErrInvalidSession; + } + + ::close(_session_fds[payload->session]); + _session_fds[payload->session] = -1; + + payload->size = 0; + + return kErrNone; +} + +/// @brief Responds to a Reset command +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(PayloadHeader* payload) +{ + for (size_t i=0; isize = 0; + + return kErrNone; +} + +/// @brief Responds to a Rename command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRename(PayloadHeader* payload) +{ + char oldpath[kMaxDataLength]; + char newpath[kMaxDataLength]; + + char *ptr = _data_as_cstring(payload); + size_t oldpath_sz = strlen(ptr); + + if (oldpath_sz == payload->size) { + // no newpath + errno = EINVAL; + return kErrFailErrno; + } + + strncpy(oldpath, ptr, kMaxDataLength); + strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength); + + if (rename(oldpath, newpath) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a RemoveDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (rmdir(dir) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CreateDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CalcFileCRC32 command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) +{ + char file_buf[256]; + uint32_t checksum = 0; + ssize_t bytes_read; + strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength); + + int fd = ::open(file_buf, O_RDONLY); + if (fd < 0) { + return kErrFailErrno; + } + + do { + bytes_read = ::read(fd, file_buf, sizeof(file_buf)); + if (bytes_read < 0) { + int r_errno = errno; + ::close(fd); + errno = r_errno; + return kErrFailErrno; + } + + checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum); + } while (bytes_read == sizeof(file_buf)); + + ::close(fd); + + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = checksum; + return kErrNone; +} + +/// @brief Returns true if the specified session is a valid open session +bool +MavlinkFTP::_valid_session(unsigned index) +{ + if ((index >= kMaxSession) || (_session_fds[index] < 0)) { + return false; + } + return true; +} + +/// @brief Returns an unused session index +int +MavlinkFTP::_find_unused_session(void) +{ + for (size_t i=0; isize < kMaxDataLength) { + payload->data[payload->size] = '\0'; + } else { + payload->data[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *)&(payload->data[0]); +} + +/// @brief Returns a unused Request entry. NULL if none available. +MavlinkFTP::Request * +MavlinkFTP::_get_request(void) +{ + _lock_request_queue(); + Request* req = reinterpret_cast(dq_remfirst(&_request_queue)); + _unlock_request_queue(); + return req; +} + +/// @brief Locks a semaphore to provide exclusive access to the request queue +void +MavlinkFTP::_lock_request_queue(void) +{ + do {} + while (sem_wait(&_request_queue_sem) != 0); +} + +/// @brief Unlocks the semaphore providing exclusive access to the request queue +void +MavlinkFTP::_unlock_request_queue(void) +{ + sem_post(&_request_queue_sem); +} + +/// @brief Returns a no longer needed request to the queue +void +MavlinkFTP::_return_request(Request *req) +{ + _lock_request_queue(); + dq_addlast(&req->work.dq, &_request_queue); + _unlock_request_queue(); +} + +/// @brief Copy file (with limited space) +int +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) +{ + char buff[512]; + int src_fd = -1, dst_fd = -1; + int op_errno = 0; + + src_fd = ::open(src_path, O_RDONLY); + if (src_fd < 0) { + return -1; + } + + dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY, 0x0777); + if (dst_fd < 0) { + op_errno = errno; + ::close(src_fd); + errno = op_errno; + return -1; + } + + while (length > 0) { + ssize_t bytes_read, bytes_written; + size_t blen = (length > sizeof(buff))? sizeof(buff) : length; + + bytes_read = ::read(src_fd, buff, blen); + if (bytes_read == 0) { + // EOF + break; + } + else if (bytes_read < 0) { + warnx("cp: read"); + op_errno = errno; + break; + } + + bytes_written = ::write(dst_fd, buff, bytes_read); + if (bytes_written != bytes_read) { + warnx("cp: short write"); + op_errno = errno; + break; + } + + length -= bytes_written; + } + + ::close(src_fd); + ::close(dst_fd); + + errno = op_errno; + return (length > 0)? -1 : 0; +} diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c285bc4052..216abe954e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,17 +30,9 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - -/** - * @file mavlink_main.h - * MAVLink 1.0 protocol interface definition. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - #pragma once +<<<<<<< HEAD #include #include #include @@ -413,3 +405,10 @@ private: Mavlink(const Mavlink&); Mavlink operator=(const Mavlink&); }; +======= +#ifdef __PX4_NUTTX +#include "mavlink_main_nuttx.h" +#else +#include "mavlink_main_linux.h" +#endif +>>>>>>> Support for building more modules with Linux diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp new file mode 100644 index 0000000000..f2d8ea90f9 --- /dev/null +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -0,0 +1,1795 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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 + * 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. + * + ****************************************************************************/ + +/** + * @file mavlink_main.cpp + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include /* isinf / isnan checks */ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_main.h" +#include "mavlink_messages.h" +#include "mavlink_receiver.h" +#include "mavlink_rate_limiter.h" + +#ifndef MAVLINK_CRC_EXTRA +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define DEFAULT_DEVICE_NAME "/dev/ttyS1" +#define MAX_DATA_RATE 20000 ///< max data rate in bytes/s +#define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate +#define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. + +static Mavlink *_mavlink_instances = nullptr; + +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + +/** + * mavlink app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); + +extern mavlink_system_t mavlink_system; + +static void usage(void); + +Mavlink::Mavlink() : + CDev("mavlink-log", MAVLINK_LOG_DEVICE), + _device_name(DEFAULT_DEVICE_NAME), + _task_should_exit(false), + next(nullptr), + _instance_id(0), + _mavlink_fd(-1), + _task_running(false), + _hil_enabled(false), + _use_hil_gps(false), + _forward_externalsp(false), + _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), + _main_loop_delay(1000), + _subscriptions(nullptr), + _streams(nullptr), + _mission_manager(nullptr), + _parameters_manager(nullptr), + _mode(MAVLINK_MODE_NORMAL), + _channel(MAVLINK_COMM_0), + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(MAV_TYPE_FIXED_WING), + _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) +{ + _instance_id = Mavlink::instance_count(); + + /* set channel according to instance id */ + switch (_instance_id) { + case 0: + _channel = MAVLINK_COMM_0; + break; + + case 1: + _channel = MAVLINK_COMM_1; + break; + + case 2: + _channel = MAVLINK_COMM_2; + break; + + case 3: + _channel = MAVLINK_COMM_3; + break; +#ifdef MAVLINK_COMM_4 + + case 4: + _channel = MAVLINK_COMM_4; + break; +#endif +#ifdef MAVLINK_COMM_5 + + case 5: + _channel = MAVLINK_COMM_5; + break; +#endif +#ifdef MAVLINK_COMM_6 + + case 6: + _channel = MAVLINK_COMM_6; + break; +#endif + + default: + px4_errx(1, "instance ID is out of range"); + break; + } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; +} + +Mavlink::~Mavlink() +{ + perf_free(_loop_perf); + perf_free(_txerr_perf); + + if (_task_running) { + /* task wakes up every 10ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + //TODO store main task handle in Mavlink instance to allow killing task + //task_delete(_mavlink_task); + break; + } + } while (_task_running); + } + + LL_DELETE(_mavlink_instances, this); +} + +void +Mavlink::count_txerr() +{ + perf_count(_txerr_perf); +} + +void +Mavlink::set_mode(enum MAVLINK_MODE mode) +{ + _mode = mode; +} + +int +Mavlink::instance_count() +{ + unsigned inst_index = 0; + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + inst_index++; + } + + return inst_index; +} + +Mavlink * +Mavlink::get_instance(unsigned instance) +{ + Mavlink *inst; + unsigned inst_index = 0; + LL_FOREACH(::_mavlink_instances, inst) { + if (instance == inst_index) { + return inst; + } + + inst_index++; + } + + return nullptr; +} + +Mavlink * +Mavlink::get_instance_for_device(const char *device_name) +{ + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + if (strcmp(inst->_device_name, device_name) == 0) { + return inst; + } + } + + return nullptr; +} + +int +Mavlink::destroy_all_instances() +{ + /* start deleting from the end */ + Mavlink *inst_to_del = nullptr; + Mavlink *next_inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (next_inst != nullptr) { + inst_to_del = next_inst; + next_inst = inst_to_del->next; + + /* set flag to stop thread and wait for all threads to finish */ + inst_to_del->_task_should_exit = true; + + while (inst_to_del->_task_running) { + printf("."); + fflush(stdout); + usleep(10000); + iterations++; + + if (iterations > 1000) { + warnx("ERROR: Couldn't stop all mavlink instances."); + return ERROR; + } + } + } + + printf("\n"); + warnx("all instances stopped"); + return OK; +} + +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + while (inst != nullptr) { + + printf("\ninstance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + +bool +Mavlink::instance_exists(const char *device_name, Mavlink *self) +{ + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + + /* don't compare with itself */ + if (inst != self && !strcmp(device_name, inst->_device_name)) { + return true; + } + + inst = inst->next; + } + + return false; +} + +void +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) +{ + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (inst != self) { + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } + } + } +} + +int +Mavlink::get_uart_fd(unsigned index) +{ + Mavlink *inst = get_instance(index); + + if (inst) { + return inst->get_uart_fd(); + } + + return -1; +} + +int +Mavlink::get_uart_fd() +{ + return _uart_fd; +} + +int +Mavlink::get_instance_id() +{ + return _instance_id; +} + +mavlink_channel_t +Mavlink::get_channel() +{ + return _channel; +} + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +int +Mavlink::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + case (int)MAVLINK_IOC_SEND_TEXT_INFO: + case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: + case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { + + const char *txt = (const char *)arg; + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (!inst->_task_should_exit) { + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; + } + } + + return OK; + } + + default: + return ENOTTY; + } +} + +void Mavlink::mavlink_update_system(void) +{ + if (!_param_initialized) { + _param_system_id = param_find("MAV_SYS_ID"); + _param_component_id = param_find("MAV_COMP_ID"); + _param_system_type = param_find("MAV_TYPE"); + _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_forward_externalsp = param_find("MAV_FWDEXTSP"); + } + + /* update system and component id */ + int32_t system_id; + param_get(_param_system_id, &system_id); + + int32_t component_id; + param_get(_param_component_id, &component_id); + + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); + } + + int32_t system_type; + param_get(_param_system_type, &system_type); + + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + _system_type = system_type; + } + + int32_t use_hil_gps; + param_get(_param_use_hil_gps, &use_hil_gps); + + _use_hil_gps = (bool)use_hil_gps; + + int32_t forward_externalsp; + param_get(_param_forward_externalsp, &forward_externalsp); + + _forward_externalsp = (bool)forward_externalsp; +} + +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + +int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); + return -EINVAL; + } + + /* open uart */ + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + + if (_uart_fd < 0) { + return _uart_fd; + } + + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); + ::close(_uart_fd); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); + ::close(_uart_fd); + return -1; + } + + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR SET CONF %s\n", uart_name); + ::close(_uart_fd); + return -1; + } + + if (!_is_usb_uart) { + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ + (void)tcgetattr(_uart_fd, &uart_config); +#ifdef CRTS_IFLOW + uart_config.c_cflag |= CRTS_IFLOW; +#else + uart_config.c_cflag |= CRTSCTS; +#endif + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + + /* setup output flow control */ + if (enable_flow_control(true)) { + warnx("hardware flow control not supported"); + } + + } else { + _flow_control_enabled = false; + } + + return _uart_fd; +} + +int +Mavlink::enable_flow_control(bool enabled) +{ + // We can't do this on USB - skip + if (_is_usb_uart) { + return OK; + } + + struct termios uart_config; + + int ret = tcgetattr(_uart_fd, &uart_config); + + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + if (!ret) { + _flow_control_enabled = enabled; + } + + return ret; +} + +int +Mavlink::set_hil_enabled(bool hil_enabled) +{ + int ret = OK; + + /* enable HIL */ + if (hil_enabled && !_hil_enabled) { + _hil_enabled = true; + configure_stream("HIL_CONTROLS", 200.0f); + } + + /* disable HIL */ + if (!hil_enabled && _hil_enabled) { + _hil_enabled = false; + configure_stream("HIL_CONTROLS", 0.0f); + + } else { + ret = ERROR; + } + + return ret; +} + +unsigned +Mavlink::get_free_tx_buf() +{ + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + +// No FIONWRITE on Linux +#ifndef __PX4_LINUX + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#endif + + if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + enable_flow_control(false); + } + } + + return buf_free; +} + +void +Mavlink::send_message(const uint8_t msgid, const void *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + pthread_mutex_lock(&_send_mutex); + + unsigned buf_free = get_free_tx_buf(); + + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < packet_len) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + /* send message to UART */ + ssize_t ret = ::write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } + + pthread_mutex_unlock(&_send_mutex); +} + +void +Mavlink::resend_message(mavlink_message_t *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + pthread_mutex_lock(&_send_mutex); + + unsigned buf_free = get_free_tx_buf(); + + _last_write_try_time = hrt_absolute_time(); + + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < packet_len) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); + + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); + + /* send message to UART */ + ssize_t ret = ::write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } + + pthread_mutex_unlock(&_send_mutex); +} + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); + + /* handle packet with parameter component */ + _parameters_manager->handle_message(msg); + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } +} + +void +Mavlink::send_statustext_info(const char *string) +{ + send_statustext(MAV_SEVERITY_INFO, string); +} + +void +Mavlink::send_statustext_critical(const char *string) +{ + send_statustext(MAV_SEVERITY_CRITICAL, string); +} + +void +Mavlink::send_statustext_emergency(const char *string) +{ + send_statustext(MAV_SEVERITY_EMERGENCY, string); +} + +void +Mavlink::send_statustext(unsigned char severity, const char *string) +{ + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; + + mavlink_logbuffer_write(&_logbuffer, &logmsg); +} + +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) +{ + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; + + LL_FOREACH(_subscriptions, sub) { + if (sub->get_topic() == topic && sub->get_instance() == instance) { + /* already subscribed */ + return sub; + } + } + + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; +} + +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + +int +Mavlink::configure_stream(const char *stream_name, const float rate) +{ + /* calculate interval in us, 0 means disabled stream */ + unsigned int interval = interval_from_rate(rate); + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (strcmp(stream_name, stream->get_name()) == 0) { + if (interval > 0) { + /* set new interval */ + stream->set_interval(interval); + + } else { + /* delete stream */ + LL_DELETE(_streams, stream); + delete stream; + } + + return OK; + } + } + + if (interval == 0) { + /* stream was not active and is requested to be disabled, do nothing */ + return OK; + } + + /* search for stream with specified name in supported streams list */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create new instance */ + stream = streams_list[i]->new_instance(this); + stream->set_interval(interval); + LL_APPEND(_streams, stream); + + return OK; + } + } + + /* if we reach here, the stream list does not contain the stream */ + warnx("stream %s not found", stream_name); + + return ERROR; +} + +void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + +void +Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) +{ + /* orb subscription must be done from the main thread, + * set _subscribe_to_stream and _subscribe_to_stream_rate fields + * which polled in mavlink main loop */ + if (!_task_should_exit) { + /* wait for previous subscription completion */ + while (_subscribe_to_stream != nullptr) { + usleep(MAIN_LOOP_DELAY / 2); + } + + /* copy stream name */ + unsigned n = strlen(stream_name) + 1; + char *s = new char[n]; + strcpy(s, stream_name); + + /* set subscription task */ + _subscribe_to_stream_rate = rate; + _subscribe_to_stream = s; + + /* wait for subscription */ + do { + usleep(MAIN_LOOP_DELAY / 2); + } while (_subscribe_to_stream != nullptr); + + delete s; + } +} + +int +Mavlink::message_buffer_init(int size) +{ + + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char *)malloc(_message_buffer.size); + + int ret; + + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + + } else { + ret = OK; + } + + return ret; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(const void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(const mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + +float +Mavlink::get_rate_mult() +{ + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); +} + +int +Mavlink::task_main(int argc, char *argv[]) +{ + int ch; + _baudrate = 57600; + _datarate = 0; + _mode = MAVLINK_MODE_NORMAL; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { + switch (ch) { + case 'b': + _baudrate = strtoul(optarg, NULL, 10); + + if (_baudrate < 9600 || _baudrate > 921600) { + warnx("invalid baud rate '%s'", optarg); + err_flag = true; + } + + break; + + case 'r': + _datarate = strtoul(optarg, NULL, 10); + + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { + warnx("invalid data rate '%s'", optarg); + err_flag = true; + } + + break; + + case 'd': + _device_name = optarg; + break; + +// case 'e': +// mavlink_link_termination_allowed = true; +// break; + + case 'm': + if (strcmp(optarg, "custom") == 0) { + _mode = MAVLINK_MODE_CUSTOM; + + } else if (strcmp(optarg, "camera") == 0) { + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; + } + + break; + + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + + case 'v': + _verbose = true; + break; + + case 'w': + _wait_to_transmit = true; + break; + + case 'x': + _ftp_on = true; + break; + + default: + err_flag = true; + break; + } + } + + if (err_flag) { + usage(); + return ERROR; + } + + if (_datarate == 0) { + /* convert bits to bytes and use 1/2 of bandwidth by default */ + _datarate = _baudrate / 20; + } + + if (_datarate > MAX_DATA_RATE) { + _datarate = MAX_DATA_RATE; + } + + if (Mavlink::instance_exists(_device_name, this)) { + warnx("%s already running", _device_name); + return ERROR; + } + + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + + /* flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + struct termios uart_config_original; + + /* default values for arguments */ + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart); + + if (_uart_fd < 0) { + warn("could not open %s", _device_name); + return ERROR; + } + + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&_logbuffer, 5); + + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on || _ftp_on) { + /* initialize message buffer if multiplexing is on or its needed for FTP. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + px4_errx(1, "msg buf:"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + + /* create the device node that's used for sending text log messages, etc. */ + register_driver(MAVLINK_LOG_DEVICE, NULL); + + /* initialize logging device */ + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + _receive_thread = MavlinkReceiver::receive_start(this); + + _task_running = true; + + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + uint64_t param_time = 0; + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); + uint64_t status_time = 0; + + struct vehicle_status_s status; + status_sub->update(&status_time, &status); + + /* add default streams depending on mode */ + + /* HEARTBEAT is constant rate stream, rate never adjusted */ + configure_stream("HEARTBEAT", 1.0f); + + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + + switch (_mode) { + case MAVLINK_MODE_NORMAL: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); + configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW_RAD", 5.0f); + break; + + case MAVLINK_MODE_ONBOARD: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("VFR_HUD", 10.0f); + configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("TIMESYNC", 10.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); + break; + + default: + break; + } + + /* set main loop delay depending on data rate to minimize CPU overhead */ + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; + + /* now the instance is fully initialized and we can bump the instance count */ + LL_APPEND(_mavlink_instances, this); + + while (!_task_should_exit) { + /* main loop */ + usleep(_main_loop_delay); + + perf_begin(_loop_perf); + + hrt_abstime t = hrt_absolute_time(); + + update_rate_mult(); + + if (param_sub->update(¶m_time, nullptr)) { + /* parameters updated */ + mavlink_update_system(); + } + + if (status_sub->update(&status_time, &status)) { + /* switch HIL mode if required */ + set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); + } + + /* check for requested subscriptions */ + if (_subscribe_to_stream != nullptr) { + if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { + if (_subscribe_to_stream_rate > 0.0f) { + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); + + } else { + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + } + + } else { + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); + } + + _subscribe_to_stream = nullptr; + } + + /* update streams */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); + } + + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { + + bool is_part; + uint8_t *read_ptr; + uint8_t *write_ptr; + + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); + + /* write second part of buffer if there is some */ + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); + message_buffer_mark_read(available); + } + + pthread_mutex_unlock(&_message_buffer_mutex); + + resend_message(&msg); + } + } + + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + + _bytes_timestamp = t; + } + + perf_end(_loop_perf); + } + + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + + /* delete streams */ + MavlinkStream *stream_to_del = nullptr; + MavlinkStream *stream_next = _streams; + + while (stream_next != nullptr) { + stream_to_del = stream_next; + stream_next = stream_to_del->next; + delete stream_to_del; + } + + _streams = nullptr; + + /* delete subscriptions */ + MavlinkOrbSubscription *sub_to_del = nullptr; + MavlinkOrbSubscription *sub_next = _subscriptions; + + while (sub_next != nullptr) { + sub_to_del = sub_next; + sub_next = sub_to_del->next; + delete sub_to_del; + } + + _subscriptions = nullptr; + + /* wait for threads to complete */ + pthread_join(_receive_thread, NULL); + + /* reset the UART flags to original state */ + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); + + /* close UART */ + ::close(_uart_fd); + + /* close mavlink logging device */ + px4_close(_mavlink_fd); + + if (_passing_on || _ftp_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } + + /* destroy log buffer */ + mavlink_logbuffer_destroy(&_logbuffer); + + warnx("exiting"); + _task_running = false; + + return OK; +} + +int Mavlink::start_helper(int argc, char *argv[]) +{ + /* create the instance in task context */ + Mavlink *instance = new Mavlink(); + + int res; + + if (!instance) { + + /* out of memory */ + res = -ENOMEM; + warnx("OUT OF MEM"); + + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + } + + return res; +} + +int +Mavlink::start(int argc, char *argv[]) +{ + // Wait for the instance count to go up one + // before returning to the shell + int ic = Mavlink::instance_count(); + + // Instantiate thread + char buf[24]; + sprintf(buf, "mavlink_if%d", ic); + + // This is where the control flow splits + // between the starting task and the spawned + // task - start_helper() only returns + // when the started task exits. + px4_task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2400, + (px4_main_t)&Mavlink::start_helper, + (char * const *)argv); + + // Ensure that this shell command + // does not return before the instance + // is fully initialized. As this is also + // the only path to create a new instance, + // this is effectively a lock on concurrent + // instance starting. XXX do a real lock. + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + + while (ic == Mavlink::instance_count() && count < limit) { + ::usleep(sleeptime); + count++; + } + + return OK; +} + +void +Mavlink::display_status() +{ + + if (_rstatus.heartbeat_time > 0) { + printf("\tGCS heartbeat:\t%lu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } + + printf("\tmavlink chan: #%u\n", _channel); + + if (_rstatus.timestamp > 0) { + + printf("\ttype:\t\t"); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("3DR RADIO\n"); + break; + + default: + printf("UNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + + } else { + printf("\tno telem status.\n"); + } + + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); + printf("\trate mult: %.3f\n", (double)_rate_mult); +} + +int +Mavlink::stream_command(int argc, char *argv[]) +{ + const char *device_name = DEFAULT_DEVICE_NAME; + float rate = -1.0f; + const char *stream_name = nullptr; + + argc -= 2; + argv += 2; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + int i = 0; + + while (i < argc) { + + if (0 == strcmp(argv[i], "-r") && i < argc - 1) { + rate = strtod(argv[i + 1], nullptr); + + if (rate < 0.0f) { + err_flag = true; + } + + i++; + + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + device_name = argv[i + 1]; + i++; + + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { + stream_name = argv[i + 1]; + i++; + + } else { + err_flag = true; + } + + i++; + } + + if (!err_flag && rate >= 0.0f && stream_name != nullptr) { + Mavlink *inst = get_instance_for_device(device_name); + + if (inst != nullptr) { + inst->configure_stream_threadsafe(stream_name, rate); + + } else { + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + px4_errx(0, "mavlink for device %s is not running", device_name); + } + + } else { + px4_errx(1, "usage: mavlink stream [-d device] -s stream -r rate"); + } + + return OK; +} + +static void usage() +{ + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); +} + +int mavlink_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + return Mavlink::start(argc, argv); + + } else if (!strcmp(argv[1], "stop")) { + warnx("mavlink stop is deprecated, use stop-all instead"); + usage(); + return 1; + + } else if (!strcmp(argv[1], "stop-all")) { + return Mavlink::destroy_all_instances(); + + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); + + } else if (!strcmp(argv[1], "stream")) { + return Mavlink::stream_command(argc, argv); + + } else { + usage(); + return 1; + } + + return 0; +} diff --git a/src/modules/mavlink/mavlink_main_linux.h b/src/modules/mavlink/mavlink_main_linux.h new file mode 100644 index 0000000000..a785c48f3c --- /dev/null +++ b/src/modules/mavlink/mavlink_main_linux.h @@ -0,0 +1,417 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file mavlink_main_linux.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Mark Charlebois + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" +#include "mavlink_messages.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" + +class Mavlink : public device::CDev +{ + +public: + /** + * Constructor + */ + Mavlink(); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + static int start(int argc, char *argv[]); + + /** + * Display the mavlink status. + */ + void display_status(); + + static int stream_command(int argc, char *argv[]); + + static int instance_count(); + + static Mavlink *new_instance(); + + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); + + static int destroy_all_instances(); + + static int get_status_all_instances(); + + static bool instance_exists(const char *device_name, Mavlink *self); + + static void forward_message(const mavlink_message_t *msg, Mavlink *self); + + static int get_uart_fd(unsigned index); + + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); + + const char *_device_name; + + enum MAVLINK_MODE { + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_ONBOARD + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } + + bool get_hil_enabled() { return _hil_enabled; } + + bool get_use_hil_gps() { return _use_hil_gps; } + + bool get_forward_externalsp() { return _forward_externalsp; } + + bool get_flow_control_enabled() { return _flow_control_enabled; } + + bool get_forwarding_on() { return _forwarding_on; } + + static int start_helper(int argc, char *argv[]); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_enabled(bool hil_enabled); + + void send_message(const uint8_t msgid, const void *msg); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); + + int get_instance_id(); + + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + + mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, float rate); + + bool _task_should_exit; /**< if true, mavlink task should exit */ + + int get_mavlink_fd() { return _mavlink_fd; } + + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level + */ + void send_statustext(unsigned char severity, const char *string); + + MavlinkStream * get_streams() const { return _streams; } + + float get_rate_mult(); + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(const void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); + + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + + unsigned get_system_type() { return _system_type; } + +protected: + Mavlink *next; + +private: + int _instance_id; + + int _mavlink_fd; + bool _task_running; + + /* states */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; + + MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; + + MAVLINK_MODE _mode; + + mavlink_channel_t _channel; + + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; + + pthread_t _receive_thread; + + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; + + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int _mavlink_param_queue_index; + + bool mavlink_link_termination_allowed; + + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; + + bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; + + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + + struct telemetry_status_s _rstatus; ///< receive status + + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; + + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; + param_t _param_forward_externalsp; + + unsigned _system_type; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + + void mavlink_update_system(); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); + + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(const mavlink_message_t *msg); + + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + + /** + * Main mavlink task. + */ + int task_main(int argc, char *argv[]); + + /* do not allow copying this class */ + Mavlink(const Mavlink&); + Mavlink operator=(const Mavlink&); +}; diff --git a/src/modules/mavlink/mavlink_main_nuttx.h b/src/modules/mavlink/mavlink_main_nuttx.h new file mode 100644 index 0000000000..f7f0ad4b26 --- /dev/null +++ b/src/modules/mavlink/mavlink_main_nuttx.h @@ -0,0 +1,415 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file mavlink_main_nuttx.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" +#include "mavlink_messages.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" + +class Mavlink +{ + +public: + /** + * Constructor + */ + Mavlink(); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + static int start(int argc, char *argv[]); + + /** + * Display the mavlink status. + */ + void display_status(); + + static int stream_command(int argc, char *argv[]); + + static int instance_count(); + + static Mavlink *new_instance(); + + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); + + static int destroy_all_instances(); + + static int get_status_all_instances(); + + static bool instance_exists(const char *device_name, Mavlink *self); + + static void forward_message(const mavlink_message_t *msg, Mavlink *self); + + static int get_uart_fd(unsigned index); + + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); + + const char *_device_name; + + enum MAVLINK_MODE { + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_ONBOARD + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } + + bool get_hil_enabled() { return _hil_enabled; } + + bool get_use_hil_gps() { return _use_hil_gps; } + + bool get_forward_externalsp() { return _forward_externalsp; } + + bool get_flow_control_enabled() { return _flow_control_enabled; } + + bool get_forwarding_on() { return _forwarding_on; } + + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + + static int start_helper(int argc, char *argv[]); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_enabled(bool hil_enabled); + + void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); + + int get_instance_id(); + + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + + mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, float rate); + + bool _task_should_exit; /**< if true, mavlink task should exit */ + + int get_mavlink_fd() { return _mavlink_fd; } + + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level + */ + void send_statustext(unsigned char severity, const char *string); + + MavlinkStream * get_streams() const { return _streams; } + + float get_rate_mult(); + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(const void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); + + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + + unsigned get_system_type() { return _system_type; } + +protected: + Mavlink *next; + +private: + int _instance_id; + + int _mavlink_fd; + bool _task_running; + + /* states */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; + + MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; + + MAVLINK_MODE _mode; + + mavlink_channel_t _channel; + + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; + + pthread_t _receive_thread; + + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; + + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int _mavlink_param_queue_index; + + bool mavlink_link_termination_allowed; + + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; + + bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; + + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + + struct telemetry_status_s _rstatus; ///< receive status + + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; + + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; + param_t _param_forward_externalsp; + + unsigned _system_type; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + + void mavlink_update_system(); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); + + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(const mavlink_message_t *msg); + + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + + /** + * Main mavlink task. + */ + int task_main(int argc, char *argv[]); + + /* do not allow copying this class */ + Mavlink(const Mavlink&); + Mavlink operator=(const Mavlink&); +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 81c76ef3f0..bfe7a4552c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -59,7 +59,6 @@ #include #include #include -#include #include #include #include @@ -992,7 +991,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; + bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS; bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; if (!onb_unix_valid && ofb_unix_valid) { @@ -1496,17 +1495,17 @@ MavlinkReceiver::receive_thread(void *arg) sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; ssize_t nread = 0; while (!_mavlink->_task_should_exit) { - if (poll(fds, 1, timeout) > 0) { + if (px4_poll(fds, 1, timeout) > 0) { /* non-blocking read. read may return negative values */ - if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { + if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index e82b8bd935..10f283e6c7 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -36,16 +36,22 @@ # MODULE_COMMAND = mavlink -SRCS += mavlink_main.cpp \ - mavlink.c \ +ifeq ($(PX$_TARGET_OS),nuttx) +SRCS += mavlink_main_nuttx.cpp \ + mavlink_ftp_nuttx.cpp +else +SRCS += mavlink_main_linux.cpp \ + mavlink_ftp_linux.cpp +endif + +SRCS += mavlink.c \ mavlink_receiver.cpp \ mavlink_mission.cpp \ mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp \ - mavlink_ftp.cpp + mavlink_rate_limiter.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index f37bc93277..d49f6fdfbf 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -38,8 +38,12 @@ MODULE_COMMAND = sensors MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" -SRCS = sensors.cpp \ - sensor_params.c +ifeq ($(PX$_TARGET_OS),nuttx) +SRCS = sensors_nuttx.cpp +else +SRCS = sensors_linux.cpp +endif +SRCS += sensor_params.c MODULE_STACKSIZE = 1200 diff --git a/src/modules/sensors/sensors_linux.cpp b/src/modules/sensors/sensors_linux.cpp new file mode 100644 index 0000000000..dea1367ef9 --- /dev/null +++ b/src/modules/sensors/sensors_linux.cpp @@ -0,0 +1,2308 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file sensors.cpp + * + * PX4 Flight Core transitional mapping layer. + * + * This app / class mapps the PX4 middleware layer / drivers to the application + * layer of the PX4 Flight Core. Individual sensors can be accessed directly as + * well instead of relying on the sensor_combined topic. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler + * @author Anton Babushkin + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Analog layout: + * FMU: + * IN2 - battery voltage + * IN3 - battery current + * IN4 - 5V sense + * IN10 - spare (we could actually trim these from the set) + * IN11 - spare (we could actually trim these from the set) + * IN12 - spare (we could actually trim these from the set) + * IN13 - aux1 + * IN14 - aux2 + * IN15 - pressure sensor + * + * IO: + * IN4 - servo supply rail + * IN5 - analog RSSI + */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#endif + +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 +#endif + +#define BATT_V_LOWPASS 0.001f +#define BATT_V_IGNORE_THRESHOLD 4.8f + +/** + * HACK - true temperature is much less than indicated temperature in baro, + * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + */ +#define PCB_TEMP_ESTIMATE_DEG 5.0f +#define STICK_ON_OFF_LIMIT 0.75f +#define MAG_ROT_VAL_INTERNAL -1 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL" + +/** + * Sensor app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int sensors_main(int argc, char *argv[]); + +class Sensors +{ +public: + /** + * Constructor + */ + Sensors(); + + /** + * Destructor, also kills the sensors task. + */ + ~Sensors(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(uint8_t func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); + + /** + * Update paramters from RC channels if the functionality is activated and the + * input has changed since the last update + * + * @param + */ + void set_params_from_rc(); + + /** + * Gather and publish RC input data. + */ + void rc_poll(); + + /* XXX should not be here - should be own driver */ + int _fd_adc; /**< ADC driver handle */ + hrt_abstime _last_adc; /**< last time we took input from the ADC */ + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _sensors_task; /**< task handle for sensor task */ + + bool _hil_enabled; /**< if true, HIL is active */ + bool _publishing; /**< if true, we are publishing sensor data */ + + int _gyro_sub; /**< raw gyro0 data subscription */ + int _accel_sub; /**< raw accel0 data subscription */ + int _mag_sub; /**< raw mag0 data subscription */ + int _gyro1_sub; /**< raw gyro1 data subscription */ + int _accel1_sub; /**< raw accel1 data subscription */ + int _mag1_sub; /**< raw mag1 data subscription */ + int _gyro2_sub; /**< raw gyro2 data subscription */ + int _accel2_sub; /**< raw accel2 data subscription */ + int _mag2_sub; /**< raw mag2 data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ + int _baro_sub; /**< raw baro data subscription */ + int _baro1_sub; /**< raw baro data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ + int _manual_control_sub; /**< notification of manual control updates */ + + orb_advert_t _sensor_pub; /**< combined sensor data topic */ + orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ + orb_advert_t _rc_pub; /**< raw r/c control topic */ + orb_advert_t _battery_pub; /**< battery status */ + orb_advert_t _airspeed_pub; /**< airspeed */ + orb_advert_t _diff_pres_pub; /**< differential_pressure */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + struct rc_channels_s _rc; /**< r/c channel data */ + struct battery_status_s _battery_status; /**< battery status */ + struct baro_report _barometer; /**< barometer data */ + struct differential_pressure_s _diff_pres; + struct airspeed_s _airspeed; + struct rc_parameter_map_s _rc_parameter_map; + + math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ + + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ + hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ + + struct { + float min[_rc_max_chan_count]; + float trim[_rc_max_chan_count]; + float max[_rc_max_chan_count]; + float rev[_rc_max_chan_count]; + float dz[_rc_max_chan_count]; + float scaling_factor[_rc_max_chan_count]; + + float diff_pres_offset_pa; + float diff_pres_analog_scale; + + int board_rotation; + int flow_rotation; + + float board_offset[3]; + + int rc_map_roll; + int rc_map_pitch; + int rc_map_yaw; + int rc_map_throttle; + int rc_map_failsafe; + + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_posctl_sw; + int rc_map_loiter_sw; + int rc_map_acro_sw; + int rc_map_offboard_sw; + + int rc_map_flaps; + + int rc_map_aux1; + int rc_map_aux2; + int rc_map_aux3; + int rc_map_aux4; + int rc_map_aux5; + + int rc_map_param[RC_PARAM_MAP_NCHAN]; + + int32_t rc_fails_thr; + float rc_assist_th; + float rc_auto_th; + float rc_posctl_th; + float rc_return_th; + float rc_loiter_th; + float rc_acro_th; + float rc_offboard_th; + bool rc_assist_inv; + bool rc_auto_inv; + bool rc_posctl_inv; + bool rc_return_inv; + bool rc_loiter_inv; + bool rc_acro_inv; + bool rc_offboard_inv; + + float battery_voltage_scaling; + float battery_current_scaling; + + float baro_qnh; + + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t min[_rc_max_chan_count]; + param_t trim[_rc_max_chan_count]; + param_t max[_rc_max_chan_count]; + param_t rev[_rc_max_chan_count]; + param_t dz[_rc_max_chan_count]; + + param_t diff_pres_offset_pa; + param_t diff_pres_analog_scale; + + param_t rc_map_roll; + param_t rc_map_pitch; + param_t rc_map_yaw; + param_t rc_map_throttle; + param_t rc_map_failsafe; + + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_posctl_sw; + param_t rc_map_loiter_sw; + param_t rc_map_acro_sw; + param_t rc_map_offboard_sw; + + param_t rc_map_flaps; + + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + param_t rc_map_aux4; + param_t rc_map_aux5; + + param_t rc_map_param[RC_PARAM_MAP_NCHAN]; + param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + + param_t rc_fails_thr; + param_t rc_assist_th; + param_t rc_auto_th; + param_t rc_posctl_th; + param_t rc_return_th; + param_t rc_loiter_th; + param_t rc_acro_th; + param_t rc_offboard_th; + + param_t battery_voltage_scaling; + param_t battery_current_scaling; + + param_t board_rotation; + param_t flow_rotation; + + param_t board_offset[3]; + + param_t baro_qnh; + + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Do accel-related initialisation. + */ + int accel_init(); + + /** + * Do gyro-related initialisation. + */ + int gyro_init(); + + /** + * Do mag-related initialisation. + */ + int mag_init(); + + /** + * Do baro-related initialisation. + */ + int baro_init(); + + /** + * Do adc-related initialisation. + */ + int adc_init(); + + /** + * Poll the accelerometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void accel_poll(struct sensor_combined_s &raw); + + /** + * Poll the gyro for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void gyro_poll(struct sensor_combined_s &raw); + + /** + * Poll the magnetometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void mag_poll(struct sensor_combined_s &raw); + + /** + * Poll the barometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void baro_poll(struct sensor_combined_s &raw); + + /** + * Poll the differential pressure sensor for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void diff_pres_poll(struct sensor_combined_s &raw); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in parameters. + */ + void parameter_update_poll(bool forced = false); + + /** + * Check for changes in rc_parameter_map + */ + void rc_parameter_map_poll(bool forced = false); + + /** + * Poll the ADC and update readings to suit. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void adc_poll(struct sensor_combined_s &raw); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main(); +}; + +namespace sensors +{ + +Sensors *g_sensors = nullptr; +} + +Sensors::Sensors() : + _fd_adc(-1), + _last_adc(0), + + _task_should_exit(true), + _sensors_task(-1), + _hil_enabled(false), + _publishing(true), + + /* subscriptions */ + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), + _gyro1_sub(-1), + _accel1_sub(-1), + _mag1_sub(-1), + _gyro2_sub(-1), + _accel2_sub(-1), + _mag2_sub(-1), + _rc_sub(-1), + _baro_sub(-1), + _baro1_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _rc_parameter_map_sub(-1), + _manual_control_sub(-1), + + /* publications */ + _sensor_pub(-1), + _manual_control_pub(-1), + _actuator_group_3_pub(-1), + _rc_pub(-1), + _battery_pub(-1), + _airspeed_pub(-1), + _diff_pres_pub(-1), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation{}, + _mag_rotation{}, + + _battery_discharged(0), + _battery_current_timestamp(0) +{ + memset(&_rc, 0, sizeof(_rc)); + memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); + + /* basic r/c parameters */ + for (unsigned i = 0; i < _rc_max_chan_count; i++) { + char nbuf[16]; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles.min[i] = param_find(nbuf); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles.trim[i] = param_find(nbuf); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles.max[i] = param_find(nbuf); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles.rev[i] = param_find(nbuf); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles.dz[i] = param_find(nbuf); + + } + + /* mandatory input switched, mapped to channels 1-4 per default */ + _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); + _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); + _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); + _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); + + _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); + _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); + + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); + _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); + + /* RC to parameter mapping for changing parameters with RC */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + char name[PARAM_ID_LEN]; + snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + _parameter_handles.rc_map_param[i] = param_find(name); + } + + /* RC thresholds */ + _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); + _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); + _parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); + _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); + _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); + _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); + _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); + _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); + + /* Differential pressure offset */ + _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); + + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); + + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + + /* Barometer QNH */ + _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + + /* fetch initial parameter values */ + parameters_update(); +} + +Sensors::~Sensors() +{ + if (_sensors_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + px4_task_delete(_sensors_task); + break; + } + } while (_sensors_task != -1); + } + + sensors::g_sensors = nullptr; +} + +int +Sensors::parameters_update() +{ + bool rc_valid = true; + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + + /* rc values */ + for (unsigned int i = 0; i < _rc_max_chan_count; i++) { + + param_get(_parameter_handles.min[i], &(_parameters.min[i])); + param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); + param_get(_parameter_handles.max[i], &(_parameters.max[i])); + param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); + param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); + + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + + /* handle blowup in the scaling factor calculation */ +#ifdef __PX4_NUTTX + if (!isfinite(tmpScaleFactor) || +#else + if (!std::isfinite(tmpScaleFactor) || +#endif + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f)) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); + /* scaling factors do not make sense, lock them down */ + _parameters.scaling_factor[i] = 0.0f; + rc_valid = false; + + } else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } + } + + /* handle wrong values */ + if (!rc_valid) { + warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + } + + const char *paramerr = "FAIL PARM LOAD"; + + /* channel mapping */ + if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("%s", paramerr); + } + + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); + } + + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); + param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); + _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); + _parameters.rc_assist_th = fabs(_parameters.rc_assist_th); + param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); + _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); + _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); + param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); + _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); + _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); + param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th)); + _parameters.rc_return_inv = (_parameters.rc_return_th < 0); + _parameters.rc_return_th = fabs(_parameters.rc_return_th); + param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); + _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); + _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); + param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th)); + _parameters.rc_acro_inv = (_parameters.rc_acro_th < 0); + _parameters.rc_acro_th = fabs(_parameters.rc_acro_th); + param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th)); + _parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0); + _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); + + /* update RC function mappings */ + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + } + + /* Airspeed offset */ + param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); + + /* scaling of ADC ticks to battery voltage */ + if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { + warnx("%s", paramerr); + } + + /* scaling of ADC ticks to battery current */ + if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { + warnx("%s", paramerr); + } + + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); + + /* set px4flow rotation */ + int flowfd; + flowfd = px4_open(PX4FLOW0_DEVICE_PATH, 0); + + if (flowfd >= 0) { + int flowret = px4_ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + + if (flowret) { + warnx("flow rotation could not be set"); + px4_close(flowfd); + return ERROR; + } + + px4_close(flowfd); + } + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + + _board_rotation = _board_rotation * board_rotation_offset; + + /* update barometer qnh setting */ + param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); + int barofd; + barofd = px4_open(BARO0_DEVICE_PATH, 0); + + if (barofd < 0) { + warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); + return ERROR; + + } else { + int baroret = px4_ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + + if (baroret) { + warnx("qnh could not be set"); + px4_close(barofd); + return ERROR; + } + + px4_close(barofd); + } + + return OK; +} + +int +Sensors::accel_init() +{ + int fd; + + fd = px4_open(ACCEL0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); + return ERROR; + + } else { + + /* set the accel internal sampling rate to default rate */ + px4_ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); + + /* set the driver to poll at default rate */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + + px4_close(fd); + } + + return OK; +} + +int +Sensors::gyro_init() +{ + int fd; + + fd = px4_open(GYRO0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); + return ERROR; + + } else { + + /* set the gyro internal sampling rate to default rate */ + px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); + + /* set the driver to poll at default rate */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + + } + + return OK; +} + +int +Sensors::mag_init() +{ + int fd; + int ret; + + fd = px4_open(MAG0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); + return ERROR; + } + + /* try different mag sampling rates */ + + + ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 150); + + if (ret == OK) { + /* set the pollrate accordingly */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); + + } else { + ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 100); + + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, 100); + + } else { + warnx("FATAL: mag sampling rate could not be set"); + return ERROR; + } + } + + px4_close(fd); + + return OK; +} + +int +Sensors::baro_init() +{ + int fd; + + fd = px4_open(BARO0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); + return ERROR; + } + + /* set the driver to poll at 150Hz */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); + + px4_close(fd); + + return OK; +} + +int +Sensors::adc_init() +{ + + _fd_adc = px4_open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + + if (_fd_adc < 0) { + warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); + return ERROR; + } + + return OK; +} + +void +Sensors::accel_poll(struct sensor_combined_s &raw) +{ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); + + raw.accelerometer_raw[0] = accel_report.x_raw; + raw.accelerometer_raw[1] = accel_report.y_raw; + raw.accelerometer_raw[2] = accel_report.z_raw; + + raw.accelerometer_timestamp = accel_report.timestamp; + raw.accelerometer_errcount = accel_report.error_count; + } + + orb_check(_accel1_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer1_m_s2[0] = vect(0); + raw.accelerometer1_m_s2[1] = vect(1); + raw.accelerometer1_m_s2[2] = vect(2); + + raw.accelerometer1_raw[0] = accel_report.x_raw; + raw.accelerometer1_raw[1] = accel_report.y_raw; + raw.accelerometer1_raw[2] = accel_report.z_raw; + + raw.accelerometer1_timestamp = accel_report.timestamp; + raw.accelerometer1_errcount = accel_report.error_count; + } + + orb_check(_accel2_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer2_m_s2[0] = vect(0); + raw.accelerometer2_m_s2[1] = vect(1); + raw.accelerometer2_m_s2[2] = vect(2); + + raw.accelerometer2_raw[0] = accel_report.x_raw; + raw.accelerometer2_raw[1] = accel_report.y_raw; + raw.accelerometer2_raw[2] = accel_report.z_raw; + + raw.accelerometer2_timestamp = accel_report.timestamp; + raw.accelerometer2_errcount = accel_report.error_count; + } +} + +void +Sensors::gyro_poll(struct sensor_combined_s &raw) +{ + bool gyro_updated; + orb_check(_gyro_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); + + raw.gyro_raw[0] = gyro_report.x_raw; + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + + raw.timestamp = gyro_report.timestamp; + raw.gyro_errcount = gyro_report.error_count; + } + + orb_check(_gyro1_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro1_rad_s[0] = vect(0); + raw.gyro1_rad_s[1] = vect(1); + raw.gyro1_rad_s[2] = vect(2); + + raw.gyro1_raw[0] = gyro_report.x_raw; + raw.gyro1_raw[1] = gyro_report.y_raw; + raw.gyro1_raw[2] = gyro_report.z_raw; + + raw.gyro1_timestamp = gyro_report.timestamp; + raw.gyro1_errcount = gyro_report.error_count; + } + + orb_check(_gyro2_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro2_rad_s[0] = vect(0); + raw.gyro2_rad_s[1] = vect(1); + raw.gyro2_rad_s[2] = vect(2); + + raw.gyro2_raw[0] = gyro_report.x_raw; + raw.gyro2_raw[1] = gyro_report.y_raw; + raw.gyro2_raw[2] = gyro_report.z_raw; + + raw.gyro2_timestamp = gyro_report.timestamp; + raw.gyro2_errcount = gyro_report.error_count; + } +} + +void +Sensors::mag_poll(struct sensor_combined_s &raw) +{ + bool mag_updated; + orb_check(_mag_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + vect = _mag_rotation[0] * vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); + + raw.magnetometer_raw[0] = mag_report.x_raw; + raw.magnetometer_raw[1] = mag_report.y_raw; + raw.magnetometer_raw[2] = mag_report.z_raw; + + raw.magnetometer_timestamp = mag_report.timestamp; + raw.magnetometer_errcount = mag_report.error_count; + } + + orb_check(_mag1_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); + + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + vect = _mag_rotation[1] * vect; + + raw.magnetometer1_ga[0] = vect(0); + raw.magnetometer1_ga[1] = vect(1); + raw.magnetometer1_ga[2] = vect(2); + + raw.magnetometer1_raw[0] = mag_report.x_raw; + raw.magnetometer1_raw[1] = mag_report.y_raw; + raw.magnetometer1_raw[2] = mag_report.z_raw; + + raw.magnetometer1_timestamp = mag_report.timestamp; + raw.magnetometer1_errcount = mag_report.error_count; + } + + orb_check(_mag2_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); + + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + vect = _mag_rotation[2] * vect; + + raw.magnetometer2_ga[0] = vect(0); + raw.magnetometer2_ga[1] = vect(1); + raw.magnetometer2_ga[2] = vect(2); + + raw.magnetometer2_raw[0] = mag_report.x_raw; + raw.magnetometer2_raw[1] = mag_report.y_raw; + raw.magnetometer2_raw[2] = mag_report.z_raw; + + raw.magnetometer2_timestamp = mag_report.timestamp; + raw.magnetometer2_errcount = mag_report.error_count; + } +} + +void +Sensors::baro_poll(struct sensor_combined_s &raw) +{ + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + + if (baro_updated) { + + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + + raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius + + raw.baro_timestamp = _barometer.timestamp; + } + + orb_check(_baro1_sub, &baro_updated); + + if (baro_updated) { + + struct baro_report baro_report; + + orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); + + raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + + raw.baro1_timestamp = baro_report.timestamp; + } +} + +void +Sensors::diff_pres_poll(struct sensor_combined_s &raw) +{ + bool updated; + orb_check(_diff_pres_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); + + raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; + raw.differential_pressure_timestamp = _diff_pres.timestamp; + raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + + _airspeed.timestamp = _diff_pres.timestamp; + + /* don't risk to feed negative airspeed into the system */ + _airspeed.indicated_airspeed_m_s = math::max(0.0f, + calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.air_temperature_celsius = air_temperature_celsius; + + /* announce the airspeed if needed, just publish else */ + if (_airspeed_pub > 0) { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); + + } else { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); + } + } +} + +void +Sensors::vehicle_control_mode_poll() +{ + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; + + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); + + if (vcontrol_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); + + /* switching from non-HIL to HIL mode */ + //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { + _hil_enabled = true; + _publishing = false; + + /* switching from HIL to non-HIL mode */ + + } else if (!_publishing && !_hil_enabled) { + _hil_enabled = false; + _publishing = true; + } + } +} + +void +Sensors::parameter_update_poll(bool forced) +{ + bool param_updated; + + /* Check if any parameter has changed */ + orb_check(_params_sub, ¶m_updated); + + if (param_updated || forced) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters */ + parameters_update(); + + /* set offset parameters to new values */ + bool failed; + int res; + char str[30]; + unsigned mag_count = 0; + unsigned gyro_count = 0; + unsigned accel_count = 0; + + /* run through all gyro sensors */ + for (unsigned s = 0; s < 3; s++) { + + res = ERROR; + (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + + int fd = open(str, 0); + + if (fd < 0) { + continue; + } + + bool config_ok = false; + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + /* initially status is ok per config */ + failed = false; + + (void)sprintf(str, "CAL_GYRO%u_ID", i); + int device_id; + failed = failed || (OK != param_get(param_find(str), &device_id)); + + if (failed) { + px4_close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct gyro_scale gscale = {}; + (void)sprintf(str, "CAL_GYRO%u_XOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_GYRO%u_YOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + + if (failed) { + warnx("%s: gyro #%u", CAL_FAILED_APPLY_CAL_MSG, gyro_count); + } else { + /* apply new scaling and offsets */ + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + if (res) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + gyro_count++; + config_ok = true; + } + } + break; + } + } + + px4_close(fd); + + if (!config_ok) { + warnx("NO CONFIG FOR GYRO #%u", s); + } + } + + /* run through all accel sensors */ + for (unsigned s = 0; s < 3; s++) { + + res = ERROR; + (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); + + int fd = open(str, 0); + + if (fd < 0) { + continue; + } + + bool config_ok = false; + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + /* initially status is ok per config */ + failed = false; + + (void)sprintf(str, "CAL_ACC%u_ID", i); + int device_id; + failed = failed || (OK != param_get(param_find(str), &device_id)); + + if (failed) { + px4_close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct accel_scale gscale = {}; + (void)sprintf(str, "CAL_ACC%u_XOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_ACC%u_YOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_ACC%u_ZOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_ACC%u_YSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + + if (failed) { + warnx("%s: acc #%u", CAL_FAILED_APPLY_CAL_MSG, accel_count); + } else { + /* apply new scaling and offsets */ + res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + if (res) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + accel_count++; + config_ok = true; + } + } + break; + } + } + + px4_close(fd); + + if (!config_ok) { + warnx("NO CONFIG FOR ACCEL #%u", s); + } + } + + /* run through all mag sensors */ + for (unsigned s = 0; s < 3; s++) { + + res = ERROR; + (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); + + int fd = px4_open(str, 0); + + if (fd < 0) { + continue; + } + + bool config_ok = false; + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + /* initially status is ok per config */ + failed = false; + + (void)sprintf(str, "CAL_MAG%u_ID", i); + int device_id; + failed = failed || (OK != param_get(param_find(str), &device_id)); + + if (failed) { + px4_close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct mag_scale gscale = {}; + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + + (void)sprintf(str, "CAL_MAG%u_ROT", i); + + if (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { + /* mag is internal */ + _mag_rotation[s] = _board_rotation; + /* reset param to -1 to indicate internal mag */ + int32_t minus_one = MAG_ROT_VAL_INTERNAL; + param_set_no_notification(param_find(str), &minus_one); + } else { + + int32_t mag_rot; + param_get(param_find(str), &mag_rot); + + /* check if this mag is still set as internal */ + if (mag_rot < 0) { + /* it was marked as internal, change to external with no rotation */ + mag_rot = 0; + param_set_no_notification(param_find(str), &mag_rot); + } + + /* handling of old setups, will be removed later (noted Feb 2015) */ + int32_t deprecated_mag_rot = 0; + param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); + + /* + * If the deprecated parameter is non-default (is != 0), + * and the new parameter is default (is == 0), then this board + * was configured already and we need to copy the old value + * to the new parameter. + * The < 0 case is special: It means that this param slot was + * used previously by an internal sensor, but the the call above + * proved that it is currently occupied by an external sensor. + * In that case we consider the orientation to be default as well. + */ + if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) { + mag_rot = deprecated_mag_rot; + param_set_no_notification(param_find(str), &mag_rot); + /* clear the old param, not supported in GUI anyway */ + deprecated_mag_rot = 0; + param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); + } + + /* handling of transition from internal to external */ + if (mag_rot < 0) { + mag_rot = 0; + } + + get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[s]); + } + + if (failed) { + warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count); + } else { + /* apply new scaling and offsets */ + res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + if (res) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + mag_count++; + config_ok = true; + } + } + break; + } + } + + px4_close(fd); + + if (!config_ok) { + warnx("NO CONFIG FOR MAG #%u", s); + } + } + + int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); + + /* this sensor is optional, abort without error */ + + if (fd >= 0) { + struct airspeed_scale airscale = { + _parameters.diff_pres_offset_pa, + 1.0f, + }; + + if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + + px4_close(fd); + } + + warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); + } +} + +void +Sensors::rc_parameter_map_poll(bool forced) +{ + bool map_updated; + orb_check(_rc_parameter_map_sub, &map_updated); + + if (map_updated) { + orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + + /* update paramter handles to which the RC channels are mapped */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + /* Set the handle by index if the index is set, otherwise use the id */ + if (_rc_parameter_map.param_index[i] >= 0) { + _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + + } else { + _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + } + + } + + warnx("rc to parameter map updated"); + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); + } + } +} + +void +Sensors::adc_poll(struct sensor_combined_s &raw) +{ + /* only read if publishing */ + if (!_publishing) { + return; + } + + hrt_abstime t = hrt_absolute_time(); + + /* rate limit to 100 Hz */ + if (t - _last_adc >= 10000) { + /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[12]; + /* read all channels available */ + int ret = px4_read(_fd_adc, &buf_adc, sizeof(buf_adc)); + + if (ret >= (int)sizeof(buf_adc[0])) { + + /* Read add channels we got */ + for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { + /* Save raw voltage values */ + if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_mapping[i] = buf_adc[i].am_channel; + } + + /* look for specific channels and process the raw voltage to measurement data */ + if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + /* Voltage in volts */ + float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); + + if (voltage > BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_v = voltage; + + /* one-time initialization of low-pass value to avoid long init delays */ + if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_filtered_v = voltage; + } + + _battery_status.timestamp = t; + _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS; + + } else { + /* mark status as invalid */ + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + } + + } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { + /* handle current only if voltage is valid */ + if (_battery_status.voltage_v > 0.0f) { + float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + + /* check measured current value */ + if (current >= 0.0f) { + _battery_status.timestamp = t; + _battery_status.current_a = current; + + if (_battery_current_timestamp != 0) { + /* initialize discharged value */ + if (_battery_status.discharged_mah < 0.0f) { + _battery_status.discharged_mah = 0.0f; + } + + _battery_discharged += current * (t - _battery_current_timestamp); + _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; + } + } + } + + _battery_current_timestamp = t; + + } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + + /* calculate airspeed, raw is the difference from */ + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) + + /** + * The voltage divider pulls the signal down, only act on + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. + */ + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { + + float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + + _diff_pres.timestamp = t; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + + (diff_pres_pa_raw * 0.1f); + _diff_pres.temperature = -1000.0f; + + /* announce the airspeed if needed, just publish else */ + if (_diff_pres_pub > 0) { + orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres); + + } else { + _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); + } + } + } + } + + _last_adc = t; + + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } + } + } +} + +float +Sensors::get_rc_value(uint8_t func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.channels[_rc.function[func]]; + + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + + } else { + return 0.0f; + } +} + +switch_pos_t +Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) +{ + if (_rc.function[func] >= 0) { + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return manual_control_setpoint_s::SWITCH_POS_ON; + + } else if (mid_inv ? value < mid_th : value > mid_th) { + return manual_control_setpoint_s::SWITCH_POS_MIDDLE; + + } else { + return manual_control_setpoint_s::SWITCH_POS_OFF; + } + + } else { + return manual_control_setpoint_s::SWITCH_POS_NONE; + } +} + +switch_pos_t +Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) +{ + if (_rc.function[func] >= 0) { + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return manual_control_setpoint_s::SWITCH_POS_ON; + + } else { + return manual_control_setpoint_s::SWITCH_POS_OFF; + } + + } else { + return manual_control_setpoint_s::SWITCH_POS_NONE; + } +} + +void +Sensors::set_params_from_rc() +{ + static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, + * maybe we need to introduce a more aggressive limit here */ + if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { + param_rc_values[i] = rc_val; + float param_val = math::constrain( + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + param_set(_parameter_handles.rc_param[i], ¶m_val); + } + } +} + +void +Sensors::rc_poll() +{ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); + + if (rc_updated) { + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* detect RC signal loss */ + bool signal_lost; + + /* check flags and require at least four channels to consider the signal valid */ + if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else { + /* signal looks good */ + signal_lost = false; + + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle + + if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping + fs_ch = _parameters.rc_map_failsafe - 1; + } + + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { + /* failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } + } + } + + unsigned channel_limit = rc_input.channel_count; + + if (channel_limit > _rc_max_chan_count) { + channel_limit = _rc_max_chan_count; + } + + /* read out and scale values from raw message even if signal is invalid */ + for (unsigned int i = 0; i < channel_limit; i++) { + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (rc_input.values[i] < _parameters.min[i]) { + rc_input.values[i] = _parameters.min[i]; + } + + if (rc_input.values[i] > _parameters.max[i]) { + rc_input.values[i] = _parameters.max[i]; + } + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..0. + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)( + _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + + } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { + _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)( + _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + + } else { + /* in the configured dead zone, output zero */ + _rc.channels[i] = 0.0f; + } + + _rc.channels[i] *= _parameters.rev[i]; + + /* handle any parameter-induced blowups */ +#ifdef __PX4_NUTTX + if (!isfinite(_rc.channels[i])) { +#else + if (!std::isfinite(_rc.channels[i])) { +#endif + _rc.channels[i] = 0.0f; + } + } + + _rc.channel_count = rc_input.channel_count; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = signal_lost; + _rc.timestamp = rc_input.timestamp_last_signal; + + /* publish rc_channels topic even if signal is invalid, for debug */ + if (_rc_pub > 0) { + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + + } else { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); + } + + if (!signal_lost) { + struct manual_control_setpoint_s manual; + memset(&manual, 0 , sizeof(manual)); + + /* fill values in manual_control_setpoint topic only if signal is valid */ + manual.timestamp = rc_input.timestamp_last_signal; + + /* limit controls */ + manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + + /* mode switches */ + manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + + /* publish manual_control_setpoint topic */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); + + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + } + + /* copy from mapped manual control to control group 3 */ + struct actuator_controls_s actuator_group_3; + memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); + + actuator_group_3.timestamp = rc_input.timestamp_last_signal; + + actuator_group_3.control[0] = manual.y; + actuator_group_3.control[1] = manual.x; + actuator_group_3.control[2] = manual.r; + actuator_group_3.control[3] = manual.z; + actuator_group_3.control[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; + + /* publish actuator_controls_3 topic */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } + + /* Update parameters from RC Channels (tuning with RC) if activated */ + static hrt_abstime last_rc_to_param_map_time = 0; + + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { + set_params_from_rc(); + last_rc_to_param_map_time = hrt_absolute_time(); + } + } + } +} + +void +Sensors::task_main_trampoline(int argc, char *argv[]) +{ + sensors::g_sensors->task_main(); +} + +void +Sensors::task_main() +{ + + /* start individual sensors */ + int ret; + ret = accel_init(); + + if (ret) { + goto exit_immediate; + } + + ret = gyro_init(); + + if (ret) { + goto exit_immediate; + } + + ret = mag_init(); + + if (ret) { + goto exit_immediate; + } + + ret = baro_init(); + + if (ret) { + goto exit_immediate; + } + + ret = adc_init(); + + if (ret) { + goto exit_immediate; + } + + /* + * do subscriptions + */ + _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); + _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); + _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); + _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); + _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); + _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); + _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); + _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); + _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vcontrol_mode_sub, 200); + + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + + /* + * do advertisements + */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + raw.timestamp = hrt_absolute_time(); + raw.adc_voltage_v[0] = 0.0f; + raw.adc_voltage_v[1] = 0.0f; + raw.adc_voltage_v[2] = 0.0f; + raw.adc_voltage_v[3] = 0.0f; + + /* set high initial error counts to deselect gyros */ + raw.gyro_errcount = 100000; + raw.gyro1_errcount = 100000; + raw.gyro2_errcount = 100000; + + /* set high initial error counts to deselect accels */ + raw.accelerometer_errcount = 100000; + raw.accelerometer1_errcount = 100000; + raw.accelerometer2_errcount = 100000; + + /* set high initial error counts to deselect mags */ + raw.magnetometer_errcount = 100000; + raw.magnetometer1_errcount = 100000; + raw.magnetometer2_errcount = 100000; + + memset(&_battery_status, 0, sizeof(_battery_status)); + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + + /* get a set of initial values */ + accel_poll(raw); + gyro_poll(raw); + mag_poll(raw); + baro_poll(raw); + diff_pres_poll(raw); + + parameter_update_poll(true /* forced */); + rc_parameter_map_poll(true /* forced */); + + /* advertise the sensor_combined topic and make the initial publication */ + _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = _gyro_sub; + fds[0].events = POLLIN; + + _task_should_exit = false; + + while (!_task_should_exit) { + + /* wait for up to 50ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_control_mode_poll(); + + /* the timestamp of the raw struct is updated by the gyro_poll() method */ + /* copy most recent sensor data */ + gyro_poll(raw); + accel_poll(raw); + mag_poll(raw); + baro_poll(raw); + + /* work out if main gyro timed out and fail over to alternate gyro */ + if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { + + /* if the secondary failed as well, go to the tertiary */ + if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { + fds[0].fd = _gyro2_sub; + } else { + fds[0].fd = _gyro1_sub; + } + } + + /* check battery voltage */ + adc_poll(raw); + + diff_pres_poll(raw); + + /* Inform other processes that new data is available to copy */ + if (_publishing) { + orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + } + + /* check parameters for updates */ + parameter_update_poll(); + + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + + /* Look for new r/c input data */ + rc_poll(); + + perf_end(_loop_perf); + } + + warnx("exiting."); + +exit_immediate: + _sensors_task = -1; + px4_task_exit(ret); +} + +int +Sensors::start() +{ + ASSERT(_sensors_task == -1); + + /* start the task */ + _sensors_task = task_spawn_cmd("sensors_task", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + (px4_main_t)&Sensors::task_main_trampoline, + nullptr); + + /* wait until the task is up and running or has failed */ + while (_sensors_task > 0 && _task_should_exit) { + usleep(100); + } + + if (_sensors_task < 0) { + return -ERROR; + } + + return OK; +} + +int sensors_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: sensors {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (sensors::g_sensors != nullptr) { + warnx("already running"); + return 0; + } + + sensors::g_sensors = new Sensors; + + if (sensors::g_sensors == nullptr) { + warnx("alloc failed"); + return 1; + } + + if (OK != sensors::g_sensors->start()) { + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + warnx("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (sensors::g_sensors == nullptr) { + warnx("not running"); + return 1; + } + + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (sensors::g_sensors) { + warnx("is running"); + return 0; + + } else { + warnx("not running"); + return 1; + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors_nuttx.cpp similarity index 100% rename from src/modules/sensors/sensors.cpp rename to src/modules/sensors/sensors_nuttx.cpp diff --git a/src/platforms/linux/include/queue.h b/src/platforms/linux/include/queue.h index c65f760a2d..4d95541e02 100644 --- a/src/platforms/linux/include/queue.h +++ b/src/platforms/linux/include/queue.h @@ -96,14 +96,6 @@ struct dq_queue_s }; typedef struct dq_queue_s dq_queue_t; -// FIXME - hack for mavlink until kernel work queue can be removed -#define LPWORK 1 -typedef struct { - dq_entry_t dq; -} work_s; -inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2); -inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2) {} - /************************************************************************ * Global Function Prototypes ************************************************************************/ diff --git a/src/platforms/linux/main.cpp b/src/platforms/linux/main.cpp index a0b2104150..790f8d6cba 100644 --- a/src/platforms/linux/main.cpp +++ b/src/platforms/linux/main.cpp @@ -41,6 +41,7 @@ #include #include #include +#include using namespace std; @@ -48,16 +49,22 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" -// FIXME - the code below only passes 1 arg for now -void run_cmd(const string &command, const string &apparg); -void run_cmd(const string &command, const string &apparg) { - const char *arg[3]; - +void run_cmd(const vector &appargs); +void run_cmd(const vector &appargs) { + // command is appargs[0] + string command = appargs[0]; + cout << "appargs.size() = " << appargs.size() << endl; if (apps.find(command) != apps.end()) { - arg[0] = (char *)command.c_str(); - arg[1] = (char *)apparg.c_str(); - arg[2] = (char *)0; - apps[command](2,(char **)arg); + const char *arg[appargs.size()+2]; + + unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { + arg[i] = (char *)appargs[i].c_str(); + ++i; + } + arg[i] = (char *)0; + cout << command << " " << i << endl; + apps[command](i,(char **)arg); } else { @@ -65,12 +72,22 @@ void run_cmd(const string &command, const string &apparg) { } } +void process_line(string &line) +{ + vector appargs(5); + + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; + cout << "Command " << appargs[0] << endl; + unsigned int i = 1; + while ( i < appargs.size() && appargs[i] != "") { + cout << " appargs[" << i << "] = " << appargs[i] << endl; + ++i; + } + run_cmd(appargs); +} + int main(int argc, char **argv) { - string mystr; - string command; - string apparg; - // Execute a command list of provided if (argc == 2) { ifstream infile(argv[1]); @@ -78,21 +95,21 @@ int main(int argc, char **argv) //vector tokens; for (string line; getline(infile, line, '\n'); ) { - stringstream(line) >> command >> apparg; - cout << "Command " << command << ", apparg " << apparg << endl; - run_cmd(command, apparg); + process_line(line); } } + string mystr; + vector appargs(2); + + appargs[0] = "list_builtins"; + while(1) { - run_cmd("list_builtins", ""); + run_cmd(appargs); cout << "Enter a command and its args:" << endl; getline (cin,mystr); - stringstream(mystr) >> command >> apparg; - run_cmd(command, apparg); + process_line(mystr); mystr = ""; - command = ""; - apparg = ""; } } diff --git a/src/platforms/linux/px4_layer/dq_addlast.c b/src/platforms/linux/px4_layer/dq_addlast.c new file mode 100644 index 0000000000..3ef08abd05 --- /dev/null +++ b/src/platforms/linux/px4_layer/dq_addlast.c @@ -0,0 +1,74 @@ +/************************************************************ + * libc/queue/dq_addlast.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_addlast + * + * Description + * dq_addlast adds 'node' to the end of 'queue' + * + ************************************************************/ + +void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) +{ + node->flink = NULL; + node->blink = queue->tail; + + if (!queue->head) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/linux/px4_layer/dq_remfirst.c b/src/platforms/linux/px4_layer/dq_remfirst.c new file mode 100644 index 0000000000..e87acc3382 --- /dev/null +++ b/src/platforms/linux/px4_layer/dq_remfirst.c @@ -0,0 +1,82 @@ +/************************************************************ + * libc/queue/dq_remfirst.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_remfirst + * + * Description: + * dq_remfirst removes 'node' from the head of 'queue' + * + ************************************************************/ + +FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) +{ + FAR dq_entry_t *ret = queue->head; + + if (ret) + { + FAR dq_entry_t *next = ret->flink; + if (!next) + { + queue->head = NULL; + queue->tail = NULL; + } + else + { + queue->head = next; + next->blink = NULL; + } + + ret->flink = NULL; + ret->blink = NULL; + } + + return ret; +} + diff --git a/src/platforms/linux/px4_layer/drv_hrt.c b/src/platforms/linux/px4_layer/drv_hrt.c index d501236d57..ad6aee4748 100644 --- a/src/platforms/linux/px4_layer/drv_hrt.c +++ b/src/platforms/linux/px4_layer/drv_hrt.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,14 +32,16 @@ ****************************************************************************/ /** - * @file drv_hrt.h + * @file drv_hrt.c * * High-resolution timer with callouts and timekeeping. */ #include +#include +#include -static hrt_abstime dummy_timer = 0; +static struct sq_queue_s callout_queue; /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 @@ -47,12 +49,20 @@ __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; __EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +static void hrt_call_reschedule(void); + +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + /* * Get absolute time. */ hrt_abstime hrt_absolute_time(void) { - return dummy_timer; + struct timespec ts; + + clock_gettime(CLOCK_MONOTONIC, &ts); + return ts_to_abstime(&ts); } /* @@ -60,7 +70,12 @@ hrt_abstime hrt_absolute_time(void) */ hrt_abstime ts_to_abstime(struct timespec *ts) { - return dummy_timer; + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; } /* @@ -77,7 +92,8 @@ void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); */ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { - return dummy_timer; + hrt_abstime delta = hrt_absolute_time() - *then; + return delta; } /* @@ -87,35 +103,10 @@ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) */ hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) { - return dummy_timer; + hrt_abstime ts = hrt_absolute_time(); + return ts; } -/* - * Call callout(arg) after delay has elapsed. - * - * If callout is NULL, this can be used to implement a timeout by testing the call - * with hrt_called(). - */ -void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) -{ -} - -/* - * Call callout(arg) at absolute time calltime. - */ -void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) -{ -} - -/* - * Call callout(arg) after delay, and then after every interval. - * - * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may - * jitter but should not drift. - */ -void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) -{ -} /* * If this returns true, the entry has been invoked and removed from the callout list, @@ -125,7 +116,7 @@ void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime inter */ bool hrt_called(struct hrt_call *entry) { - return true; + return (entry->deadline == 0); } /* @@ -133,6 +124,15 @@ bool hrt_called(struct hrt_call *entry) */ void hrt_cancel(struct hrt_call *entry) { + // FIXME - need a lock + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + // endif } /* @@ -140,6 +140,7 @@ void hrt_cancel(struct hrt_call *entry) */ void hrt_call_init(struct hrt_call *entry) { + memset(entry, 0, sizeof(*entry)); } /* @@ -151,6 +152,7 @@ void hrt_call_init(struct hrt_call *entry) */ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) { + entry->deadline = hrt_absolute_time() + delay; } /* @@ -158,5 +160,132 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) */ void hrt_init(void) { + sq_init(&callout_queue); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + //lldbg("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + //lldbg("scheduled\n"); +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + //lldbg("entry in queue\n"); + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + //lldbg("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + deadline = next->deadline; + } + } +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) + sq_rem(&entry->link, &callout_queue); + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); +} + +/* + * Call callout(arg) after delay has elapsed. + * + * If callout is NULL, this can be used to implement a timeout by testing the call + * with hrt_called(). + */ +void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/* + * Call callout(arg) after delay, and then after every interval. + * + * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may + * jitter but should not drift. + */ +void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +/* + * Call callout(arg) at absolute time calltime. + */ +void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); } diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk index 2dd89b0216..1d987418ac 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/linux/px4_layer/module.mk @@ -41,7 +41,10 @@ SRCS = \ lib_crc32.c \ drv_hrt.c \ queue.c \ + dq_addlast.c \ + dq_remfirst.c \ sq_addlast.c \ - sq_remfirst.c + sq_remfirst.c \ + sq_addafter.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index e94d51f288..059e1c9176 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -37,9 +37,12 @@ * PX4 Middleware Wrapper Linux Implementation */ +#include #include +#include #include #include +#include #include "systemlib/param/param.h" __BEGIN_DECLS @@ -49,6 +52,8 @@ struct param_info_s param_array[256]; struct param_info_s *param_info_base; struct param_info_s *param_info_limit; +long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); + __END_DECLS namespace px4 @@ -92,3 +97,19 @@ void init(int argc, char *argv[], const char *app_name) } } + + + + +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + printf("work_queue: UNIMPLEMENTED\n"); + return PX4_OK; +} + +int work_cancel(int qid, struct work_s *work) +{ + printf("work_cancel: UNIMPLEMENTED\n"); + return PX4_OK; +} + diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index 0e069dd990..f62f4227ff 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -151,21 +151,14 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int int px4_task_delete(px4_task_t id) { int rv = 0; - int i; pthread_t pid; //printf("Called px4_task_delete\n"); - // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + if (id < PX4_MAX_TASKS && taskmap[id] != 0) + pid = taskmap[id]; + else return -EINVAL; - } + // If current thread then exit, otherwise cancel if (pthread_self() == pid) { pthread_exit(0); @@ -178,6 +171,25 @@ int px4_task_delete(px4_task_t id) return rv; } +void px4_task_exit(int ret) +{ + int i; + pthread_t pid = pthread_self(); + + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) + printf("px4_task_exit: self task not found!\n"); + + pthread_exit((void *)(unsigned long)ret); +} + void px4_killall(void) { //printf("Called px4_killall\n"); diff --git a/src/platforms/linux/px4_layer/sq_addafter.c b/src/platforms/linux/px4_layer/sq_addafter.c new file mode 100644 index 0000000000..5d47feba0f --- /dev/null +++ b/src/platforms/linux/px4_layer/sq_addafter.c @@ -0,0 +1,71 @@ +/************************************************************ + * libc/queue/sq_addafter.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addafter.c + * + * Description: + * The sq_addafter function adds 'node' after 'prev' in the + * 'queue.' + * + ************************************************************/ + +void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue) +{ + if (!queue->head || prev == queue->tail) + { + sq_addlast(node, queue); + } + else + { + node->flink = prev->flink; + prev->flink = node; + } +} diff --git a/src/drivers/device/vdevice.cpp b/src/platforms/px4_adc.h similarity index 59% rename from src/drivers/device/vdevice.cpp rename to src/platforms/px4_adc.h index 487249aed5..0b123ed0c0 100644 --- a/src/drivers/device/vdevice.cpp +++ b/src/platforms/px4_adc.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,76 +32,35 @@ ****************************************************************************/ /** - * @file device.cpp + * @file px4_adc.h * - * Fundamental driver base class for the virtual device framework. + * ADC header depending on the build target */ -#include "device.h" +#pragma once -#include -#include -#include +#if defined(__PX4_ROS) +#error "ADC not supported in ROS" +#elif defined(__PX4_NUTTX) +/* + * Building for NuttX + */ +#include +#elif defined(__PX4_LINUX) -namespace device +// FIXME - this needs to be a px4_adc_msg_s type +// Curently copied from NuttX +struct adc_msg_s { + uint8_t am_channel; /* The 8-bit ADC Channel */ + int32_t am_data; /* ADC convert result (4 bytes) */ +} packed_struct; -Device::Device(const char *name) : - // public - // protected - _name(name), - _debug_enabled(false) -{ - sem_init(&_lock, 0, 1); - - /* setup a default device ID. When bus_type is UNKNOWN the - other fields are invalid */ - _device_id.devid = 0; - _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; - _device_id.devid_s.bus = 0; - _device_id.devid_s.address = 0; - _device_id.devid_s.devtype = 0; -} +// Example settings +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 -Device::~Device() -{ - sem_destroy(&_lock); -} - -int -Device::init() -{ - int ret = OK; - - return ret; -} - -void -Device::log(const char *fmt, ...) -{ - va_list ap; - - printf("[%s] ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -} - -void -Device::debug(const char *fmt, ...) -{ - va_list ap; - - if (_debug_enabled) { - printf("<%s> ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); - } -} - -} // namespace device +#else +#error "No target platform defined" +#endif diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 83bcb8882d..ac5a04c277 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -44,4 +44,10 @@ #include #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 + +extern long PX4_TICKS_PER_SEC; +#define USEC2TICKS(x) (PX4_TICKS_PER_SEC*(long)x/1000000L) + +#define px4_errx(x, ...) errx(x, __VA_ARGS__) + #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ab41625d5e..6a4b1f2cff 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -45,6 +45,8 @@ #define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) +#define PX4_ERROR (-1) +#define PX4_OK 0 #if defined(__PX4_ROS) /* @@ -111,6 +113,7 @@ typedef param_t px4_param_t; /* FIXME - Used to satisfy build */ #define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) +#define USEC2TICK(x) 0 #define getreg32(a) (*(volatile uint32_t *)(a)) diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h new file mode 100644 index 0000000000..9270cb203b --- /dev/null +++ b/src/platforms/px4_i2c.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_i2c.h + * + * Includes device headers depending on the build target + */ + +#pragma once + +#define PX4_I2C_M_READ 0x0001 /* read data, from slave to master */ + +#if defined(__PX4_ROS) +#error "Devices not supported in ROS" +#elif defined (__PX4_NUTTX) +/* + * Building for NuttX + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "up_internal.h" +#include "up_arch.h" + +#define px4_i2c_msg_t i2c_msg_s + +typedef struct i2c_dev_s px4_i2c_dev_t; + +#define px4_i2cuninitialize(x) up_i2cuninitialize(x) +#define px4_i2cinitialize(x) up_i2cinitialize(x) +#define px4_i2creset(x) up_i2creset(x) + +#define px4_interrupt_context() up_interrupt_context() + +#elif defined(__PX4_LINUX) +#include + +#define I2C_M_READ 0x0001 /* read data, from slave to master */ +#define I2C_M_TEN 0x0002 /* ten bit address */ +#define I2C_M_NORESTART 0x0080 /* message should not begin with (re-)start of transfer */ + +// NOTE - This is a copy of the NuttX i2c_msg_s structure +typedef struct { + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; +} px4_i2c_msg_t; + +struct px4_i2c_ops_t; +// NOTE - This is a copy of the NuttX i2c_ops_s structure +typedef struct { + const struct px4_i2c_ops_t *ops; /* I2C vtable */ +} px4_i2c_dev_t; + +// FIXME - Stub implementations +inline void px4_i2cuninitialize(px4_i2c_dev_t *dev); +inline void px4_i2cuninitialize(px4_i2c_dev_t *dev) {} + +inline px4_i2c_dev_t *px4_i2cinitialize(int bus); +inline px4_i2c_dev_t *px4_i2cinitialize(int bus) { return (px4_i2c_dev_t *)0; } + +inline void px4_i2creset(px4_i2c_dev_t *dev); +inline void px4_i2creset(px4_i2c_dev_t *dev) { } + +inline bool px4_interrupt_context(void); +inline bool px4_interrupt_context(void) { return false; } + +// FIXME - Empty defines for I2C ops +// Original version commented out +//#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f)) +#define I2C_SETFREQUENCY(d,f) +//#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s)) +#define SPI_SELECT(d,id,s) + +// FIXME - Stub implementation +// Original version commented out +//#define I2C_TRANSFER(d,m,c) ((d)->ops->transfer(d,m,c)) +inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count); +inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { return 0; } + +struct i2c_msg_s +{ + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; +}; +#else +#error "No target platform defined" +#endif diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 0e732df4b8..ea56c3425c 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -48,12 +49,11 @@ #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 -#define PX4_DIOC_GETPRIV 1 -#define PX4_DEVIOCSPUBBLOCK 2 -#define PX4_DEVIOCGPUBBLOCK 3 +#define PX4_DEVIOCGDEVICEID 1 -#define PX4_ERROR (-1) -#define PX4_OK 0 +#define PX4_DIOC_GETPRIV 2 +#define PX4_DEVIOCSPUBBLOCK 3 +#define PX4_DEVIOCGPUBBLOCK 4 //#define PX4_DEBUG(...) #define PX4_DEBUG(...) printf(__VA_ARGS__) diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h new file mode 100644 index 0000000000..41289aba03 --- /dev/null +++ b/src/platforms/px4_spi.h @@ -0,0 +1,35 @@ +#pragma once + +#ifdef __PX4_NUTTX +#include +#elif defined(__PX4_LINUX) +enum spi_dev_e +{ + SPIDEV_NONE = 0, /* Not a valid value */ + SPIDEV_MMCSD, /* Select SPI MMC/SD device */ + SPIDEV_FLASH, /* Select SPI FLASH device */ + SPIDEV_ETHERNET, /* Select SPI ethernet device */ + SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ + SPIDEV_WIRELESS, /* Select SPI Wireless device */ + SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ + SPIDEV_EXPANDER, /* Select SPI I/O expander device */ + SPIDEV_MUX, /* Select SPI multiplexer device */ + SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ + SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ +}; + +/* Certain SPI devices may required differnt clocking modes */ + +enum spi_mode_e +{ + SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ + SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ + SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ + SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ +}; +struct spi_dev_s +{ + int unused; +}; +#else +#endif diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 18a6ced79e..3a9adfd37e 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -46,6 +46,9 @@ #error "PX4 tasks not supported in ROS" #elif defined(__PX4_NUTTX) typedef int px4_task_t; + +#define px4_task_exit(x) _exit(x) + #elif defined(__PX4_LINUX) #include #include @@ -83,6 +86,7 @@ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name, char * const argv[]); __EXPORT int px4_task_delete(px4_task_t pid); +__EXPORT void px4_task_exit(int ret); __END_DECLS diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h new file mode 100644 index 0000000000..b482022a60 --- /dev/null +++ b/src/platforms/px4_workqueue.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * include/nuttx/wqueue.h + * + * Copyright (C) 2009, 2011-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +#pragma once + +#ifdef __PX4_ROS +#error "Work queue not supported on ROS +#elif defined(__PX4_NUTTX) +#include +#include +#include +#elif defined(__PX4_LINUX) + +#include + +#define LPWORK 1 +#define HPWORK 2 + +/* Defines the work callback */ + +typedef void (*worker_t)(void *arg); + +struct work_s +{ + struct dq_entry_s dq; /* Implements a doubly linked list */ + worker_t worker; /* Work callback */ + void *arg; /* Callback argument */ + uint32_t qtime; /* Time work queued */ + uint32_t delay; /* Delay until work performed */ +}; + +/**************************************************************************** + * Name: work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * qid - The work queue ID + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in clock ticks) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_queue(int qid, FAR struct work_s *work, worker_t worker, + FAR void *arg, uint32_t delay); + +/**************************************************************************** + * Name: work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling work_queue() + * again. + * + * Input parameters: + * qid - The work queue ID + * work - The previously queue work structure to cancel + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_cancel(int qid, FAR struct work_s *work); +#else +#error "Unknown target OS" +#endif From 88a00811121f2c636af0de9971d5c6bf5e95edcf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 14 Mar 2015 14:40:00 -0700 Subject: [PATCH 030/258] Linux: Added support for gcc-4.9.1 Fixed bug with missing quote in #error found by gcc 4.9.1 Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 4 +++- src/platforms/px4_workqueue.h | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index a9598faa39..0d26d95de6 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -62,6 +62,8 @@ CLANGVER= endif endif +USE_GCC=1 + ifeq ($(USE_GCC),1) # GCC Options: CC = gcc @@ -69,7 +71,7 @@ CXX = g++ CPP = gcc -E # GCC Version -DEV_VER_SUPPORTED = 4.6 4.8.2 +DEV_VER_SUPPORTED = 4.6 4.8.2 4.9.1 else # Clang options diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index b482022a60..b6552c204f 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -35,8 +35,8 @@ #pragma once -#ifdef __PX4_ROS -#error "Work queue not supported on ROS +#if defined(__PX4_ROS) +#error "Work queue not supported on ROS" #elif defined(__PX4_NUTTX) #include #include From 99399115f2135eb1ce64d46370f496249318cbf0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 14 Mar 2015 15:09:00 -0700 Subject: [PATCH 031/258] Linux: Minor fixes of error caught by gcc-4.9.1 - Missing static declarations for functions not used outside a file. Signed-off-by: Mark Charlebois --- src/drivers/ms5611/ms5611_linux.cpp | 1 + src/platforms/linux/main.cpp | 2 +- src/platforms/linux/vcdev_test/vcdevtest_example.cpp | 4 ++-- src/platforms/px4_config.h | 3 --- src/platforms/px4_defines.h | 12 +++++++++--- 5 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 043f16407b..2e872f7b6e 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -37,6 +37,7 @@ */ #include +#include #include #include diff --git a/src/platforms/linux/main.cpp b/src/platforms/linux/main.cpp index 790f8d6cba..ae3c547bfd 100644 --- a/src/platforms/linux/main.cpp +++ b/src/platforms/linux/main.cpp @@ -72,7 +72,7 @@ void run_cmd(const vector &appargs) { } } -void process_line(string &line) +static void process_line(string &line) { vector appargs(5); diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp index 39c804ada4..c773da8ba1 100644 --- a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/linux/vcdev_test/vcdevtest_example.cpp @@ -51,7 +51,7 @@ using namespace device; #define TESTDEV "/dev/vdevex" -int writer_main(int argc, char *argv[]) +static int writer_main(int argc, char *argv[]) { char buf[1] = { '1' }; @@ -97,7 +97,7 @@ VCDevExample::~VCDevExample() { } } -int test_pub_block(int fd, unsigned long blocked) +static int test_pub_block(int fd, unsigned long blocked) { int ret = px4_ioctl(fd, PX4_DEVIOCSPUBBLOCK, blocked); if (ret < 0) { diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index ac5a04c277..c53fd63d49 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -45,9 +45,6 @@ #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 -extern long PX4_TICKS_PER_SEC; -#define USEC2TICKS(x) (PX4_TICKS_PER_SEC*(long)x/1000000L) - #define px4_errx(x, ...) errx(x, __VA_ARGS__) #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6a4b1f2cff..091d67e056 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -112,10 +112,16 @@ typedef param_t px4_param_t; #define _PX4_IOC(x,y) _IO(x,y) /* FIXME - Used to satisfy build */ -#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) -#define USEC2TICK(x) 0 +//STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) +#define UNIQUE_ID 0x1FFF7A10 -#define getreg32(a) (*(volatile uint32_t *)(a)) +#define getreg32(a) (*(volatile uint32_t *)(a)) + +__BEGIN_DECLS +extern long PX4_TICKS_PER_SEC; +__END_DECLS + +#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)x/1000000L) #endif From ac0df5c61de058230451552bec7bf9e0056231a0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 14 Mar 2015 17:54:37 -0700 Subject: [PATCH 032/258] Linux: added HRT test, moved tests to linux/tests Also fixed naming of mavlink files for NuttX build. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 5 +- ...{mavlink_ftp.cpp => mavlink_ftp_nuttx.cpp} | 0 ...avlink_main.cpp => mavlink_main_nuttx.cpp} | 0 ...eceiver.cpp => mavlink_receiver_linux.cpp} | 0 .../mavlink/mavlink_receiver_nuttx.cpp | 1557 +++++++++++++++++ src/modules/mavlink/mavlink_tests/module.mk | 6 +- src/modules/mavlink/module.mk | 9 +- src/modules/uORB/MuORB.cpp | 8 +- src/platforms/linux/px4_layer/drv_hrt.c | 12 +- .../linux/px4_layer/px4_linux_tasks.c | 38 +- .../linux/{ => tests}/hello/hello_example.cpp | 0 .../linux/{ => tests}/hello/hello_example.h | 0 .../linux/{ => tests}/hello/hello_main.cpp | 0 .../{ => tests}/hello/hello_start_linux.cpp | 0 .../linux/{ => tests}/hello/module.mk | 0 .../linux/tests/hrt_test/hrt_test.cpp | 66 + src/platforms/linux/tests/hrt_test/hrt_test.h | 53 + .../linux/tests/hrt_test/hrt_test_main.cpp | 55 + .../tests/hrt_test/hrt_test_start_linux.cpp | 92 + src/platforms/linux/tests/hrt_test/module.mk | 43 + .../linux/{ => tests}/vcdev_test/module.mk | 0 .../vcdev_test/vcdevtest_example.cpp | 0 .../vcdev_test/vcdevtest_example.h | 0 .../{ => tests}/vcdev_test/vcdevtest_main.cpp | 0 .../vcdev_test/vcdevtest_start_linux.cpp | 0 src/platforms/px4_posix.h | 4 +- 26 files changed, 1924 insertions(+), 24 deletions(-) rename src/modules/mavlink/{mavlink_ftp.cpp => mavlink_ftp_nuttx.cpp} (100%) rename src/modules/mavlink/{mavlink_main.cpp => mavlink_main_nuttx.cpp} (100%) rename src/modules/mavlink/{mavlink_receiver.cpp => mavlink_receiver_linux.cpp} (100%) create mode 100644 src/modules/mavlink/mavlink_receiver_nuttx.cpp rename src/platforms/linux/{ => tests}/hello/hello_example.cpp (100%) rename src/platforms/linux/{ => tests}/hello/hello_example.h (100%) rename src/platforms/linux/{ => tests}/hello/hello_main.cpp (100%) rename src/platforms/linux/{ => tests}/hello/hello_start_linux.cpp (100%) rename src/platforms/linux/{ => tests}/hello/module.mk (100%) create mode 100644 src/platforms/linux/tests/hrt_test/hrt_test.cpp create mode 100644 src/platforms/linux/tests/hrt_test/hrt_test.h create mode 100644 src/platforms/linux/tests/hrt_test/hrt_test_main.cpp create mode 100644 src/platforms/linux/tests/hrt_test/hrt_test_start_linux.cpp create mode 100644 src/platforms/linux/tests/hrt_test/module.mk rename src/platforms/linux/{ => tests}/vcdev_test/module.mk (100%) rename src/platforms/linux/{ => tests}/vcdev_test/vcdevtest_example.cpp (100%) rename src/platforms/linux/{ => tests}/vcdev_test/vcdevtest_example.h (100%) rename src/platforms/linux/{ => tests}/vcdev_test/vcdevtest_main.cpp (100%) rename src/platforms/linux/{ => tests}/vcdev_test/vcdevtest_start_linux.cpp (100%) diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 8286c1c367..f53cf93197 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -54,6 +54,7 @@ MODULES += lib/conversion # Linux port # MODULES += platforms/linux/px4_layer -MODULES += platforms/linux/hello -MODULES += platforms/linux/vcdev_test +MODULES += platforms/linux/tests/hello +MODULES += platforms/linux/tests/vcdev_test +MODULES += platforms/linux/tests/hrt_test diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp_nuttx.cpp similarity index 100% rename from src/modules/mavlink/mavlink_ftp.cpp rename to src/modules/mavlink/mavlink_ftp_nuttx.cpp diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main_nuttx.cpp similarity index 100% rename from src/modules/mavlink/mavlink_main.cpp rename to src/modules/mavlink/mavlink_main_nuttx.cpp diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver_linux.cpp similarity index 100% rename from src/modules/mavlink/mavlink_receiver.cpp rename to src/modules/mavlink/mavlink_receiver_linux.cpp diff --git a/src/modules/mavlink/mavlink_receiver_nuttx.cpp b/src/modules/mavlink/mavlink_receiver_nuttx.cpp new file mode 100644 index 0000000000..51ff07d0ee --- /dev/null +++ b/src/modules/mavlink/mavlink_receiver_nuttx.cpp @@ -0,0 +1,1557 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file mavlink_receiver.cpp + * MAVLink protocol message receive and dispatch + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler + */ + +/* XXX trim includes */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +__BEGIN_DECLS + +#include "mavlink_bridge_header.h" +#include "mavlink_receiver.h" +#include "mavlink_main.h" + +__END_DECLS + +static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; + +MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : + _mavlink(parent), + status{}, + hil_local_pos{}, + hil_land_detector{}, + _control_mode{}, + _global_pos_pub(-1), + _local_pos_pub(-1), + _attitude_pub(-1), + _gps_pub(-1), + _sensors_pub(-1), + _gyro_pub(-1), + _accel_pub(-1), + _mag_pub(-1), + _baro_pub(-1), + _airspeed_pub(-1), + _battery_pub(-1), + _cmd_pub(-1), + _flow_pub(-1), + _range_pub(-1), + _offboard_control_mode_pub(-1), + _actuator_controls_pub(-1), + _global_vel_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), + _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), + _vicon_position_pub(-1), + _vision_position_pub(-1), + _telemetry_status_pub(-1), + _rc_pub(-1), + _manual_pub(-1), + _land_detector_pub(-1), + _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), + _hil_frames(0), + _old_timestamp(0), + _hil_local_proj_inited(0), + _hil_local_alt0(0.0f), + _hil_local_proj_ref{}, + _offboard_control_mode{}, + _rates_sp{}, + _time_offset_avg_alpha(0.6), + _time_offset(0) +{ + + // make sure the FTP server is started + (void)MavlinkFTP::get_server(); +} + +MavlinkReceiver::~MavlinkReceiver() +{ +} + +void +MavlinkReceiver::handle_message(mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_COMMAND_LONG: + handle_message_command_long(msg); + break; + + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); + break; + + case MAVLINK_MSG_ID_SET_MODE: + handle_message_set_mode(msg); + break; + + case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: + handle_message_vicon_position_estimate(msg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_message_set_position_target_local_ned(msg); + break; + + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_message_set_attitude_target(msg); + break; + + case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET: + handle_message_set_actuator_control_target(msg); + break; + + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); + break; + + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_message_radio_status(msg); + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + handle_message_manual_control(msg); + break; + + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(msg); + break; + + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_message_request_data_stream(msg); + break; + + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: + MavlinkFTP::get_server()->handle_message(_mavlink, msg); + break; + + case MAVLINK_MSG_ID_SYSTEM_TIME: + handle_message_system_time(msg); + break; + + case MAVLINK_MSG_ID_TIMESYNC: + handle_message_timesync(msg); + break; + + default: + break; + } + + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + * + * Accept HIL GPS messages if use_hil_gps flag is true. + * This allows to provide fake gps measurements to the system. + */ + if (_mavlink->get_hil_enabled()) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); + break; + + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: + handle_message_hil_state_quaternion(msg); + break; + + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + + default: + break; + } + } + + + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + default: + break; + } + + } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); +} + +void +MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + +void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + +void +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); + + enum Rotation flow_rot; + param_get(param_find("SENS_FLOW_ROT"),&flow_rot); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; + + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } +} + +void +MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } + + /* Use distance value for range finder report */ + struct range_finder_report r; + memset(&r, 0, sizeof(r)); + + r.timestamp = hrt_absolute_time(); + r.error_count = 0; + r.type = RANGE_FINDER_TYPE_LASER; + r.distance = flow.distance; + r.minimum_distance = 0.0f; + r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable + r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + + if (_range_pub < 0) { + _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + } else { + orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + } +} + +void +MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) +{ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; + /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = custom_mode.main_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = VEHICLE_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } +} + +void +MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) +{ + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); + + struct vehicle_vicon_position_s vicon_position; + memset(&vicon_position, 0, sizeof(vicon_position)); + + vicon_position.timestamp = hrt_absolute_time(); + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + vicon_position.roll = pos.roll; + vicon_position.pitch = pos.pitch; + vicon_position.yaw = pos.yaw; + + if (_vicon_position_pub < 0) { + _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + + } else { + orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); + } +} + +void +MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg) +{ + mavlink_set_position_target_local_ned_t set_position_target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); + + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == set_position_target_local_ned.target_system || + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0)) { + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); + offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); + offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + /* yaw ignore flag mapps to ignore_attitude */ + offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); + /* yawrate ignore flag mapps to ignore_bodyrate */ + offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); + + offboard_control_mode.timestamp = hrt_absolute_time(); + + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); + + } else { + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); + } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + if (_control_mode.flag_control_offboard_enabled) { + if (is_force_sp && offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ + struct vehicle_force_setpoint_s force_sp; + force_sp.x = set_position_target_local_ned.afx; + force_sp.y = set_position_target_local_ned.afy; + force_sp.z = set_position_target_local_ned.afz; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + struct position_setpoint_triplet_s pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others + + /* set the local pos values */ + if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = set_position_target_local_ned.x; + pos_sp_triplet.current.y = set_position_target_local_ned.y; + pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local vel values */ + if (!offboard_control_mode.ignore_velocity) { + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = set_position_target_local_ned.vx; + pos_sp_triplet.current.vy = set_position_target_local_ned.vy; + pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { + pos_sp_triplet.current.velocity_valid = false; + } + + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (!offboard_control_mode.ignore_acceleration_force) { + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; + pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; + pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; + pos_sp_triplet.current.acceleration_is_force = + is_force_sp; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw sp value */ + if (!offboard_control_mode.ignore_attitude) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value */ + if (!offboard_control_mode.ignore_bodyrate) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + //XXX handle global pos setpoints (different MAV frames) + + + if (_pos_sp_triplet_pub < 0) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } + + } + + } + + } + } +} + +void +MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) +{ + mavlink_set_actuator_control_target_t set_actuator_control_target; + mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); + + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints + + if ((mavlink_system.sysid == set_actuator_control_target.target_system || + set_actuator_control_target.target_system == 0) && + (mavlink_system.compid == set_actuator_control_target.target_component || + set_actuator_control_target.target_component == 0)) { + + /* ignore all since we are setting raw actuators here */ + offboard_control_mode.ignore_thrust = true; + offboard_control_mode.ignore_attitude = true; + offboard_control_mode.ignore_bodyrate = true; + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.timestamp = hrt_absolute_time(); + + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); + } else { + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); + } + + + /* If we are in offboard control mode, publish the actuator controls */ + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + actuator_controls.timestamp = hrt_absolute_time(); + + /* Set duty cycles for the servos in actuator_controls_0 */ + for(size_t i = 0; i < 8; i++) { + actuator_controls.control[i] = set_actuator_control_target.controls[i]; + } + + if (_actuator_controls_pub < 0) { + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } else { + orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + } + } + } + +} + +void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(msg, &pos); + + struct vision_position_estimate vision_position; + memset(&vision_position, 0, sizeof(vision_position)); + + // Use the component ID to identify the vision sensor + vision_position.id = msg->compid; + + vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time + vision_position.timestamp_computer = pos.usec; + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; + + // XXX fix this + vision_position.vx = 0.0f; + vision_position.vy = 0.0f; + vision_position.vz = 0.0f; + + math::Quaternion q; + q.from_euler(pos.roll, pos.pitch, pos.yaw); + + vision_position.q[0] = q(0); + vision_position.q[1] = q(1); + vision_position.q[2] = q(2); + vision_position.q[3] = q(3); + + if (_vision_position_pub < 0) { + _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); + + } else { + orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); + } +} + +void +MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) +{ + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); + + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == set_attitude_target.target_system || + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0)) { + + /* set correct ignore flags for thrust field: copy from mavlink message */ + _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + + /* + * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust + * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore + * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits + * set for everything else. + */ + + /* + * if attitude or body rate have been used (not ignored) previously and this message only sends + * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and + * body rates to keep the controllers running + */ + bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; + } else { + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude; + } + + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; + + _offboard_control_mode.timestamp = hrt_absolute_time(); + + if (_offboard_control_mode_pub < 0) { + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode); + + } else { + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode); + } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ + if (!(_offboard_control_mode.ignore_attitude)) { + static struct vehicle_attitude_setpoint_s att_sp = {}; + att_sp.timestamp = hrt_absolute_time(); + mavlink_quaternion_to_euler(set_attitude_target.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); + att_sp.R_valid = true; + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + att_sp.thrust = set_attitude_target.thrust; + } + att_sp.yaw_sp_move_rate = 0.0; + memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); + att_sp.q_d_valid = true; + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + } + + /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ + ///XXX add support for ignoring individual axes + if (!(_offboard_control_mode.ignore_bodyrate)) { + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.roll = set_attitude_target.body_roll_rate; + _rates_sp.pitch = set_attitude_target.body_pitch_rate; + _rates_sp.yaw = set_attitude_target.body_yaw_rate; + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + _rates_sp.thrust = set_attitude_target.thrust; + } + + if (_att_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); + } + } + } + + } + } +} + +void +MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) +{ + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + tstatus.system_id = msg->sysid; + tstatus.component_id = msg->compid; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); + + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } + } +} + +void +MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) +{ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + manual.timestamp = hrt_absolute_time(); + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; + + // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); + + if (_manual_pub < 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + } +} + +void +MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) +{ + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); + + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); + + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } + } + } +} + +void +MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) +{ + mavlink_request_data_stream_t req; + mavlink_msg_request_data_stream_decode(msg, &req); + + if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid && req.req_message_rate != 0) { + float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_mavlink->get_streams(), stream) { + if (req.req_stream_id == stream->get_id()) { + _mavlink->configure_stream_threadsafe(stream->get_name(), rate); + } + } + } +} + +void +MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) +{ + mavlink_system_time_t time; + mavlink_msg_system_time_decode(msg, &time); + + timespec tv; + clock_gettime(CLOCK_REALTIME, &tv); + + // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 + bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; + bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; + + if (!onb_unix_valid && ofb_unix_valid) { + tv.tv_sec = time.time_unix_usec / 1000000ULL; + tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; + if(clock_settime(CLOCK_REALTIME, &tv)) { + warn("failed setting clock"); + } + else { + warnx("[timesync] UTC time synced."); + } + } + +} + +void +MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) +{ + mavlink_timesync_t tsync; + mavlink_msg_timesync_decode(msg, &tsync); + + uint64_t now_ns = hrt_absolute_time() * 1000LL ; + + if (tsync.tc1 == 0) { + + mavlink_timesync_t rsync; // return timestamped sync message + + rsync.tc1 = now_ns; + rsync.ts1 = tsync.ts1; + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); + + return; + + } else if (tsync.tc1 > 0) { + + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; + int64_t dt = _time_offset - offset_ns; + + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew + _time_offset = offset_ns; + warnx("[timesync] Hard setting offset."); + } else { + smooth_time_offset(offset_ns); + } + } + +} + +void +MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); + + uint64_t timestamp = hrt_absolute_time(); + + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; + + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + gyro.timestamp = timestamp; + gyro.x_raw = imu.xgyro * 1000.0f; + gyro.y_raw = imu.ygyro * 1000.0f; + gyro.z_raw = imu.zgyro * 1000.0f; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + accel.timestamp = timestamp; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + mag.timestamp = timestamp; + mag.x_raw = imu.xmag * 1000.0f; + mag.y_raw = imu.ymag * 1000.0f; + mag.z_raw = imu.zmag * 1000.0f; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + + if (_mag_pub < 0) { + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } + + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); + + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } + } + + /* sensor combined */ + { + struct sensor_combined_s hil_sensors; + memset(&hil_sensors, 0, sizeof(hil_sensors)); + + hil_sensors.timestamp = timestamp; + + hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; + hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; + hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + + hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; + hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; + hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + hil_sensors.accelerometer_timestamp = timestamp; + + hil_sensors.adc_voltage_v[0] = 0.0f; + hil_sensors.adc_voltage_v[1] = 0.0f; + hil_sensors.adc_voltage_v[2] = 0.0f; + + hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; + hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; + hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; + hil_sensors.magnetometer_ga[0] = imu.xmag; + hil_sensors.magnetometer_ga[1] = imu.ymag; + hil_sensors.magnetometer_ga[2] = imu.zmag; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.magnetometer_timestamp = timestamp; + + hil_sensors.baro_pres_mbar = imu.abs_pressure; + hil_sensors.baro_alt_meter = imu.pressure_alt; + hil_sensors.baro_temp_celcius = imu.temperature; + hil_sensors.baro_timestamp = timestamp; + + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_timestamp = timestamp; + + /* publish combined sensor topic */ + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + + } else { + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + } + } + + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; + hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; + + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + + } else { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + } + } + + /* increment counters */ + _hil_frames++; + + /* print HIL sensors rate */ + if ((timestamp - _old_timestamp) > 10000000) { + // printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); + _old_timestamp = timestamp; + _hil_frames = 0; + } +} + +void +MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) +{ + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); + + uint64_t timestamp = hrt_absolute_time(); + + struct vehicle_gps_position_s hil_gps; + memset(&hil_gps, 0, sizeof(hil_gps)); + + hil_gps.timestamp_time = timestamp; + hil_gps.time_utc_usec = gps.time_usec; + + hil_gps.timestamp_position = timestamp; + hil_gps.lat = gps.lat; + hil_gps.lon = gps.lon; + hil_gps.alt = gps.alt; + hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m + + hil_gps.timestamp_variance = timestamp; + hil_gps.s_variance_m_s = 5.0f; + + hil_gps.timestamp_velocity = timestamp; + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m + hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m + hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m + hil_gps.vel_ned_valid = true; + hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? + + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + + } else { + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + } +} + +void +MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) +{ + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); + + uint64_t timestamp = hrt_absolute_time(); + + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* attitude */ + struct vehicle_attitude_s hil_attitude; + { + memset(&hil_attitude, 0, sizeof(hil_attitude)); + math::Quaternion q(hil_state.attitude_quaternion); + math::Matrix<3, 3> C_nb = q.to_dcm(); + math::Vector<3> euler = C_nb.to_euler(); + + hil_attitude.timestamp = timestamp; + memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); + hil_attitude.R_valid = true; + + hil_attitude.q[0] = q(0); + hil_attitude.q[1] = q(1); + hil_attitude.q[2] = q(2); + hil_attitude.q[3] = q(3); + hil_attitude.q_valid = true; + + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + + } else { + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + } + } + + /* global position */ + { + struct vehicle_global_position_s hil_global_pos; + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + + hil_global_pos.timestamp = timestamp; + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; + hil_global_pos.yaw = hil_attitude.yaw; + hil_global_pos.eph = 2.0f; + hil_global_pos.epv = 4.0f; + + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + + } else { + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + } + } + + /* local position */ + { + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + + if (!_hil_local_proj_inited) { + _hil_local_proj_inited = true; + _hil_local_alt0 = hil_state.alt / 1000.0f; + map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = lat; + hil_local_pos.ref_lon = lon; + hil_local_pos.ref_alt = _hil_local_alt0; + } + + float x; + float y; + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + hil_local_pos.timestamp = timestamp; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; + hil_local_pos.vx = hil_state.vx / 100.0f; + hil_local_pos.vy = hil_state.vy / 100.0f; + hil_local_pos.vz = hil_state.vz / 100.0f; + hil_local_pos.yaw = hil_attitude.yaw; + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + + } else { + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + } + } + + /* land detector */ + { + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + if(hil_land_detector.landed != landed) { + hil_land_detector.landed = landed; + hil_land_detector.timestamp = hrt_absolute_time(); + + if (_land_detector_pub < 0) { + _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector); + + } else { + orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector); + } + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + accel.timestamp = timestamp; + accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; + accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f; + accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; + hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; + + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + + } else { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + } + } +} + + +/** + * Receive data from UART. + */ +void * +MavlinkReceiver::receive_thread(void *arg) +{ + int uart_fd = _mavlink->get_uart_fd(); + + const int timeout = 500; + uint8_t buf[32]; + + mavlink_message_t msg; + + /* set thread name */ + char thread_name[24]; + sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); + prctl(PR_SET_NAME, thread_name, getpid()); + + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; + + ssize_t nread = 0; + + while (!_mavlink->_task_should_exit) { + if (poll(fds, 1, timeout) > 0) { + + /* non-blocking read. read may return negative values */ + if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } + + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { + /* handle generic messages and commands */ + handle_message(&msg); + + /* handle packet with parent object */ + _mavlink->handle_message(&msg); + } + } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); + } + } + + return NULL; +} + +void MavlinkReceiver::print_status() +{ + +} + +uint64_t MavlinkReceiver::to_hrt(uint64_t usec) +{ + return usec - (_time_offset / 1000) ; +} + + +void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) +{ + /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, + * the faster the moving average updates in response to + * new offset samples. + */ + + _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; +} + + +void *MavlinkReceiver::start_helper(void *context) +{ + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); + + rcv->receive_thread(NULL); + + delete rcv; + + return nullptr; +} + +pthread_t +MavlinkReceiver::receive_start(Mavlink *parent) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + // set to non-blocking read + int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); + fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); + + struct sched_param param; + (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 80; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + pthread_attr_setstacksize(&receiveloop_attr, 1800); + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); + + pthread_attr_destroy(&receiveloop_attr); + return thread; +} diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index b46d2bd355..030706cc47 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -38,8 +38,12 @@ MODULE_COMMAND = mavlink_tests SRCS = mavlink_tests.cpp \ mavlink_ftp_test.cpp \ - ../mavlink_ftp.cpp \ ../mavlink.c +ifeq ($(PX4_TARGET_NUTTX),nuttx) +SRCS += ../mavlink_ftp_nuttx.cpp +else +SRCS += ../mavlink_ftp_linux.cpp +endif INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 10f283e6c7..1740c6b776 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -36,16 +36,17 @@ # MODULE_COMMAND = mavlink -ifeq ($(PX$_TARGET_OS),nuttx) +ifeq ($(PX4_TARGET_OS),nuttx) SRCS += mavlink_main_nuttx.cpp \ - mavlink_ftp_nuttx.cpp + mavlink_ftp_nuttx.cpp \ + mavlink_receiver_nuttx.cpp else SRCS += mavlink_main_linux.cpp \ - mavlink_ftp_linux.cpp + mavlink_ftp_linux.cpp \ + mavlink_receiver_linux.cpp endif SRCS += mavlink.c \ - mavlink_receiver.cpp \ mavlink_mission.cpp \ mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index ffa51bae82..9dcab26b1c 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -197,7 +197,7 @@ ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const _priority(priority) { // enable debug() calls - _debug_enabled = true; + //_debug_enabled = true; } ORBDevNode::~ORBDevNode() @@ -702,7 +702,7 @@ namespace ORBDevMaster *g_dev; bool pubsubtest_passed = false; -bool pubsubtest_print = false; +bool pubsubtest_print = true; int pubsubtest_res = PX4_OK; struct orb_test { @@ -818,7 +818,8 @@ int pubsublatency_main(void) if (pubsubtest_print) { char fname[32]; - sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); + //sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); + sprintf(fname, "/tmp/timings%u.txt", timingsgroup); FILE *f = fopen(fname, "w"); if (f == NULL) { warnx("Error opening file!\n"); @@ -971,6 +972,7 @@ test() template int latency_test(orb_id_t T, bool print) { + test_note("---------------- LATENCY TEST ------------------"); S t; t.val = 308; t.time = hrt_absolute_time(); diff --git a/src/platforms/linux/px4_layer/drv_hrt.c b/src/platforms/linux/px4_layer/drv_hrt.c index ad6aee4748..066abe70d5 100644 --- a/src/platforms/linux/px4_layer/drv_hrt.c +++ b/src/platforms/linux/px4_layer/drv_hrt.c @@ -78,11 +78,6 @@ hrt_abstime ts_to_abstime(struct timespec *ts) return result; } -/* - * Convert absolute time to a timespec. - */ -void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); - /* * Compute the delta between a timestamp taken in the past * and now. @@ -289,3 +284,10 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo hrt_call_internal(entry, calltime, 0, callout, arg); } +#if 0 +/* + * Convert absolute time to a timespec. + */ +void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); +#endif + diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index f62f4227ff..fa832a168a 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -96,7 +96,9 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; - px4_task_t task; + pthread_t task; + pthread_attr_t attr; + struct sched_param param; // Calculate argc while (p != (char *)0) { @@ -125,14 +127,36 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; - //printf("Called px4_task_spawn_cmd\n"); - // FIXME - add handling for scheduler and priority - rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata); - - if (rv != 0) - { + rv = pthread_attr_init(&attr); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to init thread attrs\n"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to set inherit sched\n"); + return (rv < 0) ? rv : -rv; + } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to set sched policy\n"); + return (rv < 0) ? rv : -rv; + } + + param.sched_priority = priority; + + rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to set sched param\n"); + return (rv < 0) ? rv : -rv; + } + + rv = pthread_create (&task, &attr, (void *)&entry_adapter, (void *) taskdata); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to create thread\n"); + return (rv < 0) ? rv : -rv; + } + //printf("pthread_create task=%d rv=%d\n",(int)task, rv); for (i=0; i + */ + +#include +#include "hrt_test.h" +#include +#include + +px4::AppState HRTTest::appState; + +int HRTTest::main() +{ + appState.setRunning(true); + + hrt_abstime t = hrt_absolute_time(); + usleep(1000000); + hrt_abstime elt = hrt_elapsed_time(&t); + printf("Elapsed time %llu in 1 sec (usleep)\n", elt); + printf("Start time %llu\n", t); + + t = hrt_absolute_time(); + sleep(1); + elt = hrt_elapsed_time(&t); + printf("Elapsed time %llu in 1 sec (sleep)\n", elt); + printf("Start time %llu\n", t); + + return 0; +} diff --git a/src/platforms/linux/tests/hrt_test/hrt_test.h b/src/platforms/linux/tests/hrt_test/hrt_test.h new file mode 100644 index 0000000000..c4c97be6d2 --- /dev/null +++ b/src/platforms/linux/tests/hrt_test/hrt_test.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hrt_test.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include + +class HRTTest { +public: + HRTTest() {}; + + ~HRTTest() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +}; diff --git a/src/platforms/linux/tests/hrt_test/hrt_test_main.cpp b/src/platforms/linux/tests/hrt_test/hrt_test_main.cpp new file mode 100644 index 0000000000..b7c332cd3b --- /dev/null +++ b/src/platforms/linux/tests/hrt_test/hrt_test_main.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include "hrt_test.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "hello"); + + printf("hello\n"); + HRTTest test; + test.main(); + + printf("goodbye\n"); + return 0; +} diff --git a/src/platforms/linux/tests/hrt_test/hrt_test_start_linux.cpp b/src/platforms/linux/tests/hrt_test/hrt_test_start_linux.cpp new file mode 100644 index 0000000000..bc53f4e52b --- /dev/null +++ b/src/platforms/linux/tests/hrt_test/hrt_test_start_linux.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hrt_test_start_linux.cpp + * + * @author Mark Charlebois + */ +#include "hrt_test.h" +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +extern "C" __EXPORT int hrttest_main(int argc, char *argv[]); +int hrttest_main(int argc, char *argv[]) +{ + if (argc < 2) { + printf("usage: hrttest_main {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (HRTTest::appState.isRunning()) { + printf("already running\n"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("hrttest", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + HRTTest::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (HRTTest::appState.isRunning()) { + printf("is running\n"); + + } else { + printf("not started\n"); + } + + return 0; + } + + printf("usage: hrttest_main {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/linux/tests/hrt_test/module.mk b/src/platforms/linux/tests/hrt_test/module.mk new file mode 100644 index 0000000000..559427cd47 --- /dev/null +++ b/src/platforms/linux/tests/hrt_test/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = hrttest + +SRCS = hrt_test_main.cpp \ + hrt_test_start_linux.cpp \ + hrt_test.cpp + diff --git a/src/platforms/linux/vcdev_test/module.mk b/src/platforms/linux/tests/vcdev_test/module.mk similarity index 100% rename from src/platforms/linux/vcdev_test/module.mk rename to src/platforms/linux/tests/vcdev_test/module.mk diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.cpp b/src/platforms/linux/tests/vcdev_test/vcdevtest_example.cpp similarity index 100% rename from src/platforms/linux/vcdev_test/vcdevtest_example.cpp rename to src/platforms/linux/tests/vcdev_test/vcdevtest_example.cpp diff --git a/src/platforms/linux/vcdev_test/vcdevtest_example.h b/src/platforms/linux/tests/vcdev_test/vcdevtest_example.h similarity index 100% rename from src/platforms/linux/vcdev_test/vcdevtest_example.h rename to src/platforms/linux/tests/vcdev_test/vcdevtest_example.h diff --git a/src/platforms/linux/vcdev_test/vcdevtest_main.cpp b/src/platforms/linux/tests/vcdev_test/vcdevtest_main.cpp similarity index 100% rename from src/platforms/linux/vcdev_test/vcdevtest_main.cpp rename to src/platforms/linux/tests/vcdev_test/vcdevtest_main.cpp diff --git a/src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp b/src/platforms/linux/tests/vcdev_test/vcdevtest_start_linux.cpp similarity index 100% rename from src/platforms/linux/vcdev_test/vcdevtest_start_linux.cpp rename to src/platforms/linux/tests/vcdev_test/vcdevtest_start_linux.cpp diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index ea56c3425c..037ec16228 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -55,8 +55,8 @@ #define PX4_DEVIOCSPUBBLOCK 3 #define PX4_DEVIOCGPUBBLOCK 4 -//#define PX4_DEBUG(...) -#define PX4_DEBUG(...) printf(__VA_ARGS__) +#define PX4_DEBUG(...) +//#define PX4_DEBUG(...) printf(__VA_ARGS__) __BEGIN_DECLS From 4a6bd4ddc0921ec36c51cc0cf9f5dea76ae56dad Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Mar 2015 10:10:48 -0700 Subject: [PATCH 033/258] Linux: replaced getopt in ms5611_linux.cpp ms5611 uses getopt to parse args but the static variable optind was not being properly updated. Replaced use of external getopt call with simple parser; Signed-off-by: Mark Charlebois --- src/drivers/ms5611/ms5611_linux.cpp | 169 +++++++++++++++++++--------- 1 file changed, 114 insertions(+), 55 deletions(-) diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 2e872f7b6e..7f020c6c35 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -51,7 +51,6 @@ #include #include #include -#include #include #include @@ -814,11 +813,11 @@ struct ms5611_bus_option { bool start_bus(struct ms5611_bus_option &bus); struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); -void start(enum MS5611_BUS busid); -void test(enum MS5611_BUS busid); -void reset(enum MS5611_BUS busid); -void info(); -void calibrate(unsigned altitude, enum MS5611_BUS busid); +int start(enum MS5611_BUS busid); +int test(enum MS5611_BUS busid); +int reset(enum MS5611_BUS busid); +int info(); +int calibrate(unsigned altitude, enum MS5611_BUS busid); void usage(); /** @@ -918,7 +917,7 @@ start_bus(struct ms5611_bus_option &bus) * This function call only returns once the driver * is either successfully up and running or failed to start. */ -void +int start(enum MS5611_BUS busid) { uint8_t i; @@ -936,10 +935,13 @@ start(enum MS5611_BUS busid) started |= start_bus(bus_options[i]); } - if (!started) - errx(1, "driver start failed"); + if (!started) { + warnx("driver start failed"); + return 1; + } // one or more drivers started OK + return 0; } @@ -964,7 +966,7 @@ struct ms5611_bus_option &find_bus(enum MS5611_BUS busid) * make sure we can collect data from the sensor in polled * and automatic modes. */ -void +int test(enum MS5611_BUS busid) { struct ms5611_bus_option &bus = find_bus(busid); @@ -975,14 +977,18 @@ test(enum MS5611_BUS busid) int fd; fd = open(bus.devpath, O_RDONLY); - if (fd < 0) - err(1, "open failed (try 'ms5611 start' if the driver is not running)"); + if (fd < 0) { + warn("open failed (try 'ms5611 start' if the driver is not running)"); + return 1; + } /* do a simple demand read */ sz = px4_read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - err(1, "immediate read failed"); + if (sz != sizeof(report)) { + warn("immediate read failed"); + return 1; + } warnx("single read"); warnx("pressure: %10.4f", (double)report.pressure); @@ -991,12 +997,16 @@ test(enum MS5611_BUS busid) warnx("time: %lld", (long long)report.timestamp); /* set the queue depth to 10 */ - if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); + if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + warnx("failed to set queue depth"); + return 1; + } /* start the sensor polling at 2Hz */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + warnx("failed to set 2Hz poll rate"); + return 1; + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -1007,14 +1017,17 @@ test(enum MS5611_BUS busid) fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) - errx(1, "timed out waiting for sensor data"); + if (ret != 1) { + warnx("timed out waiting for sensor data"); + } /* now go get it */ sz = px4_read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - err(1, "periodic read failed"); + if (sz != sizeof(report)) { + warn("periodic read failed"); + return 1; + } warnx("periodic read %u", i); warnx("pressure: %10.4f", (double)report.pressure); @@ -1025,32 +1038,40 @@ test(enum MS5611_BUS busid) close(fd); warnx("PASS"); + return 0; } /** * Reset the driver. */ -void +int reset(enum MS5611_BUS busid) { struct ms5611_bus_option &bus = find_bus(busid); int fd; fd = open(bus.devpath, O_RDONLY); - if (fd < 0) - err(1, "failed "); + if (fd < 0) { + warn("failed "); + return 1; + } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + warn("driver reset failed"); + return 1; + } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warn("driver poll restart failed"); + return 1; + } + return 0; } /** * Print a little info about the driver. */ -void +int info() { for (uint8_t i=0; iprint_info(); } } + return 0; } /** * Calculate actual MSL pressure given current altitude */ -void +int calibrate(unsigned altitude, enum MS5611_BUS busid) { struct ms5611_bus_option &bus = find_bus(busid); @@ -1077,12 +1099,16 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) fd = open(bus.devpath, O_RDONLY); - if (fd < 0) - err(1, "open failed (try 'ms5611 start' if the driver is not running)"); + if (fd < 0) { + warn("open failed (try 'ms5611 start' if the driver is not running)"); + return 1; + } /* start the sensor polling at max */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) - errx(1, "failed to set poll rate"); + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { + warnx("failed to set poll rate"); + return 1; + } /* average a few measurements */ pressure = 0.0f; @@ -1097,14 +1123,18 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) fds.events = POLLIN; ret = poll(&fds, 1, 1000); - if (ret != 1) - errx(1, "timed out waiting for sensor data"); + if (ret != 1) { + warnx("timed out waiting for sensor data"); + return 1; + } /* now go get it */ sz = px4_read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - err(1, "sensor read failed"); + if (sz != sizeof(report)) { + warn("sensor read failed"); + return 1; + } pressure += report.pressure; } @@ -1127,10 +1157,13 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) /* save as integer Pa */ p1 *= 1000.0f; - if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) - err(1, "BAROIOCSMSLPRESSURE"); + if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { + warn("BAROIOCSMSLPRESSURE"); + return 1; + } close(fd); + return 0; } void @@ -1146,14 +1179,37 @@ usage() } // namespace +static int getopt(int argc, char *argv[], const char *options, int *myoptind) +{ + char *p = argv[*myoptind]; + int idx = 0; + if (p && options && myoptind && p[0] == '-') { + while (options[idx] != 0 && p[1] != options[idx]) + ++idx; + if (options[idx] == 0) + return (int)'?'; + *myoptind += 1; + return options[idx]; + } + return -1; +} + int ms5611_main(int argc, char *argv[]) { enum MS5611_BUS busid = MS5611_BUS_ALL; int ch; + int ret; + + if (argc < 2) { + ms5611::usage(); + return 1; + } /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XISs")) != EOF) { + int myoptind = 1; + while ((ch = getopt(argc, argv, "XISs", &myoptind)) != EOF) { + printf("ch = %d\n", ch); switch (ch) { case 'X': busid = MS5611_BUS_I2C_EXTERNAL; @@ -1173,44 +1229,47 @@ ms5611_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + const char *verb = argv[myoptind]; /* * Start/load the driver. */ if (!strcmp(verb, "start")) - ms5611::start(busid); + ret = ms5611::start(busid); /* * Test the driver/device. */ - if (!strcmp(verb, "test")) - ms5611::test(busid); + else if (!strcmp(verb, "test")) + ret = ms5611::test(busid); /* * Reset the driver. */ - if (!strcmp(verb, "reset")) - ms5611::reset(busid); + else if (!strcmp(verb, "reset")) + ret = ms5611::reset(busid); /* * Print driver information. */ - if (!strcmp(verb, "info")) - ms5611::info(); + else if (!strcmp(verb, "info")) + ret = ms5611::info(); /* * Perform MSL pressure calibration given an altitude in metres */ - if (!strcmp(verb, "calibrate")) { + else if (!strcmp(verb, "calibrate")) { if (argc < 2) errx(1, "missing altitude"); long altitude = strtol(argv[optind+1], nullptr, 10); - ms5611::calibrate(altitude, busid); + ret = ms5611::calibrate(altitude, busid); } - - warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); - return 1; + else { + ms5611::usage(); + warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); + return 1; + } + return ret; } From 598b05fcbace33ec8e4b9d5b46f0136bf693339d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Mar 2015 10:23:14 -0700 Subject: [PATCH 034/258] Linux: run threads without SCHED_FIFO if not privileged When running the process without sufficient privilege to use real time scheduling, warn the user and run with SCHED_OTHER. Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/px4_linux_tasks.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index fa832a168a..5b2c5abe33 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -153,7 +153,12 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_create (&task, &attr, (void *)&entry_adapter, (void *) taskdata); if (rv != 0) { - printf("px4_task_spawn_cmd: failed to create thread\n"); + printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + + if (rv == EPERM) { + printf("WARNING: INSUFFICIENT PRIVILEGE TO RUN REALTIME THREADS\n"); + rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata); + } return (rv < 0) ? rv : -rv; } From 2feeecdab1116b3ef09a7a903bfefb8c5d604d81 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Mar 2015 19:35:21 -0700 Subject: [PATCH 035/258] Linux: Added config and stubs to compile I2C device for Linux Not yet functional. Full implementation will provide an IOCTL interface to do bi-directional transfer. will model the interface after Linux. Signed-off-by: Mark Charlebois --- src/drivers/device/i2c.h | 128 +----- src/drivers/device/i2c_linux.cpp | 3 +- src/drivers/device/i2c_linux.h | 152 +++++++ src/drivers/device/i2c_nuttx.h | 153 +++++++ src/drivers/ms5611/ms5611_linux.cpp | 1 + src/modules/mavlink/mavlink_main.h | 375 ------------------ src/modules/mavlink/mavlink_main_linux.cpp | 3 +- .../linux/tests/hrt_test/hrt_test.cpp | 8 +- src/platforms/px4_config.h | 1 + 9 files changed, 321 insertions(+), 503 deletions(-) create mode 100644 src/drivers/device/i2c_linux.h create mode 100644 src/drivers/device/i2c_nuttx.h diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 97ab25672c..fb0be9bd5d 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,124 +30,10 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#pragma once -/** - * @file i2c.h - * - * Base class for devices connected via I2C. - */ - -#ifndef _DEVICE_I2C_H -#define _DEVICE_I2C_H - -#include "device.h" - -#include - -namespace device __EXPORT -{ - -/** - * Abstract class for character device on I2C - */ -class __EXPORT I2C : public CDev -{ - -public: - - /** - * Get the address - */ - int16_t get_address() const { return _address; } - - static int set_bus_clock(unsigned bus, unsigned clock_hz); - - static unsigned int _bus_clocks[3]; - -protected: - /** - * The number of times a read or write operation will be retried on - * error. - */ - unsigned _retries; - - /** - * The I2C bus number the device is attached to. - */ - int _bus; - - /** - * @ Constructor - * - * @param name Driver name - * @param devname Device node name - * @param bus I2C bus on which the device lives - * @param address I2C bus address, or zero if set_address will be used - * @param frequency I2C bus frequency for the device (currently not used) - * @param irq Interrupt assigned to the device (or zero if none) - */ - I2C(const char *name, - const char *devname, - int bus, - uint16_t address, - uint32_t frequency, - int irq = 0); - virtual ~I2C(); - - virtual int init(); - - /** - * Check for the presence of the device on the bus. - */ - virtual int probe(); - - /** - * Perform an I2C transaction to the device. - * - * At least one of send_len and recv_len must be non-zero. - * - * @param send Pointer to bytes to send. - * @param send_len Number of bytes to send. - * @param recv Pointer to buffer for bytes received. - * @param recv_len Number of bytes to receive. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); - - /** - * Perform a multi-part I2C transaction to the device. - * - * @param msgv An I2C message vector. - * @param msgs The number of entries in the message vector. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int transfer(px4_i2c_msg_t *msgv, unsigned msgs); - - /** - * Change the bus address. - * - * Most often useful during probe() when the driver is testing - * several possible bus addresses. - * - * @param address The new bus address to set. - */ - void set_address(uint16_t address) { - _address = address; - _device_id.devid_s.address = _address; - } - -private: - uint16_t _address; - uint32_t _frequency; - px4_i2c_dev_t *_dev; - - I2C(const device::I2C &); - I2C operator=(const device::I2C &); -}; - -} // namespace device - -#endif /* _DEVICE_I2C_H */ +#ifdef __PX4_NUTTX +#include "i2c_nuttx.h" +#else +#include "i2c_linux.h" +#endif diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 4ab3ff1723..52f336b3a5 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -51,8 +51,7 @@ I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, - uint32_t frequency, - int irq) : + uint32_t frequency) : // base class CDev(name, devname), // public diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h new file mode 100644 index 0000000000..3de6c96586 --- /dev/null +++ b/src/drivers/device/i2c_linux.h @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file i2c.h + * + * Base class for devices connected via I2C. + */ + +#ifndef _DEVICE_I2C_H +#define _DEVICE_I2C_H + +#include "device.h" + +#include + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on I2C + */ +class __EXPORT I2C : public CDev +{ + +public: + + /** + * Get the address + */ + int16_t get_address() const { return _address; } + + static int set_bus_clock(unsigned bus, unsigned clock_hz); + + static unsigned int _bus_clocks[3]; + +protected: + /** + * The number of times a read or write operation will be retried on + * error. + */ + unsigned _retries; + + /** + * The I2C bus number the device is attached to. + */ + int _bus; + + /** + * @ Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus I2C bus on which the device lives + * @param address I2C bus address, or zero if set_address will be used + * @param frequency I2C bus frequency for the device (currently not used) + * @param irq Interrupt assigned to the device (or zero if none) + */ + I2C(const char *name, + const char *devname, + int bus, + uint16_t address, + uint32_t frequency); + virtual ~I2C(); + + virtual int init(); + + /** + * Check for the presence of the device on the bus. + */ + virtual int probe(); + + /** + * Perform an I2C transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + + /** + * Perform a multi-part I2C transaction to the device. + * + * @param msgv An I2C message vector. + * @param msgs The number of entries in the message vector. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(px4_i2c_msg_t *msgv, unsigned msgs); + + /** + * Change the bus address. + * + * Most often useful during probe() when the driver is testing + * several possible bus addresses. + * + * @param address The new bus address to set. + */ + void set_address(uint16_t address) { + _address = address; + _device_id.devid_s.address = _address; + } + +private: + uint16_t _address; + uint32_t _frequency; + px4_i2c_dev_t *_dev; + + I2C(const device::I2C&); + I2C operator=(const device::I2C&); +}; + +} // namespace device + +#endif /* _DEVICE_I2C_H */ diff --git a/src/drivers/device/i2c_nuttx.h b/src/drivers/device/i2c_nuttx.h new file mode 100644 index 0000000000..97ab25672c --- /dev/null +++ b/src/drivers/device/i2c_nuttx.h @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file i2c.h + * + * Base class for devices connected via I2C. + */ + +#ifndef _DEVICE_I2C_H +#define _DEVICE_I2C_H + +#include "device.h" + +#include + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on I2C + */ +class __EXPORT I2C : public CDev +{ + +public: + + /** + * Get the address + */ + int16_t get_address() const { return _address; } + + static int set_bus_clock(unsigned bus, unsigned clock_hz); + + static unsigned int _bus_clocks[3]; + +protected: + /** + * The number of times a read or write operation will be retried on + * error. + */ + unsigned _retries; + + /** + * The I2C bus number the device is attached to. + */ + int _bus; + + /** + * @ Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus I2C bus on which the device lives + * @param address I2C bus address, or zero if set_address will be used + * @param frequency I2C bus frequency for the device (currently not used) + * @param irq Interrupt assigned to the device (or zero if none) + */ + I2C(const char *name, + const char *devname, + int bus, + uint16_t address, + uint32_t frequency, + int irq = 0); + virtual ~I2C(); + + virtual int init(); + + /** + * Check for the presence of the device on the bus. + */ + virtual int probe(); + + /** + * Perform an I2C transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + + /** + * Perform a multi-part I2C transaction to the device. + * + * @param msgv An I2C message vector. + * @param msgs The number of entries in the message vector. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(px4_i2c_msg_t *msgv, unsigned msgs); + + /** + * Change the bus address. + * + * Most often useful during probe() when the driver is testing + * several possible bus addresses. + * + * @param address The new bus address to set. + */ + void set_address(uint16_t address) { + _address = address; + _device_id.devid_s.address = _address; + } + +private: + uint16_t _address; + uint32_t _frequency; + px4_i2c_dev_t *_dev; + + I2C(const device::I2C &); + I2C operator=(const device::I2C &); +}; + +} // namespace device + +#endif /* _DEVICE_I2C_H */ diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 7f020c6c35..e914dfaec6 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -890,6 +890,7 @@ start_bus(struct ms5611_bus_option &bus) if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; + warnx("bus init failed %p", bus.dev); return false; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 216abe954e..3153255b33 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -32,383 +32,8 @@ ****************************************************************************/ #pragma once -<<<<<<< HEAD -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "mavlink_bridge_header.h" -#include "mavlink_orb_subscription.h" -#include "mavlink_stream.h" -#include "mavlink_messages.h" -#include "mavlink_mission.h" -#include "mavlink_parameters.h" - -class Mavlink -{ - -public: - /** - * Constructor - */ - Mavlink(); - - /** - * Destructor, also kills the mavlinks task. - */ - ~Mavlink(); - - /** - * Start the mavlink task. - * - * @return OK on success. - */ - static int start(int argc, char *argv[]); - - /** - * Display the mavlink status. - */ - void display_status(); - - static int stream_command(int argc, char *argv[]); - - static int instance_count(); - - static Mavlink *new_instance(); - - static Mavlink *get_instance(unsigned instance); - - static Mavlink *get_instance_for_device(const char *device_name); - - static int destroy_all_instances(); - - static int get_status_all_instances(); - - static bool instance_exists(const char *device_name, Mavlink *self); - - static void forward_message(const mavlink_message_t *msg, Mavlink *self); - - static int get_uart_fd(unsigned index); - - int get_uart_fd(); - - /** - * Get the MAVLink system id. - * - * @return The system ID of this vehicle - */ - int get_system_id(); - - /** - * Get the MAVLink component id. - * - * @return The component ID of this vehicle - */ - int get_component_id(); - - const char *_device_name; - - enum MAVLINK_MODE { - MAVLINK_MODE_NORMAL = 0, - MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD - }; - - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } - - bool get_hil_enabled() { return _hil_enabled; } - - bool get_use_hil_gps() { return _use_hil_gps; } - - bool get_forward_externalsp() { return _forward_externalsp; } - - bool get_flow_control_enabled() { return _flow_control_enabled; } - - bool get_forwarding_on() { return _forwarding_on; } - - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - - static int start_helper(int argc, char *argv[]); - - /** - * Handle parameter related messages. - */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); - - /** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ - int set_hil_enabled(bool hil_enabled); - - void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); - - /** - * Resend message as is, don't change sequence number and CRC. - */ - void resend_message(mavlink_message_t *msg); - - void handle_message(const mavlink_message_t *msg); - - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); - - int get_instance_id(); - - /** - * Enable / disable hardware flow control. - * - * @param enabled True if hardware flow control should be enabled - */ - int enable_flow_control(bool enabled); - - mavlink_channel_t get_channel(); - - void configure_stream_threadsafe(const char *stream_name, float rate); - - bool _task_should_exit; /**< if true, mavlink task should exit */ - - int get_mavlink_fd() { return _mavlink_fd; } - - /** - * Send a status text with loglevel INFO - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_info(const char *string); - - /** - * Send a status text with loglevel CRITICAL - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_critical(const char *string); - - /** - * Send a status text with loglevel EMERGENCY - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_emergency(const char *string); - - /** - * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent - * only on this mavlink connection. Useful for reporting communication specific, not system-wide info - * only to client interested in it. Message will be not sent immediately but queued in buffer as - * for mavlink_log_xxx(). - * - * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level - */ - void send_statustext(unsigned char severity, const char *string); - - MavlinkStream * get_streams() const { return _streams; } - - float get_rate_mult(); - - /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - - bool message_buffer_write(const void *ptr, int size); - - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } - - /** - * Count a transmision error - */ - void count_txerr(); - - /** - * Count transmitted bytes - */ - void count_txbytes(unsigned n) { _bytes_tx += n; }; - - /** - * Count bytes not transmitted because of errors - */ - void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; - - /** - * Count received bytes - */ - void count_rxbytes(unsigned n) { _bytes_rx += n; }; - - /** - * Get the receive status of this MAVLink link - */ - struct telemetry_status_s& get_rx_status() { return _rstatus; } - - struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } - - unsigned get_system_type() { return _system_type; } - -protected: - Mavlink *next; - -private: - int _instance_id; - - int _mavlink_fd; - bool _task_running; - - /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ - bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ - bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ - - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; - - MavlinkMissionManager *_mission_manager; - MavlinkParametersManager *_parameters_manager; - - MAVLINK_MODE _mode; - - mavlink_channel_t _channel; - - struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; - - pthread_t _receive_thread; - - bool _verbose; - bool _forwarding_on; - bool _passing_on; - bool _ftp_on; - int _uart_fd; - int _baudrate; - int _datarate; ///< data rate for normal streams (attitude, position, etc.) - int _datarate_events; ///< data rate for params, waypoints, text messages - float _rate_mult; - - /** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ - unsigned int _mavlink_param_queue_index; - - bool mavlink_link_termination_allowed; - - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; - - bool _flow_control_enabled; - uint64_t _last_write_success_time; - uint64_t _last_write_try_time; - - unsigned _bytes_tx; - unsigned _bytes_txerr; - unsigned _bytes_rx; - uint64_t _bytes_timestamp; - float _rate_tx; - float _rate_txerr; - float _rate_rx; - - struct telemetry_status_s _rstatus; ///< receive status - - struct mavlink_message_buffer { - int write_ptr; - int read_ptr; - int size; - char *data; - }; - - mavlink_message_buffer _message_buffer; - - pthread_mutex_t _message_buffer_mutex; - pthread_mutex_t _send_mutex; - - bool _param_initialized; - param_t _param_system_id; - param_t _param_component_id; - param_t _param_system_type; - param_t _param_use_hil_gps; - param_t _param_forward_externalsp; - - unsigned _system_type; - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _txerr_perf; /**< TX error counter */ - - void mavlink_update_system(); - - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - static unsigned int interval_from_rate(float rate); - - int configure_stream(const char *stream_name, const float rate); - - /** - * Adjust the stream rates based on the current rate - * - * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease - */ - void adjust_stream_rates(const float multiplier); - - int message_buffer_init(int size); - - void message_buffer_destroy(); - - int message_buffer_count(); - - int message_buffer_is_empty(); - - int message_buffer_get_ptr(void **ptr, bool *is_part); - - void message_buffer_mark_read(int n); - - void pass_message(const mavlink_message_t *msg); - - /** - * Update rate mult so total bitrate will be equal to _datarate. - */ - void update_rate_mult(); - - static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - - /** - * Main mavlink task. - */ - int task_main(int argc, char *argv[]); - - /* do not allow copying this class */ - Mavlink(const Mavlink&); - Mavlink operator=(const Mavlink&); -}; -======= #ifdef __PX4_NUTTX #include "mavlink_main_nuttx.h" #else #include "mavlink_main_linux.h" #endif ->>>>>>> Support for building more modules with Linux diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index f2d8ea90f9..694a9044d4 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -244,7 +244,8 @@ Mavlink::~Mavlink() } while (_task_running); } - LL_DELETE(_mavlink_instances, this); + if (_mavlink_instances) + LL_DELETE(_mavlink_instances, this); } void diff --git a/src/platforms/linux/tests/hrt_test/hrt_test.cpp b/src/platforms/linux/tests/hrt_test/hrt_test.cpp index d5e446bcc9..5677f4e1b2 100644 --- a/src/platforms/linux/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/linux/tests/hrt_test/hrt_test.cpp @@ -53,14 +53,14 @@ int HRTTest::main() hrt_abstime t = hrt_absolute_time(); usleep(1000000); hrt_abstime elt = hrt_elapsed_time(&t); - printf("Elapsed time %llu in 1 sec (usleep)\n", elt); - printf("Start time %llu\n", t); + printf("Elapsed time %lu in 1 sec (usleep)\n", elt); + printf("Start time %lu\n", t); t = hrt_absolute_time(); sleep(1); elt = hrt_elapsed_time(&t); - printf("Elapsed time %llu in 1 sec (sleep)\n", elt); - printf("Start time %llu\n", t); + printf("Elapsed time %lu in 1 sec (sleep)\n", elt); + printf("Start time %lu\n", t); return 0; } diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index c53fd63d49..a7b9e55000 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -44,6 +44,7 @@ #include #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 +#define PX4_I2C_BUS_ONBOARD 1 #define px4_errx(x, ...) errx(x, __VA_ARGS__) From 11e971eaa09d3a1aabe6fe70e28a5a64f9002931 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Mar 2015 20:25:39 -0700 Subject: [PATCH 036/258] Linux: min gcc version is 4.8.1 PX4 will not build with gcc-4.6. If you are running Ubuntu 12.04 still (you poor old sod) follow the directions at http://ubuntuhandbook.org/index.php/2013/08/install-gcc-4-8-via-ppa-in-ubuntu-12-04-13-04/ to install gcc-4.8.1 Alternatively you can install clang 3.4.1 for Ubuntu 12.04 from http://llvm.org/releases/3.4.1/clang+llvm-3.4.1-x86_64-unknown-ubuntu12.04.tar.xz Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 0d26d95de6..db51d7d7d8 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -62,7 +62,7 @@ CLANGVER= endif endif -USE_GCC=1 +#USE_GCC=1 ifeq ($(USE_GCC),1) # GCC Options: @@ -71,7 +71,7 @@ CXX = g++ CPP = gcc -E # GCC Version -DEV_VER_SUPPORTED = 4.6 4.8.2 4.9.1 +DEV_VER_SUPPORTED = 4.8.1 4.8.2 4.9.1 else # Clang options From 1cdc44828df669ea95ae9ee74ab70d63b4da6c15 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Mar 2015 20:35:07 -0700 Subject: [PATCH 037/258] Linux - revert to preferentially use clang over gcc Removed hardcoded requirement to use gcc Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 1 - 1 file changed, 1 deletion(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index db51d7d7d8..785d0187e3 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -48,7 +48,6 @@ HAVE_CLANG35=$(shell clang-3.5 -dumpversion) # Clang will report 4.2.1 as GCC version HAVE_CLANG=$(shell clang -dumpversion) -USE_GCC=1 #If using ubuntu 14.04 and packaged clang 4.2.1 ifeq ($(HAVE_CLANG35),4.2.1) USE_GCC=0 From 90818363eb262b9a4ceaaa76029991f0e044a48b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 16 Mar 2015 21:13:54 -0700 Subject: [PATCH 038/258] Linux: fix for undefined BO in mavlink_main_linux.cpp if termios.h is included before mathlib.h then BO is undefined. Since mathlib.h is not needed it was removed but I still don't know why this error occurs. Also added -lrt to link flags for clock_gettime Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 2 +- src/modules/mavlink/mavlink_main_linux.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 785d0187e3..d983fd24e3 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -193,7 +193,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) #EXTRA_LIBS += $(LIBM) -EXTRA_LIBS += -pthread -lm +EXTRA_LIBS += -pthread -lm -lrt # Flags we pass to the C compiler # diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index 694a9044d4..a6b49ee25f 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include From cd30b4d5caf717d77b5f5d30ba696408a6062387 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 17 Mar 2015 00:46:25 -0700 Subject: [PATCH 039/258] Linux: added I2C class Signed-off-by: Mark Charlebois --- src/drivers/blinkm/module.mk | 6 +- src/drivers/device/i2c_linux.cpp | 151 ++++++++++-------------------- src/drivers/device/i2c_linux.h | 21 +++-- src/drivers/ms5611/ms5611_i2c.cpp | 12 ++- 4 files changed, 76 insertions(+), 114 deletions(-) diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk index c906733175..712f266317 100644 --- a/src/drivers/blinkm/module.mk +++ b/src/drivers/blinkm/module.mk @@ -37,6 +37,10 @@ MODULE_COMMAND = blinkm -SRCS = blinkm.cpp +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = blinkm_nuttx.cpp +else +SRCS = blinkm_linux.cpp +endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 52f336b3a5..1a3492c8f2 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -41,17 +41,19 @@ */ #include "i2c.h" +#include +#include +#include +#include +#include namespace device { -unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 }; - I2C::I2C(const char *name, const char *devname, int bus, - uint16_t address, - uint32_t frequency) : + uint16_t address) : // base class CDev(name, devname), // public @@ -60,8 +62,8 @@ I2C::I2C(const char *name, // private _bus(bus), _address(address), - _frequency(frequency), - _dev(nullptr) + _fd(-1), + _dname(devname) { // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; @@ -73,162 +75,98 @@ I2C::I2C(const char *name, I2C::~I2C() { - if (_dev) { - px4_i2cuninitialize(_dev); - _dev = nullptr; + if (_fd >= 0) { + ::close(_fd); + _fd = -1; } } -int -I2C::set_bus_clock(unsigned bus, unsigned clock_hz) -{ - int index = bus - 1; - - if (index < 0 || index >= static_cast(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) { - return -EINVAL; - } - - if (_bus_clocks[index] > 0) { - // debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); - } - _bus_clocks[index] = clock_hz; - - return PX4_OK; -} - int I2C::init() { int ret = PX4_OK; - unsigned bus_index; - // attach to the i2c bus - _dev = px4_i2cinitialize(_bus); - - if (_dev == nullptr) { - debug("failed to init I2C"); - ret = -ENOENT; - goto out; - } - - // the above call fails for a non-existing bus index, - // so the index math here is safe. - bus_index = _bus - 1; - - // abort if the max frequency we allow (the frequency we ask) - // is smaller than the bus frequency - if (_bus_clocks[bus_index] > _frequency) { - (void)px4_i2cuninitialize(_dev); - _dev = nullptr; - log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", - _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); - ret = -EINVAL; - goto out; - } - - // set the bus frequency on the first access if it has - // not been set yet - if (_bus_clocks[bus_index] == 0) { - _bus_clocks[bus_index] = _frequency; - } - - // set frequency for this instance once to the bus speed - // the bus speed is the maximum supported by all devices on the bus, - // as we have to prioritize performance over compatibility. - // If a new device requires a lower clock speed, this has to be - // manually set via "fmu i2c " before starting any - // drivers. - // This is necessary as automatically lowering the bus speed - // for maximum compatibility could induce timing issues on - // critical sensors the adopter might be unaware of. - I2C_SETFREQUENCY(_dev, _bus_clocks[bus_index]); - - // call the probe function to check whether the device is present - ret = probe(); - - if (ret != PX4_OK) { - debug("probe failed"); - goto out; - } + // Assume the driver set the desired bus frequency. There is no standard + // way to set it from user space. // do base class init, which will create device node, etc ret = CDev::init(); if (ret != PX4_OK) { debug("cdev init failed"); - goto out; + return ret; } - // tell the world where we are - log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", - _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + _fd = ::open(_dname.c_str(), O_RDWR); + if (_fd < 0) { + warnx("could not open %s", _dname.c_str()); + return -errno; + } -out: - if ((ret != PX4_OK) && (_dev != nullptr)) { - px4_i2cuninitialize(_dev); - _dev = nullptr; - } return ret; } -int -I2C::probe() -{ - // Assume the device is too stupid to be discoverable. - return PX4_OK; -} - int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - px4_i2c_msg_t msgv[2]; + struct i2c_msg msgv[2]; unsigned msgs; + struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; do { // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - msgs = 0; if (send_len > 0) { msgv[msgs].addr = _address; msgv[msgs].flags = 0; - msgv[msgs].buffer = const_cast(send); - msgv[msgs].length = send_len; + msgv[msgs].buf = const_cast(send); + msgv[msgs].len = send_len; msgs++; } if (recv_len > 0) { msgv[msgs].addr = _address; msgv[msgs].flags = I2C_M_READ; - msgv[msgs].buffer = recv; - msgv[msgs].length = recv_len; + msgv[msgs].buf = recv; + msgv[msgs].len = recv_len; msgs++; } if (msgs == 0) return -EINVAL; - ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + packets.msgs = msgv; + packets.nmsgs = msgs; + + ret = ::ioctl(_fd, I2C_RDWR, &packets); + if (ret < 0) { + warnx("I2C transfer failed"); + return 1; + } /* success */ if (ret == PX4_OK) break; +// No way to reset device from userspace +#if 0 /* if we have already retried once, or we are going to give up, then reset the bus */ if ((retry_count >= 1) || (retry_count >= _retries)) px4_i2creset(_dev); +#endif } while (retry_count++ < _retries); return ret; - } int -I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs) +I2C::transfer(struct i2c_msg *msgv, unsigned msgs) { + struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; @@ -236,17 +174,26 @@ I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs) for (unsigned i = 0; i < msgs; i++) msgv[i].addr = _address; - do { - ret = I2C_TRANSFER(_dev, msgv, msgs); + packets.msgs = msgv; + packets.nmsgs = msgs; + + ret = ::ioctl(_fd, I2C_RDWR, &packets); + if (ret < 0) { + warnx("I2C transfer failed"); + return 1; + } /* success */ if (ret == PX4_OK) break; +// No way to reset device from userspace +#if 0 /* if we have already retried once, or we are going to give up, then reset the bus */ if ((retry_count >= 1) || (retry_count >= _retries)) px4_i2creset(_dev); +#endif } while (retry_count++ < _retries); diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h index 3de6c96586..56eaea4b96 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_linux.h @@ -43,6 +43,9 @@ #include "device.h" #include +#include +#include +#include namespace device __EXPORT { @@ -60,10 +63,6 @@ public: */ int16_t get_address() const { return _address; } - static int set_bus_clock(unsigned bus, unsigned clock_hz); - - static unsigned int _bus_clocks[3]; - protected: /** * The number of times a read or write operation will be retried on @@ -89,16 +88,16 @@ protected: I2C(const char *name, const char *devname, int bus, - uint16_t address, - uint32_t frequency); + uint16_t address); virtual ~I2C(); virtual int init(); - +#if 0 /** * Check for the presence of the device on the bus. */ virtual int probe(); +#endif /** * Perform an I2C transaction to the device. @@ -123,8 +122,9 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(px4_i2c_msg_t *msgv, unsigned msgs); + int transfer(struct i2c_msg *msgv, unsigned msgs); +#if 0 /** * Change the bus address. * @@ -137,11 +137,12 @@ protected: _address = address; _device_id.devid_s.address = _address; } +#endif private: uint16_t _address; - uint32_t _frequency; - px4_i2c_dev_t *_dev; + int _fd; + std::string _dname; I2C(const device::I2C&); I2C operator=(const device::I2C&); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 733fafbd87..dd67bc5d13 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -74,13 +74,17 @@ public: virtual int dev_read(unsigned offset, void *data, unsigned count); virtual int dev_ioctl(unsigned operation, unsigned &arg); +#ifdef __PX4_NUTTX protected: virtual int probe(); +#endif private: ms5611::prom_u &_prom; +#ifdef __PX4_NUTTX int _probe_address(uint8_t address); +#endif /** * Send a reset command to the MS5611. @@ -112,7 +116,11 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : - I2C("MS5611_I2C", nullptr, bus, 0, 400000), + I2C("MS5611_I2C", nullptr, bus, 0 +#ifdef __PX4_NUTTX +, 400000 +#endif +), _prom(prom) { } @@ -172,6 +180,7 @@ MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg) return ret; } +#ifdef __PX4_NUTTX int MS5611_I2C::probe() { @@ -206,6 +215,7 @@ MS5611_I2C::_probe_address(uint8_t address) return PX4_OK; } +#endif int From df53defca6887fc5889a5aa1fe56434b431fece8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 17 Mar 2015 12:03:54 -0700 Subject: [PATCH 040/258] Linux: Add support for blinkm to test I2C layering Running the blinkm device to test I2C Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 1 + src/drivers/blinkm/blinkm_linux.cpp | 1024 +++++++++++++++++ .../blinkm/{blinkm.cpp => blinkm_nuttx.cpp} | 0 src/drivers/drv_blinkm.h | 3 +- src/platforms/linux/include/board_config.h | 8 + src/platforms/px4_config.h | 1 - 6 files changed, 1035 insertions(+), 2 deletions(-) create mode 100644 src/drivers/blinkm/blinkm_linux.cpp rename src/drivers/blinkm/{blinkm.cpp => blinkm_nuttx.cpp} (100%) diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index f53cf93197..4ba4b7842b 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -11,6 +11,7 @@ # Board support modules # MODULES += drivers/device +MODULES += drivers/blinkm MODULES += modules/sensors MODULES += drivers/ms5611 diff --git a/src/drivers/blinkm/blinkm_linux.cpp b/src/drivers/blinkm/blinkm_linux.cpp new file mode 100644 index 0000000000..e38fb408e7 --- /dev/null +++ b/src/drivers/blinkm/blinkm_linux.cpp @@ -0,0 +1,1024 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file blinkm.cpp + * + * Driver for the BlinkM LED controller connected via I2C. + * + * Connect the BlinkM to I2C3 and put the following line to the rc startup-script: + * blinkm start + * + * To start the system monitor put in the next line after the blinkm start: + * blinkm systemmonitor + * + * + * Description: + * After startup, the Application checked how many lipo cells are connected to the System. + * The recognized number off cells, will be blinked 5 times in purple color. + * 2 Cells = 2 blinks + * ... + * 6 Cells = 6 blinks + * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. + * + * System disarmed and safe: + * The BlinkM should light solid cyan. + * + * System safety off but not armed: + * The BlinkM should light flashing orange + * + * System armed: + * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. + * + * X-X-X-X-_-_-_-_-_-_- + * ------------------------- + * G G G M + * P P P O + * S S S D + * E + * + * (X = on, _=off) + * + * The first 3 blinks indicates the status of the GPS-Signal (red): + * 0-4 satellites = X-X-X-X-X-_-_-_-_-_- + * 5 satellites = X-X-_-X-X-_-_-_-_-_- + * 6 satellites = X-_-_-X-X-_-_-_-_-_- + * >=7 satellites = _-_-_-X-X-_-_-_-_-_- + * If no GPS is found the first 3 blinks are white + * + * The fourth Blink indicates the Flightmode: + * MANUAL : amber + * STABILIZED : yellow + * HOLD : blue + * AUTO : green + * + * Battery Warning (low Battery Level): + * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X + * + * Battery Alert (critical Battery Level) + * Continuously blinking in red X-X-X-X-X-X-X-X-X-X + * + * General Error (no uOrb Data) + * Continuously blinking in white X-X-X-X-X-X-X-X-X-X + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +static const float MAX_CELL_VOLTAGE = 4.3f; +static const int LED_ONTIME = 120; +static const int LED_OFFTIME = 120; +static const int LED_BLINK = 1; +static const int LED_NOBLINK = 0; + +class BlinkM : public device::I2C +{ +public: + BlinkM(int bus, int blinkm); + virtual ~BlinkM(); + + + virtual int init(); + virtual int probe(); + virtual int setMode(int mode); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + static const char *const script_names[]; + +private: + enum ScriptID { + USER = 0, + RGB, + WHITE_FLASH, + RED_FLASH, + GREEN_FLASH, + BLUE_FLASH, + CYAN_FLASH, + MAGENTA_FLASH, + YELLOW_FLASH, + BLACK, + HUE_CYCLE, + MOOD_LIGHT, + VIRTUAL_CANDLE, + WATER_REFLECTIONS, + OLD_NEON, + THE_SEASONS, + THUNDERSTORM, + STOP_LIGHT, + MORSE_CODE + }; + + enum ledColors { + LED_OFF, + LED_RED, + LED_ORANGE, + LED_YELLOW, + LED_PURPLE, + LED_GREEN, + LED_BLUE, + LED_CYAN, + LED_WHITE, + LED_AMBER + }; + + work_s _work; + + int led_color_1; + int led_color_2; + int led_color_3; + int led_color_4; + int led_color_5; + int led_color_6; + int led_color_7; + int led_color_8; + int led_blink; + + bool systemstate_run; + + int vehicle_status_sub_fd; + int vehicle_control_mode_sub_fd; + int vehicle_gps_position_sub_fd; + int actuator_armed_sub_fd; + int safety_sub_fd; + + int num_of_cells; + int detected_cells_runcount; + + int t_led_color[8]; + int t_led_blink; + int led_thread_runcount; + int led_interval; + + bool topic_initialized; + bool detected_cells_blinked; + bool led_thread_ready; + + int num_of_used_sats; + + void setLEDColor(int ledcolor); + static void led_trampoline(void *arg); + void led(); + + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + + int fade_rgb(uint8_t r, uint8_t g, uint8_t b); + int fade_hsb(uint8_t h, uint8_t s, uint8_t b); + + int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b); + int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b); + + int set_fade_speed(uint8_t s); + + int play_script(uint8_t script_id); + int play_script(const char *script_name); + int stop_script(); + + int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3); + int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]); + int set_script(uint8_t length, uint8_t repeats); + + int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b); + + int get_firmware_version(uint8_t version[2]); +}; + +/* for now, we only support one BlinkM */ +namespace +{ + BlinkM *g_blinkm; +} + +/* list of script names, must match script ID numbers */ +const char *const BlinkM::script_names[] = { + "USER", + "RGB", + "WHITE_FLASH", + "RED_FLASH", + "GREEN_FLASH", + "BLUE_FLASH", + "CYAN_FLASH", + "MAGENTA_FLASH", + "YELLOW_FLASH", + "BLACK", + "HUE_CYCLE", + "MOOD_LIGHT", + "VIRTUAL_CANDLE", + "WATER_REFLECTIONS", + "OLD_NEON", + "THE_SEASONS", + "THUNDERSTORM", + "STOP_LIGHT", + "MORSE_CODE", + nullptr +}; + + +extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); + +BlinkM::BlinkM(int bus, int blinkm) : + I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm), + led_color_1(LED_OFF), + led_color_2(LED_OFF), + led_color_3(LED_OFF), + led_color_4(LED_OFF), + led_color_5(LED_OFF), + led_color_6(LED_OFF), + led_color_7(LED_OFF), + led_color_8(LED_OFF), + led_blink(LED_NOBLINK), + systemstate_run(false), + vehicle_status_sub_fd(-1), + vehicle_control_mode_sub_fd(-1), + vehicle_gps_position_sub_fd(-1), + actuator_armed_sub_fd(-1), + safety_sub_fd(-1), + num_of_cells(0), + detected_cells_runcount(0), + t_led_color{0}, + t_led_blink(0), + led_thread_runcount(0), + led_interval(1000), + topic_initialized(false), + detected_cells_blinked(false), + led_thread_ready(true), + num_of_used_sats(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +BlinkM::~BlinkM() +{ +} + +int +BlinkM::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + stop_script(); + set_rgb(0,0,0); + + return OK; +} + +int +BlinkM::setMode(int mode) +{ + if(mode == 1) { + if(systemstate_run == false) { + stop_script(); + set_rgb(0,0,0); + systemstate_run = true; + work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); + } + } else { + systemstate_run = false; + } + + return OK; +} + +int +BlinkM::probe() +{ + uint8_t version[2]; + int ret; + + ret = get_firmware_version(version); + + if (ret == OK) + log("found BlinkM firmware version %c%c", version[1], version[0]); + + return ret; +} + +int +BlinkM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case BLINKM_PLAY_SCRIPT_NAMED: + if (arg == 0) { + ret = EINVAL; + break; + } + ret = play_script((const char *)arg); + break; + + case BLINKM_PLAY_SCRIPT: + ret = play_script(arg); + break; + + case BLINKM_SET_USER_SCRIPT: { + if (arg == 0) { + ret = EINVAL; + break; + } + + unsigned lines = 0; + const uint8_t *script = (const uint8_t *)arg; + + while ((lines < 50) && (script[1] != 0)) { + ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); + if (ret != OK) + break; + script += 5; + } + if (ret == OK) + ret = set_script(lines, 0); + break; + } + + default: + break; + } + + return ret; +} + + +void +BlinkM::led_trampoline(void *arg) +{ + BlinkM *bm = (BlinkM *)arg; + + bm->led(); +} + + + +void +BlinkM::led() +{ + + if(!topic_initialized) { + vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(vehicle_status_sub_fd, 250); + + vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(vehicle_control_mode_sub_fd, 250); + + actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(actuator_armed_sub_fd, 250); + + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(vehicle_gps_position_sub_fd, 250); + + /* Subscribe to safety topic */ + safety_sub_fd = orb_subscribe(ORB_ID(safety)); + orb_set_interval(safety_sub_fd, 250); + + topic_initialized = true; + } + + if(led_thread_ready == true) { + if(!detected_cells_blinked) { + if(num_of_cells > 0) { + t_led_color[0] = LED_PURPLE; + } + if(num_of_cells > 1) { + t_led_color[1] = LED_PURPLE; + } + if(num_of_cells > 2) { + t_led_color[2] = LED_PURPLE; + } + if(num_of_cells > 3) { + t_led_color[3] = LED_PURPLE; + } + if(num_of_cells > 4) { + t_led_color[4] = LED_PURPLE; + } + if(num_of_cells > 5) { + t_led_color[5] = LED_PURPLE; + } + t_led_color[6] = LED_OFF; + t_led_color[7] = LED_OFF; + t_led_blink = LED_BLINK; + } else { + t_led_color[0] = led_color_1; + t_led_color[1] = led_color_2; + t_led_color[2] = led_color_3; + t_led_color[3] = led_color_4; + t_led_color[4] = led_color_5; + t_led_color[5] = led_color_6; + t_led_color[6] = led_color_7; + t_led_color[7] = led_color_8; + t_led_blink = led_blink; + } + led_thread_ready = false; + } + + if (led_thread_runcount & 1) { + if (t_led_blink) + setLEDColor(LED_OFF); + led_interval = LED_OFFTIME; + } else { + setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); + //led_interval = (led_thread_runcount & 1) : LED_ONTIME; + led_interval = LED_ONTIME; + } + + if (led_thread_runcount == 15) { + /* obtained data for the first file descriptor */ + struct vehicle_status_s vehicle_status_raw; + struct vehicle_control_mode_s vehicle_control_mode; + struct actuator_armed_s actuator_armed; + struct vehicle_gps_position_s vehicle_gps_position_raw; + struct safety_s safety; + + memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); + memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); + memset(&safety, 0, sizeof(safety)); + + bool new_data_vehicle_status; + bool new_data_vehicle_control_mode; + bool new_data_actuator_armed; + bool new_data_vehicle_gps_position; + bool new_data_safety; + + orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); + + int no_data_vehicle_status = 0; + int no_data_vehicle_control_mode = 0; + int no_data_actuator_armed = 0; + int no_data_vehicle_gps_position = 0; + + if (new_data_vehicle_status) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); + no_data_vehicle_status = 0; + } else { + no_data_vehicle_status++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + + orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); + + if (new_data_vehicle_control_mode) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); + no_data_vehicle_control_mode = 0; + } else { + no_data_vehicle_control_mode++; + if(no_data_vehicle_control_mode >= 3) + no_data_vehicle_control_mode = 3; + } + + orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); + + if (new_data_actuator_armed) { + orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); + no_data_actuator_armed = 0; + } else { + no_data_actuator_armed++; + if(no_data_actuator_armed >= 3) + no_data_actuator_armed = 3; + } + + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); + + if (new_data_vehicle_gps_position) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); + no_data_vehicle_gps_position = 0; + } else { + no_data_vehicle_gps_position++; + if(no_data_vehicle_gps_position >= 3) + no_data_vehicle_gps_position = 3; + } + + /* update safety topic */ + orb_check(safety_sub_fd, &new_data_safety); + + if (new_data_safety) { + orb_copy(ORB_ID(safety), safety_sub_fd, &safety); + } + + /* get number of used satellites in navigation */ + num_of_used_sats = vehicle_gps_position_raw.satellites_used; + + if (new_data_vehicle_status || no_data_vehicle_status < 3) { + if (num_of_cells == 0) { + /* looking for lipo cells that are connected */ + printf(" checking cells\n"); + for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { + if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; + } + printf(" cells found:%d\n", num_of_cells); + + } else { + if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + /* LED Pattern for battery critical alerting */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_RED; + led_color_5 = LED_RED; + led_color_6 = LED_RED; + led_color_7 = LED_RED; + led_color_8 = LED_RED; + led_blink = LED_BLINK; + + } else if(vehicle_status_raw.rc_signal_lost) { + /* LED Pattern for FAILSAFE */ + led_color_1 = LED_BLUE; + led_color_2 = LED_BLUE; + led_color_3 = LED_BLUE; + led_color_4 = LED_BLUE; + led_color_5 = LED_BLUE; + led_color_6 = LED_BLUE; + led_color_7 = LED_BLUE; + led_color_8 = LED_BLUE; + led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { + /* LED Pattern for battery low warning */ + led_color_1 = LED_YELLOW; + led_color_2 = LED_YELLOW; + led_color_3 = LED_YELLOW; + led_color_4 = LED_YELLOW; + led_color_5 = LED_YELLOW; + led_color_6 = LED_YELLOW; + led_color_7 = LED_YELLOW; + led_color_8 = LED_YELLOW; + led_blink = LED_BLINK; + + } else { + /* no battery warnings here */ + + if(actuator_armed.armed == false) { + /* system not armed */ + if(safety.safety_off){ + led_color_1 = LED_ORANGE; + led_color_2 = LED_ORANGE; + led_color_3 = LED_ORANGE; + led_color_4 = LED_ORANGE; + led_color_5 = LED_ORANGE; + led_color_6 = LED_ORANGE; + led_color_7 = LED_ORANGE; + led_color_8 = LED_ORANGE; + led_blink = LED_BLINK; + }else{ + led_color_1 = LED_CYAN; + led_color_2 = LED_CYAN; + led_color_3 = LED_CYAN; + led_color_4 = LED_CYAN; + led_color_5 = LED_CYAN; + led_color_6 = LED_CYAN; + led_color_7 = LED_CYAN; + led_color_8 = LED_CYAN; + led_blink = LED_NOBLINK; + } + } else { + /* armed system - initial led pattern */ + led_color_1 = LED_RED; + led_color_2 = LED_RED; + led_color_3 = LED_RED; + led_color_4 = LED_OFF; + led_color_5 = LED_OFF; + led_color_6 = LED_OFF; + led_color_7 = LED_OFF; + led_color_8 = LED_OFF; + led_blink = LED_BLINK; + + if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { + /* indicate main control state */ + if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) + led_color_4 = LED_GREEN; + /* TODO: add other Auto modes */ + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) + led_color_4 = LED_BLUE; + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) + led_color_4 = LED_YELLOW; + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) + led_color_4 = LED_WHITE; + else + led_color_4 = LED_OFF; + led_color_5 = led_color_4; + } + + if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { + /* handling used satus */ + if(num_of_used_sats >= 7) { + led_color_1 = LED_OFF; + led_color_2 = LED_OFF; + led_color_3 = LED_OFF; + } else if(num_of_used_sats == 6) { + led_color_2 = LED_OFF; + led_color_3 = LED_OFF; + } else if(num_of_used_sats == 5) { + led_color_3 = LED_OFF; + } + + } else { + /* no vehicle_gps_position data */ + led_color_1 = LED_WHITE; + led_color_2 = LED_WHITE; + led_color_3 = LED_WHITE; + + } + + } + } + } + } else { + /* LED Pattern for general Error - no vehicle_status can retrieved */ + led_color_1 = LED_WHITE; + led_color_2 = LED_WHITE; + led_color_3 = LED_WHITE; + led_color_4 = LED_WHITE; + led_color_5 = LED_WHITE; + led_color_6 = LED_WHITE; + led_color_7 = LED_WHITE; + led_color_8 = LED_WHITE; + led_blink = LED_BLINK; + + } + + /* + printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", + vehicle_status_raw.voltage_battery, + vehicle_status_raw.flag_system_armed, + vehicle_status_raw.flight_mode, + num_of_cells, + no_data_vehicle_status, + no_data_vehicle_gps_position, + num_of_used_sats, + vehicle_gps_position_raw.fix_type, + vehicle_gps_position_raw.satellites_visible); + */ + + led_thread_runcount=0; + led_thread_ready = true; + led_interval = LED_OFFTIME; + + if(detected_cells_runcount < 4){ + detected_cells_runcount++; + } else { + detected_cells_blinked = true; + } + + } else { + led_thread_runcount++; + } + + if(systemstate_run == true) { + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); + } else { + stop_script(); + set_rgb(0,0,0); + } +} + +void BlinkM::setLEDColor(int ledcolor) { + switch (ledcolor) { + case LED_OFF: // off + set_rgb(0,0,0); + break; + case LED_RED: // red + set_rgb(255,0,0); + break; + case LED_ORANGE: // orange + set_rgb(255,150,0); + break; + case LED_YELLOW: // yellow + set_rgb(200,200,0); + break; + case LED_PURPLE: // purple + set_rgb(255,0,255); + break; + case LED_GREEN: // green + set_rgb(0,255,0); + break; + case LED_BLUE: // blue + set_rgb(0,0,255); + break; + case LED_CYAN: // cyan + set_rgb(0,128,128); + break; + case LED_WHITE: // white + set_rgb(255,255,255); + break; + case LED_AMBER: // amber + set_rgb(255,65,0); + break; + } +} + +int +BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'n', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'c', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b) +{ + const uint8_t msg[4] = { 'h', h, s, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'C', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b) +{ + const uint8_t msg[4] = { 'H', h, s, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::set_fade_speed(uint8_t s) +{ + const uint8_t msg[2] = { 'f', s }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::play_script(uint8_t script_id) +{ + const uint8_t msg[4] = { 'p', script_id, 0, 0 }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::play_script(const char *script_name) +{ + /* handle HTML colour encoding */ + if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) { + char code[3]; + uint8_t r, g, b; + + code[2] = '\0'; + + code[0] = script_name[1]; + code[1] = script_name[2]; + r = strtol(code, 0, 16); + code[0] = script_name[3]; + code[1] = script_name[4]; + g = strtol(code, 0, 16); + code[0] = script_name[5]; + code[1] = script_name[6]; + b = strtol(code, 0, 16); + + stop_script(); + return set_rgb(r, g, b); + } + + for (unsigned i = 0; script_names[i] != nullptr; i++) + if (!strcasecmp(script_name, script_names[i])) + return play_script(i); + + return -1; +} + +int +BlinkM::stop_script() +{ + const uint8_t msg[1] = { 'o' }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3) +{ + const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]) +{ + const uint8_t msg[3] = { 'R', 0, line }; + uint8_t result[5]; + + int ret = transfer(msg, sizeof(msg), result, sizeof(result)); + + if (ret == OK) { + ticks = result[0]; + cmd[0] = result[1]; + cmd[1] = result[2]; + cmd[2] = result[3]; + cmd[3] = result[4]; + } + + return ret; +} + +int +BlinkM::set_script(uint8_t len, uint8_t repeats) +{ + const uint8_t msg[4] = { 'L', 0, len, repeats }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) +{ + const uint8_t msg = 'g'; + uint8_t result[3]; + + int ret = transfer(&msg, sizeof(msg), result, sizeof(result)); + + if (ret == OK) { + r = result[0]; + g = result[1]; + b = result[2]; + } + + return ret; +} + +int +BlinkM::get_firmware_version(uint8_t version[2]) +{ + const uint8_t msg = 'Z'; + + return transfer(&msg, sizeof(msg), version, 2); +} + +void blinkm_usage(); + +void blinkm_usage() { + warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --blinkmaddr blinkmaddr (9)"); +} + +int +blinkm_main(int argc, char *argv[]) +{ + + int i2cdevice = PX4_I2C_BUS_EXPANSION; + int blinkmadr = 9; + + int x; + + for (x = 1; x < argc; x++) { + if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { + if (argc > x + 1) { + i2cdevice = atoi(argv[x + 1]); + } + } + + if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) { + if (argc > x + 1) { + blinkmadr = atoi(argv[x + 1]); + } + } + + } + + if (!strcmp(argv[1], "start")) { + if (g_blinkm != nullptr) { + warnx("already started"); + return 1; + } + + g_blinkm = new BlinkM(i2cdevice, blinkmadr); + + if (g_blinkm == nullptr) { + warnx("new failed"); + return 1; + } + + if (OK != g_blinkm->init()) { + delete g_blinkm; + g_blinkm = nullptr; + warnx("init failed"); + return 1; + } + + return 0; + } + + + if (g_blinkm == nullptr) { + fprintf(stderr, "not started\n"); + blinkm_usage(); + return 0; + } + + if (!strcmp(argv[1], "systemstate")) { + g_blinkm->setMode(1); + return 0; + } + + if (!strcmp(argv[1], "ledoff")) { + g_blinkm->setMode(0); + return 0; + } + + + if (!strcmp(argv[1], "list")) { + for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) + fprintf(stderr, " %s\n", BlinkM::script_names[i]); + fprintf(stderr, " \n"); + return 0; + } + + /* things that require access to the device */ + int fd = px4_open(BLINKM0_DEVICE_PATH, 0); + if (fd < 0) { + warn("can't open BlinkM device"); + return 1; + } + + g_blinkm->setMode(0); + if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) + return 0; + + px4_close(fd); + + blinkm_usage(); + return 0; +} diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm_nuttx.cpp similarity index 100% rename from src/drivers/blinkm/blinkm.cpp rename to src/drivers/blinkm/blinkm_nuttx.cpp diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 8568436d35..13370ffab8 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -42,6 +42,7 @@ #pragma once +#include #include #include @@ -52,7 +53,7 @@ */ #define _BLINKMIOCBASE (0x2900) -#define _BLINKMIOC(_n) (_IOC(_BLINKMIOCBASE, _n)) +#define _BLINKMIOC(_n) (_PX4_IOC(_BLINKMIOCBASE, _n)) /** play the named script in *(char *)arg, repeating forever */ #define BLINKM_PLAY_SCRIPT_NAMED _BLINKMIOC(1) diff --git a/src/platforms/linux/include/board_config.h b/src/platforms/linux/include/board_config.h index 7bd465647c..81bcc8f34d 100644 --- a/src/platforms/linux/include/board_config.h +++ b/src/platforms/linux/include/board_config.h @@ -1 +1,9 @@ #define UDID_START 0x1FFF7A10 + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_EXPANSION 3 +#define PX4_I2C_BUS_LED 3 diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index a7b9e55000..c53fd63d49 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -44,7 +44,6 @@ #include #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 -#define PX4_I2C_BUS_ONBOARD 1 #define px4_errx(x, ...) errx(x, __VA_ARGS__) From 13d84ef17b3c3793579e5f84bae4fa42154571b0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 17 Mar 2015 13:57:33 -0700 Subject: [PATCH 041/258] Linux: I2C opens /dev/i2c-x For now it uses the bus number as the id. Not sure how this should actually be mapped. Seems like the I2C devices come up in random order and have random id but that a specific device can be found in the /sys/bus/i2c interface. Signed-off-by: Mark Charlebois --- src/drivers/blinkm/blinkm_linux.cpp | 4 ++++ src/drivers/device/i2c_linux.cpp | 10 ++++++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/drivers/blinkm/blinkm_linux.cpp b/src/drivers/blinkm/blinkm_linux.cpp index e38fb408e7..5a139a710a 100644 --- a/src/drivers/blinkm/blinkm_linux.cpp +++ b/src/drivers/blinkm/blinkm_linux.cpp @@ -943,6 +943,10 @@ blinkm_main(int argc, char *argv[]) int x; + if (argc < 2) { + blinkm_usage(); + return 1; + } for (x = 1; x < argc; x++) { if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { if (argc > x + 1) { diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 1a3492c8f2..e62da87979 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -97,9 +97,15 @@ I2C::init() return ret; } - _fd = ::open(_dname.c_str(), O_RDWR); + // Open the actual I2C device and map to the virtual dev name + char str[22]; + + // Fixme - not sure bus is the right mapping here + // may have to go through /sys/bus/i2c interface to find the right map + snprintf(str, sizeof(str), "/dev/i2c-%d", _bus); + _fd = ::open(str, O_RDWR); if (_fd < 0) { - warnx("could not open %s", _dname.c_str()); + warnx("could not open %s for virtual device %s", str, _dname.c_str()); return -errno; } From 5f3496353ccf19e2e6f2253ada546b35f6fc51fc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 17 Mar 2015 19:02:10 -0700 Subject: [PATCH 042/258] Linux: added support for sdlog2 Signed-off-by: Mark Charlebois --- makefiles/linux/board_linux.mk | 2 +- makefiles/linux/config_linux_default.mk | 1 + src/lib/version/version.h | 3 ++ src/modules/sdlog2/logbuffer.c | 3 +- src/modules/sdlog2/sdlog2.c | 53 ++++++++++++---------- src/modules/uORB/topics/telemetry_status.h | 4 +- src/platforms/px4_config.h | 1 + src/platforms/px4_defines.h | 6 ++- 8 files changed, 45 insertions(+), 28 deletions(-) diff --git a/makefiles/linux/board_linux.mk b/makefiles/linux/board_linux.mk index ffdfd036a5..cf5d42ff4c 100644 --- a/makefiles/linux/board_linux.mk +++ b/makefiles/linux/board_linux.mk @@ -6,6 +6,6 @@ # Configure the toolchain # CONFIG_ARCH = NATIVE -CONFIG_BOARD = FOO +CONFIG_BOARD = LINUXTEST include $(PX4_MK_DIR)/toolchain_native.mk diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 4ba4b7842b..03d406a896 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -42,6 +42,7 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB MODULES += modules/dataman +MODULES += modules/sdlog2 # # Libraries diff --git a/src/lib/version/version.h b/src/lib/version/version.h index b79fee182e..f0071f3b58 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -59,4 +59,7 @@ #define HW_ARCH "PX4_STM32F4DISCOVERY" #endif +#ifdef CONFIG_ARCH_BOARD_LINUXTEST +#define HW_ARCH "LINUXTEST" +#endif #endif /* VERSION_H_ */ diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 2da67d8a92..9e25dd2f23 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -39,6 +39,7 @@ * @author Anton Babushkin */ +#include #include #include @@ -50,7 +51,7 @@ int logbuffer_init(struct logbuffer_s *lb, int size) lb->write_ptr = 0; lb->read_ptr = 0; lb->data = malloc(lb->size); - return (lb->data == 0) ? ERROR : OK; + return (lb->data == 0) ? PX4_ERROR : PX4_OK; } int logbuffer_count(struct logbuffer_s *lb) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d19c1942be..7cca4c6fa5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -43,6 +43,7 @@ */ #include +#include #include #include #include @@ -113,7 +114,7 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" -#define PX4_EPOCH_SECS 1234567890ULL +#define PX4_EPOCH_SECS 1234567890L /** * Logging rate. @@ -292,7 +293,7 @@ sdlog2_usage(const char *reason) fprintf(stderr, "%s\n", reason); } - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t -x\n" + warnx("usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t -x\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" @@ -313,6 +314,7 @@ int sdlog2_main(int argc, char *argv[]) { if (argc < 2) { sdlog2_usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -320,7 +322,7 @@ int sdlog2_main(int argc, char *argv[]) if (thread_running) { warnx("already running"); /* this is not an error */ - exit(0); + return 0; } main_thread_should_exit = false; @@ -330,7 +332,7 @@ int sdlog2_main(int argc, char *argv[]) 3000, sdlog2_thread_main, (char * const *)argv); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { @@ -339,7 +341,7 @@ int sdlog2_main(int argc, char *argv[]) } main_thread_should_exit = true; - exit(0); + return 0; } if (!thread_running) { @@ -371,7 +373,7 @@ int sdlog2_main(int argc, char *argv[]) } sdlog2_usage("unrecognized command"); - exit(1); + return 1; } int create_log_dir() @@ -604,7 +606,8 @@ static void *logwriter_thread(void *arg) if (n < 0) { main_thread_should_exit = true; - err(1, "error writing log file"); + warn("error writing log file"); + break; } if (n > 0) { @@ -653,7 +656,7 @@ void sdlog2_start_log() /* create log dir if needed */ if (create_log_dir() != 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); - exit(1); + return; } /* initialize statistics counter */ @@ -679,7 +682,7 @@ void sdlog2_start_log() /* start log buffer emptying thread */ if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { - errx(1, "error creating logwriter thread"); + warnx("error creating logwriter thread"); } /* write all performance counters */ @@ -959,7 +962,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (check_free_space() != OK) { - errx(1, "ERR: MicroSD almost full"); + warnx("ERR: MicroSD almost full"); + return 1; } @@ -967,7 +971,8 @@ int sdlog2_thread_main(int argc, char *argv[]) int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret != 0 && errno != EEXIST) { - err(1, "ERR: failed creating log dir: %s", log_root); + warn("ERR: failed creating log dir: %s", log_root); + return 1; } /* copy conversion scripts */ @@ -985,7 +990,8 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting"); + warnx("can't allocate log buffer, exiting"); + return 1; } struct vehicle_status_s buf_status; @@ -1615,8 +1621,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (buf.triplet.current.valid) { log_msg.msg_type = LOG_GPSP_MSG; log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * (double)1e7); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * (double)1e7); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; log_msg.body.log_GPSP.type = buf.triplet.current.type; @@ -1904,7 +1910,7 @@ int file_copy(const char *file_old, const char *file_new) if (source == NULL) { warnx("ERR: open in"); - return 1; + return PX4_ERROR; } target = fopen(file_new, "w"); @@ -1912,7 +1918,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); warnx("ERR: open out"); - return 1; + return PX4_ERROR; } char buf[128]; @@ -1923,7 +1929,7 @@ int file_copy(const char *file_old, const char *file_new) if (ret <= 0) { warnx("error writing file"); - ret = 1; + ret = PX4_ERROR; break; } } @@ -1933,7 +1939,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return OK; + return PX4_OK; } int check_free_space() @@ -1941,19 +1947,20 @@ int check_free_space() /* use statfs to determine the number of blocks left */ FAR struct statfs statfs_buf; if (statfs(mountpoint, &statfs_buf) != OK) { - errx(ERROR, "ERR: statfs"); + warnx("ERR: statfs"); + return PX4_ERROR; } /* use a threshold of 50 MiB */ - if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { + if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ - return ERROR; + return PX4_ERROR; /* use a threshold of 100 MiB to send a warning */ - } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { + } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] space on MicroSD low: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); @@ -1961,7 +1968,7 @@ int check_free_space() space_warning_sent = true; } - return OK; + return PX4_OK; } void handle_command(struct vehicle_command_s *cmd) diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 93193f32ba..ee950f4db2 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -93,8 +93,8 @@ static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_I // included but telemetry_status_orb_id is not referenced. The inline works if you // choose to use it, but you can continue to just directly index into the array as well. // If you don't use the inline this ends up being a no-op with no additional code emitted. -extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index); -extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index) +//static const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index); +static inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index) { return telemetry_status_orb_id[index]; } diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index c53fd63d49..d2d1708eb6 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -44,6 +44,7 @@ #include #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 +#define CONFIG_ARCH_BOARD_LINUXTEST 1 #define px4_errx(x, ...) errx(x, __VA_ARGS__) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 091d67e056..26c9a85951 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -69,7 +69,7 @@ #elif defined(__PX4_NUTTX) || defined(__PX4_LINUX) /* - * Building for NuttX + * Building for NuttX or Linux */ #include /* Main entry point */ @@ -102,6 +102,8 @@ typedef param_t px4_param_t; #define _PX4_IOC(x,y) _IOC(x,y) +#define px4_statfs_buf_f_bavail_t int + /* Linux Specific defines */ #elif defined(__PX4_LINUX) @@ -123,6 +125,8 @@ __END_DECLS #define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)x/1000000L) +#define px4_statfs_buf_f_bavail_t unsigned long + #endif From 62eb403e4d5e4f893a6584bcf926915804e853d3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Mar 2015 09:07:37 -0700 Subject: [PATCH 043/258] Linus: print format fixes to build with clang on IFC6410 Signed-off-by: Mark Charlebois --- src/drivers/device/ringbuffer.h | 2 +- src/drivers/ms5611/ms5611_linux.cpp | 4 ++-- src/modules/mavlink/mavlink_main_linux.cpp | 2 +- src/platforms/linux/tests/hrt_test/hrt_test.cpp | 8 ++++---- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 35b38efd7d..f03171722c 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -506,7 +506,7 @@ RingBuffer::print_info(const char *name) printf("%s %u/%lu (%u/%u @ %p)\n", name, _num_items, - _num_items * _item_size, + (unsigned long)_num_items * _item_size, _head, _tail, _buf); diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index e914dfaec6..913c1ebcd3 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -445,7 +445,7 @@ MS5611::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if ((long)ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) return -EINVAL; /* update interval for next measurement */ @@ -572,7 +572,7 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + ((long)_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index a6b49ee25f..e251ceaf5e 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -1657,7 +1657,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\tGCS heartbeat:\t%lu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } printf("\tmavlink chan: #%u\n", _channel); diff --git a/src/platforms/linux/tests/hrt_test/hrt_test.cpp b/src/platforms/linux/tests/hrt_test/hrt_test.cpp index 5677f4e1b2..d5e446bcc9 100644 --- a/src/platforms/linux/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/linux/tests/hrt_test/hrt_test.cpp @@ -53,14 +53,14 @@ int HRTTest::main() hrt_abstime t = hrt_absolute_time(); usleep(1000000); hrt_abstime elt = hrt_elapsed_time(&t); - printf("Elapsed time %lu in 1 sec (usleep)\n", elt); - printf("Start time %lu\n", t); + printf("Elapsed time %llu in 1 sec (usleep)\n", elt); + printf("Start time %llu\n", t); t = hrt_absolute_time(); sleep(1); elt = hrt_elapsed_time(&t); - printf("Elapsed time %lu in 1 sec (sleep)\n", elt); - printf("Start time %lu\n", t); + printf("Elapsed time %llu in 1 sec (sleep)\n", elt); + printf("Start time %llu\n", t); return 0; } From 83df879f905019a8bef8a357bfc6e215b44b2581 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Mar 2015 09:24:57 -0700 Subject: [PATCH 044/258] Linux: fixes for compilation with gcc-4.8 on IFC6410 Signed-off-by: Mark Charlebois --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7cca4c6fa5..7d27016703 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -473,7 +473,7 @@ int open_log_file() } } - int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, 0x0777); if (fd < 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); @@ -525,7 +525,7 @@ int open_perf_file(const char* str) } } - int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC, 0x0777); if (fd < 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); From 1b4b8bb856542d3ce4450fa4b567e65a6c9119e9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Mar 2015 10:13:44 -0700 Subject: [PATCH 045/258] Linux: in printf cast uint64_t to unsigned long long When printing a uint64_t type using %llu, this works on a 32bit system, but on a 64bit machine uint64_t is an unsigned long. The compiler complains about unmatching types. The time times in PX4 should likely have been unsigned long long and not uint64_t as that type changes per architecture. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main_linux.cpp | 2 +- src/platforms/linux/tests/hrt_test/hrt_test.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index e251ceaf5e..99e689e74e 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -1657,7 +1657,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time)); } printf("\tmavlink chan: #%u\n", _channel); diff --git a/src/platforms/linux/tests/hrt_test/hrt_test.cpp b/src/platforms/linux/tests/hrt_test/hrt_test.cpp index d5e446bcc9..baa8670332 100644 --- a/src/platforms/linux/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/linux/tests/hrt_test/hrt_test.cpp @@ -53,14 +53,14 @@ int HRTTest::main() hrt_abstime t = hrt_absolute_time(); usleep(1000000); hrt_abstime elt = hrt_elapsed_time(&t); - printf("Elapsed time %llu in 1 sec (usleep)\n", elt); - printf("Start time %llu\n", t); + printf("Elapsed time %llu in 1 sec (usleep)\n", (unsigned long long)elt); + printf("Start time %llu\n", (unsigned long long)t); t = hrt_absolute_time(); sleep(1); elt = hrt_elapsed_time(&t); - printf("Elapsed time %llu in 1 sec (sleep)\n", elt); - printf("Start time %llu\n", t); + printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); + printf("Start time %llu\n", (unsigned long long)t); return 0; } From 653c14fcbb403a69f21f0af92ee5bf84b4c8109d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 19 Mar 2015 10:18:11 -0700 Subject: [PATCH 046/258] Linux: Handle nullptr passed to I2C constructor I2C class derives from CDev class which requires a devname but in at least some instances, a nullptr is passed for devname. Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_linux.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index e62da87979..4548b475af 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -63,7 +63,7 @@ I2C::I2C(const char *name, _bus(bus), _address(address), _fd(-1), - _dname(devname) + _dname() { // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; @@ -71,6 +71,9 @@ I2C::I2C(const char *name, _device_id.devid_s.address = address; // devtype needs to be filled in by the driver _device_id.devid_s.devtype = 0; + + if (devname) + _dname = devname; } I2C::~I2C() From 1f84c348fcc5a9ce967c8844ddaa9a77421ec2d9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 20 Mar 2015 19:49:53 -0700 Subject: [PATCH 047/258] fix for segv if topic has not been published If the topic has not been published, orb_copy returns a negative number which causes update() to memset the data contents to zero. In some instances data is a null pointer. This causes a segment violation crash. Added a check for data != 0 Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_orb_subscription.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 0050ec9fd4..4567f4f410 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -86,8 +86,10 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data) } if (orb_copy(_topic, _fd, data)) { - /* error copying topic data */ - memset(data, 0, _topic->o_size); + if (data) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + } return false; } else { From 526b0e68ebcc8acd6afa8a6437e318c25ccf98ea Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 22 Mar 2015 11:14:14 -0700 Subject: [PATCH 048/258] Linux: modified shell to not show _main suffix The builtin commands all have _main suffix by convention so no need to show _main. Also nsh calls the commmands without the _main suffix. Signed-off-by: Mark Charlebois --- Tools/linux_apps.py | 20 ++++++++++++++------ src/modules/mavlink/mavlink_main_linux.cpp | 2 +- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/Tools/linux_apps.py b/Tools/linux_apps.py index 9a380efd01..548dcc5e46 100755 --- a/Tools/linux_apps.py +++ b/Tools/linux_apps.py @@ -38,7 +38,7 @@ builtins = glob.glob("builtin_commands/COMMAND*") apps = [] for f in builtins: - apps.append(f.split(".")[-1]) + apps.append(f.split(".")[-1].split("_main")[0]) print print """ @@ -49,10 +49,11 @@ using namespace std; extern "C" { """ for app in apps: - print "extern int "+app+"(int argc, char *argv[]);" + print "extern int "+app+"_main(int argc, char *argv[]);" print """ -static int list_builtins(int argc, char *argv[]); +static int list_builtins_main(int argc, char *argv[]); +static int shutdown_main(int argc, char *argv[]); } @@ -63,16 +64,17 @@ static map app_map(void) map apps; """ for app in apps: - print '\tapps["'+app+'"] = '+app+';' + print '\tapps["'+app+'"] = '+app+'_main;' -print '\tapps["list_builtins"] = list_builtins;' +print '\tapps["list_builtins"] = list_builtins_main;' +print '\tapps["shutdown"] = shutdown_main;' print """ return apps; } map apps = app_map(); -static int list_builtins(int argc, char *argv[]) +static int list_builtins_main(int argc, char *argv[]) { cout << "Builtin Commands:" << endl; for (map::iterator it=apps.begin(); it!=apps.end(); ++it) @@ -80,5 +82,11 @@ static int list_builtins(int argc, char *argv[]) return 0; } + +static int shutdown_main(int argc, char *argv[]) +{ + cout << "Shutting down" << endl; + exit(0); +} """ diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index 99e689e74e..633ba8554f 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -88,7 +88,7 @@ #endif static const int ERROR = -1; -#define DEFAULT_DEVICE_NAME "/dev/ttyS1" +#define DEFAULT_DEVICE_NAME "/tmp/ttyS1" #define MAX_DATA_RATE 20000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. From 977036faf95e16365b7bb8ad09cce90ea654d18e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 24 Mar 2015 07:42:20 -0700 Subject: [PATCH 049/258] Linux: changed param to nit use errx or exit Thread based implementaton can't call errx or exit Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 +- src/systemcmds/param/param.c | 130 +++++++++++++----------- 2 files changed, 71 insertions(+), 61 deletions(-) diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 03d406a896..8ccc59feac 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -18,7 +18,7 @@ MODULES += drivers/ms5611 # # System commands # -#MODULES += systemcmds/boardinfo +MODULES += systemcmds/param # # General system control diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 7355176554..2c74e4b312 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -58,15 +58,15 @@ __EXPORT int param_main(int argc, char *argv[]); -static void do_save(const char *param_file_name); -static void do_load(const char *param_file_name); -static void do_import(const char *param_file_name); +static int do_save(const char *param_file_name); +static int do_load(const char *param_file_name); +static int do_import(const char *param_file_name); static void do_show(const char *search_string); static void do_show_print(void *arg, param_t param); -static void do_set(const char *name, const char *val, bool fail_on_not_found); -static void do_compare(const char *name, char *vals[], unsigned comparisons); -static void do_reset(const char *excludes[], int num_excludes); -static void do_reset_nostart(const char *excludes[], int num_excludes); +static int do_set(const char *name, const char *val, bool fail_on_not_found); +static int do_compare(const char *name, char *vals[], unsigned comparisons); +static int do_reset(const char *excludes[], int num_excludes); +static int do_reset_nostart(const char *excludes[], int num_excludes); int param_main(int argc, char *argv[]) @@ -74,34 +74,34 @@ param_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "save")) { if (argc >= 3) { - do_save(argv[2]); + return do_save(argv[2]); } else { if (param_save_default()) { warnx("Param export failed."); - exit(1); + return 1; } else { - exit(0); + return 0; } } } if (!strcmp(argv[1], "load")) { if (argc >= 3) { - do_load(argv[2]); + return do_load(argv[2]); } else { - do_load(param_get_default_file()); + return do_load(param_get_default_file()); } } if (!strcmp(argv[1], "import")) { if (argc >= 3) { - do_import(argv[2]); + return do_import(argv[2]); } else { - do_import(param_get_default_file()); + return do_import(param_get_default_file()); } } @@ -114,15 +114,17 @@ param_main(int argc, char *argv[]) } warnx("selected parameter default file %s", param_get_default_file()); - exit(0); + return 0; } if (!strcmp(argv[1], "show")) { if (argc >= 3) { do_show(argv[2]); + return 0; } else { do_show(NULL); + return 0; } } @@ -132,55 +134,59 @@ param_main(int argc, char *argv[]) /* if the fail switch is provided, fails the command if not found */ bool fail = !strcmp(argv[4], "fail"); - do_set(argv[2], argv[3], fail); + return do_set(argv[2], argv[3], fail); } else if (argc >= 4) { - do_set(argv[2], argv[3], false); + return do_set(argv[2], argv[3], false); } else { - errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'"); + warnx("not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'"); + return 1; } } if (!strcmp(argv[1], "compare")) { if (argc >= 4) { - do_compare(argv[2], &argv[3], argc - 3); + return do_compare(argv[2], &argv[3], argc - 3); } else { - errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); + warnx("not enough arguments.\nTry 'param compare PARAM_NAME 3'"); + return 1; } } if (!strcmp(argv[1], "reset")) { if (argc >= 3) { - do_reset((const char **) &argv[2], argc - 2); + return do_reset((const char **) &argv[2], argc - 2); } else { - do_reset(NULL, 0); + return do_reset(NULL, 0); } } if (!strcmp(argv[1], "reset_nostart")) { if (argc >= 3) { - do_reset_nostart((const char **) &argv[2], argc - 2); + return do_reset_nostart((const char **) &argv[2], argc - 2); } else { - do_reset_nostart(NULL, 0); + return do_reset_nostart(NULL, 0); } } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); + warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); + return 1; } -static void +static int do_save(const char *param_file_name) { /* create the file */ int fd = open(param_file_name, O_WRONLY | O_CREAT); if (fd < 0) { - err(1, "opening '%s' failed", param_file_name); + warn("opening '%s' failed", param_file_name); + return 1; } int result = param_export(fd, false); @@ -188,48 +194,53 @@ do_save(const char *param_file_name) if (result < 0) { (void)unlink(param_file_name); - errx(1, "error exporting to '%s'", param_file_name); + warnx("error exporting to '%s'", param_file_name); + return 1; } - exit(0); + return 0; } -static void +static int do_load(const char *param_file_name) { int fd = open(param_file_name, O_RDONLY); if (fd < 0) { - err(1, "open '%s'", param_file_name); + warn("open '%s'", param_file_name); + return 1; } int result = param_load(fd); close(fd); if (result < 0) { - errx(1, "error importing from '%s'", param_file_name); + warnx("error importing from '%s'", param_file_name); + return 1; } - exit(0); + return 0; } -static void +static int do_import(const char *param_file_name) { int fd = open(param_file_name, O_RDONLY); if (fd < 0) { - err(1, "open '%s'", param_file_name); + warn("open '%s'", param_file_name); + return 1; } int result = param_import(fd); close(fd); if (result < 0) { - errx(1, "error importing from '%s'", param_file_name); + warnx("error importing from '%s'", param_file_name); + return 1; } - exit(0); + return 0; } static void @@ -238,8 +249,6 @@ do_show(const char *search_string) printf("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, false, false); printf("\n %u parameters total, %u used.\n", param_count(), param_count_used()); - - exit(0); } static void @@ -267,7 +276,8 @@ do_show_print(void *arg, param_t param) } else if (*ss == '*') { if (*(ss + 1) != '\0') { warnx("* symbol only allowed at end of search string."); - exit(1); + // FIXME - should exit + return; } pp++; @@ -310,7 +320,7 @@ do_show_print(void *arg, param_t param) break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - printf("\n", 0 + param_type(param), param_size(param)); + printf("\n", 0 + param_type(param), param_size(param)); return; default: @@ -318,10 +328,10 @@ do_show_print(void *arg, param_t param) return; } - printf("\n", param); + printf("\n", param); } -static void +static int do_set(const char *name, const char *val, bool fail_on_not_found) { int32_t i; @@ -331,7 +341,8 @@ do_set(const char *name, const char *val, bool fail_on_not_found) /* set nothing if parameter cannot be found */ if (param == PARAM_INVALID) { /* param not found - fail silenty in scripts as it prevents booting */ - errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name); + warnx("Error: Parameter %s not found.", name); + return (fail_on_not_found) ? 1 : 0; } printf("%c %s: ", @@ -386,13 +397,14 @@ do_set(const char *name, const char *val, bool fail_on_not_found) break; default: - errx(1, "\n", 0 + param_type(param)); + warnx("\n", 0 + param_type(param)); + return 1; } - exit(0); + return 0; } -static void +static int do_compare(const char *name, char *vals[], unsigned comparisons) { int32_t i; @@ -402,7 +414,8 @@ do_compare(const char *name, char *vals[], unsigned comparisons) /* set nothing if parameter cannot be found */ if (param == PARAM_INVALID) { /* param not found */ - errx(1, "Error: Parameter %s not found.", name); + warnx("Error: Parameter %s not found.", name); + return 1; } /* @@ -451,7 +464,8 @@ do_compare(const char *name, char *vals[], unsigned comparisons) break; default: - errx(1, "\n", 0 + param_type(param)); + warnx("\n", 0 + param_type(param)); + return 1; } if (ret == 0) { @@ -460,10 +474,10 @@ do_compare(const char *name, char *vals[], unsigned comparisons) param_name(param)); } - exit(ret); + return ret; } -static void +static int do_reset(const char *excludes[], int num_excludes) { if (num_excludes > 0) { @@ -475,17 +489,14 @@ do_reset(const char *excludes[], int num_excludes) if (param_save_default()) { warnx("Param export failed."); - exit(1); - - } else { - exit(0); + return 1; } + return 0; } -static void +static int do_reset_nostart(const char *excludes[], int num_excludes) { - int32_t autostart; int32_t autoconfig; @@ -504,9 +515,8 @@ do_reset_nostart(const char *excludes[], int num_excludes) if (param_save_default()) { warnx("Param export failed."); - exit(1); + return 1; - } else { - exit(0); } + return 0; } From 2cd44a24ea23bb4b579bfade6f748e7ffd1f4505 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 24 Mar 2015 16:30:45 -0700 Subject: [PATCH 050/258] Linux: Added linker script support for param and added mc_att_control Added linker script to resolve __param_start and __param_end. Added mc_att_control to list of supported builtins. Signed-off-by: Mark Charlebois --- Makefile | 3 + Tools/linux_run.sh | 17 ++++++ linux-configs/linuxtest/scripts/ld.script | 61 +++++++++++++++++++ makefiles/linux/config_linux_default.mk | 2 +- makefiles/linux_elf.mk | 8 ++- makefiles/toolchain_native.mk | 14 ++++- .../mc_att_control/mc_att_control_main.cpp | 15 ++--- src/modules/systemlib/param/param.c | 2 +- 8 files changed, 109 insertions(+), 13 deletions(-) create mode 100755 Tools/linux_run.sh create mode 100644 linux-configs/linuxtest/scripts/ld.script diff --git a/Makefile b/Makefile index a10e5f2b33..fbf5898519 100644 --- a/Makefile +++ b/Makefile @@ -143,6 +143,9 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) +testrun: + Tools/linux_run.sh + # # Unittest targets. Builds and runs the host-level # unit tests. diff --git a/Tools/linux_run.sh b/Tools/linux_run.sh new file mode 100755 index 0000000000..4572a4e3e2 --- /dev/null +++ b/Tools/linux_run.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +if [ ! -c /tmp/ttyS0 ] || [ ! -c /tmp/ttyS1 ] + then + echo "Need to create /tmp/ttyS[01]" + echo "socat PTY,link=/tmp/ttyS0 PTY,link=/tmp/ttyS1" + exit 1 +fi + +if [ ! -d /fs/msdcard ] && [ ! -w /fs/msdcard ] + then + echo "Need to create/mount writable /fs/microsd" + echo "sudo mkdir -p /fs/msdcard" + echo "sudo chmod 777 /fs/msdcard" +fi + +Build/linux_default.build/mainapp diff --git a/linux-configs/linuxtest/scripts/ld.script b/linux-configs/linuxtest/scripts/ld.script new file mode 100644 index 0000000000..176917ff77 --- /dev/null +++ b/linux-configs/linuxtest/scripts/ld.script @@ -0,0 +1,61 @@ +/**************************************************************************** + * configs/aerocore/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +SECTIONS +{ + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = .; + KEEP(*(__param*)) + __param_end = .; + } +} diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 8ccc59feac..b766aaaf38 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -33,7 +33,7 @@ MODULES += modules/mavlink # # Vehicle Control # -#MODULES += modules/mc_att_control +MODULES += modules/mc_att_control # # Library modules diff --git a/makefiles/linux_elf.mk b/makefiles/linux_elf.mk index 18578a55c1..701d731ef3 100644 --- a/makefiles/linux_elf.mk +++ b/makefiles/linux_elf.mk @@ -41,6 +41,7 @@ # What we're going to build. # PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a +PRODUCT_SHARED_PRELINK = $(WORK_DIR)firmware.o .PHONY: firmware firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)mainapp @@ -49,8 +50,11 @@ firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)mainapp # Built product rules # -$(PRODUCT_SHARED_LIB): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) - $(call LINK_A,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS)) +$(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) + $(call PRELINKF,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS)) + +$(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) + $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) MAIN = $(PX4_BASE)/src/platforms/linux/main.cpp $(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index d983fd24e3..5d0844cca7 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -193,7 +193,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) #EXTRA_LIBS += $(LIBM) -EXTRA_LIBS += -pthread -lm -lrt +EXTRA_LIBS += -lm -lrt # Flags we pass to the C compiler # @@ -237,6 +237,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ $(EXTRADEFINES) \ $(EXTRAAFLAGS) +LDSCRIPT = $(PX4_BASE)/linux-configs/linuxtest/scripts/ld.script # Flags we pass to the linker # LDFLAGS += $(EXTRALDFLAGS) \ @@ -290,6 +291,15 @@ define PRELINK @$(MKDIR) -p $(dir $1) $(Q) $(LD) -Ur -o $1 $2 +endef +# Produce partially-linked $1 from files in $2 +# +#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold +define PRELINKF + @$(ECHO) "PRELINK: $1" + @$(MKDIR) -p $(dir $1) + $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 + endef # $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 @@ -324,7 +334,7 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - echo "$(Q) $(CXX) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS)" $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) + endef diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 5f5443ab11..c579ce8830 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -54,6 +54,7 @@ */ #include +#include #include #include #include @@ -404,7 +405,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -721,7 +722,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + if (std::isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } @@ -858,10 +859,10 @@ MulticopterAttitudeControl::task_main() control_attitude_rates(dt); /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.control[0] = (std::isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (std::isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (std::isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (std::isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; @@ -909,7 +910,7 @@ MulticopterAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, - (main_t)&MulticopterAttitudeControl::task_main_trampoline, + (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 8cc1e6e278..e42109da4d 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -71,7 +71,7 @@ /** * Array of static parameter info. */ -#if defined(_UNIT_TEST) || defined(__PX4_LINUX) +#if defined(_UNIT_TEST) extern struct param_info_s param_array[]; extern struct param_info_s *param_info_base; extern struct param_info_s *param_info_limit; From 31edc08ceaeb8a9517adab12830751ac875740a2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 24 Mar 2015 20:50:30 -0700 Subject: [PATCH 051/258] Linux: added missing -pthread flag LDFLAGS was missing -pthread Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 5d0844cca7..f790940b89 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -193,7 +193,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) #EXTRA_LIBS += $(LIBM) -EXTRA_LIBS += -lm -lrt +EXTRA_LIBS += -pthread -lm -lrt # Flags we pass to the C compiler # From bb7c8163b906ce2a4efbbc03742b38e3ec3999d0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 24 Mar 2015 20:52:38 -0700 Subject: [PATCH 052/258] Linux: commented out tests The src/platform/linux/tests modules were commented out in the config file. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index b766aaaf38..e991e3e15f 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -56,7 +56,11 @@ MODULES += lib/conversion # Linux port # MODULES += platforms/linux/px4_layer -MODULES += platforms/linux/tests/hello -MODULES += platforms/linux/tests/vcdev_test -MODULES += platforms/linux/tests/hrt_test + +# +# Unit tests +# +#MODULES += platforms/linux/tests/hello +#MODULES += platforms/linux/tests/vcdev_test +#MODULES += platforms/linux/tests/hrt_test From 192d70e5702534fa09b8f364d564476bf17dbd54 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 24 Mar 2015 20:55:06 -0700 Subject: [PATCH 053/258] Linux: removed separate path for dataman file for Linux Signed-off-by: Mark Charlebois --- src/modules/dataman/dataman.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index c125716342..75f8c6dc86 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -132,11 +132,7 @@ static sem_t g_sys_state_mutex; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; // FIXME - need a configurable path that is not OS specific -#ifdef __PX4_NUTTX static const char *k_data_manager_device_path = "/fs/microsd/dataman"; -#else -static const char *k_data_manager_device_path = "/tmp/dataman"; -#endif /* The data manager work queues */ From 340beac6726d866076807a5c9f915865ba438ccc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 25 Mar 2015 00:00:57 -0700 Subject: [PATCH 054/258] Linux: I2C virtual device Create and open I2C virtual device and support I2C_RDWR ioctl Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_linux.cpp | 30 ++++++++++++++++++++++++++++-- src/drivers/device/i2c_linux.h | 2 ++ 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 4548b475af..4399626aa7 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -100,6 +100,12 @@ I2C::init() return ret; } + _fd = px4_open(_dname.c_str(), PX4_F_RDONLY | PX4_F_WRONLY); + if (_fd < 0) { + debug("px4_open failed of device %s", _dname.c_str()); + return PX4_ERROR; + } +#if 0 // Open the actual I2C device and map to the virtual dev name char str[22]; @@ -111,6 +117,7 @@ I2C::init() warnx("could not open %s for virtual device %s", str, _dname.c_str()); return -errno; } +#endif return ret; } @@ -124,6 +131,11 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re int ret; unsigned retry_count = 0; + if (_fd < 0) { + warnx("I2C device not opened"); + return 1; + } + do { // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; @@ -150,7 +162,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re packets.msgs = msgv; packets.nmsgs = msgs; - ret = ::ioctl(_fd, I2C_RDWR, &packets); + ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); if (ret < 0) { warnx("I2C transfer failed"); return 1; @@ -187,7 +199,7 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) packets.msgs = msgv; packets.nmsgs = msgs; - ret = ::ioctl(_fd, I2C_RDWR, &packets); + ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); if (ret < 0) { warnx("I2C transfer failed"); return 1; @@ -209,4 +221,18 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) return ret; } +int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; + + switch (cmd) { + case I2C_RDWR: + warnx("I2C transfer request"); + return 0; + default: + /* give it to the superclass */ + return CDev::ioctl(handlep, cmd, arg); + } +} + } // namespace device diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h index 56eaea4b96..aae1e7f588 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_linux.h @@ -99,6 +99,8 @@ protected: virtual int probe(); #endif + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + /** * Perform an I2C transaction to the device. * From 056d5f9075ed7eaaac0ebc80de2d7938813e1465 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 25 Mar 2015 10:48:52 -0700 Subject: [PATCH 055/258] Linux: Added wqueue files from NuttX Import copies of work queue releated filed from NuttX. These are the original files. Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/dq_rem.c | 84 ++++++ src/platforms/linux/px4_layer/work_cancel.c | 125 ++++++++ src/platforms/linux/px4_layer/work_queue.c | 136 +++++++++ src/platforms/linux/px4_layer/work_thread.c | 314 ++++++++++++++++++++ 4 files changed, 659 insertions(+) create mode 100644 src/platforms/linux/px4_layer/dq_rem.c create mode 100644 src/platforms/linux/px4_layer/work_cancel.c create mode 100644 src/platforms/linux/px4_layer/work_queue.c create mode 100644 src/platforms/linux/px4_layer/work_thread.c diff --git a/src/platforms/linux/px4_layer/dq_rem.c b/src/platforms/linux/px4_layer/dq_rem.c new file mode 100644 index 0000000000..db20762c75 --- /dev/null +++ b/src/platforms/linux/px4_layer/dq_rem.c @@ -0,0 +1,84 @@ +/************************************************************ + * libc/queue/dq_rem.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_rem + * + * Descripton: + * dq_rem removes 'node' from 'queue' + * + ************************************************************/ + +void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue) +{ + FAR dq_entry_t *prev = node->blink; + FAR dq_entry_t *next = node->flink; + + if (!prev) + { + queue->head = next; + } + else + { + prev->flink = next; + } + + if (!next) + { + queue->tail = prev; + } + else + { + next->blink = prev; + } + + node->flink = NULL; + node->blink = NULL; +} + diff --git a/src/platforms/linux/px4_layer/work_cancel.c b/src/platforms/linux/px4_layer/work_cancel.c new file mode 100644 index 0000000000..2671bf2530 --- /dev/null +++ b/src/platforms/linux/px4_layer/work_cancel.c @@ -0,0 +1,125 @@ +/**************************************************************************** + * libc/wqueue/work_cancel.c + * + * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling work_queue() + * again. + * + * Input parameters: + * qid - The work queue ID + * work - The previously queue work structure to cancel + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_cancel(int qid, FAR struct work_s *work) +{ + FAR struct wqueue_s *wqueue = &g_work[qid]; + irqstate_t flags; + + DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ + + flags = irqsave(); + if (work->worker != NULL) + { + /* A little test of the integrity of the work queue */ + + DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); + DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ + + dq_rem((FAR dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + irqrestore(flags); + return OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/linux/px4_layer/work_queue.c b/src/platforms/linux/px4_layer/work_queue.c new file mode 100644 index 0000000000..c85f564e54 --- /dev/null +++ b/src/platforms/linux/px4_layer/work_queue.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * libc/wqueue/work_queue.c + * + * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * qid - The work queue ID (index) + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in clock ticks) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_queue(int qid, FAR struct work_s *work, worker_t worker, + FAR void *arg, uint32_t delay) +{ + FAR struct wqueue_s *wqueue = &g_work[qid]; + irqstate_t flags; + + DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* First, initialize the work structure */ + + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ + + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ + + flags = irqsave(); + work->qtime = clock_systimer(); /* Time work queued */ + + dq_addlast((FAR dq_entry_t *)work, &wqueue->q); + kill(wqueue->pid, SIGWORK); /* Wake up the worker thread */ + + irqrestore(flags); + return OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/linux/px4_layer/work_thread.c b/src/platforms/linux/px4_layer/work_thread.c new file mode 100644 index 0000000000..6ef8a874b0 --- /dev/null +++ b/src/platforms/linux/px4_layer/work_thread.c @@ -0,0 +1,314 @@ +/**************************************************************************** + * libc/wqueue/work_thread.c + * + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* The state of each work queue. */ + +#ifdef CONFIG_NUTTX_KERNEL + + /* Play some games in the kernel mode build to assure that different + * naming is used for the global work queue data structures. This may + * not be necessary but it safer. + * + * In this case g_work is #define'd to be either g_kernelwork or + * g_usrwork in include/nuttx/wqueue.h + */ + +# ifdef __KERNEL__ +struct wqueue_s g_kernelwork[NWORKERS]; +# else +struct wqueue_s g_usrwork[NWORKERS]; +# endif + +#else /* CONFIG_NUTTX_KERNEL */ + +struct wqueue_s g_work[NWORKERS]; + +#endif /* CONFIG_NUTTX_KERNEL */ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_process + * + * Description: + * This is the logic that performs actions placed on any work list. + * + * Input parameters: + * wqueue - Describes the work queue to be processed + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void work_process(FAR struct wqueue_s *wqueue) +{ + volatile FAR struct work_s *work; + worker_t worker; + irqstate_t flags; + FAR void *arg; + uint32_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK; + flags = irqsave(); + work = (FAR struct work_s *)wqueue->q.head; + while (work) + { + /* Is this work ready? It is ready if there is no delay or if + * the delay has elapsed. qtime is the time that the work was added + * to the work queue. It will always be greater than or equal to + * zero. Therefore a delay of zero will always execute immediately. + */ + + elapsed = clock_systimer() - work->qtime; + if (elapsed >= work->delay) + { + /* Remove the ready-to-execute work from the list */ + + (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + irqrestore(flags); + worker(arg); + + /* Now, unfortunately, since we re-enabled interrupts we don't + * know the state of the work list and we will have to start + * back at the head of the list. + */ + + flags = irqsave(); + work = (FAR struct work_s *)wqueue->q.head; + } + else + { + /* This one is not ready.. will it be ready before the next + * scheduled wakeup interval? + */ + + /* Here: elapsed < work->delay */ + remaining = work->delay - elapsed; + if (remaining < next) + { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (FAR struct work_s *)work->dq.flink; + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + + usleep(next * USEC_PER_TICK); + irqrestore(flags); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/**************************************************************************** + * Name: work_hpthread, work_lpthread, and work_usrthread + * + * Description: + * These are the worker threads that performs actions placed on the work + * lists. + * + * work_hpthread and work_lpthread: These are the kernel mode work queues + * (also build in the flat build). One of these threads also performs + * periodic garbage collection (that is otherwise performed by the idle + * thread if CONFIG_SCHED_WORKQUEUE is not defined). + * + * These worker threads are started by the OS during normal bringup. + * + * work_usrthread: This is a user mode work queue. It must be built into + * the applicatino blob during the user phase of a kernel build. The + * user work thread will then automatically be started when the system + * boots by calling through the pointer found in the header on the user + * space blob. + * + * All of these entrypoints are referenced by OS internally and should not + * not be accessed by application logic. + * + * Input parameters: + * argc, argv (not used) + * + * Returned Value: + * Does not return + * + ****************************************************************************/ + +#ifdef CONFIG_SCHED_HPWORK + +int work_hpthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + +#ifndef CONFIG_SCHED_LPWORK + sched_garbagecollection(); +#endif + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[HPWORK]); + } + + return OK; /* To keep some compilers happy */ +} + +#ifdef CONFIG_SCHED_LPWORK + +int work_lpthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + + sched_garbagecollection(); + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[LPWORK]); + } + + return OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_LPWORK */ +#endif /* CONFIG_SCHED_HPWORK */ + +#ifdef CONFIG_SCHED_USRWORK + +int work_usrthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[USRWORK]); + } + + return OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_USRWORK */ + +#endif /* CONFIG_SCHED_WORKQUEUE */ From aedf6fe6284ca19597ca9b9c0b5cfeaf8ee85995 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 25 Mar 2015 10:53:08 -0700 Subject: [PATCH 056/258] Linux: Added preliminary work queue support Based on NuttX work queue code. Not yet functional. Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/module.mk | 6 +- .../linux/px4_layer/px4_linux_impl.cpp | 66 ++++++------------- src/platforms/linux/px4_layer/work_cancel.c | 29 ++++---- src/platforms/linux/px4_layer/work_queue.c | 30 ++++----- src/platforms/linux/px4_layer/work_thread.c | 59 +++++------------ src/platforms/px4_config.h | 3 + src/platforms/px4_defines.h | 3 +- src/platforms/px4_tasks.h | 1 + src/platforms/px4_workqueue.h | 27 ++++++-- 9 files changed, 95 insertions(+), 129 deletions(-) diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk index 1d987418ac..aeb707b5c3 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/linux/px4_layer/module.mk @@ -38,6 +38,9 @@ SRCS = \ px4_linux_impl.cpp \ px4_linux_tasks.c \ + work_thread.c \ + work_queue.c \ + work_cancel.c \ lib_crc32.c \ drv_hrt.c \ queue.c \ @@ -45,6 +48,7 @@ SRCS = \ dq_remfirst.c \ sq_addlast.c \ sq_remfirst.c \ - sq_addafter.c + sq_addafter.c \ + dq_rem.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index 059e1c9176..7f06b7a916 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -56,60 +56,32 @@ long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); __END_DECLS +extern struct wqueue_s gwork[NWORKERS]; + namespace px4 { void init(int argc, char *argv[], const char *app_name) { - struct param_info_s test_1 = { - "TEST_1", - PARAM_TYPE_INT32 - }; - test_1.val.i = 2; - - struct param_info_s test_2 = { - "TEST_2", - PARAM_TYPE_INT32 - }; - test_2.val.i = 4; - - struct param_info_s rc_x = { - "RC_X", - PARAM_TYPE_INT32 - }; - rc_x.val.i = 8; - - struct param_info_s rc2_x = { - "RC2_X", - PARAM_TYPE_INT32 - }; - rc2_x.val.i = 16; - - param_array[0] = test_1; - param_array[1] = test_2; - param_array[2] = rc_x; - param_array[3] = rc2_x; - param_info_base = (struct param_info_s *) ¶m_array[0]; - param_info_limit = (struct param_info_s *) ¶m_array[4]; // needs to point at the end of the data, - // therefore number of params + 1 - printf("App name: %s\n", app_name); + + // Create high priority worker thread + g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + work_hpthread, + (char* const*)NULL); + + // Create low priority worker thread + g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2000, + work_lpthread, + (char* const*)NULL); + } } - - - -int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) -{ - printf("work_queue: UNIMPLEMENTED\n"); - return PX4_OK; -} - -int work_cancel(int qid, struct work_s *work) -{ - printf("work_cancel: UNIMPLEMENTED\n"); - return PX4_OK; -} - diff --git a/src/platforms/linux/px4_layer/work_cancel.c b/src/platforms/linux/px4_layer/work_cancel.c index 2671bf2530..6f737877d9 100644 --- a/src/platforms/linux/px4_layer/work_cancel.c +++ b/src/platforms/linux/px4_layer/work_cancel.c @@ -37,15 +37,10 @@ * Included Files ****************************************************************************/ -#include - +#include +#include #include -#include -#include -#include - -#include -#include +#include #ifdef CONFIG_SCHED_WORKQUEUE @@ -90,25 +85,25 @@ * ****************************************************************************/ -int work_cancel(int qid, FAR struct work_s *work) +int work_cancel(int qid, struct work_s *work) { - FAR struct wqueue_s *wqueue = &g_work[qid]; - irqstate_t flags; + struct wqueue_s *wqueue = &g_work[qid]; + //irqstate_t flags; - DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); /* Cancelling the work is simply a matter of removing the work structure * from the work queue. This must be done with interrupts disabled because * new work is typically added to the work queue from interrupt handlers. */ - flags = irqsave(); + //flags = irqsave(); if (work->worker != NULL) { /* A little test of the integrity of the work queue */ - DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); - DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); /* Remove the entry from the work queue and make sure that it is * mark as availalbe (i.e., the worker field is nullified). @@ -118,8 +113,8 @@ int work_cancel(int qid, FAR struct work_s *work) work->worker = NULL; } - irqrestore(flags); - return OK; + //irqrestore(flags); + return PX4_OK; } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/linux/px4_layer/work_queue.c b/src/platforms/linux/px4_layer/work_queue.c index c85f564e54..b67cfdc75b 100644 --- a/src/platforms/linux/px4_layer/work_queue.c +++ b/src/platforms/linux/px4_layer/work_queue.c @@ -37,17 +37,13 @@ * Included Files ****************************************************************************/ -#include +#include +#include +#include #include #include -#include -#include -#include - -#include -#include -#include +#include #ifdef CONFIG_SCHED_WORKQUEUE @@ -104,13 +100,11 @@ * ****************************************************************************/ -int work_queue(int qid, FAR struct work_s *work, worker_t worker, - FAR void *arg, uint32_t delay) +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) { - FAR struct wqueue_s *wqueue = &g_work[qid]; - irqstate_t flags; + struct wqueue_s *wqueue = &g_work[qid]; - DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); /* First, initialize the work structure */ @@ -123,14 +117,14 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker, * from with task logic or interrupt handlers. */ - flags = irqsave(); + //flags = irqsave(); work->qtime = clock_systimer(); /* Time work queued */ - dq_addlast((FAR dq_entry_t *)work, &wqueue->q); - kill(wqueue->pid, SIGWORK); /* Wake up the worker thread */ + dq_addlast((dq_entry_t *)work, &wqueue->q); + pthread_kill(wqueue->pid, SIGUSR1); /* Wake up the worker thread */ - irqrestore(flags); - return OK; + //irqrestore(flags); + return PX4_OK; } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/linux/px4_layer/work_thread.c b/src/platforms/linux/px4_layer/work_thread.c index 6ef8a874b0..b238979ed4 100644 --- a/src/platforms/linux/px4_layer/work_thread.c +++ b/src/platforms/linux/px4_layer/work_thread.c @@ -37,19 +37,12 @@ * Included Files ****************************************************************************/ -#include - +#include +#include #include #include #include -#include -#include -#include - -#include -#include -#include -#include +#include #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,29 +59,8 @@ ****************************************************************************/ /* The state of each work queue. */ - -#ifdef CONFIG_NUTTX_KERNEL - - /* Play some games in the kernel mode build to assure that different - * naming is used for the global work queue data structures. This may - * not be necessary but it safer. - * - * In this case g_work is #define'd to be either g_kernelwork or - * g_usrwork in include/nuttx/wqueue.h - */ - -# ifdef __KERNEL__ -struct wqueue_s g_kernelwork[NWORKERS]; -# else -struct wqueue_s g_usrwork[NWORKERS]; -# endif - -#else /* CONFIG_NUTTX_KERNEL */ - struct wqueue_s g_work[NWORKERS]; -#endif /* CONFIG_NUTTX_KERNEL */ - /**************************************************************************** * Private Variables ****************************************************************************/ @@ -115,7 +87,7 @@ static void work_process(FAR struct wqueue_s *wqueue) { volatile FAR struct work_s *work; worker_t worker; - irqstate_t flags; + //irqstate_t flags; FAR void *arg; uint32_t elapsed; uint32_t remaining; @@ -125,8 +97,9 @@ static void work_process(FAR struct wqueue_s *wqueue) * we process items in the work list. */ - next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK; - flags = irqsave(); + //next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK; + next = 100; + //flags = irqsave(); work = (FAR struct work_s *)wqueue->q.head; while (work) { @@ -158,7 +131,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * performed... we don't have any idea how long that will take! */ - irqrestore(flags); + //irqrestore(flags); worker(arg); /* Now, unfortunately, since we re-enabled interrupts we don't @@ -166,7 +139,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * back at the head of the list. */ - flags = irqsave(); + //flags = irqsave(); work = (FAR struct work_s *)wqueue->q.head; } else @@ -195,7 +168,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ usleep(next * USEC_PER_TICK); - irqrestore(flags); + //irqrestore(flags); } /**************************************************************************** @@ -258,7 +231,7 @@ int work_hpthread(int argc, char *argv[]) work_process(&g_work[HPWORK]); } - return OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #ifdef CONFIG_SCHED_LPWORK @@ -276,7 +249,7 @@ int work_lpthread(int argc, char *argv[]) * the IDLE thread (at a very, very low priority). */ - sched_garbagecollection(); + //sched_garbagecollection(); /* Then process queued work. We need to keep interrupts disabled while * we process items in the work list. @@ -285,7 +258,7 @@ int work_lpthread(int argc, char *argv[]) work_process(&g_work[LPWORK]); } - return OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #endif /* CONFIG_SCHED_LPWORK */ @@ -306,9 +279,13 @@ int work_usrthread(int argc, char *argv[]) work_process(&g_work[USRWORK]); } - return OK; /* To keep some compilers happy */ + return PX4_OK; /* To keep some compilers happy */ } #endif /* CONFIG_SCHED_USRWORK */ +int clock_systimer() +{ + return 1; +} #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index d2d1708eb6..5b1db531d6 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -44,6 +44,9 @@ #include #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 +#define CONFIG_SCHED_WORKQUEUE 1 +#define CONFIG_SCHED_HPWORK 1 +#define CONFIG_SCHED_LPWORK 1 #define CONFIG_ARCH_BOARD_LINUXTEST 1 #define px4_errx(x, ...) errx(x, __VA_ARGS__) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 26c9a85951..2777367568 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -123,7 +123,8 @@ __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)x/1000000L) +#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)(x)/1000000L) +#define USEC_PER_TICK (1000000L/PX4_TICKS_PER_SEC) #define px4_statfs_buf_f_bavail_t unsigned long diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 3a9adfd37e..02080e7133 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -55,6 +55,7 @@ typedef int px4_task_t; #define SCHED_DEFAULT SCHED_FIFO #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) #define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) typedef pthread_t px4_task_t; diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index b6552c204f..d5575b282e 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -43,10 +43,22 @@ #include #elif defined(__PX4_LINUX) +#include #include +__BEGIN_DECLS + +#define HPWORK 0 #define LPWORK 1 -#define HPWORK 2 +#define NWORKERS 2 + +struct wqueue_s +{ + pid_t pid; /* The task ID of the worker thread */ + struct dq_queue_s q; /* The queue of pending work */ +}; + +extern struct wqueue_s g_work[NWORKERS]; /* Defines the work callback */ @@ -90,8 +102,7 @@ struct work_s * ****************************************************************************/ -int work_queue(int qid, FAR struct work_s *work, worker_t worker, - FAR void *arg, uint32_t delay); +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay); /**************************************************************************** * Name: work_cancel @@ -110,7 +121,15 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker, * ****************************************************************************/ -int work_cancel(int qid, FAR struct work_s *work); +int work_cancel(int qid, struct work_s *work); + +int clock_systimer(void); + +int work_hpthread(int argc, char *argv[]); +int work_lpthread(int argc, char *argv[]); + +__END_DECLS + #else #error "Unknown target OS" #endif From cf71db74d7f7e5341fec8356ce2f2dd1c53f6156 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 25 Mar 2015 10:55:05 -0700 Subject: [PATCH 057/258] Support to specify build time OS target Now run: make PX4_TARGET_OS=nuttx or make PX4_TARGET_OS=linux To test the linux build and make sure that the required directories exist, run: make linuxrun Signed-off-by: Mark Charlebois --- Makefile | 2 +- Tools/linux_run.sh | 13 +++++++++++-- makefiles/setup.mk | 5 ++++- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index fbf5898519..4c009af00d 100644 --- a/Makefile +++ b/Makefile @@ -143,7 +143,7 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -testrun: +linuxrun: Tools/linux_run.sh # diff --git a/Tools/linux_run.sh b/Tools/linux_run.sh index 4572a4e3e2..8005b65539 100755 --- a/Tools/linux_run.sh +++ b/Tools/linux_run.sh @@ -3,7 +3,7 @@ if [ ! -c /tmp/ttyS0 ] || [ ! -c /tmp/ttyS1 ] then echo "Need to create /tmp/ttyS[01]" - echo "socat PTY,link=/tmp/ttyS0 PTY,link=/tmp/ttyS1" + echo "sudo socat PTY,link=/tmp/ttyS0 PTY,link=/tmp/ttyS1" exit 1 fi @@ -12,6 +12,15 @@ if [ ! -d /fs/msdcard ] && [ ! -w /fs/msdcard ] echo "Need to create/mount writable /fs/microsd" echo "sudo mkdir -p /fs/msdcard" echo "sudo chmod 777 /fs/msdcard" + exit 1 fi -Build/linux_default.build/mainapp +if [ ! -d /eeprom ] && [ ! -w /eeprom ] + then + echo "Need to create/mount writable /eeprom" + echo "sudo mkdir -p /eeprom" + echo "sudo chmod 777 /eeprom" + exit 1 +fi + +Build/linux_default.build/mainapp linux-config/linuxtest/init/rc.S diff --git a/makefiles/setup.mk b/makefiles/setup.mk index fd4cbb1a77..d8ba1304c9 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,8 +33,11 @@ # Path and tool setup # -#export PX4_TARGET_OS = nuttx +ifdef ($(PX4_TARGET_OS),nuttx) +export PX4_TARGET_OS = nuttx +else export PX4_TARGET_OS = linux +endif # # Some useful paths. From dffb8bb62c12c36c8cfe409a6111dfa7c9df29ba Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 25 Mar 2015 10:57:58 -0700 Subject: [PATCH 058/258] MuORB: handle case on no args passed if only uorb is called with no other args it crashes. Handle the case where no args are passed. Signed-off-by: Mark Charlebois --- src/modules/uORB/MuORB.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index 9dcab26b1c..548b36d029 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -1021,6 +1021,11 @@ info() } // namespace +static void usage() +{ + warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); +} + /* * uORB server 'main'. */ @@ -1029,6 +1034,11 @@ extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } int uorb_main(int argc, char *argv[]) { + if (argc < 2) { + usage(); + return -EINVAL; + } + /* * Start/load the driver. * @@ -1086,7 +1096,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) return info(); - warnx("unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); + usage(); return -EINVAL; } From eabc44afbe3277386e2e0bc382b0bca3a2c9c47a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 25 Mar 2015 18:28:06 -0700 Subject: [PATCH 059/258] Linux: Added work queue support and unit test PX4 uses NuttX data structures throughout so those data structures were preserved and used to implement high and low priority queues. A unit test for the work queues was added. The polling rate of the queues are set in px4_config.h in CONFIG_SCHED_WORKPERIOD. The units are milliseconds. Signed-off-by: Mark Charlebois --- linux-configs/linuxtest/init/rc.S | 4 + makefiles/linux/config_linux_default.mk | 1 + .../linux/px4_layer/px4_linux_impl.cpp | 19 ++++ .../linux/px4_layer/px4_linux_tasks.c | 29 +++++- src/platforms/linux/px4_layer/work_queue.c | 2 +- src/platforms/linux/px4_layer/work_thread.c | 18 ++-- src/platforms/linux/tests/wqueue/module.mk | 43 +++++++++ .../linux/tests/wqueue/wqueue_main.cpp | 55 +++++++++++ .../linux/tests/wqueue/wqueue_start_linux.cpp | 96 +++++++++++++++++++ .../linux/tests/wqueue/wqueue_test.cpp | 96 +++++++++++++++++++ .../linux/tests/wqueue/wqueue_test.h | 72 ++++++++++++++ src/platforms/px4_config.h | 3 + src/platforms/px4_tasks.h | 6 ++ src/platforms/px4_workqueue.h | 2 +- 14 files changed, 434 insertions(+), 12 deletions(-) create mode 100644 linux-configs/linuxtest/init/rc.S create mode 100644 src/platforms/linux/tests/wqueue/module.mk create mode 100644 src/platforms/linux/tests/wqueue/wqueue_main.cpp create mode 100644 src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp create mode 100644 src/platforms/linux/tests/wqueue/wqueue_test.cpp create mode 100644 src/platforms/linux/tests/wqueue/wqueue_test.h diff --git a/linux-configs/linuxtest/init/rc.S b/linux-configs/linuxtest/init/rc.S new file mode 100644 index 0000000000..72d31ac58e --- /dev/null +++ b/linux-configs/linuxtest/init/rc.S @@ -0,0 +1,4 @@ +uorb start +mavlink start +blink start +blink systemstate diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index e991e3e15f..ab996662d5 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -63,4 +63,5 @@ MODULES += platforms/linux/px4_layer #MODULES += platforms/linux/tests/hello #MODULES += platforms/linux/tests/vcdev_test #MODULES += platforms/linux/tests/hrt_test +#MODULES += platforms/linux/tests/wqueue diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index 7f06b7a916..a0620ce296 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include #include #include "systemlib/param/param.h" @@ -58,11 +60,28 @@ __END_DECLS extern struct wqueue_s gwork[NWORKERS]; +void sighandler(int sig) +{ + printf("Received sig %d\n", sig); +} + namespace px4 { void init(int argc, char *argv[], const char *app_name) { + int ret; + struct sigaction actions; + + memset(&actions, 0, sizeof(actions)); + sigemptyset(&actions.sa_mask); + actions.sa_flags = 0; + actions.sa_handler = sighandler; + ret = sigaction(SIGUSR2,&actions,NULL); + + if (ret < 0) { + printf("sigaction failed: %d\n", errno); + } printf("App name: %s\n", app_name); // Create high priority worker thread diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index 5b2c5abe33..0d710cad6d 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -153,16 +153,21 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_create (&task, &attr, (void *)&entry_adapter, (void *) taskdata); if (rv != 0) { - printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); if (rv == EPERM) { - printf("WARNING: INSUFFICIENT PRIVILEGE TO RUN REALTIME THREADS\n"); + printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + return (rv < 0) ? rv : -rv; + } + } + else { + return (rv < 0) ? rv : -rv; } - return (rv < 0) ? rv : -rv; } - //printf("pthread_create task=%d rv=%d\n",(int)task, rv); + printf("pthread_create task=%lu rv=%d\n",(unsigned long)task, rv); for (i=0; iqtime = clock_systimer(); /* Time work queued */ dq_addlast((dq_entry_t *)work, &wqueue->q); - pthread_kill(wqueue->pid, SIGUSR1); /* Wake up the worker thread */ + px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ //irqrestore(flags); return PX4_OK; diff --git a/src/platforms/linux/px4_layer/work_thread.c b/src/platforms/linux/px4_layer/work_thread.c index b238979ed4..9ff9ff8923 100644 --- a/src/platforms/linux/px4_layer/work_thread.c +++ b/src/platforms/linux/px4_layer/work_thread.c @@ -40,9 +40,11 @@ #include #include #include +#include #include #include #include +#include #ifdef CONFIG_SCHED_WORKQUEUE @@ -97,8 +99,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * we process items in the work list. */ - //next = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK; - next = 100; + next = CONFIG_SCHED_WORKPERIOD; //flags = irqsave(); work = (FAR struct work_s *)wqueue->q.head; while (work) @@ -110,6 +111,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ elapsed = clock_systimer() - work->qtime; + //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay); if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ @@ -132,7 +134,11 @@ static void work_process(FAR struct wqueue_s *wqueue) */ //irqrestore(flags); - worker(arg); + if (!worker) { + printf("MESSED UP: worker = 0\n"); + } + else + worker(arg); /* Now, unfortunately, since we re-enabled interrupts we don't * know the state of the work list and we will have to start @@ -167,7 +173,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * the time elapses or until we are awakened by a signal. */ - usleep(next * USEC_PER_TICK); + usleep(next); //irqrestore(flags); } @@ -284,8 +290,8 @@ int work_usrthread(int argc, char *argv[]) #endif /* CONFIG_SCHED_USRWORK */ -int clock_systimer() +uint32_t clock_systimer() { - return 1; + return (0x00000000ffffffff & hrt_absolute_time()); } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/linux/tests/wqueue/module.mk b/src/platforms/linux/tests/wqueue/module.mk new file mode 100644 index 0000000000..29f8f4cf71 --- /dev/null +++ b/src/platforms/linux/tests/wqueue/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = wqueue_test + +SRCS = wqueue_main.cpp \ + wqueue_start_linux.cpp \ + wqueue_test.cpp + diff --git a/src/platforms/linux/tests/wqueue/wqueue_main.cpp b/src/platforms/linux/tests/wqueue/wqueue_main.cpp new file mode 100644 index 0000000000..a7117cca09 --- /dev/null +++ b/src/platforms/linux/tests/wqueue/wqueue_main.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file wqueue_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include "wqueue_test.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "wqueue_test"); + + printf("wqueue hello\n"); + WQueueTest wq; + wq.main(); + + printf("goodbye\n"); + return 0; +} diff --git a/src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp b/src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp new file mode 100644 index 0000000000..7ac29a0d35 --- /dev/null +++ b/src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file wqueue_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "wqueue_test.h" +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]); +int wqueue_test_main(int argc, char *argv[]) +{ + + if (argc < 2) { + printf("usage: wqueue_test {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (WQueueTest::appState.isRunning()) { + printf("already running\n"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("wqueue", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + WQueueTest::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (WQueueTest::appState.isRunning()) { + printf("is running\n"); + + } else { + printf("not started\n"); + } + + return 0; + } + + printf("usage: wqueue_test {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/linux/tests/wqueue/wqueue_test.cpp b/src/platforms/linux/tests/wqueue/wqueue_test.cpp new file mode 100644 index 0000000000..3fc0e5a917 --- /dev/null +++ b/src/platforms/linux/tests/wqueue/wqueue_test.cpp @@ -0,0 +1,96 @@ + +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file wqueue_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include +#include "wqueue_test.h" +#include +#include + +px4::AppState WQueueTest::appState; + +void WQueueTest::hp_worker_cb(void *p) +{ + WQueueTest *wqep = (WQueueTest *)p; + + wqep->do_hp_work(); +} + +void WQueueTest::lp_worker_cb(void *p) +{ + WQueueTest *wqep = (WQueueTest *)p; + + wqep->do_lp_work(); +} + +void WQueueTest::do_lp_work() +{ + printf("done lp work\n"); + _lpwork_done = true; +} + +void WQueueTest::do_hp_work() +{ + printf("done hp work\n"); + _hpwork_done = true; +} + +int WQueueTest::main() +{ + appState.setRunning(true); + + //Put work on HP work queue + work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1); + + + //Put work on LP work queue + work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1); + + + // Wait for work to finsh + while (!appState.exitRequested() && !(_hpwork_done && _lpwork_done)) { + printf(" Sleeping...\n"); + sleep(2); + + printf(" Waiting on work...\n"); + } + + return 0; +} diff --git a/src/platforms/linux/tests/wqueue/wqueue_test.h b/src/platforms/linux/tests/wqueue/wqueue_test.h new file mode 100644 index 0000000000..6db3fc1e25 --- /dev/null +++ b/src/platforms/linux/tests/wqueue/wqueue_test.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file wqueue_test.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include +#include +#include + +class WQueueTest { +public: + WQueueTest() : + _lpwork_done(false), + _hpwork_done(false) + { + memset(&_lpwork, 0, sizeof(_lpwork)); + memset(&_hpwork, 0, sizeof(_hpwork)); + }; + + ~WQueueTest() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +private: + static void hp_worker_cb(void *p); + static void lp_worker_cb(void *p); + + void do_lp_work(void); + void do_hp_work(void); + + bool _lpwork_done; + bool _hpwork_done; + work_s _lpwork; + work_s _hpwork; +}; diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 5b1db531d6..82a79312a0 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -49,6 +49,9 @@ #define CONFIG_SCHED_LPWORK 1 #define CONFIG_ARCH_BOARD_LINUXTEST 1 +/** time in ms between checks for work in work queues **/ +#define CONFIG_SCHED_WORKPERIOD 10 + #define px4_errx(x, ...) errx(x, __VA_ARGS__) #endif diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 02080e7133..b89c6657d3 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -86,7 +86,13 @@ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name, px4_main_t entry, char * const argv[]); +/** Deletes a task - does not do resource cleanup **/ __EXPORT int px4_task_delete(px4_task_t pid); + +/** Send a signal to a task **/ +__EXPORT int px4_task_kill(px4_task_t pid, int sig); + +/** Exit current task with return value **/ __EXPORT void px4_task_exit(int ret); __END_DECLS diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index d5575b282e..686bfdf258 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -123,7 +123,7 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_ int work_cancel(int qid, struct work_s *work); -int clock_systimer(void); +uint32_t clock_systimer(void); int work_hpthread(int argc, char *argv[]); int work_lpthread(int argc, char *argv[]); From e3f137ce2373111d69d14362c2517f5bfc99bbcf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 26 Mar 2015 12:37:55 -0700 Subject: [PATCH 060/258] Fixed gcc 4.8 warnings Disabled gcc warnings that are tripped by Eigen. Removed signal code that is not needed in Linux port and was causing gcc warnings. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 4 +++- .../linux/px4_layer/px4_linux_impl.cpp | 22 ------------------- src/systemcmds/param/param.c | 2 +- 3 files changed, 4 insertions(+), 24 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index f790940b89..83a9fe5856 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -61,7 +61,7 @@ CLANGVER= endif endif -#USE_GCC=1 +USE_GCC=0 ifeq ($(USE_GCC),1) # GCC Options: @@ -162,6 +162,8 @@ ARCHWARNINGS += -Wdouble-promotion \ -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ + -Wno-error=unused-local-typedefs \ + -Wno-error=enum-compare \ -Werror=double-promotion ARCHOPTIMIZATION += -fno-strength-reduce endif diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index a0620ce296..973b85abda 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -49,39 +49,17 @@ __BEGIN_DECLS -// FIXME - This needs to be properly initialized -struct param_info_s param_array[256]; -struct param_info_s *param_info_base; -struct param_info_s *param_info_limit; - long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); __END_DECLS extern struct wqueue_s gwork[NWORKERS]; -void sighandler(int sig) -{ - printf("Received sig %d\n", sig); -} - namespace px4 { void init(int argc, char *argv[], const char *app_name) { - int ret; - struct sigaction actions; - - memset(&actions, 0, sizeof(actions)); - sigemptyset(&actions.sa_mask); - actions.sa_flags = 0; - actions.sa_handler = sighandler; - ret = sigaction(SIGUSR2,&actions,NULL); - - if (ret < 0) { - printf("sigaction failed: %d\n", errno); - } printf("App name: %s\n", app_name); // Create high priority worker thread diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 2c74e4b312..30baaeb49c 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -182,7 +182,7 @@ static int do_save(const char *param_file_name) { /* create the file */ - int fd = open(param_file_name, O_WRONLY | O_CREAT); + int fd = open(param_file_name, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("opening '%s' failed", param_file_name); From 8b985331a49a1247d7de6c37a887b019833dcb96 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 26 Mar 2015 15:43:17 -0700 Subject: [PATCH 061/258] Linux: align pointers on 64bit __param_start and __param end need to be 8 byte aligned on 64bit machines. Changed linker script to 8 byte align __param section. Signed-off-by: Mark Charlebois --- linux-configs/linuxtest/scripts/ld.script | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux-configs/linuxtest/scripts/ld.script b/linux-configs/linuxtest/scripts/ld.script index 176917ff77..2e92f20a69 100644 --- a/linux-configs/linuxtest/scripts/ld.script +++ b/linux-configs/linuxtest/scripts/ld.script @@ -53,7 +53,7 @@ SECTIONS /* * Construction data for parameters. */ - __param ALIGN(4): { + __param ALIGN(8): { __param_start = .; KEEP(*(__param*)) __param_end = .; From 1805c21226133f239d9206dc0633730dfec150f3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 26 Mar 2015 16:01:30 -0700 Subject: [PATCH 062/258] Linux: default to clang build The build will now fail if clang is not found. To force the use of GCC, use: make USE_GCC=1 The toolchain makefile was modified so it no longer checks for various versions of clang if USE_GCC=1 is passed. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 83a9fe5856..2332529a76 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -1,7 +1,7 @@ # # Copyright (C) 2012-2014 PX4 Development Team. All rights reuint32_tserved. # -# 2005 Modified for clang and GCC on Linux: +# 2005 Modified for clang and GCC on Linux: # Author: Mark Charlebois # # Redistribution and use in source and binary forms, with or without @@ -42,6 +42,9 @@ # # Set to 1 for GCC-4.8.2 and to 0 for Clang-3.5 (Ubuntu 14.04) +USE_GCC?=0 + +ifneq ($(USE_GCC),1) HAVE_CLANG35=$(shell clang-3.5 -dumpversion) @@ -61,7 +64,13 @@ CLANGVER= endif endif -USE_GCC=0 +# If no version of clang was found +ifeq ($(HAVE_CLANG35),) +ifeq ($(HAVE_CLANG35),) +$(error Clang not found. Try make USE_GCC=1) +endif +endif +endif # USE_GCC is not 1 ifeq ($(USE_GCC),1) # GCC Options: @@ -115,7 +124,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Dnoreturn_function= \ -I/usr/include/eigen3 \ -I$(PX4_BASE)/src/platforms/linux/include \ - -Wno-error=shadow + -Wno-error=shadow # optimisation flags # @@ -162,10 +171,8 @@ ARCHWARNINGS += -Wdouble-promotion \ -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ - -Wno-error=unused-local-typedefs \ - -Wno-error=enum-compare \ - -Werror=double-promotion -ARCHOPTIMIZATION += -fno-strength-reduce + -Werror=double-promotion +ARCHOPTIMIZATION += -fno-strength-reduce endif # -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+ @@ -184,7 +191,9 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ # Add compiler specific options ifeq ($(USE_GCC),1) ARCHCWARNINGS += -Wold-style-declaration \ - -Wmissing-parameter-type + -Wmissing-parameter-type \ + -Wno-error=unused-local-typedefs \ + -Wno-error=enum-compare endif # C++-specific warnings @@ -194,7 +203,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) -#EXTRA_LIBS += $(LIBM) +#EXTRA_LIBS += $(LIBM) EXTRA_LIBS += -pthread -lm -lrt # Flags we pass to the C compiler @@ -243,7 +252,7 @@ LDSCRIPT = $(PX4_BASE)/linux-configs/linuxtest/scripts/ld.script # Flags we pass to the linker # LDFLAGS += $(EXTRALDFLAGS) \ - $(addprefix -L,$(LIB_DIRS)) + $(addprefix -L,$(LIB_DIRS)) # Compiler support library # @@ -252,7 +261,7 @@ LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) # Files that the final link depends on # #LINK_DEPS += $(LDSCRIPT) -LINK_DEPS += +LINK_DEPS += # Files to include to get automated dependencies # @@ -319,7 +328,7 @@ define LINK_A @$(ECHO) "LINK_A: $1" @$(MKDIR) -p $(dir $1) echo "$(Q) $(AR) $1 $2" - $(Q) $(AR) $1 $2 + $(Q) $(AR) $1 $2 endef # Link the objects in $2 into the shared library $1 From 994065ef47d99747ae024d7c09b274c6332b3503 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 26 Mar 2015 16:57:56 -0700 Subject: [PATCH 063/258] Linux: dont check stacksize in sdlog2 stacksize check in sdlog2 fails for x86_64 Signed-off-by: Mark Charlebois --- src/modules/sdlog2/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 6964acf339..30fc846a27 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,6 +46,8 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os +ifeq ($(PX4_TARGET_OS),nuttx) EXTRACFLAGS = -Wframe-larger-than=1400 +endif EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' From 3ab76caa78d9f504056d94cf8568ed5e8c9e0077 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 26 Mar 2015 17:00:16 -0700 Subject: [PATCH 064/258] Linux: Changed non-fatal px4_errx to warnx px4_errx kills the process, so if possible we want to end the thread but not the process. Using warnx and return exits gracefully. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main_linux.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index 633ba8554f..4f5890c92f 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -1323,7 +1323,8 @@ Mavlink::task_main(int argc, char *argv[]) * marker ring buffer approach. */ if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { - px4_errx(1, "msg buf:"); + warnx("msg buf:"); + return 1; } /* initialize message buffer mutex */ @@ -1747,11 +1748,13 @@ Mavlink::stream_command(int argc, char *argv[]) // If the link is not running we should complain, but not fall over // because this is so easy to get wrong and not fatal. Warning is sufficient. - px4_errx(0, "mavlink for device %s is not running", device_name); + warnx("mavlink for device %s is not running", device_name); + return 0; } } else { - px4_errx(1, "usage: mavlink stream [-d device] -s stream -r rate"); + warnx("usage: mavlink stream [-d device] -s stream -r rate"); + return 1; } return OK; From 509f7f9fdbe70c85ba6e078af269180b93b6ce57 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 28 Mar 2015 03:16:55 +0000 Subject: [PATCH 065/258] Linux: changed to use submodule version of eigen vs system version Eigen no longer needs to be installed on the build machine as it is downloaded as a submodule. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 2332529a76..3ef60a4f65 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -122,7 +122,7 @@ endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -D__PX4_LINUX \ -Dnoreturn_function= \ - -I/usr/include/eigen3 \ + -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/linux/include \ -Wno-error=shadow From a2a113ee283f3560a3cb0d26ad75bd2cc7d19b5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Mar 2015 00:55:04 +0100 Subject: [PATCH 066/258] Ported mc_att_control --- .../mc_att_control/mc_att_control_main.cpp | 38 +++++++++++-------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c579ce8830..93d2116b46 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -894,10 +894,8 @@ MulticopterAttitudeControl::task_main() perf_end(_loop_perf); } - warnx("exit"); - _control_task = -1; - _exit(0); + return; } int @@ -924,43 +922,53 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: mc_att_control {start|stop|status}"); + warnx("usage: mc_att_control {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { - if (mc_att_control::g_control != nullptr) - errx(1, "already running"); + if (mc_att_control::g_control != nullptr) { + warnx("already running"); + return 1; + } mc_att_control::g_control = new MulticopterAttitudeControl; - if (mc_att_control::g_control == nullptr) - errx(1, "alloc failed"); + if (mc_att_control::g_control == nullptr) { + warnx("alloc failed"); + return 1; + } if (OK != mc_att_control::g_control->start()) { delete mc_att_control::g_control; mc_att_control::g_control = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { - if (mc_att_control::g_control == nullptr) - errx(1, "not running"); + if (mc_att_control::g_control == nullptr) { + warnx("not running"); + return 1; + } delete mc_att_control::g_control; mc_att_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (mc_att_control::g_control) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } From b4d52327e848c4c99b6dc643d356024faf15f68d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Mar 2015 03:02:15 +0200 Subject: [PATCH 067/258] att EKF: Enable execution on Linux --- .../attitude_estimator_ekf_main.cpp | 28 ++++++++++++------- .../attitude_estimator_ekf_params.c | 1 + 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 30439fad58..0a55756163 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -101,11 +100,11 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); - exit(1); } /** @@ -120,6 +119,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -137,29 +137,29 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) 7700, attitude_estimator_ekf_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); - exit(0); + return 0; } else { warnx("not started"); - exit(1); + return 1; } - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } /* @@ -536,7 +536,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) debugOutput); /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + if (std::isfinite(euler[0]) && std::isfinite(euler[1]) && std::isfinite(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); @@ -546,7 +546,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } if (last_data > 0 && raw.timestamp - last_data > 30000) { +<<<<<<< HEAD warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data); +======= + warnx("data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); +>>>>>>> att EKF: Enable execution on Linux } last_data = raw.timestamp; @@ -583,8 +587,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; +<<<<<<< HEAD if (isfinite(att.q[0]) && isfinite(att.q[1]) && isfinite(att.q[2]) && isfinite(att.q[3])) { +======= + if (std::isfinite(att.roll) && std::isfinite(att.pitch) && std::isfinite(att.yaw)) { +>>>>>>> att EKF: Enable execution on Linux // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index fe480e12b7..328236d231 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -41,6 +41,7 @@ #include "attitude_estimator_ekf_params.h" #include +#include /* Extended Kalman Filter covariances */ From 7bdaec9ff05425faaa7f007cc46b2a08958b9d0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Mar 2015 03:02:31 +0200 Subject: [PATCH 068/258] att + pos EKF: Enable execution on Linux --- .../ekf_att_pos_estimator_main.cpp | 90 ++++++++----------- .../estimator_22states.cpp | 24 ++--- 2 files changed, 50 insertions(+), 64 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e1446bbe9b..ecd67ecf7a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -44,6 +44,7 @@ #include "estimator_22states.h" #include +#include #include #include #include @@ -281,7 +282,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_estimator_task); + px4_task_delete(_estimator_task); break; } } while (_estimator_task != -1); @@ -490,7 +491,8 @@ void AttitudePositionEstimatorEKF::task_main() _filter_start_time = hrt_absolute_time(); if (!_ekf) { - errx(1, "OUT OF MEM!"); + warnx("OUT OF MEM!"); + return; } /* @@ -696,10 +698,8 @@ void AttitudePositionEstimatorEKF::task_main() _task_running = false; - warnx("exiting.\n"); - _estimator_task = -1; - _exit(0); + return; } void AttitudePositionEstimatorEKF::initializeGPS() @@ -1039,7 +1039,7 @@ int AttitudePositionEstimatorEKF::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 7500, - (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, + (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); if (_estimator_task < 0) { @@ -1155,7 +1155,7 @@ void AttitudePositionEstimatorEKF::pollData() float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ - if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + if (!std::isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } @@ -1167,9 +1167,9 @@ void AttitudePositionEstimatorEKF::pollData() int last_gyro_main = _gyro_main; - if (isfinite(_sensor_combined.gyro_rad_s[0]) && - isfinite(_sensor_combined.gyro_rad_s[1]) && - isfinite(_sensor_combined.gyro_rad_s[2]) && + if (std::isfinite(_sensor_combined.gyro_rad_s[0]) && + std::isfinite(_sensor_combined.gyro_rad_s[1]) && + std::isfinite(_sensor_combined.gyro_rad_s[2]) && (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; @@ -1178,9 +1178,9 @@ void AttitudePositionEstimatorEKF::pollData() _gyro_main = 0; _gyro_valid = true; - } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && - isfinite(_sensor_combined.gyro1_rad_s[1]) && - isfinite(_sensor_combined.gyro1_rad_s[2])) { + } else if (std::isfinite(_sensor_combined.gyro1_rad_s[0]) && + std::isfinite(_sensor_combined.gyro1_rad_s[1]) && + std::isfinite(_sensor_combined.gyro1_rad_s[2])) { _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; @@ -1528,25 +1528,29 @@ int AttitudePositionEstimatorEKF::trip_nan() int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); + warnx("usage: ekf_att_pos_estimator {start|stop|status|logging}"); + return 1; } if (!strcmp(argv[1], "start")) { if (estimator::g_estimator != nullptr) { - errx(1, "already running"); + warnx("already running"); + return 1; } estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != estimator::g_estimator->start()) { delete estimator::g_estimator; estimator::g_estimator = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ @@ -1558,64 +1562,46 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) printf("\n"); - exit(0); + return 0; + } + + if (estimator::g_estimator == nullptr) { + warnx("not running"); + return 1; } if (!strcmp(argv[1], "stop")) { - if (estimator::g_estimator == nullptr) { - errx(1, "not running"); - } delete estimator::g_estimator; estimator::g_estimator = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { - if (estimator::g_estimator) { - warnx("running"); + warnx("running"); - estimator::g_estimator->print_status(); + estimator::g_estimator->print_status(); - exit(0); - - } else { - errx(1, "not running"); - } + return 0; } if (!strcmp(argv[1], "trip")) { - if (estimator::g_estimator) { - int ret = estimator::g_estimator->trip_nan(); + int ret = estimator::g_estimator->trip_nan(); - exit(ret); - - } else { - errx(1, "not running"); - } + return ret; } if (!strcmp(argv[1], "logging")) { - if (estimator::g_estimator) { - int ret = estimator::g_estimator->enable_logging(true); + int ret = estimator::g_estimator->enable_logging(true); - exit(ret); - - } else { - errx(1, "not running"); - } + return ret; } if (!strcmp(argv[1], "debug")) { - if (estimator::g_estimator) { - int debug = strtoul(argv[2], NULL, 10); - int ret = estimator::g_estimator->set_debuglevel(debug); + int debug = strtoul(argv[2], NULL, 10); + int ret = estimator::g_estimator->set_debuglevel(debug); - exit(ret); - - } else { - errx(1, "not running"); - } + return ret; } warnx("unrecognized command"); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 967402fff7..6235aa5b8b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2392,9 +2392,9 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error { for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) { - if (isfinite(storedStates[i][bestStoreIndex])) { + if (std::isfinite(storedStates[i][bestStoreIndex])) { statesForFusion[i] = storedStates[i][bestStoreIndex]; - } else if (isfinite(states[i])) { + } else if (std::isfinite(states[i])) { statesForFusion[i] = states[i]; } else { // There is not much we can do here, except reporting the error we just @@ -2406,7 +2406,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) else // otherwise output current state { for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - if (isfinite(states[i])) { + if (std::isfinite(states[i])) { statesForFusion[i] = states[i]; } else { ret++; @@ -2630,7 +2630,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } - if (!isfinite(val)) { + if (!std::isfinite(val)) { ekf_debug("constrain: non-finite!"); } @@ -2922,21 +2922,21 @@ bool AttPosEKF::StatesNaN() { bool err = false; // check all integrators - if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { + if (!std::isfinite(summedDelAng.x) || !std::isfinite(summedDelAng.y) || !std::isfinite(summedDelAng.z)) { current_ekf_state.angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles - if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { + if (!std::isfinite(correctedDelAng.x) || !std::isfinite(correctedDelAng.y) || !std::isfinite(correctedDelAng.z)) { current_ekf_state.angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles - if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { + if (!std::isfinite(summedDelVel.x) || !std::isfinite(summedDelVel.y) || !std::isfinite(summedDelVel.z)) { current_ekf_state.summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; @@ -2946,7 +2946,7 @@ bool AttPosEKF::StatesNaN() { // check all states and covariance matrices for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { - if (!isfinite(KH[i][j])) { + if (!std::isfinite(KH[i][j])) { current_ekf_state.KHNaN = true; err = true; @@ -2954,7 +2954,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // intermediate result used for covariance updates - if (!isfinite(KHP[i][j])) { + if (!std::isfinite(KHP[i][j])) { current_ekf_state.KHPNaN = true; err = true; @@ -2962,7 +2962,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // intermediate result used for covariance updates - if (!isfinite(P[i][j])) { + if (!std::isfinite(P[i][j])) { current_ekf_state.covarianceNaN = true; err = true; @@ -2970,7 +2970,7 @@ bool AttPosEKF::StatesNaN() { } // covariance matrix } - if (!isfinite(Kfusion[i])) { + if (!std::isfinite(Kfusion[i])) { current_ekf_state.kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); @@ -2978,7 +2978,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // Kalman gains - if (!isfinite(states[i])) { + if (!std::isfinite(states[i])) { current_ekf_state.statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); From 07b16ffa01905bb60ea93f914588f9dc38fa2c46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Mar 2015 03:03:12 +0200 Subject: [PATCH 069/258] Enable estimators in config --- makefiles/linux/config_linux_default.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index ab996662d5..4b4cf7fc71 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -28,7 +28,8 @@ MODULES += modules/mavlink # # Estimation modules (EKF/ SO3 / other filters) # -#MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator # # Vehicle Control From 8c8f57b5b4e45e8b84ee4341607dd8f5c07bde35 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 15:03:21 -0700 Subject: [PATCH 070/258] Fixed std::isfinite vs isfinite differences Added PX4_ISFINITE(x) to px4_defines.h to handle the differences on NuttX and Linux. This change also picked up some file renaming for virtual character devices Signed-off-by: Mark Charlebois --- src/drivers/device/module.mk | 4 +-- src/drivers/device/{vcdev.cpp => vdev.cpp} | 0 src/drivers/device/{vdevice.h => vdev.h} | 0 .../{vcdev_posix.cpp => vdev_posix.cpp} | 0 .../attitude_estimator_ekf_main.cpp | 17 ++++-------- .../ekf_att_pos_estimator_main.cpp | 15 ++++++----- .../estimator_22states.cpp | 27 ++++++++++--------- .../mc_att_control/mc_att_control_main.cpp | 11 ++++---- src/platforms/px4_defines.h | 4 +++ 9 files changed, 39 insertions(+), 39 deletions(-) rename src/drivers/device/{vcdev.cpp => vdev.cpp} (100%) rename src/drivers/device/{vdevice.h => vdev.h} (100%) rename src/drivers/device/{vcdev_posix.cpp => vdev_posix.cpp} (100%) diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index a3660d70a5..c927426ae7 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -44,8 +44,8 @@ SRCS = \ spi.cpp endif ifeq ($(PX4_TARGET_OS),linux) -SRCS = vcdev.cpp \ +SRCS = vdev.cpp \ device.cpp \ - vcdev_posix.cpp \ + vdev_posix.cpp \ i2c_linux.cpp endif diff --git a/src/drivers/device/vcdev.cpp b/src/drivers/device/vdev.cpp similarity index 100% rename from src/drivers/device/vcdev.cpp rename to src/drivers/device/vdev.cpp diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdev.h similarity index 100% rename from src/drivers/device/vdevice.h rename to src/drivers/device/vdev.h diff --git a/src/drivers/device/vcdev_posix.cpp b/src/drivers/device/vdev_posix.cpp similarity index 100% rename from src/drivers/device/vcdev_posix.cpp rename to src/drivers/device/vdev_posix.cpp diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 0a55756163..affe6a68a4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -42,6 +42,7 @@ */ #include +#include #include #include #include @@ -536,7 +537,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) debugOutput); /* swap values for next iteration, check for fatal inputs */ - if (std::isfinite(euler[0]) && std::isfinite(euler[1]) && std::isfinite(euler[2])) { + if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); @@ -546,11 +547,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } if (last_data > 0 && raw.timestamp - last_data > 30000) { -<<<<<<< HEAD - warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data); -======= - warnx("data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); ->>>>>>> att EKF: Enable execution on Linux + warnx("sensor data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); } last_data = raw.timestamp; @@ -587,12 +584,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; -<<<<<<< HEAD - if (isfinite(att.q[0]) && isfinite(att.q[1]) - && isfinite(att.q[2]) && isfinite(att.q[3])) { -======= - if (std::isfinite(att.roll) && std::isfinite(att.pitch) && std::isfinite(att.yaw)) { ->>>>>>> att EKF: Enable execution on Linux + if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) + && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index ecd67ecf7a..41c62875bb 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -44,6 +44,7 @@ #include "estimator_22states.h" #include +#include #include #include #include @@ -1155,7 +1156,7 @@ void AttitudePositionEstimatorEKF::pollData() float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ - if (!std::isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } @@ -1167,9 +1168,9 @@ void AttitudePositionEstimatorEKF::pollData() int last_gyro_main = _gyro_main; - if (std::isfinite(_sensor_combined.gyro_rad_s[0]) && - std::isfinite(_sensor_combined.gyro_rad_s[1]) && - std::isfinite(_sensor_combined.gyro_rad_s[2]) && + if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) && + PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) && + PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) && (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; @@ -1178,9 +1179,9 @@ void AttitudePositionEstimatorEKF::pollData() _gyro_main = 0; _gyro_valid = true; - } else if (std::isfinite(_sensor_combined.gyro1_rad_s[0]) && - std::isfinite(_sensor_combined.gyro1_rad_s[1]) && - std::isfinite(_sensor_combined.gyro1_rad_s[2])) { + } else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) && + PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) && + PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) { _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 6235aa5b8b..d3200047ed 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -37,6 +37,7 @@ * @author Lorenz Meier */ +#include #include "estimator_22states.h" #include #include @@ -2392,9 +2393,9 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error { for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) { - if (std::isfinite(storedStates[i][bestStoreIndex])) { + if (PX4_ISFINITE(storedStates[i][bestStoreIndex])) { statesForFusion[i] = storedStates[i][bestStoreIndex]; - } else if (std::isfinite(states[i])) { + } else if (PX4_ISFINITE(states[i])) { statesForFusion[i] = states[i]; } else { // There is not much we can do here, except reporting the error we just @@ -2406,7 +2407,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) else // otherwise output current state { for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { - if (std::isfinite(states[i])) { + if (PX4_ISFINITE(states[i])) { statesForFusion[i] = states[i]; } else { ret++; @@ -2630,7 +2631,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } - if (!std::isfinite(val)) { + if (!PX4_ISFINITE(val)) { ekf_debug("constrain: non-finite!"); } @@ -2710,7 +2711,7 @@ void AttPosEKF::ConstrainStates() // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) // Constrain dtIMUfilt - if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) { + if (!PX4_ISFINITE(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) { dtIMUfilt = dtIMU; } @@ -2922,21 +2923,21 @@ bool AttPosEKF::StatesNaN() { bool err = false; // check all integrators - if (!std::isfinite(summedDelAng.x) || !std::isfinite(summedDelAng.y) || !std::isfinite(summedDelAng.z)) { + if (!PX4_ISFINITE(summedDelAng.x) || !PX4_ISFINITE(summedDelAng.y) || !PX4_ISFINITE(summedDelAng.z)) { current_ekf_state.angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles - if (!std::isfinite(correctedDelAng.x) || !std::isfinite(correctedDelAng.y) || !std::isfinite(correctedDelAng.z)) { + if (!PX4_ISFINITE(correctedDelAng.x) || !PX4_ISFINITE(correctedDelAng.y) || !PX4_ISFINITE(correctedDelAng.z)) { current_ekf_state.angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles - if (!std::isfinite(summedDelVel.x) || !std::isfinite(summedDelVel.y) || !std::isfinite(summedDelVel.z)) { + if (!PX4_ISFINITE(summedDelVel.x) || !PX4_ISFINITE(summedDelVel.y) || !PX4_ISFINITE(summedDelVel.z)) { current_ekf_state.summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; @@ -2946,7 +2947,7 @@ bool AttPosEKF::StatesNaN() { // check all states and covariance matrices for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { - if (!std::isfinite(KH[i][j])) { + if (!PX4_ISFINITE(KH[i][j])) { current_ekf_state.KHNaN = true; err = true; @@ -2954,7 +2955,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // intermediate result used for covariance updates - if (!std::isfinite(KHP[i][j])) { + if (!PX4_ISFINITE(KHP[i][j])) { current_ekf_state.KHPNaN = true; err = true; @@ -2962,7 +2963,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // intermediate result used for covariance updates - if (!std::isfinite(P[i][j])) { + if (!PX4_ISFINITE(P[i][j])) { current_ekf_state.covarianceNaN = true; err = true; @@ -2970,7 +2971,7 @@ bool AttPosEKF::StatesNaN() { } // covariance matrix } - if (!std::isfinite(Kfusion[i])) { + if (!PX4_ISFINITE(Kfusion[i])) { current_ekf_state.kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); @@ -2978,7 +2979,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // Kalman gains - if (!std::isfinite(states[i])) { + if (!PX4_ISFINITE(states[i])) { current_ekf_state.statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 93d2116b46..37f7eae671 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -54,6 +54,7 @@ */ #include +#include #include #include #include @@ -722,7 +723,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - if (std::isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } @@ -859,10 +860,10 @@ MulticopterAttitudeControl::task_main() control_attitude_rates(dt); /* publish actuator controls */ - _actuators.control[0] = (std::isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (std::isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (std::isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (std::isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2777367568..1cca8e55de 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -104,6 +104,8 @@ typedef param_t px4_param_t; #define px4_statfs_buf_f_bavail_t int +#define PX4_ISFINITE(x) isfinite(x) + /* Linux Specific defines */ #elif defined(__PX4_LINUX) @@ -165,6 +167,8 @@ __END_DECLS #define M_DEG_TO_RAD 0.01745329251994 #define M_RAD_TO_DEG 57.2957795130823 +#define PX4_ISFINITE(x) std::isfinite(x) + #endif /* Defines for all platforms */ From ccd18929fc7a9d573c0a439ee9545a232efeaa66 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 15:06:15 -0700 Subject: [PATCH 071/258] Linux: changed CDev to VDev for virtual device implementation To avoid confusion when a real device and a virtual device is being used, changed CDev to VDev for Linux. Signed-off-by: Mark Charlebois --- src/drivers/device/device.h | 2 +- src/drivers/device/i2c_linux.cpp | 6 +- src/drivers/device/i2c_linux.h | 27 +----- src/drivers/device/spi.h | 4 + src/drivers/device/vdev.cpp | 97 +++++++++++----------- src/drivers/device/vdev.h | 18 ++-- src/drivers/device/vdev_posix.cpp | 18 ++-- src/drivers/ms5611/ms5611_linux.cpp | 10 +-- src/modules/mavlink/mavlink_main_linux.cpp | 2 +- src/modules/mavlink/mavlink_main_linux.h | 2 +- src/modules/uORB/MuORB.cpp | 22 ++--- 11 files changed, 95 insertions(+), 113 deletions(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 98851edc83..4d941b752d 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -36,6 +36,6 @@ #ifdef __PX4_NUTTX #include "device_nuttx.h" #elif defined (__PX4_LINUX) -#include "vdevice.h" +#include "vdev.h" #endif diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 4399626aa7..4fe2c415c1 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -55,7 +55,7 @@ I2C::I2C(const char *name, int bus, uint16_t address) : // base class - CDev(name, devname), + VDev(name, devname), // public // protected _retries(0), @@ -93,7 +93,7 @@ I2C::init() // way to set it from user space. // do base class init, which will create device node, etc - ret = CDev::init(); + ret = VDev::init(); if (ret != PX4_OK) { debug("cdev init failed"); @@ -231,7 +231,7 @@ int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) return 0; default: /* give it to the superclass */ - return CDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(handlep, cmd, arg); } } diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h index aae1e7f588..a49c03267d 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_linux.h @@ -40,7 +40,7 @@ #ifndef _DEVICE_I2C_H #define _DEVICE_I2C_H -#include "device.h" +#include "vdev.h" #include #include @@ -53,7 +53,7 @@ namespace device __EXPORT /** * Abstract class for character device on I2C */ -class __EXPORT I2C : public CDev +class __EXPORT I2C : public VDev { public: @@ -92,12 +92,6 @@ protected: virtual ~I2C(); virtual int init(); -#if 0 - /** - * Check for the presence of the device on the bus. - */ - virtual int probe(); -#endif virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); @@ -126,25 +120,10 @@ protected: */ int transfer(struct i2c_msg *msgv, unsigned msgs); -#if 0 - /** - * Change the bus address. - * - * Most often useful during probe() when the driver is testing - * several possible bus addresses. - * - * @param address The new bus address to set. - */ - void set_address(uint16_t address) { - _address = address; - _device_id.devid_s.address = _address; - } -#endif - private: uint16_t _address; int _fd; - std::string _dname; + std::string _dname; I2C(const device::I2C&); I2C operator=(const device::I2C&); diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 4822880144..2f44f3cafa 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -50,7 +50,11 @@ namespace device __EXPORT /** * Abstract class for character device on SPI */ +#ifdef __PX4_NUTTX class __EXPORT SPI : public CDev +#else +class __EXPORT SPI : public VDev +#endif { protected: /** diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index da1ff05764..1e69384e68 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -38,7 +38,7 @@ */ #include "px4_posix.h" -#include "device.h" +#include "vdev.h" #include "drivers/drv_device.h" #include @@ -49,7 +49,6 @@ namespace device { int px4_errno; - struct px4_dev_t { char *name; @@ -73,7 +72,7 @@ static px4_dev_t *devmap[PX4_MAX_DEV]; * directly, so we have to bounce them through this dispatch table. */ -CDev::CDev(const char *name, +VDev::VDev(const char *name, const char *devname) : // base class Device(name), @@ -89,14 +88,14 @@ CDev::CDev(const char *name, _pollset[i] = nullptr; } -CDev::~CDev() +VDev::~VDev() { if (_registered) unregister_driver(_devname); } int -CDev::register_class_devname(const char *class_devname) +VDev::register_class_devname(const char *class_devname) { if (class_devname == nullptr) { return -EINVAL; @@ -119,7 +118,7 @@ CDev::register_class_devname(const char *class_devname) } int -CDev::register_driver(const char *name, void *data) +VDev::register_driver(const char *name, void *data) { int ret = -ENOSPC; @@ -145,7 +144,7 @@ CDev::register_driver(const char *name, void *data) } int -CDev::unregister_driver(const char *name) +VDev::unregister_driver(const char *name) { int ret = -ENOSPC; @@ -165,7 +164,7 @@ CDev::unregister_driver(const char *name) } int -CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +VDev::unregister_class_devname(const char *class_devname, unsigned class_instance) { char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); @@ -180,7 +179,7 @@ CDev::unregister_class_devname(const char *class_devname, unsigned class_instanc } int -CDev::init() +VDev::init() { // base class init first int ret = Device::init(); @@ -206,11 +205,11 @@ out: * Default implementations of the character device interface */ int -CDev::open(px4_dev_handle_t *handlep) +VDev::open(px4_dev_handle_t *handlep) { int ret = PX4_OK; - debug("CDev::open"); + debug("VDev::open"); lock(); /* increment the open count */ _open_count++; @@ -230,16 +229,16 @@ CDev::open(px4_dev_handle_t *handlep) } int -CDev::open_first(px4_dev_handle_t *handlep) +VDev::open_first(px4_dev_handle_t *handlep) { - debug("CDev::open_first"); + debug("VDev::open_first"); return PX4_OK; } int -CDev::close(px4_dev_handle_t *handlep) +VDev::close(px4_dev_handle_t *handlep) { - debug("CDev::close"); + debug("VDev::close"); int ret = PX4_OK; lock(); @@ -262,68 +261,66 @@ CDev::close(px4_dev_handle_t *handlep) } int -CDev::close_last(px4_dev_handle_t *handlep) +VDev::close_last(px4_dev_handle_t *handlep) { - debug("CDev::close_last"); + debug("VDev::close_last"); return PX4_OK; } ssize_t -CDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +VDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) { - debug("CDev::read"); + debug("VDev::read"); return -ENOSYS; } ssize_t -CDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +VDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) { - debug("CDev::write"); + debug("VDev::write"); return -ENOSYS; } off_t -CDev::seek(px4_dev_handle_t *handlep, off_t offset, int whence) +VDev::seek(px4_dev_handle_t *handlep, off_t offset, int whence) { return -ENOSYS; } int -CDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) +VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) { - debug("CDev::ioctl"); + int ret = -ENOTTY; + + debug("VDev::ioctl"); switch (cmd) { /* fetch a pointer to the driver's private data */ case PX4_DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; - return PX4_OK; + ret = PX4_OK; break; case PX4_DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); - return PX4_OK; + ret = PX4_OK; break; case PX4_DEVIOCGPUBBLOCK: - return _pub_blocked; + ret = _pub_blocked; + break; + case PX4_DEVIOCGDEVICEID: + ret = (int)_device_id.devid; + default: break; } -#if 0 - /* try the superclass. The different ioctl() function form - * means we need to copy arg */ - unsigned arg2 = arg; - int ret = Device::ioctl(cmd, arg2); - if (ret != -ENODEV) - return ret; -#endif return -ENOTTY; } int -CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) +VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) { int ret = PX4_OK; - debug("CDev::Poll %s", setup ? "setup" : "teardown"); + debug("VDev::Poll %s", setup ? "setup" : "teardown"); /* * Lock against pollnotify() (and possibly other callers) @@ -336,7 +333,7 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * benefit. */ fds->priv = (void *)handlep; - debug("CDev::poll: fds->priv = %p", handlep); + debug("VDev::poll: fds->priv = %p", handlep); /* * Handle setup requests. @@ -369,9 +366,9 @@ CDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) } void -CDev::poll_notify(pollevent_t events) +VDev::poll_notify(pollevent_t events) { - debug("CDev::poll_notify events = %0x", events); + debug("VDev::poll_notify events = %0x", events); /* lock against poll() as well as other wakeups */ lock(); @@ -384,9 +381,9 @@ CDev::poll_notify(pollevent_t events) } void -CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - debug("CDev::poll_notify_one"); + debug("VDev::poll_notify_one"); int value; sem_getvalue(fds->sem, &value); @@ -402,20 +399,20 @@ CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) } pollevent_t -CDev::poll_state(px4_dev_handle_t *handlep) +VDev::poll_state(px4_dev_handle_t *handlep) { - debug("CDev::poll_notify"); + debug("VDev::poll_notify"); /* by default, no poll events to report */ return 0; } int -CDev::store_poll_waiter(px4_pollfd_struct_t *fds) +VDev::store_poll_waiter(px4_pollfd_struct_t *fds) { /* * Look for a free slot. */ - debug("CDev::store_poll_waiter"); + debug("VDev::store_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -430,9 +427,9 @@ CDev::store_poll_waiter(px4_pollfd_struct_t *fds) } int -CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) +VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { - debug("CDev::remove_poll_waiter"); + debug("VDev::remove_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -446,12 +443,12 @@ CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) return -EINVAL; } -CDev *CDev::getDev(const char *path) +VDev *VDev::getDev(const char *path) { int i=0; for (; iname, path) == 0)) { - return (CDev *)(devmap[i]->cdev); + return (VDev *)(devmap[i]->cdev); } } return NULL; diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index bf823f5b5d..9334c06b7c 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -218,9 +218,9 @@ private: }; /** - * Abstract class for any character device + * Abstract class for any virtual character device */ -class __EXPORT CDev : public Device +class __EXPORT VDev : public Device { public: /** @@ -229,12 +229,12 @@ public: * @param name Driver name * @param devname Device node name */ - CDev(const char *name, const char *devname); + VDev(const char *name, const char *devname); /** * Destructor */ - virtual ~CDev(); + virtual ~VDev(); virtual int init(); @@ -333,7 +333,7 @@ public: */ virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); - static CDev *getDev(const char *path); + static VDev *getDev(const char *path); protected: @@ -452,14 +452,15 @@ private: int remove_poll_waiter(px4_pollfd_struct_t *fds); /* do not allow copying this class */ - CDev(const CDev&); - CDev operator=(const CDev&); + VDev(const VDev&); + VDev operator=(const VDev&); }; +#if 0 /** * Abstract class for character device accessed via PIO */ -class __EXPORT PIO : public CDev +class __EXPORT VPIO : public CDev { public: /** @@ -519,6 +520,7 @@ protected: private: unsigned long _base; }; +#endif } // namespace device diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index fab308ef07..bb119624c5 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -84,7 +84,7 @@ inline bool valid_fd(int fd) int px4_open(const char *path, int flags) { - CDev *dev = CDev::getDev(path); + VDev *dev = VDev::getDev(path); int ret = 0; int i; @@ -117,7 +117,7 @@ int px4_close(int fd) { int ret; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_close fd = %d\n", fd); ret = dev->close(filemap[fd]); filemap[fd] = NULL; @@ -135,7 +135,7 @@ ssize_t px4_read(int fd, void *buffer, size_t buflen) { int ret; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_read fd = %d\n", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); } @@ -152,7 +152,7 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen) { int ret = PX4_ERROR; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_write fd = %d\n", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); } @@ -169,7 +169,7 @@ int px4_ioctl(int fd, int cmd, unsigned long arg) { int ret = PX4_ERROR; if (valid_fd(fd)) { - CDev *dev = (CDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->cdev); PX4_DEBUG("px4_ioctl fd = %d\n", fd); ret = dev->ioctl(filemap[fd], cmd, arg); } @@ -206,8 +206,8 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // If fd is valid if (valid_fd(fds[i].fd)) { - CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; - PX4_DEBUG("px4_poll: CDev->poll(setup) %d\n", fds[i].fd); + VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);; + PX4_DEBUG("px4_poll: VDev->poll(setup) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) @@ -250,8 +250,8 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // If fd is valid if (valid_fd(fds[i].fd)) { - CDev *dev = (CDev *)(filemap[fds[i].fd]->cdev);; - PX4_DEBUG("px4_poll: CDev->poll(teardown) %d\n", fds[i].fd); + VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);; + PX4_DEBUG("px4_poll: VDev->poll(teardown) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 913c1ebcd3..490601b743 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -100,7 +100,7 @@ static const int ERROR = -1; #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" -class MS5611 : public device::CDev +class MS5611 : public device::VDev { public: MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); @@ -210,7 +210,7 @@ protected: extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : - CDev("MS5611", path), + VDev("MS5611", path), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -259,9 +259,9 @@ MS5611::init() { int ret; - ret = CDev::init(); + ret = VDev::init(); if (ret != OK) { - debug("CDev init failed"); + debug("VDev init failed"); goto out; } @@ -505,7 +505,7 @@ MS5611::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return CDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(handlep, cmd, arg); } void diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index 4f5890c92f..3a5684110b 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -110,7 +110,7 @@ extern mavlink_system_t mavlink_system; static void usage(void); Mavlink::Mavlink() : - CDev("mavlink-log", MAVLINK_LOG_DEVICE), + VDev("mavlink-log", MAVLINK_LOG_DEVICE), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), next(nullptr), diff --git a/src/modules/mavlink/mavlink_main_linux.h b/src/modules/mavlink/mavlink_main_linux.h index a785c48f3c..693d8451ca 100644 --- a/src/modules/mavlink/mavlink_main_linux.h +++ b/src/modules/mavlink/mavlink_main_linux.h @@ -61,7 +61,7 @@ #include "mavlink_mission.h" #include "mavlink_parameters.h" -class Mavlink : public device::CDev +class Mavlink : public device::VDev { public: diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index 548b36d029..791a639e59 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -114,7 +114,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance /** * Per-object device instance. */ -class ORBDevNode : public device::CDev +class ORBDevNode : public device::VDev { public: ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); @@ -188,7 +188,7 @@ ORBDevNode::SubscriberData *ORBDevNode::handlep_to_sd(device::px4_dev_handle_t * } ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : - CDev(name, path), + VDev(name, path), _meta(meta), _data(nullptr), _last_update(0), @@ -230,7 +230,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) /* now complete the open */ if (ret == PX4_OK) { - ret = CDev::open(handlep); + ret = VDev::open(handlep); /* open failed - not the publisher anymore */ if (ret != PX4_OK) @@ -259,10 +259,10 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) handlep->priv = (void *)sd; - ret = CDev::open(handlep); + ret = VDev::open(handlep); if (ret != PX4_OK) { - PX4_DEBUG("ERROR: CDev::open failed\n"); + PX4_DEBUG("ERROR: VDev::open failed\n"); delete sd; } @@ -292,7 +292,7 @@ ORBDevNode::close(device::px4_dev_handle_t *handlep) } } - return CDev::close(handlep); + return VDev::close(handlep); } ssize_t @@ -412,7 +412,7 @@ ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return CDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(handlep, cmd, arg); } } @@ -468,7 +468,7 @@ ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) * If the topic looks updated to the subscriber, go ahead and notify them. */ if (appears_updated(sd)) - CDev::poll_notify_one(fds, events); + VDev::poll_notify_one(fds, events); } bool @@ -569,7 +569,7 @@ ORBDevNode::update_deferred_trampoline(void *arg) * Used primarily to create new objects via the ORBIOCCREATE * ioctl. */ -class ORBDevMaster : public device::CDev +class ORBDevMaster : public device::VDev { public: ORBDevMaster(Flavor f); @@ -581,7 +581,7 @@ private: }; ORBDevMaster::ORBDevMaster(Flavor f) : - CDev((f == PUBSUB) ? "obj_master" : "param_master", + VDev((f == PUBSUB) ? "obj_master" : "param_master", (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), _flavor(f) { @@ -688,7 +688,7 @@ ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar default: /* give it to the superclass */ - return CDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(handlep, cmd, arg); } } From 78dd177e8613b3da84dff8f8fa65e6369f9e87ef Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 16:02:46 -0700 Subject: [PATCH 072/258] Linux: removed documentation of parameter The parameter is not present in the linux implementation so removed the documentation for the parameter. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 9334c06b7c..586971ed29 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -167,7 +167,6 @@ protected: * Constructor * * @param name Driver name - * @param irq Interrupt assigned to the device. */ Device(const char *name); From e3efe71444e7955307d55469d75919da2aa6497b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 16:04:21 -0700 Subject: [PATCH 073/258] Linux: I2C implementation with simulator stub If PX4_I2C_SIMULATE is set to 1, then the actual I2C device will not be opened and all transfers will succeed. If PX4_I2C_SIMULATE is false and transfer() is called, then the appropriate ioctl is make on the actual device. if I2C::ioctl is called via px4_ioctl() then the command fails and a warning is printed to use I2C::transfer Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_linux.cpp | 77 +++++++++++++++----------------- src/drivers/device/i2c_linux.h | 2 - 2 files changed, 37 insertions(+), 42 deletions(-) diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 4fe2c415c1..1a8bf1d185 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -47,6 +47,9 @@ #include #include +#define PX4_SIMULATE_I2C 1 +static int simulate = PX4_SIMULATE_I2C; + namespace device { @@ -62,8 +65,7 @@ I2C::I2C(const char *name, // private _bus(bus), _address(address), - _fd(-1), - _dname() + _fd(-1) { // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; @@ -71,9 +73,6 @@ I2C::I2C(const char *name, _device_id.devid_s.address = address; // devtype needs to be filled in by the driver _device_id.devid_s.devtype = 0; - - if (devname) - _dname = devname; } I2C::~I2C() @@ -100,24 +99,24 @@ I2C::init() return ret; } - _fd = px4_open(_dname.c_str(), PX4_F_RDONLY | PX4_F_WRONLY); + _fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY); if (_fd < 0) { - debug("px4_open failed of device %s", _dname.c_str()); + debug("px4_open failed of device %s", get_devname()); return PX4_ERROR; } -#if 0 - // Open the actual I2C device and map to the virtual dev name - char str[22]; - // Fixme - not sure bus is the right mapping here - // may have to go through /sys/bus/i2c interface to find the right map - snprintf(str, sizeof(str), "/dev/i2c-%d", _bus); - _fd = ::open(str, O_RDWR); - if (_fd < 0) { - warnx("could not open %s for virtual device %s", str, _dname.c_str()); - return -errno; - } -#endif + if (simulate) { + _fd = 0; + } + else { + // Open the actual I2C device and map to the virtual dev name + _fd = ::open(get_devname(), O_RDWR); + if (_fd < 0) { + warnx("could not open %s", get_devname()); + px4_errno = errno; + return PX4_ERROR; + } + } return ret; } @@ -162,23 +161,22 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re packets.msgs = msgv; packets.nmsgs = msgs; - ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); - if (ret < 0) { - warnx("I2C transfer failed"); - return 1; - } + if (simulate) { + printf("I2C SIM: transfer_4 on %s\n", get_devname()); + ret = PX4_OK; + } + else { + ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + if (ret < 0) { + warnx("I2C transfer failed"); + return 1; + } + } /* success */ if (ret == PX4_OK) break; -// No way to reset device from userspace -#if 0 - /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) - px4_i2creset(_dev); -#endif - } while (retry_count++ < _retries); return ret; @@ -199,7 +197,13 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) packets.msgs = msgv; packets.nmsgs = msgs; - ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + if (simulate) { + printf("I2C SIM: transfer_2 on %s\n", get_devname()); + ret = PX4_OK; + } + else { + ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + } if (ret < 0) { warnx("I2C transfer failed"); return 1; @@ -209,13 +213,6 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) if (ret == PX4_OK) break; -// No way to reset device from userspace -#if 0 - /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) - px4_i2creset(_dev); -#endif - } while (retry_count++ < _retries); return ret; @@ -227,7 +224,7 @@ int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) switch (cmd) { case I2C_RDWR: - warnx("I2C transfer request"); + warnx("Use I2C::transfer, not ioctl"); return 0; default: /* give it to the superclass */ diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h index a49c03267d..09b141173a 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_linux.h @@ -82,8 +82,6 @@ protected: * @param devname Device node name * @param bus I2C bus on which the device lives * @param address I2C bus address, or zero if set_address will be used - * @param frequency I2C bus frequency for the device (currently not used) - * @param irq Interrupt assigned to the device (or zero if none) */ I2C(const char *name, const char *devname, From 058d4408cc13768b30cdaed6d4cfe8f113f15a86 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 16:15:24 -0700 Subject: [PATCH 074/258] Linux: removed debug output from shell The command shell was spewing debug infor about the command and parameters. Removed the debug output. Signed-off-by: Mark Charlebois --- src/platforms/linux/main.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/platforms/linux/main.cpp b/src/platforms/linux/main.cpp index ae3c547bfd..f3b1ecc221 100644 --- a/src/platforms/linux/main.cpp +++ b/src/platforms/linux/main.cpp @@ -53,7 +53,6 @@ void run_cmd(const vector &appargs); void run_cmd(const vector &appargs) { // command is appargs[0] string command = appargs[0]; - cout << "appargs.size() = " << appargs.size() << endl; if (apps.find(command) != apps.end()) { const char *arg[appargs.size()+2]; @@ -63,7 +62,7 @@ void run_cmd(const vector &appargs) { ++i; } arg[i] = (char *)0; - cout << command << " " << i << endl; + //cout << command << " " << i << endl; apps[command](i,(char **)arg); } else @@ -77,12 +76,6 @@ static void process_line(string &line) vector appargs(5); stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; - cout << "Command " << appargs[0] << endl; - unsigned int i = 1; - while ( i < appargs.size() && appargs[i] != "") { - cout << " appargs[" << i << "] = " << appargs[i] << endl; - ++i; - } run_cmd(appargs); } From 894611df040aaa1f085eef7be403d3ba0e85bbdf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 16:56:16 -0700 Subject: [PATCH 075/258] Linux: Fixed argc check in sensors_main Was checking for argc < 1, and should be argc < 2. Signed-off-by: Mark Charlebois --- src/modules/sensors/sensors_linux.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors_linux.cpp b/src/modules/sensors/sensors_linux.cpp index dea1367ef9..01a0c0dee1 100644 --- a/src/modules/sensors/sensors_linux.cpp +++ b/src/modules/sensors/sensors_linux.cpp @@ -2253,8 +2253,9 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { - if (argc < 1) { - errx(1, "usage: sensors {start|stop|status}"); + if (argc < 2) { + warnx("usage: sensors {start|stop|status}"); + return 0; } if (!strcmp(argv[1], "start")) { From f62ea8aeae74bc4732f36e2a052f6ca1317b8238 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 18:10:29 -0700 Subject: [PATCH 076/258] Linux: added read and write function overrides for i2c Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_linux.cpp | 25 +++++++++++++++++++++++-- src/drivers/device/i2c_linux.h | 2 ++ 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 1a8bf1d185..3929c79794 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -67,6 +67,7 @@ I2C::I2C(const char *name, _address(address), _fd(-1) { + printf("I2C::I2C name = %s devname = %s\n", name, devname); // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; _device_id.devid_s.bus = bus; @@ -95,7 +96,7 @@ I2C::init() ret = VDev::init(); if (ret != PX4_OK) { - debug("cdev init failed"); + debug("VDev::init failed"); return ret; } @@ -106,7 +107,7 @@ I2C::init() } if (simulate) { - _fd = 0; + _fd = 10000; } else { // Open the actual I2C device and map to the virtual dev name @@ -232,4 +233,24 @@ int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) } } +ssize_t I2C::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + if (simulate) { + // FIXME no idea what this should be + printf ("2C SIM I2C::read\n"); + return 0; + } + + return ::read(_fd, buffer, buflen); +} + +ssize_t I2C::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +{ + if (simulate) { + return buflen; + } + + return ::write(_fd, buffer, buflen); +} + } // namespace device diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h index 09b141173a..c0a94aff01 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_linux.h @@ -91,6 +91,8 @@ protected: virtual int init(); + virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); /** From 92d90f7780d69ed7106da53cf43ec4056760af89 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 18:13:59 -0700 Subject: [PATCH 077/258] Linux: ms5611 open, close changed to px4_open, px4_close Calls to open and close were used instead of px4_open and px4_close. Signed-off-by: Mark Charlebois --- src/drivers/ms5611/ms5611_linux.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 490601b743..4acb2f251b 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -894,7 +894,7 @@ start_bus(struct ms5611_bus_option &bus) return false; } - int fd = open(bus.devpath, O_RDONLY); + int fd = px4_open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ if (fd == -1) { @@ -902,12 +902,12 @@ start_bus(struct ms5611_bus_option &bus) return false; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - close(fd); + px4_close(fd); warnx("failed setting default poll rate"); return false; } - close(fd); + px4_close(fd); return true; } @@ -959,7 +959,6 @@ struct ms5611_bus_option &find_bus(enum MS5611_BUS busid) } // FIXME - This is fatal to all threads errx(1, "bus %u not started", (unsigned)busid); - } /** @@ -977,7 +976,7 @@ test(enum MS5611_BUS busid) int fd; - fd = open(bus.devpath, O_RDONLY); + fd = px4_open(bus.devpath, O_RDONLY); if (fd < 0) { warn("open failed (try 'ms5611 start' if the driver is not running)"); return 1; @@ -1098,7 +1097,7 @@ calibrate(unsigned altitude, enum MS5611_BUS busid) int fd; - fd = open(bus.devpath, O_RDONLY); + fd = px4_open(bus.devpath, O_RDONLY); if (fd < 0) { warn("open failed (try 'ms5611 start' if the driver is not running)"); From 4780353cd80408f3307de0e801b42d021bc93e43 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Apr 2015 18:16:38 -0700 Subject: [PATCH 078/258] ms5611_i2c: don't create with nullptr for devname I have not been able to unravel why nullptr is passed as the device path to the constructor of ms5611_i2c. This crashes the VDev code as it expects to create a virtual driver with the device path passed as devname. It causes VDev to do a strncmp with null. Using /vdev/ms5611_i2c as the name for the now. Signed-off-by: Mark Charlebois --- src/drivers/ms5611/ms5611_i2c.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index dd67bc5d13..257e42a2de 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -116,9 +116,11 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : - I2C("MS5611_I2C", nullptr, bus, 0 + I2C("MS5611_I2C", #ifdef __PX4_NUTTX -, 400000 +nullptr, bus, 0, 400000 +#else +"/vdev/MS5611_I2C", bus, 0 #endif ), _prom(prom) From 76a6f194761b1bc4ff17730f38cf688442c2fe88 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 6 Apr 2015 19:31:03 -0700 Subject: [PATCH 079/258] Linux: Added device simulator to simulate an I2C or SPI device The simulated device satisfies the factory pattern used by MS5611 to create a specific I2C or SPI device instance. For now the functions just return true, but should/could return simulated data. Signed-off-by: Mark Charlebois --- src/drivers/device/module.mk | 3 +- src/drivers/device/sim.cpp | 115 ++++++++++++ src/drivers/device/sim.h | 112 ++++++++++++ src/drivers/device/vdev.h | 14 +- src/drivers/ms5611/module.mk | 2 +- src/drivers/ms5611/ms5611.h | 1 + src/drivers/ms5611/ms5611_linux.cpp | 36 ++-- src/drivers/ms5611/ms5611_sim.cpp | 194 +++++++++++++++++++++ src/platforms/linux/include/board_config.h | 2 +- 9 files changed, 457 insertions(+), 22 deletions(-) create mode 100644 src/drivers/device/sim.cpp create mode 100644 src/drivers/device/sim.h create mode 100644 src/drivers/ms5611/ms5611_sim.cpp diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index c927426ae7..488e5e759e 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -47,5 +47,6 @@ ifeq ($(PX4_TARGET_OS),linux) SRCS = vdev.cpp \ device.cpp \ vdev_posix.cpp \ - i2c_linux.cpp + i2c_linux.cpp \ + sim.cpp endif diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp new file mode 100644 index 0000000000..f1040ea8e7 --- /dev/null +++ b/src/drivers/device/sim.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file sim.cpp + * + * Base class for simulated devices. + * + * @todo Bus frequency changes; currently we do nothing with the value + * that is supplied. Should we just depend on the bus knowing? + */ + +#include +#include +#include +#include "sim.h" + +namespace device +{ + +SIM::SIM(const char *name, + const char *devname, + int bus, + uint16_t address) : + // base class + Device(name), + // public + // protected + // private + _bus(bus), + _address(address), + _devname(devname) +{ + + printf("SIM::SIM name = %s devname = %s\n", name, devname); + // fill in _device_id fields for a SIM device + _device_id.devid_s.bus_type = DeviceBusType_SIM; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; +} + +SIM::~SIM() +{ +} + +int +SIM::init() +{ + int ret = PX4_OK; + + // Assume the driver set the desired bus frequency. There is no standard + // way to set it from user space. + + // do base class init, which registers the virtual driver + ret = Device::init(); + + if (ret != PX4_OK) { + debug("VDev::init failed"); + return ret; + } + + return ret; +} + +int +SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + if (send_len > 0) { + printf("SIM: sending %d bytes\n", send_len); + } + + if (recv_len > 0) { + printf("SIM: receiving %d bytes\n", recv_len); + + // TODO - write data to recv; + } + + printf("I2C SIM: transfer_4 on %s\n", _devname); + + return PX4_OK; +} + +} // namespace device diff --git a/src/drivers/device/sim.h b/src/drivers/device/sim.h new file mode 100644 index 0000000000..05e4fa78c7 --- /dev/null +++ b/src/drivers/device/sim.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file sim.h + * + * Base class for devices on simulation bus. + */ + +#pragma once + +#include "vdev.h" + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on SIM + */ +class __EXPORT SIM : public Device +{ + +public: + + /** + * Get the address + */ + int16_t get_address() const { return _address; } + +protected: + /** + * The number of times a read or write operation will be retried on + * error. + */ + unsigned _retries; + + /** + * The SIM bus number the device is attached to. + */ + int _bus; + + /** + * @ Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus SIM bus on which the device lives + * @param address SIM bus address, or zero if set_address will be used + */ + SIM(const char *name, + const char *devname, + int bus, + uint16_t address); + virtual ~SIM(); + + virtual int init(); + + /** + * Perform an SIM transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + +private: + uint16_t _address; + const char * _devname; + + SIM(const device::SIM&); + SIM operator=(const device::SIM&); +}; + +} // namespace device + diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 586971ed29..e98d13209a 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -71,10 +71,9 @@ struct px4_dev_handle_t { }; /** - * Fundamental base class for all device drivers. + * Fundamental base class for all physical drivers (I2C, SPI). * - * This class handles the basic "being a driver" things, including - * interrupt registration and dispatch. + * This class provides the basic driver template for I2C and SPI devices */ class __EXPORT Device { @@ -107,7 +106,7 @@ public: * @param count The number of items to read. * @return The number of items read on success, negative errno otherwise. */ - int dev_read(unsigned address, void *data, unsigned count); + virtual int dev_read(unsigned address, void *data, unsigned count); /** * Write directly to the device. @@ -119,7 +118,7 @@ public: * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. */ - int dev_write(unsigned address, void *data, unsigned count); + virtual int dev_write(unsigned address, void *data, unsigned count); /** * Perform a device-specific operation. @@ -128,7 +127,7 @@ public: * @param arg An argument to the operation. * @return Negative errno on error, OK or positive value on success. */ - int dev_ioctl(unsigned operation, unsigned &arg); + virtual int dev_ioctl(unsigned operation, unsigned &arg); /* device bus types for DEVID @@ -138,6 +137,7 @@ public: DeviceBusType_I2C = 1, DeviceBusType_SPI = 2, DeviceBusType_UAVCAN = 3, + DeviceBusType_SIM = 4, }; /* @@ -452,7 +452,7 @@ private: /* do not allow copying this class */ VDev(const VDev&); - VDev operator=(const VDev&); + //VDev operator=(const VDev&); }; #if 0 diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index ba1ac7efcf..59316f4bfe 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -40,7 +40,7 @@ MODULE_COMMAND = ms5611 ifeq ($(PX4_TARGET_OS),nuttx) SRCS = ms5611_nuttx.cpp ms5611_spi.cpp ms5611_i2c.cpp else -SRCS = ms5611_linux.cpp ms5611_spi.cpp ms5611_i2c.cpp +SRCS = ms5611_linux.cpp ms5611_spi.cpp ms5611_i2c.cpp ms5611_sim.cpp endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index d129891bda..1d6239467d 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -82,4 +82,5 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +extern device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 4acb2f251b..4ee61a1277 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -71,7 +71,8 @@ enum MS5611_BUS { MS5611_BUS_I2C_INTERNAL, MS5611_BUS_I2C_EXTERNAL, MS5611_BUS_SPI_INTERNAL, - MS5611_BUS_SPI_EXTERNAL + MS5611_BUS_SPI_EXTERNAL, + MS5611_BUS_SIM_EXTERNAL }; /* oddly, ERROR is not defined for c++ */ @@ -258,6 +259,7 @@ int MS5611::init() { int ret; + warnx("MS5611::init"); ret = VDev::init(); if (ret != OK) { @@ -287,6 +289,7 @@ MS5611::init() /* do temperature first */ if (OK != measure()) { ret = -EIO; + warnx("temp measure failed"); break; } @@ -294,12 +297,14 @@ MS5611::init() if (OK != collect()) { ret = -EIO; + warnx("temp collect failed"); break; } /* now do a pressure measurement */ if (OK != measure()) { ret = -EIO; + warnx("pressure collect failed"); break; } @@ -307,6 +312,7 @@ MS5611::init() if (OK != collect()) { ret = -EIO; + warnx("pressure collect failed"); break; } @@ -318,10 +324,10 @@ MS5611::init() _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic < 0) { + if (_baro_topic == (orb_advert_t)(-1)) { warnx("failed to create sensor_baro publication"); } + //warnx("sensor_baro publication %ld", _baro_topic); } while (0); @@ -735,8 +741,13 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + if (_baro_topic != (orb_advert_t)(-1)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } + else { + printf("MS5611::collect _baro_topic not initialized\n"); + } } if (_reports->force(&report)) { @@ -796,6 +807,7 @@ struct ms5611_bus_option { uint8_t busnum; MS5611 *dev; } bus_options[] = { +#if 0 #if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, #endif @@ -808,6 +820,10 @@ struct ms5611_bus_option { #ifdef PX4_I2C_BUS_EXPANSION { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, #endif +#endif +#ifdef PX4_SIM_BUS_TEST + { MS5611_BUS_SIM_EXTERNAL, "/dev/ms5611_sim", &MS5611_sim_interface, PX4_SIM_BUS_TEST, NULL }, +#endif }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) @@ -1173,8 +1189,7 @@ usage() warnx("options:"); warnx(" -X (external I2C bus)"); warnx(" -I (intternal I2C bus)"); - warnx(" -S (external SPI bus)"); - warnx(" -s (internal SPI bus)"); + warnx(" -S (Simulation bus)"); } } // namespace @@ -1208,7 +1223,7 @@ ms5611_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; - while ((ch = getopt(argc, argv, "XISs", &myoptind)) != EOF) { + while ((ch = getopt(argc, argv, "XIS", &myoptind)) != EOF) { printf("ch = %d\n", ch); switch (ch) { case 'X': @@ -1218,10 +1233,7 @@ ms5611_main(int argc, char *argv[]) busid = MS5611_BUS_I2C_INTERNAL; break; case 'S': - busid = MS5611_BUS_SPI_EXTERNAL; - break; - case 's': - busid = MS5611_BUS_SPI_INTERNAL; + busid = MS5611_BUS_SIM_EXTERNAL; break; default: ms5611::usage(); diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp new file mode 100644 index 0000000000..782712bc4d --- /dev/null +++ b/src/drivers/ms5611/ms5611_sim.cpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + + /** + * @file ms5611_i2c.cpp + * + * SIM interface for MS5611 + */ + +/* XXX trim includes */ +#include +#include + +#include +#include +#include +#include +//#include +#include +#include + +#include +#include "ms5611.h" +#include "board_config.h" + +device::Device *MS5611_sim_interface(ms5611::prom_u &prom_buf); + +class MS5611_SIM : public device::SIM +{ +public: + MS5611_SIM(uint8_t bus, ms5611::prom_u &prom_buf); + virtual ~MS5611_SIM(); + + virtual int init(); + virtual int dev_read(unsigned offset, void *data, unsigned count); + virtual int dev_ioctl(unsigned operation, unsigned &arg); + +private: + ms5611::prom_u &_prom; + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return PX4_OK if the PROM reads successfully. + */ + int _read_prom(); + +}; + +device::Device * +MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) +{ + return new MS5611_SIM(busnum, prom_buf); +} + +MS5611_SIM::MS5611_SIM(uint8_t bus, ms5611::prom_u &prom) : + SIM("MS5611_SIM", "/vdev/MS5611_SIM", bus, 0), + _prom(prom) +{ +} + +MS5611_SIM::~MS5611_SIM() +{ +} + +int +MS5611_SIM::init() +{ + return SIM::init(); +} + +int +MS5611_SIM::dev_read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == PX4_OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +MS5611_SIM::dev_ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + return ret; +} + +int +MS5611_SIM::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +MS5611_SIM::_measure(unsigned addr) +{ + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + +int +MS5611_SIM::_read_prom() +{ + int ret = 1; + // TODO input simlation data + return ret; +} diff --git a/src/platforms/linux/include/board_config.h b/src/platforms/linux/include/board_config.h index 81bcc8f34d..afbdd98cd2 100644 --- a/src/platforms/linux/include/board_config.h +++ b/src/platforms/linux/include/board_config.h @@ -4,6 +4,6 @@ * I2C busses */ #define PX4_I2C_BUS_ESC 1 -#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_SIM_BUS_TEST 2 #define PX4_I2C_BUS_EXPANSION 3 #define PX4_I2C_BUS_LED 3 From 440fc306a999588359542e7d88106a1d8187bf1a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 00:06:18 -0700 Subject: [PATCH 080/258] Added accelerometer simulator The simulator satisfies the dependencies for an accelerometer being present. The accel code compiles but is not fully functional. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 + src/drivers/device/module.mk | 6 +- src/drivers/device/ringbuffer.cpp | 385 +++++ src/drivers/device/ringbuffer.h | 347 +---- src/drivers/drv_sensor.h | 2 + .../mathlib/math/filter/LowPassFilter2p.cpp | 1 + src/platforms/linux/drivers/accel/accel.cpp | 1367 +++++++++++++++++ src/platforms/linux/drivers/accel/module.mk | 7 + 8 files changed, 1772 insertions(+), 345 deletions(-) create mode 100644 src/drivers/device/ringbuffer.cpp create mode 100644 src/platforms/linux/drivers/accel/accel.cpp create mode 100644 src/platforms/linux/drivers/accel/module.mk diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 4b4cf7fc71..2781cba50c 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -49,6 +49,7 @@ MODULES += modules/sdlog2 # Libraries # MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion @@ -57,6 +58,7 @@ MODULES += lib/conversion # Linux port # MODULES += platforms/linux/px4_layer +MODULES += platforms/linux/drivers/accel # # Unit tests diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 488e5e759e..4e0b563e54 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -41,12 +41,14 @@ SRCS = \ cdev_nuttx.cpp \ i2c_nuttx.cpp \ pio.cpp \ - spi.cpp + spi.cpp \ + ringbuffer.cpp endif ifeq ($(PX4_TARGET_OS),linux) SRCS = vdev.cpp \ device.cpp \ vdev_posix.cpp \ i2c_linux.cpp \ - sim.cpp + sim.cpp \ + ringbuffer.cpp endif diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp new file mode 100644 index 0000000000..427876463f --- /dev/null +++ b/src/drivers/device/ringbuffer.cpp @@ -0,0 +1,385 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file ringbuffer.cpp + * + * A flexible ringbuffer class. + */ + +#include "ringbuffer.h" +#include + +RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : + _num_items(num_items), + _item_size(item_size), + _buf(new char[(_num_items+1) * item_size]), + _head(_num_items), + _tail(_num_items) +{} + +RingBuffer::~RingBuffer() +{ + if (_buf != nullptr) + delete[] _buf; +} + +unsigned +RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _num_items : (index - 1); +} + +bool +RingBuffer::empty() +{ + return _tail == _head; +} + +bool +RingBuffer::full() +{ + return _next(_head) == _tail; +} + +unsigned +RingBuffer::size() +{ + return (_buf != nullptr) ? _num_items : 0; +} + +void +RingBuffer::flush() +{ + while (!empty()) + get(NULL); +} + +bool +RingBuffer::put(const void *val, size_t val_size) +{ + unsigned next = _next(_head); + if (next != _tail) { + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + memcpy(&_buf[_head * _item_size], val, val_size); + _head = next; + return true; + } else { + return false; + } +} + +bool +RingBuffer::put(int8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(float val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(double val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::force(const void *val, size_t val_size) +{ + bool overwrote = false; + + for (;;) { + if (put(val, val_size)) + break; + get(NULL); + overwrote = true; + } + return overwrote; +} + +bool +RingBuffer::force(int8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(float val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(double val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::get(void *val, size_t val_size) +{ + if (_tail != _head) { + unsigned candidate; + unsigned next; + + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + + do { + /* decide which element we think we're going to read */ + candidate = _tail; + + /* and what the corresponding next index will be */ + next = _next(candidate); + + /* go ahead and read from this index */ + if (val != NULL) + memcpy(val, &_buf[candidate * _item_size], val_size); + + /* if the tail pointer didn't change, we got our item */ + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); + + return true; + } else { + return false; + } +} + +bool +RingBuffer::get(int8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(float &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(double &val) +{ + return get(&val, sizeof(val)); +} + +unsigned +RingBuffer::space(void) +{ + unsigned tail, head; + + /* + * Make a copy of the head/tail pointers in a fashion that + * may err on the side of under-estimating the free space + * in the buffer in the case that the buffer is being updated + * asynchronously with our check. + * If the head pointer changes (reducing space) while copying, + * re-try the copy. + */ + do { + head = _head; + tail = _tail; + } while (head != _head); + + return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); +} + +unsigned +RingBuffer::count(void) +{ + /* + * Note that due to the conservative nature of space(), this may + * over-estimate the number of items in the buffer. + */ + return _num_items - space(); +} + +bool +RingBuffer::resize(unsigned new_size) +{ + char *old_buffer; + char *new_buffer = new char [(new_size+1) * _item_size]; + if (new_buffer == nullptr) { + return false; + } + old_buffer = _buf; + _buf = new_buffer; + _num_items = new_size; + _head = new_size; + _tail = new_size; + delete[] old_buffer; + return true; +} + +void +RingBuffer::print_info(const char *name) +{ + printf("%s %u/%lu (%u/%u @ %p)\n", + name, + _num_items, + (unsigned long)_num_items * _item_size, + _head, + _tail, + _buf); +} diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index f03171722c..876bb3a8f2 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -39,6 +39,10 @@ #pragma once +#include +#include +#include + class RingBuffer { public: RingBuffer(unsigned ring_size, size_t entry_size); @@ -168,346 +172,3 @@ private: RingBuffer operator=(const RingBuffer&); }; -RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : - _num_items(num_items), - _item_size(item_size), - _buf(new char[(_num_items+1) * item_size]), - _head(_num_items), - _tail(_num_items) -{} - -RingBuffer::~RingBuffer() -{ - if (_buf != nullptr) - delete[] _buf; -} - -unsigned -RingBuffer::_next(unsigned index) -{ - return (0 == index) ? _num_items : (index - 1); -} - -bool -RingBuffer::empty() -{ - return _tail == _head; -} - -bool -RingBuffer::full() -{ - return _next(_head) == _tail; -} - -unsigned -RingBuffer::size() -{ - return (_buf != nullptr) ? _num_items : 0; -} - -void -RingBuffer::flush() -{ - while (!empty()) - get(NULL); -} - -bool -RingBuffer::put(const void *val, size_t val_size) -{ - unsigned next = _next(_head); - if (next != _tail) { - if ((val_size == 0) || (val_size > _item_size)) - val_size = _item_size; - memcpy(&_buf[_head * _item_size], val, val_size); - _head = next; - return true; - } else { - return false; - } -} - -bool -RingBuffer::put(int8_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint8_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(int16_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint16_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(int32_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint32_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(int64_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(uint64_t val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(float val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::put(double val) -{ - return put(&val, sizeof(val)); -} - -bool -RingBuffer::force(const void *val, size_t val_size) -{ - bool overwrote = false; - - for (;;) { - if (put(val, val_size)) - break; - get(NULL); - overwrote = true; - } - return overwrote; -} - -bool -RingBuffer::force(int8_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint8_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(int16_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint16_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(int32_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint32_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(int64_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(uint64_t val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(float val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::force(double val) -{ - return force(&val, sizeof(val)); -} - -bool -RingBuffer::get(void *val, size_t val_size) -{ - if (_tail != _head) { - unsigned candidate; - unsigned next; - - if ((val_size == 0) || (val_size > _item_size)) - val_size = _item_size; - - do { - /* decide which element we think we're going to read */ - candidate = _tail; - - /* and what the corresponding next index will be */ - next = _next(candidate); - - /* go ahead and read from this index */ - if (val != NULL) - memcpy(val, &_buf[candidate * _item_size], val_size); - - /* if the tail pointer didn't change, we got our item */ - } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); - - return true; - } else { - return false; - } -} - -bool -RingBuffer::get(int8_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint8_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(int16_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint16_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(int32_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint32_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(int64_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(uint64_t &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(float &val) -{ - return get(&val, sizeof(val)); -} - -bool -RingBuffer::get(double &val) -{ - return get(&val, sizeof(val)); -} - -unsigned -RingBuffer::space(void) -{ - unsigned tail, head; - - /* - * Make a copy of the head/tail pointers in a fashion that - * may err on the side of under-estimating the free space - * in the buffer in the case that the buffer is being updated - * asynchronously with our check. - * If the head pointer changes (reducing space) while copying, - * re-try the copy. - */ - do { - head = _head; - tail = _tail; - } while (head != _head); - - return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); -} - -unsigned -RingBuffer::count(void) -{ - /* - * Note that due to the conservative nature of space(), this may - * over-estimate the number of items in the buffer. - */ - return _num_items - space(); -} - -bool -RingBuffer::resize(unsigned new_size) -{ - char *old_buffer; - char *new_buffer = new char [(new_size+1) * _item_size]; - if (new_buffer == nullptr) { - return false; - } - old_buffer = _buf; - _buf = new_buffer; - _num_items = new_size; - _head = new_size; - _tail = new_size; - delete[] old_buffer; - return true; -} - -void -RingBuffer::print_info(const char *name) -{ - printf("%s %u/%lu (%u/%u @ %p)\n", - name, - _num_items, - (unsigned long)_num_items * _item_size, - _head, - _tail, - _buf); -} diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index b1148ce76f..4b5775d223 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -54,9 +54,11 @@ #define DRV_MAG_DEVTYPE_HMC5883 0x01 #define DRV_MAG_DEVTYPE_LSM303D 0x02 +#define DRV_MAG_DEVTYPE_ACCELSIM 0x03 #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_ACC_DEVTYPE_ACCELSIM 0x14 #define DRV_GYR_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 #define DRV_RNG_DEVTYPE_MB12XX 0x31 diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index c709a28348..7e2778e445 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -37,6 +37,7 @@ /// @brief A class to implement a second order low pass filter /// Author: Leonard Hall +#include #include "LowPassFilter2p.hpp" #include "math.h" diff --git a/src/platforms/linux/drivers/accel/accel.cpp b/src/platforms/linux/drivers/accel/accel.cpp new file mode 100644 index 0000000000..30ec5dc420 --- /dev/null +++ b/src/platforms/linux/drivers/accel/accel.cpp @@ -0,0 +1,1367 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file accel.cpp + * Driver for a simulated accelerometer / magnetometer. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define ACCELSIM_DEVICE_PATH_ACCEL "/vdev/sim_accel" +#define ACCELSIM_DEVICE_PATH_ACCEL_EXT "/vdev/sim_accel_ext" +#define ACCELSIM_DEVICE_PATH_MAG "/vdev/sim_mag" + +#define ADDR_WHO_AM_I 0x0F + +#define ACCELSIM_ACCEL_DEFAULT_RATE 800 +#define ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define ACCELSIM_ONE_G 9.80665f + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) + +extern "C" { __EXPORT int accel_main(int argc, char *argv[]); } + + +class ACCELSIM_mag; + +class ACCELSIM : public device::VDev +{ +public: + ACCELSIM(const char* path, enum Rotation rotation); + virtual ~ACCELSIM(); + + virtual int init(); + + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + //void print_info(); + + /** + * dump register values + */ + void print_registers(); + +protected: + friend class ACCELSIM_mag; + + virtual ssize_t mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + int transfer(uint8_t *send, uint8_t *recv, unsigned len); +private: + + ACCELSIM_mag *_mag; + + struct hrt_call _accel_call; + struct hrt_call _mag_call; + + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + RingBuffer *_accel_reports; + RingBuffer *_mag_reports; + + struct accel_scale _accel_scale; + unsigned _accel_range_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_onchip_filter_bandwith; + + struct mag_scale _mag_scale; + unsigned _mag_range_ga; + float _mag_range_scale; + unsigned _mag_samplerate; + + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + int _accel_class_instance; + + unsigned _accel_read; + unsigned _mag_read; + + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_sample_perf; + perf_counter_t _accel_reschedules; + perf_counter_t _bad_registers; + perf_counter_t _bad_values; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + + enum Rotation _rotation; + + // values used to + float _last_accel[3]; + uint8_t _constant_accel_count; + + // last temperature value + float _last_temperature; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define ACCELSIM_NUM_CHECKED_REGISTERS 1 + static const uint8_t _checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[ACCELSIM_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + void reset(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ + static void mag_measure_trampoline(void *arg); + + /** + * Fetch accel measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Fetch mag measurements from the sensor and update the report ring. + */ + void mag_measure(); + + /** + * Read a register from the ACCELSIM + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the ACCELSIM + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the ACCELSIM + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the ACCELSIM, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the ACCELSIM accel measurement range. + * + * @param max_g The measurement range of the accel is in g (9.81m/s^2) + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_range(unsigned max_g); + + /** + * Set the ACCELSIM mag measurement range. + * + * @param max_ga The measurement range of the mag is in Ga + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int mag_set_range(unsigned max_g); + + /** + * Set the driver lowpass filter bandwidth. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); + + /** + * Set the ACCELSIM internal accel sampling frequency. + * + * @param frequency The internal accel sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int accel_set_samplerate(unsigned frequency); + + /** + * Set the ACCELSIM internal mag sampling frequency. + * + * @param frequency The internal mag sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int mag_set_samplerate(unsigned frequency); + + /* this class cannot be copied */ + ACCELSIM(const ACCELSIM&); + ACCELSIM operator=(const ACCELSIM&); +}; + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t ACCELSIM::_checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I }; + +/** + * Helper class implementing the mag driver node. + */ +class ACCELSIM_mag : public device::VDev +{ +public: + ACCELSIM_mag(ACCELSIM *parent); + ~ACCELSIM_mag(); + + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + virtual int init(); + +protected: + friend class ACCELSIM; + + void parent_poll_notify(); +private: + ACCELSIM *_parent; + + orb_advert_t _mag_topic; + int _mag_orb_class_instance; + int _mag_class_instance; + + void measure(); + + void measure_trampoline(void *arg); + + /* this class does not allow copying due to ptr data members */ + ACCELSIM_mag(const ACCELSIM_mag&); + ACCELSIM_mag operator=(const ACCELSIM_mag&); +}; + + +ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : + VDev("ACCELSIM", path), + _mag(new ACCELSIM_mag(this)), + _accel_call{}, + _mag_call{}, + _call_accel_interval(0), + _call_mag_interval(0), + _accel_reports(nullptr), + _mag_reports(nullptr), + _accel_scale{}, + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(0), + _accel_onchip_filter_bandwith(0), + _mag_scale{}, + _mag_range_ga(0.0f), + _mag_range_scale(0.0f), + _mag_samplerate(0), + _accel_topic(-1), + _accel_orb_class_instance(-1), + _accel_class_instance(-1), + _accel_read(0), + _mag_read(0), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "sim_accel_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "sim_mag_read")), + _accel_reschedules(perf_alloc(PC_COUNT, "sim_accel_resched")), + _bad_registers(perf_alloc(PC_COUNT, "sim_bad_registers")), + _bad_values(perf_alloc(PC_COUNT, "sim_bad_values")), + _accel_filter_x(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _rotation(rotation), + _constant_accel_count(0), + _last_temperature(0), + _checked_next(0) +{ + + + // enable debug() calls + _debug_enabled = true; + + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; + + /* Prime _mag with parents devid. */ + _mag->_device_id.devid = _device_id.devid; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; + + + // default scale factors + _accel_scale.x_offset = 0.0f; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0.0f; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0.0f; + _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0.0f; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0.0f; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0.0f; + _mag_scale.z_scale = 1.0f; +} + +ACCELSIM::~ACCELSIM() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_accel_reports != nullptr) + delete _accel_reports; + if (_mag_reports != nullptr) + delete _mag_reports; + + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + + delete _mag; + + /* delete the perf counter */ + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); + perf_free(_bad_registers); + perf_free(_bad_values); + perf_free(_accel_reschedules); +} + +int +ACCELSIM::init() +{ + int ret = ERROR; + + /* do SIM init first */ + if (VDev::init() != OK) { + warnx("SIM init failed"); + goto out; + } + + /* allocate basic report buffers */ + _accel_reports = new RingBuffer(2, sizeof(accel_report)); + + if (_accel_reports == nullptr) + goto out; + + _mag_reports = new RingBuffer(2, sizeof(mag_report)); + + if (_mag_reports == nullptr) + goto out; + + reset(); + + /* do VDev init for the mag device node */ + ret = _mag->init(); + if (ret != OK) { + warnx("MAG init failed"); + goto out; + } + + /* fill report structures */ + measure(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); + + /* measurement will have generated a report, publish */ + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + + if (_mag->_mag_topic < 0) { + warnx("ADVERT ERR"); + } + + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + + if (_accel_topic < 0) { + warnx("ADVERT ERR"); + } + +out: + return ret; +} + +void +ACCELSIM::reset() +{ +} + +int +ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + return PX4_OK; +} + +ssize_t +ACCELSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + accel_report *arb = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_accel_interval > 0) { + /* + * While there is space in the caller's buffer, and reports, copy them. + */ + while (count--) { + if (_accel_reports->get(arb)) { + ret += sizeof(*arb); + arb++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + measure(); + + /* measurement will have generated a report, copy it out */ + if (_accel_reports->get(arb)) + ret = sizeof(*arb); + + return ret; +} + +ssize_t +ACCELSIM::mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + mag_report *mrb = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + */ + while (count--) { + if (_mag_reports->get(mrb)) { + ret += sizeof(*mrb); + mrb++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _mag_reports->flush(); + _mag->measure(); + + /* measurement will have generated a report, copy it out */ + if (_mag_reports->get(mrb)) + ret = sizeof(*mrb); + + return ret; +} + +int +ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_accel_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + return ioctl(handlep, SENSORIOCSPOLLRATE, 1600); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(handlep, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 500) + return -EINVAL; + + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_accel_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_accel_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _accel_reports->size(); + + case SENSORIOCRESET: + reset(); + return OK; + + case ACCELIOCSSAMPLERATE: + return accel_set_samplerate(arg); + + case ACCELIOCGSAMPLERATE: + return _accel_samplerate; + + case ACCELIOCSLOWPASS: { + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCSRANGE: + /* arg needs to be in G */ + return accel_set_range(arg); + + case ACCELIOCGRANGE: + /* convert to m/s^2 and return rounded in G */ + return (unsigned long)((_accel_range_m_s2)/ACCELSIM_ONE_G + 0.5f); + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + default: + /* give it to the superclass */ + return VDev::ioctl(handlep, cmd, arg); + } +} + +int +ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* 100 Hz is max for mag */ + return mag_ioctl(handlep, SENSORIOCSPOLLRATE, 100); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_mag_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + if (!_mag_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _mag_reports->size(); + + case SENSORIOCRESET: + reset(); + return OK; + + case MAGIOCSSAMPLERATE: + return mag_set_samplerate(arg); + + case MAGIOCGSAMPLERATE: + return _mag_samplerate; + + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: + return mag_set_range(arg); + + case MAGIOCGRANGE: + return _mag_range_ga; + + case MAGIOCGEXTERNAL: + /* Even if this sensor is on the "external" SPI bus + * it is still fixed to the autopilot assembly, + * so always return 0. + */ + return 0; + + default: + /* give it to the superclass */ + return VDev::ioctl(handlep, cmd, arg); + } +} + +uint8_t +ACCELSIM::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + cmd[1] = 0; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +ACCELSIM::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +ACCELSIM::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; iflush(); + _mag_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&ACCELSIM::mag_measure_trampoline, this); +} + +void +ACCELSIM::stop() +{ + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); +} + +void +ACCELSIM::measure_trampoline(void *arg) +{ + ACCELSIM *dev = (ACCELSIM *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +ACCELSIM::mag_measure_trampoline(void *arg) +{ + ACCELSIM *dev = (ACCELSIM *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + +void +ACCELSIM::measure() +{ + /* status register and data as read back from the device */ + +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_accel_report; +#pragma pack(pop) + + accel_report accel_report; + + /* start the performance counter */ + perf_begin(_accel_sample_perf); + + /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); + raw_accel_report.cmd = DIR_READ; + transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + accel_report.timestamp = hrt_absolute_time(); + + // use the temperature from the last mag reading + accel_report.temperature = _last_temperature; + + // report the error count as the sum of the number of bad + // register reads and bad values. This allows the higher level + // code to decide if it should use this sensor based on + // whether it has had failures + accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + + accel_report.x_raw = raw_accel_report.x; + accel_report.y_raw = raw_accel_report.y; + accel_report.z_raw = raw_accel_report.z; + + float xraw_f = raw_accel_report.x; + float yraw_f = raw_accel_report.y; + float zraw_f = raw_accel_report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + /* + we have logs where the accelerometers get stuck at a fixed + large value. We want to detect this and mark the sensor as + being faulty + */ + if (fabsf(_last_accel[0] - x_in_new) < 0.001f && + fabsf(_last_accel[1] - y_in_new) < 0.001f && + fabsf(_last_accel[2] - z_in_new) < 0.001f && + fabsf(x_in_new) > 20 && + fabsf(y_in_new) > 20 && + fabsf(z_in_new) > 20) { + _constant_accel_count += 1; + } else { + _constant_accel_count = 0; + } + if (_constant_accel_count > 100) { + // we've had 100 constant accel readings with large + // values. The sensor is almost certainly dead. We + // will raise the error_count so that the top level + // flight code will know to avoid this sensor, but + // we'll still give the data so that it can be logged + // and viewed + perf_count(_bad_values); + _constant_accel_count = 0; + } + + _last_accel[0] = x_in_new; + _last_accel[1] = y_in_new; + _last_accel[2] = z_in_new; + + accel_report.x = _accel_filter_x.apply(x_in_new); + accel_report.y = _accel_filter_y.apply(y_in_new); + accel_report.z = _accel_filter_z.apply(z_in_new); + + accel_report.scaling = _accel_range_scale; + accel_report.range_m_s2 = _accel_range_m_s2; + + _accel_reports->force(&accel_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } + + _accel_read++; + + /* stop the perf counter */ + perf_end(_accel_sample_perf); +} + +void +ACCELSIM::mag_measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + int16_t temperature; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; +#pragma pack(pop) + + mag_report mag_report; + memset(&mag_report, 0, sizeof(mag_report)); + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); + raw_mag_report.cmd = DIR_READ; + transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + mag_report.timestamp = hrt_absolute_time(); + + mag_report.x_raw = raw_mag_report.x; + mag_report.y_raw = raw_mag_report.y; + mag_report.z_raw = raw_mag_report.z; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + /* apply user specified rotation */ + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = (float)_mag_range_ga; + mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + + /* remember the temperature. The datasheet isn't clear, but it + * seems to be a signed offset from 25 degrees C in units of 0.125C + */ + _last_temperature = 25 + (raw_mag_report.temperature * 0.125f); + mag_report.temperature = _last_temperature; + + _mag_reports->force(&mag_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } + + _mag_read++; + + /* stop the perf counter */ + perf_end(_mag_sample_perf); +} + +ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : + VDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(-1), + _mag_orb_class_instance(-1), + _mag_class_instance(-1) +{ +} + +ACCELSIM_mag::~ACCELSIM_mag() +{ + if (_mag_class_instance != -1) + unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); +} + +int +ACCELSIM_mag::init() +{ + int ret; + + ret = VDev::init(); + if (ret != OK) + goto out; + + _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + +out: + return ret; +} + +void +ACCELSIM_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +ACCELSIM_mag::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + return _parent->mag_read(handlep, buffer, buflen); +} + +int +ACCELSIM_mag::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(handlep, cmd, arg); + break; + default: + return _parent->mag_ioctl(handlep, cmd, arg); + } +} + +void +ACCELSIM_mag::measure() +{ + _parent->mag_measure(); +} + +void +ACCELSIM_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); +} + +/** + * Local functions in support of the shell command. + */ +namespace accel +{ + +ACCELSIM *g_dev; + +int start(enum Rotation rotation); +int info(); +void usage(); + +/** + * Start the driver. + * + * This function call only returns once the driver is + * up and running or failed to detect the sensor. + */ +int +start(enum Rotation rotation) +{ + int fd, fd_mag; + if (g_dev != nullptr) { + warnx( "already started"); + return 0; + } + + /* create the driver */ + g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); + + if (g_dev == nullptr) { + warnx("failed instantiating ACCELSIM obj"); + goto fail; + } + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + fd_mag = open(ACCELSIM_DEVICE_PATH_MAG, O_RDONLY); + + /* don't fail if open cannot be opened */ + if (0 <= fd_mag) { + if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + } + + close(fd); + close(fd_mag); + + return 0; +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + warnx("driver start failed"); + return 1; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + warnx("driver not running\n"); + return 1; + } + + printf("state @ %p\n", g_dev); + //g_dev->print_info(); + + return 0; +} + +void +usage() +{ + warnx("Usage: accel 'start', 'info'"); + warnx("options:"); + warnx(" -R rotation"); +} + +} // namespace + +int +accel_main(int argc, char *argv[]) +{ + int ch; + enum Rotation rotation = ROTATION_NONE; + int ret; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + default: + accel::usage(); + return 0; + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) + ret = accel::start(rotation); + + /* + * Print driver information. + */ + else if (!strcmp(verb, "info")) + ret = accel::info(); + + else { + accel::usage(); + return 1; + } + return ret; +} diff --git a/src/platforms/linux/drivers/accel/module.mk b/src/platforms/linux/drivers/accel/module.mk new file mode 100644 index 0000000000..71c6c0ebdd --- /dev/null +++ b/src/platforms/linux/drivers/accel/module.mk @@ -0,0 +1,7 @@ +# +# Simulated accel/mag driver +# + +MODULE_COMMAND = accel +SRCS = accel.cpp + From 80c5812861fa4251e0b84de4d992d9e75af779e7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 00:08:23 -0700 Subject: [PATCH 081/258] Linux: run socat as user not as root If sudo is used to run socat the tty cannot be opened by a regular user Signed-off-by: Mark Charlebois --- Tools/linux_run.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/linux_run.sh b/Tools/linux_run.sh index 8005b65539..5207aebdfb 100755 --- a/Tools/linux_run.sh +++ b/Tools/linux_run.sh @@ -3,7 +3,7 @@ if [ ! -c /tmp/ttyS0 ] || [ ! -c /tmp/ttyS1 ] then echo "Need to create /tmp/ttyS[01]" - echo "sudo socat PTY,link=/tmp/ttyS0 PTY,link=/tmp/ttyS1" + echo "socat PTY,link=/tmp/ttyS0 PTY,link=/tmp/ttyS1" exit 1 fi From b86b334c2fb1795da1214ef12be330e607a29076 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 00:13:48 -0700 Subject: [PATCH 082/258] Linux: Updated copyright on simulated accelrometer source Signed-off-by: Mark Charlebois --- src/platforms/linux/drivers/accel/accel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/linux/drivers/accel/accel.cpp b/src/platforms/linux/drivers/accel/accel.cpp index 30ec5dc420..7e97d77b26 100644 --- a/src/platforms/linux/drivers/accel/accel.cpp +++ b/src/platforms/linux/drivers/accel/accel.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From eab32572f42f8e3e715b952512b6f5df9041f848 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 12:17:27 -0700 Subject: [PATCH 083/258] Linux: Added thread safe getopt The getopt command uses global variables and is not thread safe. Created a minimal px4_getopt version that supports options with or without an arg, and random placement of options on the command line. This version modifies the order of the args in argv as does the POSIX version of getopt. This assumes that argv[0] is the program name. Nuttx may not support that properly in task_spawn. Signed-off-by: Mark Charlebois --- makefiles/linux.mk | 4 +- src/drivers/ms5611/ms5611_linux.cpp | 19 +-- src/modules/mavlink/mavlink_main_linux.cpp | 25 ++-- src/platforms/common/module.mk | 6 + src/platforms/common/px4_getopt.c | 152 ++++++++++++++++++++ src/platforms/linux/drivers/accel/accel.cpp | 19 ++- src/platforms/px4_getopt.h | 46 ++++++ 7 files changed, 235 insertions(+), 36 deletions(-) create mode 100644 src/platforms/common/module.mk create mode 100644 src/platforms/common/px4_getopt.c create mode 100644 src/platforms/px4_getopt.h diff --git a/makefiles/linux.mk b/makefiles/linux.mk index b424521a4b..d050259c57 100644 --- a/makefiles/linux.mk +++ b/makefiles/linux.mk @@ -34,5 +34,7 @@ # building firmware. # -MODULES += platforms/linux/px4_layer +MODULES += \ + platforms/common \ + platforms/linux/px4_layer diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp index 4ee61a1277..9e99d926e2 100644 --- a/src/drivers/ms5611/ms5611_linux.cpp +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -1194,21 +1195,6 @@ usage() } // namespace -static int getopt(int argc, char *argv[], const char *options, int *myoptind) -{ - char *p = argv[*myoptind]; - int idx = 0; - if (p && options && myoptind && p[0] == '-') { - while (options[idx] != 0 && p[1] != options[idx]) - ++idx; - if (options[idx] == 0) - return (int)'?'; - *myoptind += 1; - return options[idx]; - } - return -1; -} - int ms5611_main(int argc, char *argv[]) { @@ -1223,7 +1209,8 @@ ms5611_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; - while ((ch = getopt(argc, argv, "XIS", &myoptind)) != EOF) { + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) { printf("ch = %d\n", ch); switch (ch) { case 'X': diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index 3a5684110b..e9bb7b1308 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -1199,38 +1200,36 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = 0; _mode = MAVLINK_MODE_NORMAL; - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; + int myoptind=1; + const char *myoptarg = NULL; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': - _baudrate = strtoul(optarg, NULL, 10); + _baudrate = strtoul(myoptarg, NULL, 10); if (_baudrate < 9600 || _baudrate > 921600) { - warnx("invalid baud rate '%s'", optarg); + warnx("invalid baud rate '%s'", myoptarg); err_flag = true; } break; case 'r': - _datarate = strtoul(optarg, NULL, 10); + _datarate = strtoul(myoptarg, NULL, 10); if (_datarate < 10 || _datarate > MAX_DATA_RATE) { - warnx("invalid data rate '%s'", optarg); + warnx("invalid data rate '%s'", myoptarg); err_flag = true; } break; case 'd': - _device_name = optarg; + _device_name = myoptarg; break; // case 'e': @@ -1238,13 +1237,13 @@ Mavlink::task_main(int argc, char *argv[]) // break; case 'm': - if (strcmp(optarg, "custom") == 0) { + if (strcmp(myoptarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; - } else if (strcmp(optarg, "camera") == 0) { + } else if (strcmp(myoptarg, "camera") == 0) { // left in here for compatibility _mode = MAVLINK_MODE_ONBOARD; - } else if (strcmp(optarg, "onboard") == 0) { + } else if (strcmp(myoptarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; } diff --git a/src/platforms/common/module.mk b/src/platforms/common/module.mk new file mode 100644 index 0000000000..0e5f463e69 --- /dev/null +++ b/src/platforms/common/module.mk @@ -0,0 +1,6 @@ +# +# Common OS porting APIs +# + +SRCS = px4_getopt.c + diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c new file mode 100644 index 0000000000..3078351727 --- /dev/null +++ b/src/platforms/common/px4_getopt.c @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_getopt.cpp + * Minimal, thread safe version of getopt + */ + +#include +#include + +// check if p is a valid option and if the option takes an arg +static char isvalidopt(char p, const char *options, int *takesarg) +{ + int idx = 0; + *takesarg = 0; + while (options[idx] != 0 && p != options[idx]) + ++idx; + if (options[idx] == 0) + return '?'; + if (options[idx+1] == ':') { + *takesarg = 1; + } + return options[idx]; +} + +// reorder argv and put non-options at the end +static int reorder(int argc, char **argv, const char *options) +{ + char *tmp_argv[argc]; + char c; + int idx = 1; + int tmpidx = 1; + int takesarg; + + // move the options to the front + while (idx < argc && argv[idx] != 0) { + if (argv[idx][0] == '-') { + c = isvalidopt(argv[idx][1], options, &takesarg); + if (c == '?') + return 1; + tmp_argv[tmpidx] = argv[idx]; + tmpidx++; + if (takesarg) { + tmp_argv[tmpidx] = argv[idx+1]; + printf("tmp_argv[%d] = %s\n", tmpidx, tmp_argv[tmpidx]); + tmpidx++; + idx++; + } + } + idx++; + } + + // Add non-options to the end + idx = 1; + while (idx < argc && argv[idx] != 0) { + if (argv[idx][0] == '-') { + c = isvalidopt(argv[idx][1], options, &takesarg); + if (c == '?') + return c; + if (takesarg) { + idx++; + } + } + else { + tmp_argv[tmpidx] = argv[idx]; + tmpidx++; + } + idx++; + } + + // Reorder argv + for (idx=1; idx < argc; idx++) { + argv[idx] = tmp_argv[idx]; + } + + return 0; +} + +// +// px4_getopt +// +// returns: +// the valid option character +// '?' if any option is unknown +// -1 if no remaining options +// +// If the option takes an arg, myoptarg will be updated accordingly. +// After each call to px4_getopt, myoptind in incremented to the next +// unparsed arg index. +// Argv is changed to put all options and option args at the beginning, +// followed by non-options. +// +int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const char **myoptarg) +{ + char *p; + char c; + int takesarg; + + if (*myoptind == 1) + if (reorder(argc, argv, options) != 0) + return (int)'?'; + + p = argv[*myoptind]; + + if (*myoptarg == 0) + *myoptarg = argv[*myoptind]; + + if (p && options && myoptind && p[0] == '-') { + c = isvalidopt(p[1], options, &takesarg); + if (c == '?') + return (int)c; + *myoptind += 1; + if (takesarg) { + *myoptarg = argv[*myoptind]; + *myoptind += 1; + } + return (int)c; + } + return -1; +} + diff --git a/src/platforms/linux/drivers/accel/accel.cpp b/src/platforms/linux/drivers/accel/accel.cpp index 7e97d77b26..6efb4772cb 100644 --- a/src/platforms/linux/drivers/accel/accel.cpp +++ b/src/platforms/linux/drivers/accel/accel.cpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include @@ -496,7 +496,7 @@ ACCELSIM::init() _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); - if (_accel_topic < 0) { + if (_accel_topic == (orb_advert_t)(-1)) { warnx("ADVERT ERR"); } @@ -1069,7 +1069,12 @@ ACCELSIM::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + + // The first call to measure() is from init() and _accel_topic is not + // yet initialized + if (_accel_topic != (orb_advert_t)(-1)) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } } _accel_read++; @@ -1332,12 +1337,14 @@ accel_main(int argc, char *argv[]) int ch; enum Rotation rotation = ROTATION_NONE; int ret; + int myoptind = 1; + const char * myoptarg = NULL; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "R:")) != EOF) { + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': - rotation = (enum Rotation)atoi(optarg); + rotation = (enum Rotation)atoi(myoptarg); break; default: accel::usage(); @@ -1345,7 +1352,7 @@ accel_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + const char *verb = argv[myoptind]; /* * Start/load the driver. diff --git a/src/platforms/px4_getopt.h b/src/platforms/px4_getopt.h new file mode 100644 index 0000000000..7c85ef32d2 --- /dev/null +++ b/src/platforms/px4_getopt.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_getopt.h + * Thread safe version of getopt + */ + +#pragma once + +__BEGIN_DECLS + +int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const char **myoptarg); + +__END_DECLS + From 892012aa15156e9e33978f527994be37ce469938 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 12:37:56 -0700 Subject: [PATCH 084/258] SIM: made transfer a virtual method Drivers simulating HW can implement specific behavior for calls to transfer. Signed-off-by: Mark Charlebois --- src/drivers/device/sim.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/sim.h b/src/drivers/device/sim.h index 05e4fa78c7..59f7446b47 100644 --- a/src/drivers/device/sim.h +++ b/src/drivers/device/sim.h @@ -97,7 +97,7 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(const uint8_t *send, unsigned send_len, + virtual int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); private: From c6498de3e1384bf8df4981c68e8df12daebaf3ad Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 12:50:24 -0700 Subject: [PATCH 085/258] SIM: pushed transfer stub for ms5611_sim The MS5611_SIM class is supposed to simulate data from a real ms5611. An externl simulator could provide an interface to call to get data that would be returned from a transfer() call. Signed-off-by: Mark Charlebois --- src/drivers/device/sim.h | 4 ++-- src/drivers/ms5611/ms5611_sim.cpp | 11 +++++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/sim.h b/src/drivers/device/sim.h index 59f7446b47..139967f6e8 100644 --- a/src/drivers/device/sim.h +++ b/src/drivers/device/sim.h @@ -97,8 +97,8 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); private: uint16_t _address; diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp index 782712bc4d..b99e35b676 100644 --- a/src/drivers/ms5611/ms5611_sim.cpp +++ b/src/drivers/ms5611/ms5611_sim.cpp @@ -65,6 +65,8 @@ public: virtual int dev_read(unsigned offset, void *data, unsigned count); virtual int dev_ioctl(unsigned operation, unsigned &arg); + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); private: ms5611::prom_u &_prom; @@ -192,3 +194,12 @@ MS5611_SIM::_read_prom() // TODO input simlation data return ret; } + +int +MS5611_SIM::transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len) +{ + // TODO add Simulation data connection so calls retrieve + // data from the simulator + return 0; +} From 1d676b2e10afce659afeeac134a1b0b76b88b4b4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 13:02:40 -0700 Subject: [PATCH 086/258] Moved linux/drivers/accel to linux/drivers/accelsim Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 +- src/platforms/linux/drivers/accel/module.mk | 7 ------- .../{accel/accel.cpp => accelsim/accelsim.cpp} | 18 +++++++++--------- src/platforms/linux/drivers/accelsim/module.mk | 7 +++++++ 4 files changed, 17 insertions(+), 17 deletions(-) delete mode 100644 src/platforms/linux/drivers/accel/module.mk rename src/platforms/linux/drivers/{accel/accel.cpp => accelsim/accelsim.cpp} (99%) create mode 100644 src/platforms/linux/drivers/accelsim/module.mk diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 2781cba50c..218fb4a605 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -58,7 +58,7 @@ MODULES += lib/conversion # Linux port # MODULES += platforms/linux/px4_layer -MODULES += platforms/linux/drivers/accel +MODULES += platforms/linux/drivers/accelsim # # Unit tests diff --git a/src/platforms/linux/drivers/accel/module.mk b/src/platforms/linux/drivers/accel/module.mk deleted file mode 100644 index 71c6c0ebdd..0000000000 --- a/src/platforms/linux/drivers/accel/module.mk +++ /dev/null @@ -1,7 +0,0 @@ -# -# Simulated accel/mag driver -# - -MODULE_COMMAND = accel -SRCS = accel.cpp - diff --git a/src/platforms/linux/drivers/accel/accel.cpp b/src/platforms/linux/drivers/accelsim/accelsim.cpp similarity index 99% rename from src/platforms/linux/drivers/accel/accel.cpp rename to src/platforms/linux/drivers/accelsim/accelsim.cpp index 6efb4772cb..1b5e76839a 100644 --- a/src/platforms/linux/drivers/accel/accel.cpp +++ b/src/platforms/linux/drivers/accelsim/accelsim.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file accel.cpp + * @file accelsim.cpp * Driver for a simulated accelerometer / magnetometer. */ @@ -87,7 +87,7 @@ static const int ERROR = -1; #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) -extern "C" { __EXPORT int accel_main(int argc, char *argv[]); } +extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } class ACCELSIM_mag; @@ -1236,7 +1236,7 @@ ACCELSIM_mag::measure_trampoline(void *arg) /** * Local functions in support of the shell command. */ -namespace accel +namespace accelsim { ACCELSIM *g_dev; @@ -1324,7 +1324,7 @@ info() void usage() { - warnx("Usage: accel 'start', 'info'"); + warnx("Usage: accelsim 'start', 'info'"); warnx("options:"); warnx(" -R rotation"); } @@ -1332,7 +1332,7 @@ usage() } // namespace int -accel_main(int argc, char *argv[]) +accelsim_main(int argc, char *argv[]) { int ch; enum Rotation rotation = ROTATION_NONE; @@ -1347,7 +1347,7 @@ accel_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(myoptarg); break; default: - accel::usage(); + accelsim::usage(); return 0; } } @@ -1358,16 +1358,16 @@ accel_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(verb, "start")) - ret = accel::start(rotation); + ret = accelsim::start(rotation); /* * Print driver information. */ else if (!strcmp(verb, "info")) - ret = accel::info(); + ret = accelsim::info(); else { - accel::usage(); + accelsim::usage(); return 1; } return ret; diff --git a/src/platforms/linux/drivers/accelsim/module.mk b/src/platforms/linux/drivers/accelsim/module.mk new file mode 100644 index 0000000000..dc980c5cc8 --- /dev/null +++ b/src/platforms/linux/drivers/accelsim/module.mk @@ -0,0 +1,7 @@ +# +# Simulated accel/mag driver +# + +MODULE_COMMAND = accelsim +SRCS = accelsim.cpp + From f0312d2da038f04b57c808fff3972ab0efb535be Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 7 Apr 2015 16:09:43 -0700 Subject: [PATCH 087/258] Linux: Added commands to show threads and devices The list_tasks and list_devices commands will show lists of running px4 threads and created virtual device nodes. The list_builtins command was removes and the list of commands will be shown if return is pressed. Signed-off-by: Mark Charlebois --- Tools/linux_apps.py | 30 ++++++-- src/drivers/device/vdev.cpp | 11 +++ src/drivers/device/vdev.h | 7 +- src/drivers/device/vdev_posix.cpp | 17 +++-- src/platforms/linux/main.cpp | 12 ++-- .../linux/px4_layer/px4_linux_tasks.c | 68 +++++++++++++------ src/platforms/px4_posix.h | 1 + src/platforms/px4_tasks.h | 3 + 8 files changed, 106 insertions(+), 43 deletions(-) diff --git a/Tools/linux_apps.py b/Tools/linux_apps.py index 548dcc5e46..b45d3f8888 100755 --- a/Tools/linux_apps.py +++ b/Tools/linux_apps.py @@ -44,16 +44,25 @@ print print """ #include #include + +#define __EXPORT + +#include +#include + using namespace std; +extern void px4_show_devices(void); + extern "C" { """ for app in apps: print "extern int "+app+"_main(int argc, char *argv[]);" print """ -static int list_builtins_main(int argc, char *argv[]); static int shutdown_main(int argc, char *argv[]); +static int list_tasks_main(int argc, char *argv[]); +static int list_devices_main(int argc, char *argv[]); } @@ -66,21 +75,20 @@ static map app_map(void) for app in apps: print '\tapps["'+app+'"] = '+app+'_main;' -print '\tapps["list_builtins"] = list_builtins_main;' print '\tapps["shutdown"] = shutdown_main;' +print '\tapps["list_tasks"] = list_tasks_main;' +print '\tapps["list_devices"] = list_devices_main;' print """ return apps; } map apps = app_map(); -static int list_builtins_main(int argc, char *argv[]) +static void list_builtins(void) { cout << "Builtin Commands:" << endl; for (map::iterator it=apps.begin(); it!=apps.end(); ++it) cout << '\t' << it->first << endl; - - return 0; } static int shutdown_main(int argc, char *argv[]) @@ -88,5 +96,17 @@ static int shutdown_main(int argc, char *argv[]) cout << "Shutting down" << endl; exit(0); } + +static int list_tasks_main(int argc, char *argv[]) +{ + px4_show_tasks(); + return 0; +} + +static int list_devices_main(int argc, char *argv[]) +{ + px4_show_devices(); + return 0; +} """ diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 1e69384e68..157da97b82 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -454,5 +454,16 @@ VDev *VDev::getDev(const char *path) return NULL; } +void VDev::showDevices() +{ + int i=0; + printf("Devices:\n"); + for (; iname); + } + } +} + } // namespace device diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index e98d13209a..62ae479c62 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -64,10 +64,10 @@ struct px4_dev_handle_t { int fd; int flags; void *priv; - void *cdev; + void *vdev; - px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), cdev(NULL) {} - px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), cdev(c) {} + px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), vdev(NULL) {} + px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), vdev(c) {} }; /** @@ -333,6 +333,7 @@ public: virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); static VDev *getDev(const char *path); + static void showDevices(void); protected: diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index bb119624c5..df19fd8392 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -117,7 +117,7 @@ int px4_close(int fd) { int ret; if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_close fd = %d\n", fd); ret = dev->close(filemap[fd]); filemap[fd] = NULL; @@ -135,7 +135,7 @@ ssize_t px4_read(int fd, void *buffer, size_t buflen) { int ret; if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_read fd = %d\n", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); } @@ -152,7 +152,7 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen) { int ret = PX4_ERROR; if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_write fd = %d\n", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); } @@ -169,7 +169,7 @@ int px4_ioctl(int fd, int cmd, unsigned long arg) { int ret = PX4_ERROR; if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->cdev); + VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_ioctl fd = %d\n", fd); ret = dev->ioctl(filemap[fd], cmd, arg); } @@ -206,7 +206,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // If fd is valid if (valid_fd(fds[i].fd)) { - VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);; + VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; PX4_DEBUG("px4_poll: VDev->poll(setup) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); @@ -250,7 +250,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) // If fd is valid if (valid_fd(fds[i].fd)) { - VDev *dev = (VDev *)(filemap[fds[i].fd]->cdev);; + VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; PX4_DEBUG("px4_poll: VDev->poll(teardown) %d\n", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); @@ -269,5 +269,10 @@ cleanup: return count; } +void px4_show_devices() +{ + VDev::showDevices(); +} + } diff --git a/src/platforms/linux/main.cpp b/src/platforms/linux/main.cpp index f3b1ecc221..8ae5ee58d5 100644 --- a/src/platforms/linux/main.cpp +++ b/src/platforms/linux/main.cpp @@ -48,6 +48,7 @@ using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" +#include "px4_middleware.h" void run_cmd(const vector &appargs); void run_cmd(const vector &appargs) { @@ -62,12 +63,12 @@ void run_cmd(const vector &appargs) { ++i; } arg[i] = (char *)0; - //cout << command << " " << i << endl; apps[command](i,(char **)arg); } else { cout << "Invalid command" << endl; + list_builtins(); } } @@ -85,21 +86,16 @@ int main(int argc, char **argv) if (argc == 2) { ifstream infile(argv[1]); - //vector tokens; - for (string line; getline(infile, line, '\n'); ) { process_line(line); } } string mystr; - vector appargs(2); - - appargs[0] = "list_builtins"; - while(1) { - run_cmd(appargs); + px4::init(argc, argv, "mainapp"); + while(1) { cout << "Enter a command and its args:" << endl; getline (cin,mystr); process_line(mystr); diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index 0d710cad6d..f25cfa3a39 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -42,24 +42,28 @@ #include #include #include +#include #include #include -#include #include -#include #include #include #include -//#include - #include #define MAX_CMD_LEN 100 #define PX4_MAX_TASKS 100 -static pthread_t taskmap[PX4_MAX_TASKS] = {}; +typedef struct +{ + pthread_t pid; + const char *name; + bool isused; +} task_entry; + +static task_entry taskmap[PX4_MAX_TASKS] = {}; typedef struct { @@ -69,15 +73,16 @@ typedef struct // strings are allocated after the } pthdata_t; -void entry_adapter ( void *ptr ); -void entry_adapter ( void *ptr ) +static void entry_adapter ( void *ptr ) { pthdata_t *data; data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); free(ptr); - pthread_exit(0); + printf("Before px4_task_exit\n"); + px4_task_exit(0); + printf("After px4_task_exit\n"); } void @@ -170,9 +175,10 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int printf("pthread_create task=%lu rv=%d\n",(unsigned long)task, rv); for (i=0; i=PX4_MAX_TASKS) printf("px4_task_exit: self task not found!\n"); + else + printf("px4_task_exit: %s\n", taskmap[i].name); pthread_exit((void *)(unsigned long)ret); } @@ -229,7 +237,7 @@ void px4_killall(void) //printf("Called px4_killall\n"); for (int i=0; i Date: Tue, 7 Apr 2015 16:28:50 -0700 Subject: [PATCH 088/258] Linux: fixed registration of a class of device Previously it created 4 instances instead of the next available slot. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 157da97b82..87fbbe6e74 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -108,6 +108,7 @@ VDev::register_class_devname(const char *class_devname) char name[32]; snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, (void *)this); + if (ret == OK) break; class_instance++; } @@ -171,6 +172,7 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc for (int i=0;iname,name) == 0) { delete devmap[i]; + debug("Unregistered class DEV %s", name); devmap[i] = NULL; return PX4_OK; } From a6950eb7d3cd4b8eb251d5bb7094deb1c82cf7dc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 8 Apr 2015 09:13:55 -0700 Subject: [PATCH 089/258] Linux: fixed bad return value Function was always returning -ENOTTY instead of the "ret" variable. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 87fbbe6e74..89275a919c 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -315,7 +315,7 @@ VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) break; } - return -ENOTTY; + return ret; } int From 76e28ac952199896e324132ca3fa4230a07b652c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 8 Apr 2015 09:17:21 -0700 Subject: [PATCH 090/258] Use of != compare of floats in AttitudeEKF.c This is a bug and is unsafe. I am not going to change the code but it needs to be changed to a cast to int or a <= as it is unsafe to check for equality with 0.0F. Disabled warning for GCC 4.9 for now. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 3ef60a4f65..151223b66c 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -42,7 +42,7 @@ # # Set to 1 for GCC-4.8.2 and to 0 for Clang-3.5 (Ubuntu 14.04) -USE_GCC?=0 +USE_GCC?=1 ifneq ($(USE_GCC),1) @@ -193,7 +193,8 @@ ifeq ($(USE_GCC),1) ARCHCWARNINGS += -Wold-style-declaration \ -Wmissing-parameter-type \ -Wno-error=unused-local-typedefs \ - -Wno-error=enum-compare + -Wno-error=enum-compare \ + -Wno-error=float-equal endif # C++-specific warnings From d137c09b0bbd09446a9ea31ef77fe924ef859f6f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 8 Apr 2015 09:34:43 -0700 Subject: [PATCH 091/258] Linux: removed debug output for task creation Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/px4_linux_tasks.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index f25cfa3a39..3c26acdcb5 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -160,7 +160,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int if (rv != 0) { if (rv == EPERM) { - printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); + //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata); if (rv != 0) { printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); @@ -172,8 +172,6 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int } } - printf("pthread_create task=%lu rv=%d\n",(unsigned long)task, rv); - for (i=0; i Date: Wed, 8 Apr 2015 09:35:36 -0700 Subject: [PATCH 092/258] ACCELSIM: fixed calls to open, close, ioctl The calls to open, close, ioctl should have been px4_open, px4_close and px4_ioctl. Signed-off-by: Mark Charlebois --- .../linux/drivers/accelsim/accelsim.cpp | 51 +++++++++++-------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/src/platforms/linux/drivers/accelsim/accelsim.cpp b/src/platforms/linux/drivers/accelsim/accelsim.cpp index 1b5e76839a..1225a5f60d 100644 --- a/src/platforms/linux/drivers/accelsim/accelsim.cpp +++ b/src/platforms/linux/drivers/accelsim/accelsim.cpp @@ -1268,29 +1268,40 @@ start(enum Rotation rotation) goto fail; } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { + warnx("failed init of ACCELSIM obj"); goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - fd_mag = open(ACCELSIM_DEVICE_PATH_MAG, O_RDONLY); - - /* don't fail if open cannot be opened */ - if (0 <= fd_mag) { - if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } } - close(fd); - close(fd_mag); + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); + + if (fd < 0) { + warnx("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + goto fail; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + px4_close(fd); + goto fail; + } + + fd_mag = px4_open(ACCELSIM_DEVICE_PATH_MAG, O_RDONLY); + + /* don't fail if mag dev cannot be opened */ + if (0 <= fd_mag) { + if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + } + } + else + { + warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + } + + px4_close(fd); + px4_close(fd_mag); return 0; fail: From 5d988381e629281ae5435aeda3b4ca102f6e6f7b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 8 Apr 2015 14:40:20 -0700 Subject: [PATCH 093/258] Linux: Created new linker script from scratch Signed-off-by: Mark Charlebois --- linux-configs/linuxtest/scripts/ld.script | 25 +++++------------------ 1 file changed, 5 insertions(+), 20 deletions(-) diff --git a/linux-configs/linuxtest/scripts/ld.script b/linux-configs/linuxtest/scripts/ld.script index 2e92f20a69..32478e1e14 100644 --- a/linux-configs/linuxtest/scripts/ld.script +++ b/linux-configs/linuxtest/scripts/ld.script @@ -1,8 +1,8 @@ /**************************************************************************** - * configs/aerocore/common/ld.script + * ld.script * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Author: Mark Charlebois * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -14,7 +14,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -33,27 +33,12 @@ * ****************************************************************************/ -/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and - * 256Kb of SRAM. SRAM is split up into three blocks: - * - * 1) 112Kb of SRAM beginning at address 0x2000:0000 - * 2) 16Kb of SRAM beginning at address 0x2001:c000 - * 3) 64Kb of SRAM beginning at address 0x2002:0000 - * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 - * - * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 - * where the code expects to begin execution by jumping to the entry point in - * the 0x0800:0000 address range. - * - * The first 0x4000 of flash is reserved for the bootloader. - */ - SECTIONS { /* * Construction data for parameters. */ - __param ALIGN(8): { + __param : ALIGN(8) { __param_start = .; KEEP(*(__param*)) __param_end = .; From ec1b77c9e1fcca6c0984539d72e8c324f063a624 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 8 Apr 2015 14:50:27 -0700 Subject: [PATCH 094/258] Linux: GCC static data is 16byte aligned, messes up param GCC 4.8 and higher implement 16 byte static data alignment on 64-bit. This means that the 24-byte param_info_s variables are 16 byte aligned by GCC and that messes up the assumption that the address of the second parameter is at ¶m[0]+sizeof(param[0]). When compiled with clang it is true, with gcc is is not true. See https://llvm.org/bugs/show_bug.cgi?format=multiple&id=18006 The fix is needed for GCC >=4.8 only. Clang works fine without this. Added __attribute__((aligned(16))) to first member of param_info_s. Signed-off-by: Mark Charlebois --- src/modules/systemlib/param/param.h | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index b29a7e51d6..8f53a5d730 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -374,6 +374,7 @@ union param_value_u { void *p; int32_t i; float f; + long long x; }; /** @@ -383,7 +384,23 @@ union param_value_u { * instead. */ struct param_info_s { - const char *name; + const char *name + +// GCC 4.8 and higher don't implement proper alignment of static data on +// 64-bit. This means that the 24-byte param_info_s variables are +// 16 byte aligned by GCC and that messes up the assumption that +// sequential items in the __param segment can be addressed as an array. +// The assumption is that the address of the second parameter is at +// ¶m[0]+sizeof(param[0]). When compiled with clang it is +// true, with gcc is is not true. +// See https://llvm.org/bugs/show_bug.cgi?format=multiple&id=18006 +// The following hack is for GCC >=4.8 only. Clang works fine without +// this. +#ifdef __PX4_LINUX + __attribute__((aligned(16))); +#else + ; +#endif param_type_t type; union param_value_u val; }; From 410f86b76745a1f40982f6a9da0f0fcab0d4e8b9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 8 Apr 2015 16:18:11 -0700 Subject: [PATCH 095/258] Linux: Added simulated gyro The code is based on mpu6000. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 1 + src/drivers/drv_sensor.h | 20 +- .../linux/drivers/gyrosim/gyrosim.cpp | 2055 +++++++++++++++++ src/platforms/linux/drivers/gyrosim/module.mk | 46 + 4 files changed, 2113 insertions(+), 9 deletions(-) create mode 100644 src/platforms/linux/drivers/gyrosim/gyrosim.cpp create mode 100644 src/platforms/linux/drivers/gyrosim/module.mk diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 218fb4a605..b7bfd9392f 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -59,6 +59,7 @@ MODULES += lib/conversion # MODULES += platforms/linux/px4_layer MODULES += platforms/linux/drivers/accelsim +MODULES += platforms/linux/drivers/gyrosim # # Unit tests diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 4b5775d223..a98bcafc7a 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -52,17 +52,19 @@ * Used to create a unique device id for redundant and combo sensors */ -#define DRV_MAG_DEVTYPE_HMC5883 0x01 -#define DRV_MAG_DEVTYPE_LSM303D 0x02 +#define DRV_MAG_DEVTYPE_HMC5883 0x01 +#define DRV_MAG_DEVTYPE_LSM303D 0x02 #define DRV_MAG_DEVTYPE_ACCELSIM 0x03 -#define DRV_ACC_DEVTYPE_LSM303D 0x11 -#define DRV_ACC_DEVTYPE_BMA180 0x12 -#define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_ACC_DEVTYPE_LSM303D 0x11 +#define DRV_ACC_DEVTYPE_BMA180 0x12 +#define DRV_ACC_DEVTYPE_MPU6000 0x13 #define DRV_ACC_DEVTYPE_ACCELSIM 0x14 -#define DRV_GYR_DEVTYPE_MPU6000 0x21 -#define DRV_GYR_DEVTYPE_L3GD20 0x22 -#define DRV_RNG_DEVTYPE_MB12XX 0x31 -#define DRV_RNG_DEVTYPE_LL40LS 0x32 +#define DRV_ACC_DEVTYPE_GYROSIM 0x15 +#define DRV_GYR_DEVTYPE_MPU6000 0x21 +#define DRV_GYR_DEVTYPE_L3GD20 0x22 +#define DRV_GYR_DEVTYPE_GYROSIM 0x23 +#define DRV_RNG_DEVTYPE_MB12XX 0x31 +#define DRV_RNG_DEVTYPE_LL40LS 0x32 /* * ioctl() definitions diff --git a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp new file mode 100644 index 0000000000..1091b7e930 --- /dev/null +++ b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp @@ -0,0 +1,2055 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file gyrosim.cpp + * + * Driver for the simulated gyro + * + * @author Andrew Tridgell + * @author Pat Hickey + * @author Mark Charlebois + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +#define MPU_DEVICE_PATH_ACCEL "/dev/gyrosim_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/gyrosim_gyro" + +// MPU 6000 registers +#define MPUREG_WHOAMI 0x75 +#define MPUREG_SMPLRT_DIV 0x19 +#define MPUREG_CONFIG 0x1A +#define MPUREG_GYRO_CONFIG 0x1B +#define MPUREG_ACCEL_CONFIG 0x1C +#define MPUREG_FIFO_EN 0x23 +#define MPUREG_INT_PIN_CFG 0x37 +#define MPUREG_INT_ENABLE 0x38 +#define MPUREG_INT_STATUS 0x3A +#define MPUREG_ACCEL_XOUT_H 0x3B +#define MPUREG_ACCEL_XOUT_L 0x3C +#define MPUREG_ACCEL_YOUT_H 0x3D +#define MPUREG_ACCEL_YOUT_L 0x3E +#define MPUREG_ACCEL_ZOUT_H 0x3F +#define MPUREG_ACCEL_ZOUT_L 0x40 +#define MPUREG_TEMP_OUT_H 0x41 +#define MPUREG_TEMP_OUT_L 0x42 +#define MPUREG_GYRO_XOUT_H 0x43 +#define MPUREG_GYRO_XOUT_L 0x44 +#define MPUREG_GYRO_YOUT_H 0x45 +#define MPUREG_GYRO_YOUT_L 0x46 +#define MPUREG_GYRO_ZOUT_H 0x47 +#define MPUREG_GYRO_ZOUT_L 0x48 +#define MPUREG_USER_CTRL 0x6A +#define MPUREG_PWR_MGMT_1 0x6B +#define MPUREG_PWR_MGMT_2 0x6C +#define MPUREG_FIFO_COUNTH 0x72 +#define MPUREG_FIFO_COUNTL 0x73 +#define MPUREG_FIFO_R_W 0x74 +#define MPUREG_PRODUCT_ID 0x0C +#define MPUREG_TRIM1 0x0D +#define MPUREG_TRIM2 0x0E +#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 + +// Configuration bits MPU 3000 and MPU 6000 (not revised)? +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 +#define BITS_DLPF_CFG_MASK 0x07 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_RAW_RDY_EN 0x01 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_INT_STATUS_DATA 0x01 + +// Product ID Description for GYROSIM +// high 4 bits low 4 bits +// Product Name Product Revision +#define GYROSIMES_REV_C4 0x14 +#define GYROSIMES_REV_C5 0x15 +#define GYROSIMES_REV_D6 0x16 +#define GYROSIMES_REV_D7 0x17 +#define GYROSIMES_REV_D8 0x18 +#define GYROSIM_REV_C4 0x54 +#define GYROSIM_REV_C5 0x55 +#define GYROSIM_REV_D6 0x56 +#define GYROSIM_REV_D7 0x57 +#define GYROSIM_REV_D8 0x58 +#define GYROSIM_REV_D9 0x59 +#define GYROSIM_REV_D10 0x5A + +#define GYROSIM_ACCEL_DEFAULT_RANGE_G 8 +#define GYROSIM_ACCEL_DEFAULT_RATE 1000 +#define GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define GYROSIM_GYRO_DEFAULT_RANGE_G 8 +#define GYROSIM_GYRO_DEFAULT_RATE 1000 +#define GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define GYROSIM_DEFAULT_ONCHIP_FILTER_FREQ 42 + +#define GYROSIM_ONE_G 9.80665f + +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + +/* + the GYROSIM can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define GYROSIM_LOW_BUS_SPEED 1000*1000 +#define GYROSIM_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ + +class GYROSIM_gyro; + +class GYROSIM : public device::VDev +{ +public: + GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation); + virtual ~GYROSIM(); + + virtual int init(); + + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + int transfer(uint8_t *send, uint8_t *recv, unsigned len); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + +protected: + friend class GYROSIM_gyro; + + virtual ssize_t gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + +private: + GYROSIM_gyro *_gyro; + uint8_t _product; /** product code */ + + struct hrt_call _call; + unsigned _call_interval; + + RingBuffer *_accel_reports; + + struct accel_scale _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + int _accel_class_instance; + + RingBuffer *_gyro_reports; + + struct gyro_scale _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + + unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; + perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + perf_counter_t _good_transfers; + perf_counter_t _reset_retries; + perf_counter_t _system_latency_perf; + perf_counter_t _controller_latency_perf; + + uint8_t _register_wait; + uint64_t _reset_wait; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + + enum Rotation _rotation; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define GYROSIM_NUM_CHECKED_REGISTERS 9 + static const uint8_t _checked_registers[GYROSIM_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[GYROSIM_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + + // use this to avoid processing measurements when in factory + // self test + volatile bool _in_factory_test; + + // last temperature reading for print_info() + float _last_temperature; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Read a register from the GYROSIM + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg, uint32_t speed=GYROSIM_LOW_BUS_SPEED); + uint16_t read_reg16(unsigned reg); + + /** + * Write a register in the GYROSIM + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the GYROSIM + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the GYROSIM, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the GYROSIM measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_accel_range(unsigned max_g); + + /** + * Swap a 16-bit value read from the GYROSIM to native byte order. + */ + uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + + /** + * Measurement self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Gyro self test + * + * @return 0 on success, 1 on failure + */ + int gyro_self_test(); + + /* + set low pass filter frequency + */ + void _set_dlpf_filter(uint16_t frequency_hz); + + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(unsigned desired_sample_rate_hz); + + /* + check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + GYROSIM(const GYROSIM&); + GYROSIM operator=(const GYROSIM&); + +#pragma pack(push, 1) + /** + * Report conversation within the GYROSIM, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) +}; + +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint8_t GYROSIM::_checked_registers[GYROSIM_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_PWR_MGMT_1, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG }; + + + +/** + * Helper class implementing the gyro driver node. + */ +class GYROSIM_gyro : public device::VDev +{ +public: + GYROSIM_gyro(GYROSIM *parent, const char *path); + ~GYROSIM_gyro(); + + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + virtual int init(); + +protected: + friend class GYROSIM; + + void parent_poll_notify(); + +private: + GYROSIM *_parent; + orb_advert_t _gyro_topic; + int _gyro_orb_class_instance; + int _gyro_class_instance; + + /* do not allow to copy this class due to pointer data members */ + GYROSIM_gyro(const GYROSIM_gyro&); + GYROSIM_gyro operator=(const GYROSIM_gyro&); +}; + +/** driver 'main' command */ +extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } + +GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : + VDev("GYROSIM", path_accel), + _gyro(new GYROSIM_gyro(this, path_gyro)), + _product(GYROSIMES_REV_C4), + _call{}, + _call_interval(0), + _accel_reports(nullptr), + _accel_scale{}, + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(-1), + _accel_orb_class_instance(-1), + _accel_class_instance(-1), + _gyro_reports(nullptr), + _gyro_scale{}, + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "gyrosim_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "gyrosim_gyro_read")), + _sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")), + _bad_transfers(perf_alloc(PC_COUNT, "gyrosim_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "gyrosim_bad_registers")), + _good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), + _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _register_wait(0), + _reset_wait(0), + _accel_filter_x(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(GYROSIM_ACCEL_DEFAULT_RATE, GYROSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _rotation(rotation), + _checked_next(0), + _in_factory_test(false), + _last_temperature(0) +{ + // disable debug() calls + _debug_enabled = false; + + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; + + // default accel scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + // default gyro scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + memset(&_call, 0, sizeof(_call)); + _checked_values[0] = _product; +} + +GYROSIM::~GYROSIM() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the gyro subdriver */ + delete _gyro; + + /* free any existing reports */ + if (_accel_reports != nullptr) + delete _accel_reports; + if (_gyro_reports != nullptr) + delete _gyro_reports; + + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); + perf_free(_bad_transfers); + perf_free(_bad_registers); + perf_free(_good_transfers); +} + +int +GYROSIM::init() +{ + int ret; + + /* do VDevinit first */ + ret = VDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + warnx("VDev setup failed"); + return ret; + } + + /* allocate basic report buffers */ + _accel_reports = new RingBuffer(2, sizeof(accel_report)); + if (_accel_reports == nullptr) { + warnx("_accel_reports creation failed"); + goto out; + } + + _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); + if (_gyro_reports == nullptr) { + warnx("_gyro_reports creation failed"); + goto out; + } + + if (reset() != OK) { + warnx("reset failed"); + goto out; + } + + /* Initialize offsets and scales */ + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + + /* do VDev init for the gyro device node, keep it optional */ + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + measure(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, ORB_PRIO_HIGH); + + if (_accel_topic < 0) { + warnx("ADVERT FAIL"); + } + + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); + + if (_gyro->_gyro_topic < 0) { + warnx("ADVERT FAIL"); + } + +out: + return ret; +} + +int GYROSIM::reset() +{ + return OK; +} + +int +GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + return PX4_OK; +} + +/* + set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro +*/ +void +GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) +{ + if (desired_sample_rate_hz == 0 || + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz = GYROSIM_GYRO_DEFAULT_RATE; + } + + uint8_t div = 1000 / desired_sample_rate_hz; + if(div>200) div=200; + if(div<1) div=1; + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; +} + +/* + set the DLPF filter frequency. This affects both accel and gyro. + */ +void +GYROSIM::_set_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + /* + choose next highest filter frequency available + */ + if (frequency_hz == 0) { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } else if (frequency_hz <= 5) { + filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { + filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { + filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 42) { + filter = BITS_DLPF_CFG_42HZ; + } else if (frequency_hz <= 98) { + filter = BITS_DLPF_CFG_98HZ; + } else if (frequency_hz <= 188) { + filter = BITS_DLPF_CFG_188HZ; + } else if (frequency_hz <= 256) { + filter = BITS_DLPF_CFG_256HZ_NOLPF2; + } else { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } + write_checked_reg(MPUREG_CONFIG, filter); +} + +ssize_t +GYROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(accel_report); + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _accel_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_accel_reports->empty()) + return -EAGAIN; + + perf_count(_accel_reads); + + /* copy reports out of our buffer to the caller */ + accel_report *arp = reinterpret_cast(buffer); + int transferred = 0; + while (count--) { + if (!_accel_reports->get(arp)) + break; + transferred++; + arp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(accel_report)); +} + +int +GYROSIM::self_test() +{ + if (perf_event_count(_sample_perf) == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; +} + +int +GYROSIM::accel_self_test() +{ + if (self_test()) + return 1; + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + return 1; + + return 0; +} + +int +GYROSIM::gyro_self_test() +{ + if (self_test()) + return 1; + + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ + const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) + return 1; + + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + return 1; + + if (fabsf(_gyro_scale.y_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + return 1; + + if (fabsf(_gyro_scale.z_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + return 1; + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + + return 0; +} + + + +/* + perform a self-test comparison to factory trim values. This takes + about 200ms and will return OK if the current values are within 14% + of the expected values (as per datasheet) + */ +int +GYROSIM::factory_self_test() +{ + _in_factory_test = true; + uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); + uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); + const uint16_t repeats = 100; + int ret = OK; + + // gyro self test has to be done at 250DPS + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); + + struct MPUReport mpu_report; + float accel_baseline[3]; + float gyro_baseline[3]; + float accel[3]; + float gyro[3]; + float accel_ftrim[3]; + float gyro_ftrim[3]; + + // get baseline values without self-test enabled + //set_frequency(GYROSIM_HIGH_BUS_SPEED); + + memset(accel_baseline, 0, sizeof(accel_baseline)); + memset(gyro_baseline, 0, sizeof(gyro_baseline)); + memset(accel, 0, sizeof(accel)); + memset(gyro, 0, sizeof(gyro)); + + for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); + atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); + atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + gtrim[0] = trims[0] & 0x1F; + gtrim[1] = trims[1] & 0x1F; + gtrim[2] = trims[2] & 0x1F; + + // convert factory trims to right units + for (uint8_t i=0; i<3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + } + // Y gyro trim is negative + gyro_ftrim[1] *= -1; + + for (uint8_t i=0; i<3; i++) { + float diff = accel[i]-accel_baseline[i]; + float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*accel_baseline[i]), + (int)(1000*accel[i]), + (int)(1000*diff), + (int)(1000*accel_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + for (uint8_t i=0; i<3; i++) { + float diff = gyro[i]-gyro_baseline[i]; + float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*gyro_baseline[i]), + (int)(1000*gyro[i]), + (int)(1000*(gyro[i]-gyro_baseline[i])), + (int)(1000*gyro_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + + write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); + + _in_factory_test = false; + if (ret == OK) { + ::printf("PASSED\n"); + } + + return ret; +} + + +ssize_t +GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(gyro_report); + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _gyro_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_gyro_reports->empty()) + return -EAGAIN; + + perf_count(_gyro_reads); + + /* copy reports out of our buffer to the caller */ + gyro_report *grp = reinterpret_cast(buffer); + int transferred = 0; + while (count--) { + if (!_gyro_reports->get(grp)) + break; + transferred++; + grp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(gyro_report)); +} + +int +GYROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCRESET: + return reset(); + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + return ioctl(handlep, SENSORIOCSPOLLRATE, 1000); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(handlep, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _set_dlpf_filter(cutoff_freq_hz); + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + + + float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _set_dlpf_filter(cutoff_freq_hz_gyro); + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _accel_reports->size(); + + case ACCELIOCGSAMPLERATE: + return _sample_rate; + + case ACCELIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; + + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); + // set software filtering + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + case ACCELIOCSSCALE: + { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + case ACCELIOCSRANGE: + return set_accel_range(arg); + + case ACCELIOCGRANGE: + return (unsigned long)((_accel_range_m_s2)/GYROSIM_ONE_G + 0.5f); + + case ACCELIOCSELFTEST: + return accel_self_test(); + + default: + /* give it to the superclass */ + return VDev::ioctl(handlep, cmd, arg); + } +} + +int +GYROSIM::gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCGPOLLRATE: + case SENSORIOCRESET: + return ioctl(handlep, cmd, arg); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + if (!_gyro_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _gyro_reports->size(); + + case GYROIOCGSAMPLERATE: + return _sample_rate; + + case GYROIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; + + case GYROIOCGLOWPASS: + return _gyro_filter_x.get_cutoff_freq(); + case GYROIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); + _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale)); + return OK; + + case GYROIOCGSCALE: + /* copy scale out */ + memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; + + case GYROIOCSRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _gyro_range_scale = xx + // _gyro_range_rad_s = xx + return -EINVAL; + case GYROIOCGRANGE: + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); + + case GYROIOCSELFTEST: + return gyro_self_test(); + + default: + /* give it to the superclass */ + return VDev::ioctl(handlep, cmd, arg); + } +} + +uint8_t +GYROSIM::read_reg(unsigned reg, uint32_t speed) +{ + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + + // general register transfer at low clock speed + //set_frequency(speed); + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +uint16_t +GYROSIM::read_reg16(unsigned reg) +{ + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; + + // general register transfer at low clock speed + //set_frequency(GYROSIM_LOW_BUS_SPEED); + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +void +GYROSIM::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + // general register transfer at low clock speed + //set_frequency(GYROSIM_LOW_BUS_SPEED); + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +GYROSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +void +GYROSIM::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; i 8) { // 16g - AFS_SEL = 3 + afs_sel = 3; + lsb_per_g = 2048; + max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 + afs_sel = 2; + lsb_per_g = 4096; + max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 + afs_sel = 1; + lsb_per_g = 8192; + max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 + afs_sel = 0; + lsb_per_g = 16384; + max_accel_g = 2; + } + + write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); + _accel_range_scale = (GYROSIM_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * GYROSIM_ONE_G; + + return OK; +} + +void +GYROSIM::start() +{ + /* make sure we are stopped first */ + stop(); + + /* discard any stale data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); +} + +void +GYROSIM::stop() +{ + hrt_cancel(&_call); +} + +void +GYROSIM::measure_trampoline(void *arg) +{ + GYROSIM *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +GYROSIM::check_registers(void) +{ + /* + we read the register at full speed, even though it isn't + listed as a high speed register. The low speed requirement + for some registers seems to be a propgation delay + requirement for changing sensor configuration, which should + not apply to reading a single register. It is also a better + test of VDev bus health to read at the same speed as we read + the data registers. + */ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next], GYROSIM_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the VDev bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the gyrosim in a + // bizarre state where it has all correct + // register values but large offsets on the + // accel axes + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + _register_wait = 20; + } + _checked_next = (_checked_next+1) % GYROSIM_NUM_CHECKED_REGISTERS; +} + +void +GYROSIM::measure() +{ + if (_in_factory_test) { + // don't publish any data while in factory test mode + return; + } + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct MPUReport mpu_report; + struct Report { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the GYROSIM in one pass. + */ + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + check_registers(); + + // sensor transfer at high clock speed + //set_frequency(GYROSIM_HIGH_BUS_SPEED); + + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + return; + + /* + * Convert from big to little endian + */ + + report.accel_x = int16_t_from_bytes(mpu_report.accel_x); + report.accel_y = int16_t_from_bytes(mpu_report.accel_y); + report.accel_z = int16_t_from_bytes(mpu_report.accel_z); + + report.temp = int16_t_from_bytes(mpu_report.temp); + + report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); + report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); + report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a VDev bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } + + + /* + * Swap axes and negate y + */ + int16_t accel_xt = report.accel_y; + int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); + + /* + * Apply the swap + */ + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; + + /* + * Report buffers. + */ + accel_report arb; + gyro_report grb; + + /* + * Adjust and scale results to m/s^2. + */ + grb.timestamp = arb.timestamp = hrt_absolute_time(); + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + /* NOTE: Axes have been swapped to match the board a few lines above. */ + + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; + + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); + + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; + + _last_temperature = (report.temp) / 361.0f + 35.0f; + + arb.temperature_raw = report.temp; + arb.temperature = _last_temperature; + + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; + + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); + + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; + + grb.temperature_raw = report.temp; + grb.temperature = _last_temperature; + + _accel_reports->force(&arb); + _gyro_reports->force(&grb); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + _gyro->parent_poll_notify(); + + if (!(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + perf_begin(_system_latency_perf); + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +GYROSIM::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; igyro_read(handlep, buffer, buflen); +} + +int +GYROSIM_gyro::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(handlep, cmd, arg); + break; + default: + return _parent->gyro_ioctl(handlep, cmd, arg); + } +} + +/** + * Local functions in support of the shell command. + */ +namespace gyrosim +{ + +GYROSIM *g_dev_sim; // on simulated bus + +int start(enum Rotation); +int stop(); +int test(); +int reset(); +int info(); +int regdump(); +int factorytest(); +void usage(); + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +int +start(enum Rotation rotation) +{ + int fd; + GYROSIM **g_dev_ptr = &g_dev_sim; + const char *path_accel = MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = MPU_DEVICE_PATH_GYRO; + + if (*g_dev_ptr != nullptr) { + /* if already started, the still command succeeded */ + warnx("already started"); + return 0; + } + + /* create the driver */ + *g_dev_ptr = new GYROSIM(path_accel, path_gyro, rotation); + + if (*g_dev_ptr == nullptr) + goto fail; + + if (OK != (*g_dev_ptr)->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(path_accel, O_RDONLY); + + if (fd < 0) + goto fail; + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + px4_close(fd); + + return 0; +fail: + + if (*g_dev_ptr != nullptr) { + delete (*g_dev_ptr); + *g_dev_ptr = nullptr; + } + + warnx("driver start failed"); + return 1; +} + +int +stop() +{ + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr != nullptr) { + delete *g_dev_ptr; + *g_dev_ptr = nullptr; + } else { + /* warn, but not an error */ + warnx("already stopped."); + } + return 0; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + const char *path_accel = MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = MPU_DEVICE_PATH_GYRO; + accel_report a_report; + gyro_report g_report; + ssize_t sz; + + /* get the driver */ + int fd = px4_open(path_accel, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'gyrosim start')", + path_accel); + + /* get the driver */ + int fd_gyro = px4_open(path_gyro, O_RDONLY); + + if (fd_gyro < 0) + err(1, "%s open failed", path_gyro); + + /* reset to manual polling */ + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + err(1, "reset to manual polling"); + + /* do a simple demand read */ + sz = read(fd, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) { + warnx("ret: %zd, expected: %ld", sz, sizeof(a_report)); + err(1, "immediate acc read failed"); + } + + warnx("single read"); + warnx("time: %lld", (long long)a_report.timestamp); + warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); + warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); + warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); + warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, + (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); + + /* do a simple demand read */ + sz = read(fd_gyro, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) { + warnx("ret: %zd, expected: %ld", sz, sizeof(g_report)); + err(1, "immediate gyro read failed"); + } + + warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyro x: \t%d\traw", (int)g_report.x_raw); + warnx("gyro y: \t%d\traw", (int)g_report.y_raw); + warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + + warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); + warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + + + /* XXX add poll-rate tests here too */ + + reset(); + warnx("PASS"); + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + const char *path_accel = MPU_DEVICE_PATH_ACCEL; + int fd = px4_open(path_accel, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + px4_close(fd); + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { + warnx("driver not running"); + return 1; + } + + printf("state @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_info(); + + return 0; +} + +/** + * Dump the register information + */ +int +regdump() +{ + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { + warnx("driver not running"); + return 1; + } + + printf("regdump @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_registers(); + + return 0; +} + +/** + * Dump the register information + */ +int +factorytest() +{ + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { + warnx("driver not running"); + return 1; + } + + (*g_dev_ptr)->factory_self_test(); + + return 0; +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest'"); + warnx("options:"); + warnx(" -R rotation"); +} + +} // namespace + +int +gyrosim_main(int argc, char *argv[]) +{ + int ch; + enum Rotation rotation = ROTATION_NONE; + int ret; + + /* jump over start/off/etc and look at options first */ + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + default: + gyrosim::usage(); + return 0; + } + } + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + + */ + if (!strcmp(verb, "start")) { + ret = gyrosim::start(rotation); + } + + else if (!strcmp(verb, "stop")) { + ret = gyrosim::stop(); + } + + /* + * Test the driver/device. + */ + else if (!strcmp(verb, "test")) { + ret = gyrosim::test(); + } + + /* + * Reset the driver. + */ + else if (!strcmp(verb, "reset")) { + ret = gyrosim::reset(); + } + + /* + * Print driver information. + */ + else if (!strcmp(verb, "info")) { + ret = gyrosim::info(); + } + + /* + * Print register information. + */ + else if (!strcmp(verb, "regdump")) { + ret = gyrosim::regdump(); + } + + else if (!strcmp(verb, "factorytest")) { + ret = gyrosim::factorytest(); + } + + else { + gyrosim::usage(); + ret = 1; + } + + return ret; +} diff --git a/src/platforms/linux/drivers/gyrosim/module.mk b/src/platforms/linux/drivers/gyrosim/module.mk new file mode 100644 index 0000000000..7bd37c2e2a --- /dev/null +++ b/src/platforms/linux/drivers/gyrosim/module.mk @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Makefile to build the MPU6000 driver. +# + +MODULE_COMMAND = gyrosim + +SRCS = gyrosim.cpp + +MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os From 2eaa2f06e7717f3b19e808f917c5306a19fa16c7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 8 Apr 2015 22:30:58 -0700 Subject: [PATCH 096/258] Linux: fised printf param to work on 32 and 64 bit targets Use %zd instead of %d or %ld for sizeof(x). Signed-off-by: Mark Charlebois --- src/platforms/linux/drivers/gyrosim/gyrosim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp index 1091b7e930..378c026f2a 100644 --- a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp @@ -1850,7 +1850,7 @@ test() sz = read(fd, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { - warnx("ret: %zd, expected: %ld", sz, sizeof(a_report)); + warnx("ret: %zd, expected: %zd", sz, sizeof(a_report)); err(1, "immediate acc read failed"); } @@ -1869,7 +1869,7 @@ test() sz = read(fd_gyro, &g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { - warnx("ret: %zd, expected: %ld", sz, sizeof(g_report)); + warnx("ret: %zd, expected: %zd", sz, sizeof(g_report)); err(1, "immediate gyro read failed"); } From ddf75dd55ae17d70a65bfb58a3b1fe70fff09783 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 9 Apr 2015 10:24:41 -0700 Subject: [PATCH 097/258] Linux: added ADC simulator The sensor module is now able to run after the simulation modules are started. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 1 + src/platforms/linux/drivers/adcsim/adcsim.cpp | 303 ++++++++++++++++++ src/platforms/linux/drivers/adcsim/module.mk | 42 +++ src/platforms/px4_adc.h | 6 +- 4 files changed, 350 insertions(+), 2 deletions(-) create mode 100644 src/platforms/linux/drivers/adcsim/adcsim.cpp create mode 100644 src/platforms/linux/drivers/adcsim/module.mk diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index b7bfd9392f..4cd7ac4e27 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -60,6 +60,7 @@ MODULES += lib/conversion MODULES += platforms/linux/px4_layer MODULES += platforms/linux/drivers/accelsim MODULES += platforms/linux/drivers/gyrosim +MODULES += platforms/linux/drivers/adcsim # # Unit tests diff --git a/src/platforms/linux/drivers/adcsim/adcsim.cpp b/src/platforms/linux/drivers/adcsim/adcsim.cpp new file mode 100644 index 0000000000..f676d03d9e --- /dev/null +++ b/src/platforms/linux/drivers/adcsim/adcsim.cpp @@ -0,0 +1,303 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file adc.cpp + * + * Driver for the STM32 ADCSIM. + * + * This is a low-rate driver, designed for sampling things like voltages + * and so forth. It avoids the gross complexity of the NuttX ADCSIM driver. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + + +class ADCSIM : public device::VDev +{ +public: + ADCSIM(uint32_t channels); + ~ADCSIM(); + + virtual int init(); + + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t len); + +protected: + virtual int open_first(device::px4_dev_handle_t *handlep); + virtual int close_last(device::px4_dev_handle_t *handlep); + +private: + static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ + + hrt_call _call; + perf_counter_t _sample_perf; + + unsigned _channel_count; + adc_msg_s *_samples; /**< sample buffer */ + + orb_advert_t _to_system_power; + + /** work trampoline */ + static void _tick_trampoline(void *arg); + + /** worker function */ + void _tick(); + + /** + * Sample a single channel and return the measured value. + * + * @param channel The channel to sample. + * @return The sampled value, or 0xffff if + * sampling failed. + */ + uint16_t _sample(unsigned channel); + + // update system_power ORB topic, only on FMUv2 + void update_system_power(void); +}; + +ADCSIM::ADCSIM(uint32_t channels) : + VDev("adc", ADCSIM0_DEVICE_PATH), + _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), + _channel_count(0), + _samples(nullptr), + _to_system_power(0) +{ + _debug_enabled = true; + + /* always enable the temperature sensor */ + channels |= 1 << 16; + + /* allocate the sample array */ + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _channel_count++; + } + } + _samples = new adc_msg_s[_channel_count]; + + /* prefill the channel numbers in the sample array */ + if (_samples != nullptr) { + unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { + if (channels & (1 << i)) { + _samples[index].am_channel = i; + _samples[index].am_data = 0; + index++; + } + } + } +} + +ADCSIM::~ADCSIM() +{ + if (_samples != nullptr) + delete _samples; +} + +int +ADCSIM::init() +{ + debug("init done"); + + /* create the device node */ + return VDev::init(); +} + +int +ADCSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t +ADCSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t len) +{ + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) + len = maxsize; + + /* block interrupts while copying samples to avoid racing with an update */ + memcpy(buffer, _samples, len); + + return len; +} + +int +ADCSIM::open_first(device::px4_dev_handle_t *handlep) +{ + /* get fresh data */ + _tick(); + + /* and schedule regular updates */ + hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + + return 0; +} + +int +ADCSIM::close_last(device::px4_dev_handle_t *handlep) +{ + hrt_cancel(&_call); + return 0; +} + +void +ADCSIM::_tick_trampoline(void *arg) +{ + (reinterpret_cast(arg))->_tick(); +} + +void +ADCSIM::_tick() +{ + /* scan the channel set and sample each */ + for (unsigned i = 0; i < _channel_count; i++) + _samples[i].am_data = _sample(_samples[i].am_channel); + update_system_power(); +} + +void +ADCSIM::update_system_power(void) +{ +} + +uint16_t +ADCSIM::_sample(unsigned channel) +{ + perf_begin(_sample_perf); + + uint16_t result = 1; + + perf_end(_sample_perf); + return result; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int adc_main(int argc, char *argv[]); + +namespace +{ +ADCSIM *g_adc; + +int +test(void) +{ + + int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + if (fd < 0) { + warnx("can't open ADCSIM device"); + return 1; + } + + for (unsigned i = 0; i < 50; i++) { + adc_msg_s data[12]; + ssize_t count = read(fd, data, sizeof(data)); + + if (count < 0) { + warnx("read error"); + return 1; + } + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + printf ("%d: %u ", data[j].am_channel, data[j].am_data); + } + + printf("\n"); + usleep(500000); + } + + px4_close(fd); + return 0; +} +} + +int +adc_main(int argc, char *argv[]) +{ + int ret = 0; + + if (g_adc == nullptr) { +#ifdef CONFIG_ARCH_BOARD_LINUXTEST + /* XXX this hardcodes the default channel set for LINUXTEST - should be configurable */ + g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif + + if (g_adc == nullptr) { + warnx("couldn't allocate the ADCSIM driver"); + return 1; + } + + if (g_adc->init() != OK) { + delete g_adc; + warnx("ADCSIM init failed"); + return 1; + } + } + + if (argc > 1) { + if (!strcmp(argv[1], "test")) + ret = test(); + } + + return ret; +} diff --git a/src/platforms/linux/drivers/adcsim/module.mk b/src/platforms/linux/drivers/adcsim/module.mk new file mode 100644 index 0000000000..9b8461fd77 --- /dev/null +++ b/src/platforms/linux/drivers/adcsim/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# STM32 ADC driver +# + +MODULE_COMMAND = adc + +SRCS = adcsim.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/px4_adc.h b/src/platforms/px4_adc.h index 0b123ed0c0..bda39d0369 100644 --- a/src/platforms/px4_adc.h +++ b/src/platforms/px4_adc.h @@ -39,6 +39,8 @@ #pragma once +#include + #if defined(__PX4_ROS) #error "ADC not supported in ROS" #elif defined(__PX4_NUTTX) @@ -54,13 +56,13 @@ struct adc_msg_s { uint8_t am_channel; /* The 8-bit ADC Channel */ int32_t am_data; /* ADC convert result (4 bytes) */ -} packed_struct; +} __attribute__ ((packed)); // Example settings #define ADC_BATTERY_VOLTAGE_CHANNEL 10 #define ADC_BATTERY_CURRENT_CHANNEL -1 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 - +#define ADCSIM0_DEVICE_PATH "/dev/adc0" #else #error "No target platform defined" #endif From 87a8289a222a2e96776a53b800d5a74f26c80827 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 10 Apr 2015 11:23:17 -0700 Subject: [PATCH 098/258] Linux: changed adc to adcsim and add barosim The name of the app was adc but should have been adcsim. Added a barometer simulator. This will allow ms56711_linux to depend on real devices and not simulation. Signed-off-by: Mark Charlebois --- Tools/linux_run.sh | 2 +- linux-configs/linuxtest/init/rc.S | 8 +- makefiles/linux/config_linux_default.mk | 3 +- .../linux/drivers/accelsim/accelsim.cpp | 2 +- src/platforms/linux/drivers/adcsim/adcsim.cpp | 8 +- src/platforms/linux/drivers/adcsim/module.mk | 2 +- src/platforms/linux/drivers/barosim/baro.cpp | 1186 +++++++++++++++++ .../linux/drivers/barosim/baro_sim.cpp | 205 +++ src/platforms/linux/drivers/barosim/barosim.h | 84 ++ src/platforms/linux/drivers/barosim/module.mk | 42 + 10 files changed, 1532 insertions(+), 10 deletions(-) create mode 100644 src/platforms/linux/drivers/barosim/baro.cpp create mode 100644 src/platforms/linux/drivers/barosim/baro_sim.cpp create mode 100644 src/platforms/linux/drivers/barosim/barosim.h create mode 100644 src/platforms/linux/drivers/barosim/module.mk diff --git a/Tools/linux_run.sh b/Tools/linux_run.sh index 5207aebdfb..ebe7a77745 100755 --- a/Tools/linux_run.sh +++ b/Tools/linux_run.sh @@ -23,4 +23,4 @@ if [ ! -d /eeprom ] && [ ! -w /eeprom ] exit 1 fi -Build/linux_default.build/mainapp linux-config/linuxtest/init/rc.S +Build/linux_default.build/mainapp linux-configs/linuxtest/init/rc.S diff --git a/linux-configs/linuxtest/init/rc.S b/linux-configs/linuxtest/init/rc.S index 72d31ac58e..dc8d3c3a0f 100644 --- a/linux-configs/linuxtest/init/rc.S +++ b/linux-configs/linuxtest/init/rc.S @@ -1,4 +1,8 @@ uorb start +barosim start +adcsim start +accelsim start +gyrosim start mavlink start -blink start -blink systemstate +sensors start +list_devices diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index 4cd7ac4e27..c11815f110 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -13,7 +13,7 @@ MODULES += drivers/device MODULES += drivers/blinkm MODULES += modules/sensors -MODULES += drivers/ms5611 +#MODULES += drivers/ms5611 # # System commands @@ -61,6 +61,7 @@ MODULES += platforms/linux/px4_layer MODULES += platforms/linux/drivers/accelsim MODULES += platforms/linux/drivers/gyrosim MODULES += platforms/linux/drivers/adcsim +MODULES += platforms/linux/drivers/barosim # # Unit tests diff --git a/src/platforms/linux/drivers/accelsim/accelsim.cpp b/src/platforms/linux/drivers/accelsim/accelsim.cpp index 1225a5f60d..f2d30a9e06 100644 --- a/src/platforms/linux/drivers/accelsim/accelsim.cpp +++ b/src/platforms/linux/drivers/accelsim/accelsim.cpp @@ -390,7 +390,7 @@ ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; diff --git a/src/platforms/linux/drivers/adcsim/adcsim.cpp b/src/platforms/linux/drivers/adcsim/adcsim.cpp index f676d03d9e..83419cdad2 100644 --- a/src/platforms/linux/drivers/adcsim/adcsim.cpp +++ b/src/platforms/linux/drivers/adcsim/adcsim.cpp @@ -111,13 +111,13 @@ private: }; ADCSIM::ADCSIM(uint32_t channels) : - VDev("adc", ADCSIM0_DEVICE_PATH), + VDev("adcsim", ADCSIM0_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr), _to_system_power(0) { - _debug_enabled = true; + //_debug_enabled = true; /* always enable the temperature sensor */ channels |= 1 << 16; @@ -231,7 +231,7 @@ ADCSIM::_sample(unsigned channel) /* * Driver 'main' command. */ -extern "C" __EXPORT int adc_main(int argc, char *argv[]); +extern "C" __EXPORT int adcsim_main(int argc, char *argv[]); namespace { @@ -272,7 +272,7 @@ test(void) } int -adc_main(int argc, char *argv[]) +adcsim_main(int argc, char *argv[]) { int ret = 0; diff --git a/src/platforms/linux/drivers/adcsim/module.mk b/src/platforms/linux/drivers/adcsim/module.mk index 9b8461fd77..a8ad55effc 100644 --- a/src/platforms/linux/drivers/adcsim/module.mk +++ b/src/platforms/linux/drivers/adcsim/module.mk @@ -35,7 +35,7 @@ # STM32 ADC driver # -MODULE_COMMAND = adc +MODULE_COMMAND = adcsim SRCS = adcsim.cpp diff --git a/src/platforms/linux/drivers/barosim/baro.cpp b/src/platforms/linux/drivers/barosim/baro.cpp new file mode 100644 index 0000000000..05040c0909 --- /dev/null +++ b/src/platforms/linux/drivers/barosim/baro.cpp @@ -0,0 +1,1186 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file baro.cpp + * Driver for the simulated barometric pressure sensor + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "barosim.h" + +enum BAROSIM_BUS { + BAROSIM_BUS_SIM_EXTERNAL +}; + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) + +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +/* + * BAROSIM internal constants and data structures. + */ + +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */ +#define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ + +class BAROSIM : public device::VDev +{ +public: + BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path); + ~BAROSIM(); + + virtual int init(); + + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + Device *_interface; + + barosim::prom_s _prom; + + struct work_s _work; + unsigned _measure_ticks; + + RingBuffer *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per BAROSIM datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + float _P; + float _T; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in Pa */ + + orb_advert_t _baro_topic; + int _orb_class_instance; + int _class_instance; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Initialize the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + virtual int measure(); + + /** + * Collect the result of the most recent measurement. + */ + virtual int collect(); +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int barosim_main(int argc, char *argv[]); + +BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path) : + VDev("BAROSIM", path), + _interface(interface), + _prom(prom_buf.s), + _measure_ticks(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _baro_topic(-1), + _orb_class_instance(-1), + _class_instance(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "barosim_measure")), + _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "barosim_buffer_overflows")) +{ + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +BAROSIM::~BAROSIM() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + if (_class_instance != -1) + unregister_class_devname(get_devname(), _class_instance); + + /* free any existing reports */ + if (_reports != nullptr) + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + + delete _interface; +} + +int +BAROSIM::init() +{ + int ret; + warnx("BAROSIM::init"); + + ret = VDev::init(); + if (ret != OK) { + debug("VDev init failed"); + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(baro_report)); + + if (_reports == nullptr) { + debug("can't get memory for reports"); + ret = -ENOMEM; + goto out; + } + + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); + + struct baro_report brp; + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + _reports->flush(); + + /* this do..while is goto without goto */ + do { + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + warnx("temp measure failed"); + break; + } + + usleep(BAROSIM_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + warnx("temp collect failed"); + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + warnx("pressure collect failed"); + break; + } + + usleep(BAROSIM_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + warnx("pressure collect failed"); + break; + } + + /* state machine will have generated a report, copy it out */ + _reports->get(&brp); + + ret = OK; + + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + + if (_baro_topic == (orb_advert_t)(-1)) { + warnx("failed to create sensor_baro publication"); + } + //warnx("sensor_baro publication %ld", _baro_topic); + + } while (0); + +out: + return ret; +} + +ssize_t +BAROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(brp)) { + ret += sizeof(*brp); + brp++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _measure_phase = 0; + _reports->flush(); + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(BAROSIM_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(BAROSIM_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(brp)) + ret = sizeof(*brp); + + } while (0); + + return ret; +} + +int +BAROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if ((long)ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + if (!_reports->resize(arg)) { + return -ENOMEM; + } + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* + * Since we are initialized, we do not need to do anything, since the + * PROM is correctly read and the part does not need to be configured. + */ + return OK; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return VDev::ioctl(handlep, cmd, arg); +} + +void +BAROSIM::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&BAROSIM::cycle_trampoline, this, 1); +} + +void +BAROSIM::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +BAROSIM::cycle_trampoline(void *arg) +{ + BAROSIM *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void +BAROSIM::cycle() +{ + int ret; + unsigned dummy; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + /* issue a reset command to the sensor */ + _interface->dev_ioctl(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + ((long)_measure_ticks > USEC2TICK(BAROSIM_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&BAROSIM::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* issue a reset command to the sensor */ + _interface->dev_ioctl(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&BAROSIM::cycle_trampoline, + this, + USEC2TICK(BAROSIM_CONVERSION_INTERVAL)); +} + +int +BAROSIM::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + */ + ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); + if (OK != ret) + perf_count(_comms_errors); + + perf_end(_measure_perf); + + return ret; +} + +int +BAROSIM::collect() +{ + int ret; + uint32_t raw; + + perf_begin(_sample_perf); + + struct baro_report report; + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + ret = _interface->dev_read(0, (void *)&raw, 0); + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); + + /* temperature compensation */ + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; + + /* generate a new report */ + report.temperature = _TEMP / 100.0f; + report.pressure = P / 100.0f; /* convert to millibar */ + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: barosim_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: barosim_read: 963 events, 208066us elapsed, min 202us max 241us + */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* publish it */ + if (!(_pub_blocked)) { + if (_baro_topic != (orb_advert_t)(-1)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } + else { + printf("BAROSIM::collect _baro_topic not initialized\n"); + } + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, BAROSIM_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +void +BAROSIM::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", (long long)_SENS); + printf("OFF: %lld\n", (long long)_OFF); + printf("P: %.3f\n", (double)_P); + printf("T: %.3f\n", (double)_T); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + + printf("factory_setup %u\n", _prom.factory_setup); + printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.serial_and_crc); +} + +/** + * Local functions in support of the shell command. + */ +namespace barosim +{ + +/* + list of supported bus configurations + */ +struct barosim_bus_option { + enum BAROSIM_BUS busid; + const char *devpath; + BAROSIM_constructor interface_constructor; + uint8_t busnum; + BAROSIM *dev; +} bus_options[] = { + { BAROSIM_BUS_SIM_EXTERNAL, "/vdev/baro_sim", &BAROSIM_sim_interface, PX4_SIM_BUS_TEST, NULL }, +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +bool start_bus(struct barosim_bus_option &bus); +int start(); +int test(); +int reset(); +int info(); +int calibrate(unsigned altitude); +void usage(); + +/** + * BAROSIM crc4 cribbed from the datasheet + */ +bool +crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + + +/** + * Start the driver. + */ +bool +start_bus(struct barosim_bus_option &bus) +{ + if (bus.dev != nullptr) { + warnx("bus option already started"); + return false; + } + + prom_u prom_buf; + device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new BAROSIM(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + warnx("bus init failed %p", bus.dev); + return false; + } + + int fd = px4_open(bus.devpath, O_RDONLY); + + /* set the poll rate to default, starts automatic data collection */ + if (fd == -1) { + warnx("can't open baro device"); + return false; + } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + px4_close(fd); + warnx("failed setting default poll rate"); + return false; + } + + px4_close(fd); + return true; +} + + +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +int +start() +{ + bool started = false; + + started |= start_bus(bus_options[0]); + + if (!started) { + warnx("driver start failed"); + return 1; + } + + // driver started OK + return 0; +} + + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + struct barosim_bus_option &bus = bus_options[0]; + struct baro_report report; + ssize_t sz; + int ret; + + int fd; + + fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { + warn("open failed (try 'barosim start' if the driver is not running)"); + return 1; + } + + /* do a simple demand read */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("immediate read failed"); + return 1; + } + + warnx("single read"); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature: %8.4f", (double)report.temperature); + warnx("time: %lld", (long long)report.timestamp); + + /* set the queue depth to 10 */ + if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + warnx("failed to set queue depth"); + return 1; + } + + /* start the sensor polling at 2Hz */ + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + warnx("failed to set 2Hz poll rate"); + return 1; + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + warnx("timed out waiting for sensor data"); + } + + /* now go get it */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + return 1; + } + + warnx("periodic read %u", i); + warnx("pressure: %10.4f", (double)report.pressure); + warnx("altitude: %11.4f", (double)report.altitude); + warnx("temperature: %8.4f", (double)report.temperature); + warnx("time: %lld", (long long)report.timestamp); + } + + close(fd); + warnx("PASS"); + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + struct barosim_bus_option &bus = bus_options[0]; + int fd; + + fd = open(bus.devpath, O_RDONLY); + if (fd < 0) { + warn("failed "); + return 1; + } + + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + warn("driver reset failed"); + return 1; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warn("driver poll restart failed"); + return 1; + } + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + for (uint8_t i=0; iprint_info(); + } + } + return 0; +} + +/** + * Calculate actual MSL pressure given current altitude + */ +int +calibrate(unsigned altitude) +{ + struct barosim_bus_option &bus = bus_options[0]; + struct baro_report report; + float pressure; + float p1; + + int fd; + + fd = px4_open(bus.devpath, O_RDONLY); + + if (fd < 0) { + warn("open failed (try 'barosim start' if the driver is not running)"); + return 1; + } + + /* start the sensor polling at max */ + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { + warnx("failed to set poll rate"); + return 1; + } + + /* average a few measurements */ + pressure = 0.0f; + + for (unsigned i = 0; i < 20; i++) { + struct pollfd fds; + int ret; + ssize_t sz; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 1000); + + if (ret != 1) { + warnx("timed out waiting for sensor data"); + return 1; + } + + /* now go get it */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("sensor read failed"); + return 1; + } + + pressure += report.pressure; + } + + pressure /= 20; /* average */ + pressure /= 10; /* scale from millibar to kPa */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); + + p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); + + warnx("calculated MSL pressure %10.4fkPa", (double)p1); + + /* save as integer Pa */ + p1 *= 1000.0f; + + if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { + warn("BAROIOCSMSLPRESSURE"); + return 1; + } + + close(fd); + return 0; +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate '"); +} + +} // namespace + +int +barosim_main(int argc, char *argv[]) +{ + int ret; + + if (argc < 2) { + barosim::usage(); + return 1; + } + + const char *verb = argv[1]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) + ret = barosim::start(); + + /* + * Test the driver/device. + */ + else if (!strcmp(verb, "test")) + ret = barosim::test(); + + /* + * Reset the driver. + */ + else if (!strcmp(verb, "reset")) + ret = barosim::reset(); + + /* + * Print driver information. + */ + else if (!strcmp(verb, "info")) + ret = barosim::info(); + + /* + * Perform MSL pressure calibration given an altitude in metres + */ + else if (!strcmp(verb, "calibrate")) { + if (argc < 3) { + warnx("missing altitude"); + barosim::usage(); + return 1; + } + + long altitude = strtol(argv[2], nullptr, 10); + + ret = barosim::calibrate(altitude); + } + else { + barosim::usage(); + return 1; + } + return ret; +} diff --git a/src/platforms/linux/drivers/barosim/baro_sim.cpp b/src/platforms/linux/drivers/barosim/baro_sim.cpp new file mode 100644 index 0000000000..6e4e1ca0b0 --- /dev/null +++ b/src/platforms/linux/drivers/barosim/baro_sim.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + + /** + * @file baro_sim.cpp + * + * Simulation interface for barometer + */ + +/* XXX trim includes */ +#include +#include + +#include +#include +#include +#include +//#include +#include +#include + +#include +#include "barosim.h" +#include "board_config.h" + +device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf); + +class BARO_SIM : public device::SIM +{ +public: + BARO_SIM(uint8_t bus, barosim::prom_u &prom_buf); + virtual ~BARO_SIM(); + + virtual int init(); + virtual int dev_read(unsigned offset, void *data, unsigned count); + virtual int dev_ioctl(unsigned operation, unsigned &arg); + + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); +private: + barosim::prom_u &_prom; + + /** + * Send a reset command to the barometer simulator. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the barometer simulator. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return PX4_OK if the PROM reads successfully. + */ + int _read_prom(); + +}; + +device::Device * +BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) +{ + return new BARO_SIM(busnum, prom_buf); +} + +BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : + SIM("BARO_SIM", "/vdev/BARO_SIM", bus, 0), + _prom(prom) +{ +} + +BARO_SIM::~BARO_SIM() +{ +} + +int +BARO_SIM::init() +{ + return SIM::init(); +} + +int +BARO_SIM::dev_read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == PX4_OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +BARO_SIM::dev_ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + return ret; +} + +int +BARO_SIM::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +BARO_SIM::_measure(unsigned addr) +{ + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + +int +BARO_SIM::_read_prom() +{ + int ret = 1; + // TODO input simlation data + return ret; +} + +int +BARO_SIM::transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len) +{ + // TODO add Simulation data connection so calls retrieve + // data from the simulator + return 0; +} diff --git a/src/platforms/linux/drivers/barosim/barosim.h b/src/platforms/linux/drivers/barosim/barosim.h new file mode 100644 index 0000000000..8eaa0c21f5 --- /dev/null +++ b/src/platforms/linux/drivers/barosim/barosim.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file barosim.h + * + * Shared defines for the ms5611 driver. + */ + +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + +/* interface ioctls */ +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + +namespace barosim +{ + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct prom_s { + uint16_t factory_setup; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t serial_and_crc; +}; + +/** + * Grody hack for crc4() + */ +union prom_u { + uint16_t c[8]; + prom_s s; +}; +#pragma pack(pop) + +extern bool crc4(uint16_t *n_prom); + +} /* namespace */ + +/* interface factories */ +extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +typedef device::Device *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); diff --git a/src/platforms/linux/drivers/barosim/module.mk b/src/platforms/linux/drivers/barosim/module.mk new file mode 100644 index 0000000000..5a4cb4ebb9 --- /dev/null +++ b/src/platforms/linux/drivers/barosim/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# MS5611 driver +# + +MODULE_COMMAND = barosim + +SRCS = baro.cpp baro_sim.cpp + +MAXOPTIMIZATION = -Os From edf8677c3703dcf599fed04c4b8c1a998582c3a1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 10 Apr 2015 22:12:31 -0700 Subject: [PATCH 099/258] Linux: use string to store task name Converted px4_linux_tasks to C++ so the task struct can use a string. Sometimes the name string was in the stack of the calling function and goes out of scope. Signed-off-by: Mark Charlebois --- src/platforms/linux/px4_layer/module.mk | 2 +- ...{px4_linux_tasks.c => px4_linux_tasks.cpp} | 22 +++++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) rename src/platforms/linux/px4_layer/{px4_linux_tasks.c => px4_linux_tasks.cpp} (93%) diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk index aeb707b5c3..de6678eb92 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/linux/px4_layer/module.mk @@ -37,7 +37,7 @@ SRCS = \ px4_linux_impl.cpp \ - px4_linux_tasks.c \ + px4_linux_tasks.cpp \ work_thread.c \ work_queue.c \ work_cancel.c \ diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.cpp similarity index 93% rename from src/platforms/linux/px4_layer/px4_linux_tasks.c rename to src/platforms/linux/px4_layer/px4_linux_tasks.cpp index 3c26acdcb5..a210f430ea 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.cpp @@ -50,20 +50,22 @@ #include #include +#include #include #define MAX_CMD_LEN 100 #define PX4_MAX_TASKS 100 -typedef struct +struct task_entry { pthread_t pid; - const char *name; + std::string name; bool isused; -} task_entry; + task_entry() : isused(false) {} +}; -static task_entry taskmap[PX4_MAX_TASKS] = {}; +static task_entry taskmap[PX4_MAX_TASKS]; typedef struct { @@ -73,7 +75,7 @@ typedef struct // strings are allocated after the } pthdata_t; -static void entry_adapter ( void *ptr ) +static void *entry_adapter ( void *ptr ) { pthdata_t *data; data = (pthdata_t *) ptr; @@ -83,6 +85,8 @@ static void entry_adapter ( void *ptr ) printf("Before px4_task_exit\n"); px4_task_exit(0); printf("After px4_task_exit\n"); + + return NULL; } void @@ -156,12 +160,12 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, (void *)&entry_adapter, (void *) taskdata); + rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); if (rv != 0) { if (rv == EPERM) { //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, (void *)&entry_adapter, (void *) taskdata); + rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); if (rv != 0) { printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); return (rv < 0) ? rv : -rv; @@ -225,7 +229,7 @@ void px4_task_exit(int ret) if (i>=PX4_MAX_TASKS) printf("px4_task_exit: self task not found!\n"); else - printf("px4_task_exit: %s\n", taskmap[i].name); + printf("px4_task_exit: %s\n", taskmap[i].name.c_str()); pthread_exit((void *)(unsigned long)ret); } @@ -267,7 +271,7 @@ void px4_show_tasks() for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %lu\n", taskmap[idx].name, taskmap[idx].pid); + printf(" %-10s %lu\n", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } From 639afeb28f92bb289cd449aaa2caf7bd87fc18e7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 10 Apr 2015 22:16:30 -0700 Subject: [PATCH 100/258] Fixed issue with argc check Was checking if argc < 1 and then accessing argv[1]. Fixed by checking if argc < 2. Signed-off-by: Mark Charlebois --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index affe6a68a4..3a09b44bb1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -128,7 +128,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (thread_running) { warnx("already running\n"); /* this is not an error */ - exit(0); + return 0; } thread_should_exit = false; From a1501fa368ca37852e502c9d8b7333b2ec65c7c8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 13 Apr 2015 13:14:37 -0700 Subject: [PATCH 101/258] Linux: Default to use clang Fixed to use clang 3.4.2 on Ubuntu 12.04 Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 151223b66c..3a65682a5e 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -42,7 +42,7 @@ # # Set to 1 for GCC-4.8.2 and to 0 for Clang-3.5 (Ubuntu 14.04) -USE_GCC?=1 +USE_GCC?=0 ifneq ($(USE_GCC),1) @@ -51,13 +51,13 @@ HAVE_CLANG35=$(shell clang-3.5 -dumpversion) # Clang will report 4.2.1 as GCC version HAVE_CLANG=$(shell clang -dumpversion) -#If using ubuntu 14.04 and packaged clang 4.2.1 +#If using ubuntu 14.04 and packaged clang 3.5 ifeq ($(HAVE_CLANG35),4.2.1) USE_GCC=0 CLANGVER=-3.5 else -#If using ubuntu 12.04 and downloaded clang 3.4.1 +#If using ubuntu 12.04 and downloaded clang 3.4.2 ifeq ($(HAVE_CLANG),4.2.1) USE_GCC=0 CLANGVER= @@ -66,7 +66,7 @@ endif # If no version of clang was found ifeq ($(HAVE_CLANG35),) -ifeq ($(HAVE_CLANG35),) +ifeq ($(HAVE_CLANG),) $(error Clang not found. Try make USE_GCC=1) endif endif From f00dc44475a1bdbca108aa3c76271393da0a6985 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 14 Apr 2015 14:38:28 -0700 Subject: [PATCH 102/258] Linux: fixed px4_task_t to be int px4_task_t is negative for failure conditions. It was set mistakenly to pthread_t (which is unsigned) for LInux. Signed-off-by: Mark Charlebois --- src/platforms/px4_tasks.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 791097c873..28289f4e55 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -58,7 +58,7 @@ typedef int px4_task_t; #define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) #define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) -typedef pthread_t px4_task_t; +typedef int px4_task_t; typedef struct { int argc; From ac1679dbc3d8fae80b9eaeae2fb2a28c6d3cc675 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 14 Apr 2015 14:44:38 -0700 Subject: [PATCH 103/258] Added simulator Simulator listens for UDP input data at port 9876. Data is for now comma separated. Not yet connected to the various sim classes: accelsim, gyrosim, magsim. Barometer measurements not yet supported. Signed-off-by: Mark Charlebois --- src/modules/simulator/module.mk | 40 ++++++ src/modules/simulator/simulator.cpp | 189 ++++++++++++++++++++++++++++ src/modules/simulator/simulator.h | 86 +++++++++++++ 3 files changed, 315 insertions(+) create mode 100644 src/modules/simulator/module.mk create mode 100644 src/modules/simulator/simulator.cpp create mode 100644 src/modules/simulator/simulator.h diff --git a/src/modules/simulator/module.mk b/src/modules/simulator/module.mk new file mode 100644 index 0000000000..483204d5e4 --- /dev/null +++ b/src/modules/simulator/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2015 MArk Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Makefile to build simulator +# + +MODULE_COMMAND = simulator + +SRCS = simulator.cpp diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp new file mode 100644 index 0000000000..c4c6ffbbd6 --- /dev/null +++ b/src/modules/simulator/simulator.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file simulator.cpp + * A device simulator + */ + +#include +#include +#include +#include +#include +#include +#include "simulator.h" + +Simulator *Simulator::_instance = NULL; + +Simulator::Simulator() : _max_readers(3) +{ + sem_init(&_lock, 0, _max_readers); +} + +Simulator *Simulator::getInstance() +{ + return _instance; +} + +int Simulator::getSample(sim_dev_t dev, sample &val) +{ + int ret; + + switch (dev) { + case SIM_GYRO: + read_lock(); + val = _gyro[_readidx]; + read_unlock(); + ret = 0; + break; + case SIM_ACCEL: + read_lock(); + val = _accel[_readidx]; + read_unlock(); + ret = 0; + break; + case SIM_MAG: + read_lock(); + val = _mag[_readidx]; + read_unlock(); + ret = 0; + break; + default: + ret = 1; + } + return ret; +} + +int Simulator::start(int argc, char *argv[]) +{ + int ret = 0; + _instance = new Simulator(); + if (_instance) + _instance->updateSamples(); + else { + printf("Simulator creation failed\n"); + ret = 1; + } + return ret; +} + + +void Simulator::updateSamples() +{ + // get samples from external provider + struct sockaddr_in myaddr; + struct sockaddr_in srcaddr; + socklen_t addrlen = sizeof(srcaddr); + int len, fd; + const int buflen = 200; + const int port = 9876; + unsigned char buf[buflen]; + int writeidx, num; + + if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + printf("create socket failed\n"); + return; + } + + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(port); + + if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + printf("bind failed\n"); + return; + } + + for (;;) { + len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); + if (len > 0) { + writeidx = !_readidx; + buf[len] = 0; + printf("received: %s\n", buf); + // FIXME - temp hack to read data, not safe - bad bad bad + num = sscanf((const char *)buf, "%f,%f,%f,%f,%f,%f,%f,%f,%f", + &_gyro[writeidx].x, &_gyro[writeidx].y, &_gyro[writeidx].z, + &_accel[writeidx].x, &_accel[writeidx].y, &_accel[writeidx].z, + &_mag[writeidx].x, &_mag[writeidx].y, &_mag[writeidx].z); + if (num != 9) { + printf("Only read %d items\n", num); + } + else { + // Swap read and write buffers + write_lock(); + _readidx = !_readidx; + write_unlock(); + } + } + } +} + +void Simulator::read_lock() +{ + sem_wait(&_lock); +} +void Simulator::read_unlock() +{ + sem_post(&_lock); +} +void Simulator::write_lock() +{ + for (int i=0; i<_max_readers; i++) { + sem_wait(&_lock); + } +} +void Simulator::write_unlock() +{ + for (int i=0; i<_max_readers; i++) { + sem_post(&_lock); + } +} + +extern "C" { + +int simulator_main(int argc, char *argv[]) +{ + return (int)(px4_task_spawn_cmd("Simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + Simulator::start, + nullptr) < 0); + + return 0; +} + +} + diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h new file mode 100644 index 0000000000..4c20768caf --- /dev/null +++ b/src/modules/simulator/simulator.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file simulator.h + * A device simulator + */ + +#pragma once + +#include + +class Simulator { +public: + static Simulator *getInstance(); + + enum sim_dev_t { + SIM_GYRO, + SIM_ACCEL, + SIM_MAG + }; + + struct sample { + float x; + float y; + float z; + sample() : x(0), y(0), z(0) {} + sample(float a, float b, float c) : x(a), y(b), z(c) {} + }; + + static int start(int argc, char *argv[]); + + int getSample(sim_dev_t dev, sample &val); +private: + Simulator(); + ~Simulator() { _instance=NULL; } + + void updateSamples(); + + void read_lock(); + void read_unlock(); + void write_lock(); + void write_unlock(); + + int _readidx; + + sample _accel[2]; + sample _gyro[2]; + sample _mag[2]; + + static Simulator *_instance; + sem_t _lock; + + const int _max_readers; +}; + From facc2faf044369114a6d4b39430487f967c0be8b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 09:13:04 -0700 Subject: [PATCH 104/258] Linux: added hil support The HIL driver now runs in the Linux build Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 2 + src/drivers/drv_gpio.h | 7 +- src/drivers/drv_mixer.h | 3 +- src/drivers/drv_pwm_output.h | 67 +- src/drivers/hil/hil_linux.cpp | 860 ++++++++++++++++++++++++ src/drivers/hil/module.mk | 6 +- 6 files changed, 909 insertions(+), 36 deletions(-) create mode 100644 src/drivers/hil/hil_linux.cpp diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index c11815f110..fd3c691b09 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -12,6 +12,7 @@ # MODULES += drivers/device MODULES += drivers/blinkm +MODULES += drivers/hil MODULES += modules/sensors #MODULES += drivers/ms5611 @@ -44,6 +45,7 @@ MODULES += modules/systemlib/mixer MODULES += modules/uORB MODULES += modules/dataman MODULES += modules/sdlog2 +MODULES += modules/simulator # # Libraries diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index f18c8162d7..e11a035c20 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -114,9 +114,14 @@ /* no GPIO driver on the PX4_STM32F4DISCOVERY board */ #endif +#ifdef CONFIG_ARCH_BOARD_LINUXTEST +/* no GPIO driver on the LINUXTEST board */ +#endif + #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ - !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) + !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) \ + !defined(CONFIG_ARCH_BOARD_LINUXTEST) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/drv_mixer.h b/src/drivers/drv_mixer.h index a6cec3edc2..8886a6c1a0 100644 --- a/src/drivers/drv_mixer.h +++ b/src/drivers/drv_mixer.h @@ -53,6 +53,7 @@ #ifndef _DRV_MIXER_H #define _DRV_MIXER_H +#include #include #include @@ -62,7 +63,7 @@ * ioctl() definitions */ #define _MIXERIOCBASE (0x2500) -#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n)) +#define _MIXERIOC(_n) (_PX4_IOC(_MIXERIOCBASE, _n)) /** get the number of mixable outputs */ #define MIXERIOCGETOUTPUTCOUNT _MIXERIOC(0) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6271ad2086..8c7efaf224 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -43,6 +43,7 @@ #pragma once +#include #include #include @@ -149,102 +150,102 @@ ORB_DECLARE(output_pwm); #define _PWM_SERVO_BASE 0x2a00 /** arm all servo outputs handle by this driver */ -#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) +#define PWM_SERVO_ARM _PX4_IOC(_PWM_SERVO_BASE, 0) /** disarm all servo outputs (stop generating pulses) */ -#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) +#define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1) /** get default servo update rate */ #define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) /** set alternate servo update rate */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3) +#define PWM_SERVO_SET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 3) /** get alternate servo update rate */ -#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) +#define PWM_SERVO_GET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 4) /** get the number of servos in *(unsigned *)arg */ -#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 5) +#define PWM_SERVO_GET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 5) /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ -#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6) +#define PWM_SERVO_SET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 6) /** check the selected update rates */ -#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7) +#define PWM_SERVO_GET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 7) /** set the 'ARM ok' bit, which activates the safety switch */ -#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8) +#define PWM_SERVO_SET_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 8) /** clear the 'ARM ok' bit, which deactivates the safety switch */ -#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9) +#define PWM_SERVO_CLEAR_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 9) /** start DSM bind */ -#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10) +#define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10) #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ #define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) +#define DSM_BIND_POWER_UP _PX4_IOC(_PWM_SERVO_BASE, 11) /** set the PWM value for failsafe */ -#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12) +#define PWM_SERVO_SET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 12) /** get the PWM value for failsafe */ -#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13) +#define PWM_SERVO_GET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 13) /** set the PWM value when disarmed - should be no PWM (zero) by default */ -#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14) +#define PWM_SERVO_SET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 14) /** get the PWM value when disarmed */ -#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15) +#define PWM_SERVO_GET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 15) /** set the minimum PWM value the output will send */ -#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16) +#define PWM_SERVO_SET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 16) /** get the minimum PWM value the output will send */ -#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17) +#define PWM_SERVO_GET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 17) /** set the maximum PWM value the output will send */ -#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18) +#define PWM_SERVO_SET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 18) /** get the maximum PWM value the output will send */ -#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +#define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19) /** set the number of servos in (unsigned)arg - allows change of * split between servos and GPIO */ -#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) +#define PWM_SERVO_SET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 20) /** set the lockdown override flag to enable outputs in HIL */ -#define PWM_SERVO_SET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 21) +#define PWM_SERVO_SET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 21) /** get the lockdown override flag to enable outputs in HIL */ -#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) +#define PWM_SERVO_GET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 22) /** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _PX4_IOC(_PWM_SERVO_BASE, 23) /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) +#define PWM_SERVO_SET_FORCE_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 24) /** make failsafe non-recoverable (termination) if it occurs */ -#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) +#define PWM_SERVO_SET_TERMINATION_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 25) /** force safety switch on (to enable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 26) /** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ -#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) +#define PWM_SERVO_SET_RC_CONFIG _PX4_IOC(_PWM_SERVO_BASE, 27) /** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) +#define PWM_SERVO_SET_OVERRIDE_OK _PX4_IOC(_PWM_SERVO_BASE, 28) /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) +#define PWM_SERVO_CLEAR_OVERRIDE_OK _PX4_IOC(_PWM_SERVO_BASE, 29) /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ -#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) +#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 30) /* * @@ -255,15 +256,15 @@ ORB_DECLARE(output_pwm); */ /** set a single servo to a specific value */ -#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) +#define PWM_SERVO_SET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x20 + _servo) /** get a single specific servo value */ -#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) +#define PWM_SERVO_GET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x40 + _servo) /** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels * whose update rates must be the same. */ -#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n) +#define PWM_SERVO_GET_RATEGROUP(_n) _PX4_IOC(_PWM_SERVO_BASE, 0x60 + _n) /* * Low-level PWM output interface. diff --git a/src/drivers/hil/hil_linux.cpp b/src/drivers/hil/hil_linux.cpp new file mode 100644 index 0000000000..274a4369f5 --- /dev/null +++ b/src/drivers/hil/hil_linux.cpp @@ -0,0 +1,860 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hil_linux.cpp + * + * Driver/configurator for the virtual HIL port. + * + * This virtual driver emulates PWM / servo outputs for setups where + * the connected hardware does not provide enough or no PWM outputs. + * + * Its only function is to take actuator_control uORB messages, + * mix them with any loaded mixer and output the result to the + * actuator_output uORB topic. HIL can also be performed with normal + * PWM outputs, a special flag prevents the outputs to be operated + * during HIL mode. If HIL is not performed with a standalone FMU, + * but in a real system, it is NOT recommended to use this virtual + * driver. Use instead the normal FMU or IO driver. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +class HIL : public device::VDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_8PWM, + MODE_12PWM, + MODE_16PWM, + MODE_NONE + }; + HIL(); + virtual ~HIL(); + + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + virtual int init(); + + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + +private: + static const unsigned _max_actuators = 4; + + Mode _mode; + int _update_rate; + int _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + orb_advert_t _t_outputs; + unsigned _num_outputs; + bool _primary_pwm_device; + + volatile bool _task_should_exit; + bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int pwm_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + +}; + +namespace +{ + +HIL *g_hil; + +} // namespace + +HIL::HIL() : + VDev("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), + _mode(MODE_NONE), + _update_rate(50), + _current_update_rate(0), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _num_outputs(0), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +HIL::~HIL() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + px4_task_delete(_task); + break; + } + + } while (_task != -1); + } + + // XXX already claimed with CDEV + // /* clean up the alternate device node */ + // if (_primary_pwm_device) + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_hil = nullptr; +} + +int +HIL::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = VDev::init(); + + if (ret != PX4_OK) + return ret; + + // XXX already claimed with CDEV + ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == PX4_OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + // gpio_reset(); + + /* start the HIL interface task */ + _task = px4_task_spawn_cmd("fmuhil", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1200, + (px4_main_t)&HIL::task_main_trampoline, + nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return PX4_OK; +} + +void +HIL::task_main_trampoline(int argc, char *argv[]) +{ + g_hil->task_main(); +} + +int +HIL::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: + debug("MODE_2PWM"); + /* multi-port with flow control lines as PWM */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_4PWM: + debug("MODE_4PWM"); + /* multi-port as 4 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_8PWM: + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_NONE: + debug("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + _update_rate = 10; + break; + + default: + return -EINVAL; + } + _mode = mode; + return PX4_OK; +} + +int +HIL::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) + return -EINVAL; + + _update_rate = rate; + return PX4_OK; +} + +void +HIL::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_LOW); + + px4_pollfd_struct_t fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs; + + /* select the number of virtual outputs */ + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + // XXX only support the lower 8 - trivial to extend + num_outputs = 8; + break; + + case MODE_NONE: + default: + num_outputs = 0; + break; + } + + log("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + if (update_rate_in_ms < 2) + update_rate_in_ms = 2; + orb_set_interval(_t_actuators, update_rate_in_ms); + // up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ + int ret = px4_poll(&fds[0], 2, 1000); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } + } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + } + } + + px4_close(_t_actuators); + px4_close(_t_armed); + + /* make sure servos are off */ + // up_pwm_servo_deinit(); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; +} + +int +HIL::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + +int +HIL::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + int ret; + + debug("ioctl 0x%04x 0x%08x", cmd, arg); + + // /* try it as a GPIO ioctl first */ + // ret = HIL::gpio_ioctl(handlep, cmd, arg); + // if (ret != -ENOTTY) + // return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch(_mode) { + case MODE_2PWM: + case MODE_4PWM: + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + ret = HIL::pwm_ioctl(handlep, cmd, arg); + break; + default: + ret = -ENOTTY; + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let VDev have it */ + if (ret == -ENOTTY) + ret = VDev::ioctl(handlep, cmd, arg); + + return ret; +} + +int +HIL::pwm_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + int ret = PX4_OK; + // int channel; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + // up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + // up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate + g_hil->set_pwm_rate(arg); + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + // channel = cmd - PWM_SERVO_SET(0); +// up_pwm_servo_set(channel, arg); XXX + + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): { + // channel = cmd - PWM_SERVO_SET(0); + // *(servo_position_t *)arg = up_pwm_servo_get(channel); + break; + } + + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + *(uint32_t *)arg = (1 << channel); + break; + } + + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + + } else { + *(unsigned *)arg = 2; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers == nullptr) { + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; + } + + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNDEFINED = 0, + PORT1_MODE_UNSET, + PORT1_FULL_PWM, + PORT1_PWM_AND_SERIAL, + PORT1_PWM_AND_GPIO, + PORT2_MODE_UNSET, + PORT2_8PWM, + PORT2_12PWM, + PORT2_16PWM, +}; + +PortMode g_port_mode; + +int +hil_new_mode(PortMode new_mode) +{ + // uint32_t gpio_bits; + + +// /* reset to all-inputs */ +// g_hil->ioctl(0, GPIO_RESET, 0); + + // gpio_bits = 0; + + HIL::Mode servo_mode = HIL::MODE_NONE; + + switch (new_mode) { + case PORT_MODE_UNDEFINED: + case PORT1_MODE_UNSET: + case PORT2_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT1_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = HIL::MODE_4PWM; + break; + + case PORT1_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; +// /* set RX/TX multi-GPIOs to serial mode */ +// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT1_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; + break; + + case PORT2_8PWM: + /* select 8-pin PWM mode */ + servo_mode = HIL::MODE_8PWM; + break; + + case PORT2_12PWM: + /* select 12-pin PWM mode */ + servo_mode = HIL::MODE_12PWM; + break; + + case PORT2_16PWM: + /* select 16-pin PWM mode */ + servo_mode = HIL::MODE_16PWM; + break; + } + +// /* adjust GPIO config for serial mode(s) */ +// if (gpio_bits != 0) +// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_hil->set_mode(servo_mode); + + return PX4_OK; +} + +int +hil_start(void) +{ + int ret = PX4_OK; + + if (g_hil == nullptr) { + + g_hil = new HIL; + + if (g_hil == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_hil->init(); + + if (ret != PX4_OK) { + delete g_hil; + g_hil = nullptr; + } + } + } + + return ret; +} + +int +test(void) +{ + int fd; + + fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); + + if (fd < 0) { + puts("open fail"); + return -ENODEV; + } + + px4_ioctl(fd, PWM_SERVO_ARM, 0); + px4_ioctl(fd, PWM_SERVO_SET(0), 1000); + + px4_close(fd); + + return PX4_OK; +} + +int +fake(int argc, char *argv[]) +{ + if (argc < 5) { + puts("hil fake (values -100 .. 100)"); + return -EINVAL; + } + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle < 0) { + puts("advertise failed"); + return 1; + } + + return 0; +} + +} // namespace + +extern "C" __EXPORT int hil_main(int argc, char *argv[]); + +int +hil_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNDEFINED; + const char *verb = argv[1]; + int ret = PX4_OK; + + if (hil_start() != PX4_OK) { + warnx("failed to start the HIL driver"); + return 1; + } + + /* + * Mode switches. + */ + + // this was all cut-and-pasted from the FMU driver; it's junk + if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT1_FULL_PWM; + + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT1_PWM_AND_SERIAL; + + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT1_PWM_AND_GPIO; + + } else if (!strcmp(verb, "mode_port2_pwm8")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(verb, "mode_port2_pwm12")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(verb, "mode_port2_pwm16")) { + new_mode = PORT2_8PWM; + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNDEFINED) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return PX4_OK; + + /* switch modes */ + return hil_new_mode(new_mode); + } + + if (!strcmp(verb, "test")) { + ret = test(); + } + + else if (!strcmp(verb, "fake")) { + ret = fake(argc - 1, argv + 1); + } + + else { + fprintf(stderr, "HIL: unrecognized command, try:\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + ret = -EINVAL; + } + return ret; +} diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index f1fc49fb3d..d9137e99b1 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -37,6 +37,10 @@ MODULE_COMMAND = hil +ifdef ($(PX4_TARGET_OS),nuttx) SRCS = hil.cpp - MAXOPTIMIZATION = -Os +else +SRCS = hil_linux.cpp +endif + From 89bc1b86b21eedf23ae156e8bd2ede711c7e6cfa Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 12:38:50 -0700 Subject: [PATCH 105/258] Linux: connected gyrosim to Simulator Simulator gets incoming MPU data and gives raw MPU data to the gyrosim sensor when read. Signed-off-by: Mark Charlebois --- src/modules/simulator/simulator.cpp | 93 ++++---- src/modules/simulator/simulator.h | 19 +- .../linux/drivers/gyrosim/gyrosim.cpp | 220 +++--------------- 3 files changed, 99 insertions(+), 233 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index c4c6ffbbd6..3372f25f4b 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -37,6 +37,8 @@ */ #include +#include +#include #include #include #include @@ -44,6 +46,8 @@ #include #include "simulator.h" +static px4_task_t g_sim_task = -1; + Simulator *Simulator::_instance = NULL; Simulator::Simulator() : _max_readers(3) @@ -56,33 +60,15 @@ Simulator *Simulator::getInstance() return _instance; } -int Simulator::getSample(sim_dev_t dev, sample &val) +bool Simulator::getMPUReport(uint8_t *buf, int len) { - int ret; - - switch (dev) { - case SIM_GYRO: - read_lock(); - val = _gyro[_readidx]; - read_unlock(); - ret = 0; - break; - case SIM_ACCEL: - read_lock(); - val = _accel[_readidx]; - read_unlock(); - ret = 0; - break; - case SIM_MAG: - read_lock(); - val = _mag[_readidx]; - read_unlock(); - ret = 0; - break; - default: - ret = 1; - } - return ret; + if (len != sizeof(MPUReport)) { + return false; + } + read_lock(); + memcpy(buf, &_mpureport[_readidx], sizeof(MPUReport)); + read_unlock(); + return true; } int Simulator::start(int argc, char *argv[]) @@ -109,7 +95,7 @@ void Simulator::updateSamples() const int buflen = 200; const int port = 9876; unsigned char buf[buflen]; - int writeidx, num; + int writeidx; if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { printf("create socket failed\n"); @@ -130,22 +116,18 @@ void Simulator::updateSamples() len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { writeidx = !_readidx; - buf[len] = 0; - printf("received: %s\n", buf); - // FIXME - temp hack to read data, not safe - bad bad bad - num = sscanf((const char *)buf, "%f,%f,%f,%f,%f,%f,%f,%f,%f", - &_gyro[writeidx].x, &_gyro[writeidx].y, &_gyro[writeidx].z, - &_accel[writeidx].x, &_accel[writeidx].y, &_accel[writeidx].z, - &_mag[writeidx].x, &_mag[writeidx].y, &_mag[writeidx].z); - if (num != 9) { - printf("Only read %d items\n", num); - } - else { + if (len == sizeof(MPUReport)) { + printf("received: MPU data\n"); + memcpy((void *)&_mpureport[writeidx], (void *)buf, len); + // Swap read and write buffers write_lock(); _readidx = !_readidx; write_unlock(); } + else { + printf("bad packet: len = %d\n", len); + } } } } @@ -171,18 +153,47 @@ void Simulator::write_unlock() } } +static void usage() +{ + warnx("Usage: simulator {start|stop}"); +} + extern "C" { int simulator_main(int argc, char *argv[]) { - return (int)(px4_task_spawn_cmd("Simulator", + int ret = 0; + if (argc != 2) { + usage(); + return 1; + } + if (strcmp(argv[1], "start") == 0) { + if (g_sim_task >= 0) { + warnx("Simulator already started"); + return 0; + } + g_sim_task = px4_task_spawn_cmd("Simulator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, Simulator::start, - nullptr) < 0); + nullptr); + } + else if (strcmp(argv[1], "stop") == 0) { + if (g_sim_task < 0) { + warnx("Simulator not running"); + } + else { + px4_task_delete(g_sim_task); + g_sim_task = -1; + } + } + else { + usage(); + ret = -EINVAL; + } - return 0; + return ret; } } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 4c20768caf..333a729281 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -60,7 +60,7 @@ public: static int start(int argc, char *argv[]); - int getSample(sim_dev_t dev, sample &val); + bool getMPUReport(uint8_t *buf, int len); private: Simulator(); ~Simulator() { _instance=NULL; } @@ -82,5 +82,22 @@ private: sem_t _lock; const int _max_readers; + +#pragma pack(push, 1) + /** + * Report conversation within the GYROSIM, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) + MPUReport _mpureport[2]; }; diff --git a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp index 378c026f2a..d117cab9a1 100644 --- a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp @@ -59,6 +59,8 @@ #include #include +#include + #include #include #include @@ -207,13 +209,6 @@ public: void print_registers(); - /** - * Test behaviour against factory offsets - * - * @return 0 on success, 1 on failure - */ - int factory_self_test(); - protected: friend class GYROSIM_gyro; @@ -273,10 +268,6 @@ private: uint8_t _checked_values[GYROSIM_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; - // use this to avoid processing measurements when in factory - // self test - volatile bool _in_factory_test; - // last temperature reading for print_info() float _last_temperature; @@ -419,6 +410,8 @@ private: uint8_t gyro_z[2]; }; #pragma pack(pop) + + uint8_t _regdata[108]; }; /* @@ -507,7 +500,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _checked_next(0), - _in_factory_test(false), _last_temperature(0) { // disable debug() calls @@ -661,6 +653,30 @@ int GYROSIM::reset() int GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + uint8_t cmd = send[0]; + uint8_t reg = cmd & 0x7F; + const uint8_t MPUREAD = MPUREG_INT_STATUS | DIR_READ; + + if (cmd == MPUREAD) { + // Get data from the simulator + Simulator *sim = Simulator::getInstance(); + if (sim == NULL) + return ENODEV; + + // FIXME - not sure what interrupt status should be + recv[1] = 0; + // skip cmd and status bytes + sim->getMPUReport(&recv[2], len-2); + } + else if (cmd & DIR_READ) + { + printf("Reading %u bytes from register %u\n", len-1, reg); + memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); + } + else { + printf("Writing %u bytes to register %u\n", len-1, reg); + memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + } return PX4_OK; } @@ -835,157 +851,6 @@ GYROSIM::gyro_self_test() return 0; } - - -/* - perform a self-test comparison to factory trim values. This takes - about 200ms and will return OK if the current values are within 14% - of the expected values (as per datasheet) - */ -int -GYROSIM::factory_self_test() -{ - _in_factory_test = true; - uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); - uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); - const uint16_t repeats = 100; - int ret = OK; - - // gyro self test has to be done at 250DPS - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - - struct MPUReport mpu_report; - float accel_baseline[3]; - float gyro_baseline[3]; - float accel[3]; - float gyro[3]; - float accel_ftrim[3]; - float gyro_ftrim[3]; - - // get baseline values without self-test enabled - //set_frequency(GYROSIM_HIGH_BUS_SPEED); - - memset(accel_baseline, 0, sizeof(accel_baseline)); - memset(gyro_baseline, 0, sizeof(gyro_baseline)); - memset(accel, 0, sizeof(accel)); - memset(gyro, 0, sizeof(gyro)); - - for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); - atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); - atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); - gtrim[0] = trims[0] & 0x1F; - gtrim[1] = trims[1] & 0x1F; - gtrim[2] = trims[2] & 0x1F; - - // convert factory trims to right units - for (uint8_t i=0; i<3; i++) { - accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); - gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); - } - // Y gyro trim is negative - gyro_ftrim[1] *= -1; - - for (uint8_t i=0; i<3; i++) { - float diff = accel[i]-accel_baseline[i]; - float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; - ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", - (unsigned)i, - (int)(1000*accel_baseline[i]), - (int)(1000*accel[i]), - (int)(1000*diff), - (int)(1000*accel_ftrim[i]), - (int)err); - if (fabsf(err) > 14) { - ::printf("FAIL\n"); - ret = -EIO; - } - } - for (uint8_t i=0; i<3; i++) { - float diff = gyro[i]-gyro_baseline[i]; - float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; - ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", - (unsigned)i, - (int)(1000*gyro_baseline[i]), - (int)(1000*gyro[i]), - (int)(1000*(gyro[i]-gyro_baseline[i])), - (int)(1000*gyro_ftrim[i]), - (int)err); - if (fabsf(err) > 14) { - ::printf("FAIL\n"); - ret = -EIO; - } - } - - write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); - write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); - - _in_factory_test = false; - if (ret == OK) { - ::printf("PASSED\n"); - } - - return ret; -} - - ssize_t GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) { @@ -1426,11 +1291,6 @@ GYROSIM::check_registers(void) void GYROSIM::measure() { - if (_in_factory_test) { - // don't publish any data while in factory test mode - return; - } - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -1746,7 +1606,6 @@ int test(); int reset(); int info(); int regdump(); -int factorytest(); void usage(); /** @@ -1952,27 +1811,10 @@ regdump() return 0; } -/** - * Dump the register information - */ -int -factorytest() -{ - GYROSIM **g_dev_ptr = &g_dev_sim; - if (*g_dev_ptr == nullptr) { - warnx("driver not running"); - return 1; - } - - (*g_dev_ptr)->factory_self_test(); - - return 0; -} - void usage() { - warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest'"); + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'"); warnx("options:"); warnx(" -R rotation"); } @@ -2042,10 +1884,6 @@ gyrosim_main(int argc, char *argv[]) ret = gyrosim::regdump(); } - else if (!strcmp(verb, "factorytest")) { - ret = gyrosim::factorytest(); - } - else { gyrosim::usage(); ret = 1; From 93c39e6d1dd9bdaf2b77487a7fa02050a4ca49e0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 16:12:08 -0700 Subject: [PATCH 106/258] Linux: Added barosim support to simulator Signed-off-by: Mark Charlebois --- src/modules/simulator/simulator.cpp | 104 ++++++++++------ src/modules/simulator/simulator.h | 117 +++++++++++++----- .../linux/drivers/barosim/baro_sim.cpp | 12 ++ 3 files changed, 163 insertions(+), 70 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 3372f25f4b..12be371339 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -50,11 +50,55 @@ static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = NULL; -Simulator::Simulator() : _max_readers(3) +SimulatorReport::SimulatorReport(int readers, int reportLen) : + _max_readers(readers), + _report_len(reportLen) { sem_init(&_lock, 0, _max_readers); } +bool SimulatorReport::copyData(void *inbuf, void *outbuf, int len) +{ + if (len != _report_len) { + return false; + } + read_lock(); + memcpy(inbuf, outbuf, _report_len); + read_unlock(); + return true; +} + +bool SimulatorReport::writeData(void *inbuf, void *outbuf, int len) +{ + if (len != _report_len) { + return false; + } + memcpy(inbuf, outbuf, _report_len); + swapBuffers(); + return true; +} + +void SimulatorReport::read_lock() +{ + sem_wait(&_lock); +} +void SimulatorReport::read_unlock() +{ + sem_post(&_lock); +} +void SimulatorReport::write_lock() +{ + for (int i=0; i<_max_readers; i++) { + sem_wait(&_lock); + } +} +void SimulatorReport::write_unlock() +{ + for (int i=0; i<_max_readers; i++) { + sem_post(&_lock); + } +} + Simulator *Simulator::getInstance() { return _instance; @@ -62,13 +106,17 @@ Simulator *Simulator::getInstance() bool Simulator::getMPUReport(uint8_t *buf, int len) { - if (len != sizeof(MPUReport)) { - return false; - } - read_lock(); - memcpy(buf, &_mpureport[_readidx], sizeof(MPUReport)); - read_unlock(); - return true; + return _mpu.copyData(buf, &_mpu._data[_mpu.getReadIdx()], len); +} + +bool Simulator::getRawAccelReport(uint8_t *buf, int len) +{ + return _accel.copyData(buf, &_accel._data[_accel.getReadIdx()], len); +} + +bool Simulator::getBaroSample(uint8_t *buf, int len) +{ + return _baro.copyData(buf, &_baro._data[_baro.getReadIdx()], len); } int Simulator::start(int argc, char *argv[]) @@ -95,7 +143,6 @@ void Simulator::updateSamples() const int buflen = 200; const int port = 9876; unsigned char buf[buflen]; - int writeidx; if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { printf("create socket failed\n"); @@ -115,15 +162,17 @@ void Simulator::updateSamples() for (;;) { len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { - writeidx = !_readidx; - if (len == sizeof(MPUReport)) { + if (len == sizeof(MPUReport::RawMPUData)) { printf("received: MPU data\n"); - memcpy((void *)&_mpureport[writeidx], (void *)buf, len); - - // Swap read and write buffers - write_lock(); - _readidx = !_readidx; - write_unlock(); + _mpu.writeData(&_mpu._data[_mpu.getWriteIdx()], buf, len); + } + else if (len == sizeof(RawAccelReport::RawAccelData)) { + printf("received: accel data\n"); + _accel.writeData(&_accel._data[_accel.getWriteIdx()], buf, len); + } + else if (len == sizeof(BaroReport::RawBaroData)) { + printf("received: accel data\n"); + _baro.writeData(&_baro._data[_baro.getWriteIdx()], buf, len); } else { printf("bad packet: len = %d\n", len); @@ -132,27 +181,6 @@ void Simulator::updateSamples() } } -void Simulator::read_lock() -{ - sem_wait(&_lock); -} -void Simulator::read_unlock() -{ - sem_post(&_lock); -} -void Simulator::write_lock() -{ - for (int i=0; i<_max_readers; i++) { - sem_wait(&_lock); - } -} -void Simulator::write_unlock() -{ - for (int i=0; i<_max_readers; i++) { - sem_post(&_lock); - } -} - static void usage() { warnx("Usage: simulator {start|stop}"); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 333a729281..c9e8a73c97 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -40,6 +40,85 @@ #include +class SimulatorReport { +public: + SimulatorReport(int readers, int reportLen); + ~SimulatorReport() {}; + + int getReadIdx() { return _readidx; } + int getWriteIdx() { return !_readidx; } + + bool copyData(void *inbuf, void *outbuf, int len); + bool writeData(void *inbuf, void *outbuf, int len); + +protected: + void read_lock(); + void read_unlock(); + void write_lock(); + void write_unlock(); + + void swapBuffers() + { + write_lock(); + _readidx = !_readidx; + write_unlock(); + } + + int _readidx; + sem_t _lock; + const int _max_readers; + const int _report_len; +}; + +class RawAccelReport : public SimulatorReport { +public: + RawAccelReport() : SimulatorReport(1, sizeof(RawAccelData)) {} + ~RawAccelReport() {} + +// FIXME - what is the endianness of these on actual device? +#pragma pack(push, 1) + struct RawAccelData { + int16_t x; + int16_t y; + int16_t z; + }; +#pragma pack(pop) + + RawAccelData _data[2]; +}; + +class MPUReport : public SimulatorReport { +public: + MPUReport() : SimulatorReport(1, sizeof(RawMPUData)) {} + ~MPUReport() {} + +#pragma pack(push, 1) + struct RawMPUData { + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) + + RawMPUData _data[2]; +}; + +class BaroReport : public SimulatorReport { +public: + BaroReport() : SimulatorReport(1, sizeof(RawBaroData)) {} + ~BaroReport() {} + + struct RawBaroData { + uint8_t d[3]; + }; + + RawBaroData _data[2]; +}; + class Simulator { public: static Simulator *getInstance(); @@ -60,44 +139,18 @@ public: static int start(int argc, char *argv[]); + bool getRawAccelReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); + bool getBaroSample(uint8_t *buf, int len); private: - Simulator(); + Simulator() {} ~Simulator() { _instance=NULL; } void updateSamples(); - void read_lock(); - void read_unlock(); - void write_lock(); - void write_unlock(); - - int _readidx; - - sample _accel[2]; - sample _gyro[2]; - sample _mag[2]; - static Simulator *_instance; - sem_t _lock; - - const int _max_readers; - -#pragma pack(push, 1) - /** - * Report conversation within the GYROSIM, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - }; -#pragma pack(pop) - MPUReport _mpureport[2]; + RawAccelReport _accel; + MPUReport _mpu; + BaroReport _baro; }; diff --git a/src/platforms/linux/drivers/barosim/baro_sim.cpp b/src/platforms/linux/drivers/barosim/baro_sim.cpp index 6e4e1ca0b0..a9374137fe 100644 --- a/src/platforms/linux/drivers/barosim/baro_sim.cpp +++ b/src/platforms/linux/drivers/barosim/baro_sim.cpp @@ -50,6 +50,7 @@ #include #include +#include #include "barosim.h" #include "board_config.h" @@ -201,5 +202,16 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len, { // TODO add Simulation data connection so calls retrieve // data from the simulator + + if (send_len != 1 || send[0] != 0 || recv_len != 3) { + return 1; + } + else { + Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { + return -ENODEV; + } + sim->getBaroSample(recv, recv_len); + } return 0; } From 55581cc43820ca27731e03208b9c1efc4f6d3860 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 16:13:00 -0700 Subject: [PATCH 107/258] Linux: adcsim fixes Call to read should have been px4_read. Signed-off-by: Mark Charlebois --- src/platforms/linux/drivers/adcsim/adcsim.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/platforms/linux/drivers/adcsim/adcsim.cpp b/src/platforms/linux/drivers/adcsim/adcsim.cpp index 83419cdad2..78ffbf1db7 100644 --- a/src/platforms/linux/drivers/adcsim/adcsim.cpp +++ b/src/platforms/linux/drivers/adcsim/adcsim.cpp @@ -32,12 +32,12 @@ ****************************************************************************/ /** - * @file adc.cpp + * @file adcsim.cpp * - * Driver for the STM32 ADCSIM. + * Driver for the ADCSIM. * - * This is a low-rate driver, designed for sampling things like voltages - * and so forth. It avoids the gross complexity of the NuttX ADCSIM driver. + * This is a designed for simulating sampling things like voltages + * and so forth. */ #include @@ -249,7 +249,7 @@ test(void) for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = read(fd, data, sizeof(data)); + ssize_t count = px4_read(fd, data, sizeof(data)); if (count < 0) { warnx("read error"); From 2f5bfe0c16d8c185f2824a14710d9c4738fc61a9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 16:14:13 -0700 Subject: [PATCH 108/258] Linux: quelch clang-3.5 not found warnings Quelch stderr when looking for clang-3.5. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 3a65682a5e..f26678486a 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -46,10 +46,10 @@ USE_GCC?=0 ifneq ($(USE_GCC),1) -HAVE_CLANG35=$(shell clang-3.5 -dumpversion) +HAVE_CLANG35:=$(shell clang-3.5 -dumpversion 2>/dev/null) # Clang will report 4.2.1 as GCC version -HAVE_CLANG=$(shell clang -dumpversion) +HAVE_CLANG:=$(shell clang -dumpversion) #If using ubuntu 14.04 and packaged clang 3.5 ifeq ($(HAVE_CLANG35),4.2.1) From 88dc6ec1e51b54338d44f1e40e9d174eddd6c79f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 17:11:15 -0700 Subject: [PATCH 109/258] Simulator: use template for Reports Signed-off-by: Mark Charlebois --- src/modules/simulator/simulator.cpp | 69 +++----------- src/modules/simulator/simulator.h | 137 ++++++++++++++-------------- 2 files changed, 81 insertions(+), 125 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 12be371339..9984896525 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -46,59 +46,12 @@ #include #include "simulator.h" +using namespace simulator; + static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = NULL; -SimulatorReport::SimulatorReport(int readers, int reportLen) : - _max_readers(readers), - _report_len(reportLen) -{ - sem_init(&_lock, 0, _max_readers); -} - -bool SimulatorReport::copyData(void *inbuf, void *outbuf, int len) -{ - if (len != _report_len) { - return false; - } - read_lock(); - memcpy(inbuf, outbuf, _report_len); - read_unlock(); - return true; -} - -bool SimulatorReport::writeData(void *inbuf, void *outbuf, int len) -{ - if (len != _report_len) { - return false; - } - memcpy(inbuf, outbuf, _report_len); - swapBuffers(); - return true; -} - -void SimulatorReport::read_lock() -{ - sem_wait(&_lock); -} -void SimulatorReport::read_unlock() -{ - sem_post(&_lock); -} -void SimulatorReport::write_lock() -{ - for (int i=0; i<_max_readers; i++) { - sem_wait(&_lock); - } -} -void SimulatorReport::write_unlock() -{ - for (int i=0; i<_max_readers; i++) { - sem_post(&_lock); - } -} - Simulator *Simulator::getInstance() { return _instance; @@ -106,17 +59,17 @@ Simulator *Simulator::getInstance() bool Simulator::getMPUReport(uint8_t *buf, int len) { - return _mpu.copyData(buf, &_mpu._data[_mpu.getReadIdx()], len); + return _mpu.copyData(buf, len); } bool Simulator::getRawAccelReport(uint8_t *buf, int len) { - return _accel.copyData(buf, &_accel._data[_accel.getReadIdx()], len); + return _accel.copyData(buf, len); } bool Simulator::getBaroSample(uint8_t *buf, int len) { - return _baro.copyData(buf, &_baro._data[_baro.getReadIdx()], len); + return _baro.copyData(buf, len); } int Simulator::start(int argc, char *argv[]) @@ -162,17 +115,17 @@ void Simulator::updateSamples() for (;;) { len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { - if (len == sizeof(MPUReport::RawMPUData)) { + if (len == sizeof(RawMPUData)) { printf("received: MPU data\n"); - _mpu.writeData(&_mpu._data[_mpu.getWriteIdx()], buf, len); + _mpu.writeData(buf); } - else if (len == sizeof(RawAccelReport::RawAccelData)) { + else if (len == sizeof(RawAccelData)) { printf("received: accel data\n"); - _accel.writeData(&_accel._data[_accel.getWriteIdx()], buf, len); + _accel.writeData(buf); } - else if (len == sizeof(BaroReport::RawBaroData)) { + else if (len == sizeof(RawBaroData)) { printf("received: accel data\n"); - _baro.writeData(&_baro._data[_baro.getWriteIdx()], buf, len); + _baro.writeData(buf); } else { printf("bad packet: len = %d\n", len); diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index c9e8a73c97..e07cc0f296 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -40,83 +40,85 @@ #include -class SimulatorReport { +namespace simulator { + +// FIXME - what is the endianness of these on actual device? +#pragma pack(push, 1) +struct RawAccelData { + int16_t x; + int16_t y; + int16_t z; +}; +#pragma pack(pop) + +#pragma pack(push, 1) +struct RawMPUData { + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; +}; +#pragma pack(pop) + +struct RawBaroData { + uint8_t d[3]; +}; + +template class Report { public: - SimulatorReport(int readers, int reportLen); - ~SimulatorReport() {}; + Report(int readers) : + _max_readers(readers), + _report_len(sizeof(RType)) + { + sem_init(&_lock, 0, _max_readers); + } - int getReadIdx() { return _readidx; } - int getWriteIdx() { return !_readidx; } + ~Report() {}; - bool copyData(void *inbuf, void *outbuf, int len); - bool writeData(void *inbuf, void *outbuf, int len); + bool copyData(void *outbuf, int len) + { + if (len != _report_len) { + return false; + } + read_lock(); + memcpy(outbuf, &_buf[_readidx], _report_len); + read_unlock(); + return true; + } + void writeData(void *inbuf) + { + write_lock(); + memcpy(&_buf[!_readidx], inbuf, _report_len); + _readidx = !_readidx; + write_unlock(); + } protected: - void read_lock(); - void read_unlock(); - void write_lock(); - void write_unlock(); - - void swapBuffers() + void read_lock() { sem_wait(&_lock); } + void read_unlock() { sem_post(&_lock); } + void write_lock() { - write_lock(); - _readidx = !_readidx; - write_unlock(); + for (int i=0; i<_max_readers; i++) { + sem_wait(&_lock); + } + } + void write_unlock() + { + for (int i=0; i<_max_readers; i++) { + sem_post(&_lock); + } } int _readidx; sem_t _lock; const int _max_readers; const int _report_len; + RType _buf[2]; }; -class RawAccelReport : public SimulatorReport { -public: - RawAccelReport() : SimulatorReport(1, sizeof(RawAccelData)) {} - ~RawAccelReport() {} - -// FIXME - what is the endianness of these on actual device? -#pragma pack(push, 1) - struct RawAccelData { - int16_t x; - int16_t y; - int16_t z; - }; -#pragma pack(pop) - - RawAccelData _data[2]; -}; - -class MPUReport : public SimulatorReport { -public: - MPUReport() : SimulatorReport(1, sizeof(RawMPUData)) {} - ~MPUReport() {} - -#pragma pack(push, 1) - struct RawMPUData { - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - }; -#pragma pack(pop) - - RawMPUData _data[2]; -}; - -class BaroReport : public SimulatorReport { -public: - BaroReport() : SimulatorReport(1, sizeof(RawBaroData)) {} - ~BaroReport() {} - - struct RawBaroData { - uint8_t d[3]; - }; - - RawBaroData _data[2]; }; class Simulator { @@ -143,14 +145,15 @@ public: bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); private: - Simulator() {} + Simulator() : _accel(1), _mpu(1), _baro(1) {} ~Simulator() { _instance=NULL; } void updateSamples(); static Simulator *_instance; - RawAccelReport _accel; - MPUReport _mpu; - BaroReport _baro; + + simulator::Report _accel; + simulator::Report _mpu; + simulator::Report _baro; }; From bba26c34303269c0cb1e25f593e968e1aedf8a85 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 18:53:59 -0700 Subject: [PATCH 110/258] Linux: enabled commander module The commander module now compiles for Linux. state_machine_helper_linux.cpp iterates over the virtual devices vs all devices under /dev as per NuttX when disabling publishing. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 1 + src/drivers/device/vdev.cpp | 8 + src/drivers/device/vdev.h | 1 + src/drivers/device/vdev_posix.cpp | 5 + src/drivers/drv_led.h | 6 +- src/drivers/drv_rgbled.h | 2 +- src/drivers/drv_tone_alarm.h | 2 +- .../commander/airspeed_calibration.cpp | 1 + .../commander/calibration_routines.cpp | 3 +- src/modules/commander/commander.cpp | 2 +- src/modules/commander/commander_helper.cpp | 5 +- src/modules/commander/gyro_calibration.cpp | 1 + .../commander/gyro_calibration_linux.cpp | 274 +++++++ .../commander/mag_calibration_linux.cpp | 470 ++++++++++++ src/modules/commander/module.mk | 16 +- .../commander/state_machine_helper_linux.cpp | 675 ++++++++++++++++++ src/modules/systemlib/cpuload.c | 5 + src/modules/systemlib/cpuload.h | 4 +- src/platforms/px4_config.h | 3 + src/platforms/px4_posix.h | 1 + 20 files changed, 1471 insertions(+), 14 deletions(-) create mode 100644 src/modules/commander/gyro_calibration_linux.cpp create mode 100644 src/modules/commander/mag_calibration_linux.cpp create mode 100644 src/modules/commander/state_machine_helper_linux.cpp diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index fd3c691b09..e40a58d46a 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -46,6 +46,7 @@ MODULES += modules/uORB MODULES += modules/dataman MODULES += modules/sdlog2 MODULES += modules/simulator +MODULES += modules/commander # # Libraries diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 89275a919c..e5004b6841 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -467,5 +467,13 @@ void VDev::showDevices() } } +const char *VDev::devList(unsigned int *next) +{ + for (;*nextname; + return NULL; +} + } // namespace device diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 62ae479c62..1ce48285f7 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -334,6 +334,7 @@ public: static VDev *getDev(const char *path); static void showDevices(void); + static const char *devList(unsigned int *next); protected: diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index df19fd8392..50df5535d2 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -274,5 +274,10 @@ void px4_show_devices() VDev::showDevices(); } +const char * px4_get_device_names(unsigned int *handle) +{ + return VDev::devList(handle); +} + } diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index f3e8164714..3f5897bd00 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -52,9 +52,9 @@ #define LED_BLUE 0 #define LED_SAFETY 2 -#define LED_ON _IOC(_LED_BASE, 0) -#define LED_OFF _IOC(_LED_BASE, 1) -#define LED_TOGGLE _IOC(_LED_BASE, 2) +#define LED_ON _PX4_IOC(_LED_BASE, 0) +#define LED_OFF _PX4_IOC(_LED_BASE, 1) +#define LED_TOGGLE _PX4_IOC(_LED_BASE, 2) __BEGIN_DECLS diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 0633768684..4e3de36abd 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -50,7 +50,7 @@ */ #define _RGBLEDIOCBASE (0x2900) -#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) +#define _RGBLEDIOC(_n) (_PX4_IOC(_RGBLEDIOCBASE, _n)) /** play the named script in *(char *)arg, repeating forever */ #define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 31aee266e1..b2e02cdb8f 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -65,7 +65,7 @@ #define TONEALARM0_DEVICE_PATH "/dev/tone_alarm0" #define _TONE_ALARM_BASE 0x7400 -#define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) +#define TONE_SET_ALARM _PX4_IOC(_TONE_ALARM_BASE, 1) /* structure describing one note in a tone pattern */ struct tone_note { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 747d915ff1..773c62b3e1 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -41,6 +41,7 @@ #include "commander_helper.h" #include +#include #include #include #include diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 192b8c7270..98a354e0cd 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include @@ -53,7 +54,7 @@ #include "commander_helper.h" // FIXME: Fix return codes -static const int ERROR = -1; +//static const int ERROR = -1; int sphere_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 48b5070440..31baedb2fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -53,7 +53,7 @@ #include #include #include -#include +//#include #include #include #include diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a5e4d19725..c06523ce44 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -41,6 +41,7 @@ * */ +#include #include #include #include @@ -110,7 +111,7 @@ int battery_init() bat_capacity_h = param_find("BAT_CAPACITY"); bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); - return OK; + return PX4_OK; } int buzzer_init() @@ -130,7 +131,7 @@ int buzzer_init() return ERROR; } - return OK; + return PX4_OK; } void buzzer_deinit() diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 291ccfe804..a2b0900ef6 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -42,6 +42,7 @@ #include "commander_helper.h" #include +#include #include #include #include diff --git a/src/modules/commander/gyro_calibration_linux.cpp b/src/modules/commander/gyro_calibration_linux.cpp new file mode 100644 index 0000000000..f490487f3b --- /dev/null +++ b/src/modules/commander/gyro_calibration_linux.cpp @@ -0,0 +1,274 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.cpp + * + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "calibration_messages.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static const char *sensor_name = "gyro"; + +int do_gyro_calibration(int mavlink_fd) +{ + const unsigned max_gyros = 3; + + int32_t device_id[3]; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); + + struct gyro_scale gyro_scale_zero = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + struct gyro_scale gyro_scale[max_gyros] = {}; + + int res = OK; + + /* store board ID */ + uint32_t mcu_id[3]; + mcu_unique_id(&mcu_id[0]); + + /* store last 32bit number - not unique, but unique in a given set */ + (void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); + + char str[30]; + + for (unsigned s = 0; s < max_gyros; s++) { + + /* ensure all scale fields are initialized tha same as the first struct */ + (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); + + sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + /* reset all offsets to zero and all scales to one */ + int fd = px4_open(str, 0); + + if (fd < 0) { + continue; + } + + device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + px4_close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); + } + } + + unsigned calibration_counter[max_gyros] = { 0 }; + const unsigned calibration_count = 5000; + + struct gyro_report gyro_report_0 = {}; + + if (res == OK) { + /* determine gyro mean values */ + unsigned poll_errcount = 0; + + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro[max_gyros]; + px4_pollfd_struct_t fds[max_gyros]; + + for (unsigned s = 0; s < max_gyros; s++) { + sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + fds[s].fd = sub_sensor_gyro[s]; + fds[s].events = POLLIN; + } + + struct gyro_report gyro_report; + + /* use first gyro to pace, but count correctly per-gyro for statistics */ + while (calibration_counter[0] < calibration_count) { + /* wait blocking for new data */ + + int poll_ret = px4_poll(&fds[0], max_gyros, 1000); + + if (poll_ret > 0) { + + for (unsigned s = 0; s < max_gyros; s++) { + bool changed; + orb_check(sub_sensor_gyro[s], &changed); + + if (changed) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); + } + + gyro_scale[s].x_offset += gyro_report.x; + gyro_scale[s].y_offset += gyro_report.y; + gyro_scale[s].z_offset += gyro_report.z; + calibration_counter[s]++; + } + + if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count); + } + } + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } + } + + for (unsigned s = 0; s < max_gyros; s++) { + px4_close(sub_sensor_gyro[s]); + + gyro_scale[s].x_offset /= calibration_counter[s]; + gyro_scale[s].y_offset /= calibration_counter[s]; + gyro_scale[s].z_offset /= calibration_counter[s]; + } + } + + if (res == OK) { + /* check offsets */ + float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; + float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; + float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + + /* maximum allowable calibration error in radians */ + const float maxoff = 0.002f; + + if (!isfinite(gyro_scale[0].x_offset) || + !isfinite(gyro_scale[0].y_offset) || + !isfinite(gyro_scale[0].z_offset) || + fabsf(xdiff) > maxoff || + fabsf(ydiff) > maxoff || + fabsf(zdiff) > maxoff) { + mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration"); + res = ERROR; + } + } + + if (res == OK) { + /* set offset parameters to new values */ + bool failed = false; + + for (unsigned s = 0; s < max_gyros; s++) { + + /* if any reasonable amount of data is missing, skip */ + if (calibration_counter[s] < calibration_count / 2) { + continue; + } + + (void)sprintf(str, "CAL_GYRO%u_XOFF", s); + failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset))); + (void)sprintf(str, "CAL_GYRO%u_YOFF", s); + failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset))); + (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); + failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset))); + (void)sprintf(str, "CAL_GYRO%u_ID", s); + failed |= (OK != param_set(param_find(str), &(device_id[s]))); + + /* apply new scaling and offsets */ + (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + int fd = px4_open(str, 0); + + if (fd < 0) { + failed = true; + continue; + } + + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); + px4_close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + if (failed) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + res = ERROR; + } + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + } + + return res; +} diff --git a/src/modules/commander/mag_calibration_linux.cpp b/src/modules/commander/mag_calibration_linux.cpp new file mode 100644 index 0000000000..49ccbf3ef1 --- /dev/null +++ b/src/modules/commander/mag_calibration_linux.cpp @@ -0,0 +1,470 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file mag_calibration.cpp + * + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" +#include "calibration_messages.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static const char *sensor_name = "mag"; +static const unsigned max_mags = 3; + +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); +int mag_calibration_worker(detect_orientation_return orientation, void* worker_data); + +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + unsigned done_count; + int sub_mag[max_mags]; + unsigned int calibration_points_perside; + unsigned int calibration_interval_perside_seconds; + uint64_t calibration_interval_perside_useconds; + unsigned int calibration_counter_total; + bool side_data_collected[detect_orientation_side_count]; + float* x[max_mags]; + float* y[max_mags]; + float* z[max_mags]; +} mag_worker_data_t; + + +int do_mag_calibration(int mavlink_fd) +{ + mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int result = OK; + + // Determine which mags are available and reset each + + int32_t device_ids[max_mags]; + char str[30]; + + for (size_t i=0; imavlink_fd, "Rotate vehicle around the detected orientation"); + mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); + sleep(2); + + uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + unsigned poll_errcount = 0; + + calibration_counter_side = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter_side < worker_data->calibration_points_perside) { + + // Wait clocking for new data on all mags + px4_pollfd_struct_t fds[max_mags]; + size_t fd_count = 0; + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + fds[fd_count].fd = worker_data->sub_mag[cur_mag]; + fds[fd_count].events = POLLIN; + fd_count++; + } + } + int poll_ret = px4_poll(fds, fd_count, 1000); + + if (poll_ret > 0) { + for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + struct mag_report mag; + + orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; + + } + } + + worker_data->calibration_counter_total++; + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "%s %s side calibration: progress <%u>", + sensor_name, + detect_orientation_str(orientation), + (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } else { + poll_errcount++; + } + + if (poll_errcount > worker_data->calibration_points_perside * 3) { + result = ERROR; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); + break; + } + } + + // Mark the opposite side as collected as well. No need to collect opposite side since it + // would generate similar points. + detect_orientation_return alternateOrientation = orientation; + switch (orientation) { + case DETECT_ORIENTATION_TAIL_DOWN: + alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN; + break; + case DETECT_ORIENTATION_NOSE_DOWN: + alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN; + break; + case DETECT_ORIENTATION_LEFT: + alternateOrientation = DETECT_ORIENTATION_RIGHT; + break; + case DETECT_ORIENTATION_RIGHT: + alternateOrientation = DETECT_ORIENTATION_LEFT; + break; + case DETECT_ORIENTATION_UPSIDE_DOWN: + alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP; + break; + case DETECT_ORIENTATION_RIGHTSIDE_UP: + alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN; + break; + case DETECT_ORIENTATION_ERROR: + warnx("Invalid orientation in mag_calibration_worker"); + break; + } + worker_data->side_data_collected[alternateOrientation] = true; + mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation)); + + worker_data->done_count++; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); + + return result; +} + +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) +{ + int result = OK; + + mag_worker_data_t worker_data; + + worker_data.mavlink_fd = mavlink_fd; + worker_data.done_count = 0; + worker_data.calibration_counter_total = 0; + worker_data.calibration_points_perside = 80; + worker_data.calibration_interval_perside_seconds = 20; + worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; + + // Initialize to collect all sides + for (size_t cur_side=0; cur_side<6; cur_side++) { + worker_data.side_data_collected[cur_side] = false; + } + + for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); + result = ERROR; + } + } + + + // Setup subscriptions to mag sensors + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + px4_close(worker_data.sub_mag[cur_mag]); + } + } + + // Calculate calibration values for each mag + + + float sphere_x[max_mags]; + float sphere_y[max_mags]; + float sphere_z[max_mags]; + float sphere_radius[max_mags]; + + // Sphere fit the data to get calibration values + if (result == OK) { + for (unsigned cur_mag=0; cur_mag= 0) { + px4_close(fd_mag); + } + + if (result == OK) { + bool failed = false; + + /* set parameters */ + (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); + (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); + (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); + (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); + (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); + (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); + + if (failed) { + mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag); + result = ERROR; + } else { + mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga", + cur_mag, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f", + cur_mag, + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); + } + } + } + } + } + + return result; +} diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4437041e26..c9de1ffa2f 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -38,17 +38,25 @@ MODULE_COMMAND = commander SRCS = commander.cpp \ commander_params.c \ - state_machine_helper.cpp \ commander_helper.cpp \ calibration_routines.cpp \ - accelerometer_calibration.cpp \ - gyro_calibration.cpp \ - mag_calibration.cpp \ baro_calibration.cpp \ + accelerometer_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp \ PreflightCheck.cpp +ifdef ($(PX4_TARGET_OS),nuttx) +SRCS += + state_machine_helper.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp +else +SRCS += state_machine_helper_linux.cpp \ + gyro_calibration_linux.cpp \ + mag_calibration_linux.cpp +endif + MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os diff --git a/src/modules/commander/state_machine_helper_linux.cpp b/src/modules/commander/state_machine_helper_linux.cpp new file mode 100644 index 0000000000..97c2f11043 --- /dev/null +++ b/src/modules/commander/state_machine_helper_linux.cpp @@ -0,0 +1,675 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file state_machine_helper.cpp + * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "state_machine_helper.h" +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +// This array defines the arming state transitions. The rows are the new state, and the columns +// are the current state. Using new state and current state you can index into the array which +// will be true for a valid transition or false for a invalid transition. In some cases even +// though the transition is marked as true additional checks must be made. See arming_state_transition +// code for those checks. +static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI +}; + +// You can index into the array with an arming_state_t in order to get it's textual representation +static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", +}; + +transition_result_t +arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status + const struct safety_s *safety, ///< current safety settings + arming_state_t new_arming_state, ///< arming state requested + struct actuator_armed_s *armed, ///< current armed status + bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing + const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none +{ + // Double check that our static arrays are still valid + ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); + ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); + + transition_result_t ret = TRANSITION_DENIED; + arming_state_t current_arming_state = status->arming_state; + bool feedback_provided = false; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_arming_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + /* + * Get sensing state if necessary + */ + int prearm_ret = OK; + + /* only perform the check if we have to */ + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + prearm_ret = prearm_check(status, mavlink_fd); + } + + /* + * Perform an atomic state update + */ + //irqstate_t flags = irqsave(); + + /* enforce lockdown in HIL */ + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { + armed->lockdown = true; + + } else { + armed->lockdown = false; + } + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + // Do not perform pre-arm checks if coming from in air restore + // Allow if vehicle_status_s::HIL_STATE_ON + if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + + // Fail transition if pre-arm check fails + if (prearm_ret) { + /* the prearm check already prints the reject reason */ + feedback_provided = true; + valid_transition = false; + + // Fail transition if we need safety switch press + } else if (safety->safety_switch_available && !safety->safety_off) { + + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + feedback_provided = true; + valid_transition = false; + } + + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + feedback_provided = true; + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { + // Check avionics rail voltages + if (status->avionics_power_rail_voltage < 4.75f) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + valid_transition = false; + } else if (status->avionics_power_rail_voltage < 4.9f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } else if (status->avionics_power_rail_voltage > 5.4f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } + } + } + + } + + } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; + } + } + + // HIL can always go to standby + if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + feedback_provided = true; + valid_transition = false; + } + + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + } + + /* end of atomic state update */ + //irqrestore(flags); + } + + if (ret == TRANSITION_DENIED) { + const char * str = "INVAL: %s - %s"; + /* only print to console here by default as this is too technical to be useful during operation */ + warnx(str, state_names[status->arming_state], state_names[new_arming_state]); + + /* print to MAVLink if we didn't provide any feedback yet */ + if (!feedback_provided) { + mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + } + } + + return ret; +} + +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + + } else { + return false; + } +} + +transition_result_t +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* transition may be denied even if the same state is requested because conditions may have changed */ + switch (new_main_state) { + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + + case vehicle_status_s::MAIN_STATE_ALTCTL: + /* need at minimum altitude estimate */ + /* TODO: add this for fixedwing as well */ + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_POSCTL: + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: + /* need global position and home position */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + break; + + case vehicle_status_s::MAIN_STATE_OFFBOARD: + + /* need offboard signal */ + if (!status->offboard_control_signal_lost) { + ret = TRANSITION_CHANGED; + } + + break; + + case vehicle_status_s::MAIN_STATE_MAX: + default: + break; + } + if (ret == TRANSITION_CHANGED) { + if (status->main_state != new_main_state) { + status->main_state = new_main_state; + } else { + ret = TRANSITION_NOT_CHANGED; + } + } + + return ret; +} + +/** + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; + + if (current_status->hil_state == new_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + switch (new_state) { + case vehicle_status_s::HIL_STATE_OFF: + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); + ret = TRANSITION_DENIED; + break; + + case vehicle_status_s::HIL_STATE_ON: + if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; + + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } + + + int sensfd = px4_open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); + + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + } else { + mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + ret = TRANSITION_DENIED; + } + break; + + default: + warnx("Unknown HIL state"); + break; + } + } + + if (ret == TRANSITION_CHANGED) { + current_status->hil_state = new_state; + current_status->timestamp = hrt_absolute_time(); + // XXX also set lockdown here + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + } + return ret; +} + +/** + * Check failsafe and main status and set navigation status for navigator accordingly + */ +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, + const bool stay_in_failsafe) +{ + navigation_state_t nav_state_old = status->nav_state; + + bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); + status->failsafe = false; + + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + } else { + switch (status->main_state) { + case vehicle_status_s::MAIN_STATE_ACRO: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; + break; + + case vehicle_status_s::MAIN_STATE_MANUAL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + break; + + case vehicle_status_s::MAIN_STATE_ALTCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + break; + + case vehicle_status_s::MAIN_STATE_POSCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + break; + } + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + + /* go into failsafe + * - if commanded to do so + * - if we have an engine failure + * - depending on datalink, RC and if the mission is finished */ + + /* first look at the commands */ + if (status->engine_failure_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->data_link_lost_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->gps_failure_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->rc_signal_lost_cmd) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ + } else if (status->gps_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ + } else if (!stay_in_failsafe){ + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: + /* go into failsafe on a engine failure */ + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } else { + /* everything is perfect */ + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } + break; + + case vehicle_status_s::MAIN_STATE_AUTO_RTL: + /* require global position and home, also go into failsafe on an engine failure */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } + break; + + case vehicle_status_s::MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ + if (status->offboard_control_signal_lost && !status->rc_signal_lost) { + status->failsafe = true; + + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; + } + default: + break; + } + + return status->nav_state != nav_state_old; +} + +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +{ + int ret; + bool failed = false; + + int fd = px4_open(ACCEL0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); + failed = true; + goto system_eval; + } + + ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); + failed = true; + goto system_eval; + } + + /* check measurement result range */ + struct accel_report acc; + ret = px4_read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); + /* this is frickin' fatal */ + failed = true; + goto system_eval; + } + } else { + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); + /* this is frickin' fatal */ + failed = true; + goto system_eval; + } + + /* Perform airspeed check only if circuit breaker is not + * engaged and it's not a rotary wing */ + if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { + /* accel done, close it */ + px4_close(fd); + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); + failed = true; + goto system_eval; + } + + if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + // XXX do not make this fatal yet + } + } + +system_eval: + px4_close(fd); + return (failed); +} diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 4f58ac71a7..fe08da71fd 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -61,6 +61,8 @@ #ifdef CONFIG_SCHED_INSTRUMENTATION +#ifdef __PX4_NUTTX + __EXPORT void sched_note_start(FAR struct tcb_s *tcb); __EXPORT void sched_note_stop(FAR struct tcb_s *tcb); __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb); @@ -167,4 +169,7 @@ void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb) } } +#else +__EXPORT struct system_load_s system_load; +#endif #endif /* CONFIG_SCHED_INSTRUMENTATION */ diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index 16d132fdb8..dc173baa79 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -37,13 +37,15 @@ __BEGIN_DECLS -#include +#include struct system_load_taskinfo_s { uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load uint64_t curr_start_time; ///< Start time of the current scheduling slot uint64_t start_time; ///< FIRST start time of task +#ifdef __PX4_NUTTX FAR struct tcb_s *tcb; ///< +#endif bool valid; ///< Task is currently active / valid }; diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 82a79312a0..5388f08f1d 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -52,6 +52,9 @@ /** time in ms between checks for work in work queues **/ #define CONFIG_SCHED_WORKPERIOD 10 +#define CONFIG_SCHED_INSTRUMENTATION 1 +#define CONFIG_MAX_TASKS 32 + #define px4_errx(x, ...) errx(x, __VA_ARGS__) #endif diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index f88deab466..2eff37f88e 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -84,5 +84,6 @@ __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __EXPORT void px4_show_devices(void); +__EXPORT const char * px4_get_device_names(unsigned int *handle); __END_DECLS From 694427e4ba01f978cf0a7b634bf144b553e02fda Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 19:38:59 -0700 Subject: [PATCH 111/258] Converted commander to use px4_posix calls Signed-off-by: Mark Charlebois --- .../commander/accelerometer_calibration.cpp | 21 ++++----- .../commander/airspeed_calibration.cpp | 45 ++++++++++--------- src/modules/commander/commander.cpp | 30 +++++++------ src/modules/commander/commander_helper.cpp | 39 ++++++++-------- src/modules/commander/gyro_calibration.cpp | 21 ++++----- src/modules/commander/mag_calibration.cpp | 26 ++++++----- src/modules/commander/module.mk | 10 ++--- 7 files changed, 99 insertions(+), 93 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 409c2ea003..4248ad282b 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -127,6 +127,7 @@ #include "calibration_routines.h" #include "commander_helper.h" +#include #include #include #include @@ -190,16 +191,16 @@ int do_accel_calibration(int mavlink_fd) for (unsigned s = 0; s < max_accel_sens; s++) { sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ - fd = open(str, 0); + fd = px4_open(str, 0); if (fd < 0) { continue; } - device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); + device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); - close(fd); + res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + px4_close(fd); if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); @@ -268,14 +269,14 @@ int do_accel_calibration(int mavlink_fd) } sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i); - fd = open(str, 0); + fd = px4_open(str, 0); if (fd < 0) { mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist"); res = ERROR; } else { - res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); - close(fd); + res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + px4_close(fd); } if (res != OK) { @@ -366,7 +367,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } - close(worker_data.subs[i]); + px4_close(worker_data.subs[i]); } } @@ -390,7 +391,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac */ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { - struct pollfd fds[max_accel_sens]; + px4_pollfd_struct_t fds[max_accel_sens]; for (unsigned i = 0; i < max_accel_sens; i++) { fds[i].fd = subs[i]; @@ -405,7 +406,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a /* use the first sensor to pace the readout, but do per-sensor counts */ while (counts[0] < samples_num) { - int poll_ret = poll(&fds[0], max_accel_sens, 1000); + int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000); if (poll_ret > 0) { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 773c62b3e1..dd78676af6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -40,6 +40,7 @@ #include "calibration_messages.h" #include "commander_helper.h" +#include #include #include #include @@ -90,17 +91,17 @@ int do_airspeed_calibration(int mavlink_fd) }; bool paramreset_successful = false; - int fd = open(AIRSPEED0_DEVICE_PATH, 0); + int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); if (fd > 0) { - if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + if (OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; } else { mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } - close(fd); + px4_close(fd); } if (!paramreset_successful) { @@ -110,14 +111,14 @@ int do_airspeed_calibration(int mavlink_fd) param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } @@ -130,11 +131,11 @@ int do_airspeed_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; - int poll_ret = poll(fds, 1, 1000); + int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); @@ -149,7 +150,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } @@ -158,19 +159,19 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { - int fd_scale = open(AIRSPEED0_DEVICE_PATH, 0); + int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { - if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); } - close(fd_scale); + px4_close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -180,13 +181,13 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } else { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -204,11 +205,11 @@ int do_airspeed_calibration(int mavlink_fd) while (calibration_counter < maxcount) { /* wait blocking for new data */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; fds[0].events = POLLIN; - int poll_ret = poll(fds, 1, 1000); + int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); @@ -228,14 +229,14 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); - close(diff_pres_sub); + px4_close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -243,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); - close(diff_pres_sub); + px4_close(diff_pres_sub); feedback_calibration_failed(mavlink_fd); return ERROR; @@ -256,14 +257,14 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { feedback_calibration_failed(mavlink_fd); - close(diff_pres_sub); + px4_close(diff_pres_sub); return ERROR; } @@ -271,6 +272,6 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); - close(diff_pres_sub); + px4_close(diff_pres_sub); return OK; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 31baedb2fc..97873e462c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -264,6 +264,7 @@ int commander_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -271,7 +272,7 @@ int commander_main(int argc, char *argv[]) if (thread_running) { warnx("commander already running"); /* this is not an error */ - exit(0); + return 0; } thread_should_exit = false; @@ -286,13 +287,14 @@ int commander_main(int argc, char *argv[]) usleep(200); } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (!thread_running) { - errx(0, "commander already stopped"); + warnx("commander already stopped"); + return 0; } thread_should_exit = true; @@ -304,18 +306,18 @@ int commander_main(int argc, char *argv[]) warnx("terminated."); - exit(0); + return 0; } /* commands needing the app to run below */ if (!thread_running) { warnx("\tcommander not started"); - exit(1); + return 1; } if (!strcmp(argv[1], "status")) { print_status(); - exit(0); + return 0; } if (!strcmp(argv[1], "calibrate")) { @@ -332,9 +334,10 @@ int commander_main(int argc, char *argv[]) } if (calib_ret) { - errx(1, "calibration failed, exiting."); + warnx("calibration failed, exiting."); + return 0; } else { - exit(0); + return 0; } } else { warnx("missing argument"); @@ -346,25 +349,25 @@ int commander_main(int argc, char *argv[]) int checkres = prearm_check(&status, mavlink_fd_local); close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); - exit(0); + return 0; } if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); close(mavlink_fd_local); - exit(0); + return 0; } if (!strcmp(argv[1], "disarm")) { int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); arm_disarm(false, mavlink_fd_local, "command line"); close(mavlink_fd_local); - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } void usage(const char *reason) @@ -374,7 +377,6 @@ void usage(const char *reason) } fprintf(stderr, "usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); - exit(1); } void print_status() @@ -935,7 +937,7 @@ int commander_thread_main(int argc, char *argv[]) if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); - exit(ERROR); + px4_task_exit(ERROR); } /* armed topic */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c06523ce44..e2680bf9ad 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -42,6 +42,7 @@ */ #include +#include #include #include #include @@ -124,10 +125,10 @@ int buzzer_init() tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; - buzzer = open(TONEALARM0_DEVICE_PATH, O_WRONLY); + buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { - warnx("Buzzer: open fail\n"); + warnx("Buzzer: px4_open fail\n"); return ERROR; } @@ -136,12 +137,12 @@ int buzzer_init() void buzzer_deinit() { - close(buzzer); + px4_close(buzzer); } void set_tune_override(int tune) { - ioctl(buzzer, TONE_SET_ALARM, tune); + px4_ioctl(buzzer, TONE_SET_ALARM, tune); } void set_tune(int tune) @@ -152,7 +153,7 @@ void set_tune(int tune) if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ if (tune != tune_current || new_tune_duration != 0) { - ioctl(buzzer, TONE_SET_ALARM, tune); + px4_ioctl(buzzer, TONE_SET_ALARM, tune); } tune_current = tune; @@ -230,22 +231,22 @@ int led_init() blink_msg_end = 0; /* first open normal LEDs */ - leds = open(LED0_DEVICE_PATH, 0); + leds = px4_open(LED0_DEVICE_PATH, 0); if (leds < 0) { - warnx("LED: open fail\n"); + warnx("LED: px4_open fail\n"); return ERROR; } /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ - (void)ioctl(leds, LED_ON, LED_BLUE); + (void)px4_ioctl(leds, LED_ON, LED_BLUE); /* switch blue off */ led_off(LED_BLUE); /* we consider the amber led mandatory */ - if (ioctl(leds, LED_ON, LED_AMBER)) { - warnx("Amber LED: ioctl fail\n"); + if (px4_ioctl(leds, LED_ON, LED_AMBER)) { + warnx("Amber LED: px4_ioctl fail\n"); return ERROR; } @@ -253,7 +254,7 @@ int led_init() led_off(LED_AMBER); /* then try RGB LEDs, this can fail on FMUv1*/ - rgbleds = open(RGBLED0_DEVICE_PATH, 0); + rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0); if (rgbleds < 0) { warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); @@ -265,11 +266,11 @@ int led_init() void led_deinit() { if (leds >= 0) { - close(leds); + px4_close(leds); } if (rgbleds >= 0) { - close(rgbleds); + px4_close(rgbleds); } } @@ -278,7 +279,7 @@ int led_toggle(int led) if (leds < 0) { return leds; } - return ioctl(leds, LED_TOGGLE, led); + return px4_ioctl(leds, LED_TOGGLE, led); } int led_on(int led) @@ -286,7 +287,7 @@ int led_on(int led) if (leds < 0) { return leds; } - return ioctl(leds, LED_ON, led); + return px4_ioctl(leds, LED_ON, led); } int led_off(int led) @@ -294,7 +295,7 @@ int led_off(int led) if (leds < 0) { return leds; } - return ioctl(leds, LED_OFF, led); + return px4_ioctl(leds, LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) @@ -303,7 +304,7 @@ void rgbled_set_color(rgbled_color_t color) if (rgbleds < 0) { return; } - ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) @@ -312,7 +313,7 @@ void rgbled_set_mode(rgbled_mode_t mode) if (rgbleds < 0) { return; } - ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) @@ -321,7 +322,7 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) if (rgbleds < 0) { return; } - ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index a2b0900ef6..b5d5bb23c7 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -41,6 +41,7 @@ #include "calibration_messages.h" #include "commander_helper.h" +#include #include #include #include @@ -96,16 +97,16 @@ int do_gyro_calibration(int mavlink_fd) sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { continue; } - device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); + device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - close(fd); + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + px4_close(fd); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); @@ -123,7 +124,7 @@ int do_gyro_calibration(int mavlink_fd) /* subscribe to gyro sensor topic */ int sub_sensor_gyro[max_gyros]; - struct pollfd fds[max_gyros]; + px4_pollfd_struct_t fds[max_gyros]; for (unsigned s = 0; s < max_gyros; s++) { sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); @@ -137,7 +138,7 @@ int do_gyro_calibration(int mavlink_fd) while (calibration_counter[0] < calibration_count) { /* wait blocking for new data */ - int poll_ret = poll(&fds[0], max_gyros, 1000); + int poll_ret = px4_poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -175,7 +176,7 @@ int do_gyro_calibration(int mavlink_fd) } for (unsigned s = 0; s < max_gyros; s++) { - close(sub_sensor_gyro[s]); + px4_close(sub_sensor_gyro[s]); gyro_scale[s].x_offset /= calibration_counter[s]; gyro_scale[s].y_offset /= calibration_counter[s]; @@ -225,15 +226,15 @@ int do_gyro_calibration(int mavlink_fd) /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { failed = true; continue; } - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); - close(fd); + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); + px4_close(fd); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 198acb0272..49ccbf3ef1 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -42,7 +42,9 @@ #include "calibration_routines.h" #include "calibration_messages.h" +#include #include +#include #include #include #include @@ -119,16 +121,16 @@ int do_mag_calibration(int mavlink_fd) // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); - int fd = open(str, O_RDONLY); + int fd = px4_open(str, O_RDONLY); if (fd < 0) { continue; } // Get device id for this mag - device_ids[cur_mag] = ioctl(fd, DEVIOCGDEVICEID, 0); + device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); // Reset mag scale - result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); + result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (result != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag); @@ -136,7 +138,7 @@ int do_mag_calibration(int mavlink_fd) if (result == OK) { /* calibrate range */ - result = ioctl(fd, MAGIOCCALIBRATE, fd); + result = px4_ioctl(fd, MAGIOCCALIBRATE, fd); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag); @@ -145,7 +147,7 @@ int do_mag_calibration(int mavlink_fd) } } - close(fd); + px4_close(fd); } if (result == OK) { @@ -192,7 +194,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) calibration_counter_side < worker_data->calibration_points_perside) { // Wait clocking for new data on all mags - struct pollfd fds[max_mags]; + px4_pollfd_struct_t fds[max_mags]; size_t fd_count = 0; for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { @@ -201,7 +203,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data) fd_count++; } } - int poll_ret = poll(fds, fd_count, 1000); + int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { for (size_t cur_mag=0; cur_mag= 0) { - close(worker_data.sub_mag[cur_mag]); + px4_close(worker_data.sub_mag[cur_mag]); } } @@ -398,14 +400,14 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); - fd_mag = open(str, 0); + fd_mag = px4_open(str, 0); if (fd_mag < 0) { mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag); result = ERROR; } if (result == OK) { - result = ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale); + result = px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag); result = ERROR; @@ -417,7 +419,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; - result = ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale); + result = px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag); result = ERROR; @@ -426,7 +428,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) // Mag device no longer needed if (fd_mag >= 0) { - close(fd_mag); + px4_close(fd_mag); } if (result == OK) { diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index c9de1ffa2f..4ef361a826 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -40,6 +40,8 @@ SRCS = commander.cpp \ commander_params.c \ commander_helper.cpp \ calibration_routines.cpp \ + mag_calibration.cpp \ + gyro_calibration.cpp \ baro_calibration.cpp \ accelerometer_calibration.cpp \ rc_calibration.cpp \ @@ -48,13 +50,9 @@ SRCS = commander.cpp \ ifdef ($(PX4_TARGET_OS),nuttx) SRCS += - state_machine_helper.cpp \ - gyro_calibration.cpp \ - mag_calibration.cpp + state_machine_helper.cpp else -SRCS += state_machine_helper_linux.cpp \ - gyro_calibration_linux.cpp \ - mag_calibration_linux.cpp +SRCS += state_machine_helper_linux.cpp endif MODULE_STACKSIZE = 5000 From d2f0572ce65ac0b120c4c6f5b680b3aa2dfa7e3e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 20:34:45 -0700 Subject: [PATCH 112/258] Linux: added builtins to show devices and topics list_devices will list virtual devices starting with "/dev/". list_topics will list topics ("/obj/") Signed-off-by: Mark Charlebois --- Tools/linux_apps.py | 8 + src/drivers/device/vdev.cpp | 23 +- src/drivers/device/vdev.h | 2 + src/drivers/device/vdev_posix.cpp | 10 + .../commander/gyro_calibration_linux.cpp | 274 ---------- .../commander/mag_calibration_linux.cpp | 470 ------------------ src/platforms/px4_posix.h | 2 + 7 files changed, 43 insertions(+), 746 deletions(-) delete mode 100644 src/modules/commander/gyro_calibration_linux.cpp delete mode 100644 src/modules/commander/mag_calibration_linux.cpp diff --git a/Tools/linux_apps.py b/Tools/linux_apps.py index b45d3f8888..f48977c49a 100755 --- a/Tools/linux_apps.py +++ b/Tools/linux_apps.py @@ -63,6 +63,7 @@ print """ static int shutdown_main(int argc, char *argv[]); static int list_tasks_main(int argc, char *argv[]); static int list_devices_main(int argc, char *argv[]); +static int list_topics_main(int argc, char *argv[]); } @@ -78,6 +79,7 @@ for app in apps: print '\tapps["shutdown"] = shutdown_main;' print '\tapps["list_tasks"] = list_tasks_main;' print '\tapps["list_devices"] = list_devices_main;' +print '\tapps["list_topics"] = list_topics_main;' print """ return apps; } @@ -108,5 +110,11 @@ static int list_devices_main(int argc, char *argv[]) px4_show_devices(); return 0; } + +static int list_topics_main(int argc, char *argv[]) +{ + px4_show_topics(); + return 0; +} """ diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index e5004b6841..0d02d2b420 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -461,16 +461,35 @@ void VDev::showDevices() int i=0; printf("Devices:\n"); for (; iname, "/dev/", 5) == 0) { printf(" %s\n", devmap[i]->name); } } } +void VDev::showTopics() +{ + int i=0; + printf("Devices:\n"); + for (; iname, "/obj/", 5) == 0) { + printf(" %s\n", devmap[i]->name); + } + } +} + +const char *VDev::topicList(unsigned int *next) +{ + for (;*nextname, "/obj/", 5) == 0) + return devmap[(*next)++]->name; + return NULL; +} + const char *VDev::devList(unsigned int *next) { for (;*nextname, "/dev/", 5) == 0) return devmap[(*next)++]->name; return NULL; } diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 1ce48285f7..6448b8aa98 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -334,7 +334,9 @@ public: static VDev *getDev(const char *path); static void showDevices(void); + static void showTopics(void); static const char *devList(unsigned int *next); + static const char *topicList(unsigned int *next); protected: diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 50df5535d2..31f141267f 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -274,10 +274,20 @@ void px4_show_devices() VDev::showDevices(); } +void px4_show_topics() +{ + VDev::showTopics(); +} + const char * px4_get_device_names(unsigned int *handle) { return VDev::devList(handle); } +const char * px4_get_topic_names(unsigned int *handle) +{ + return VDev::topicList(handle); +} + } diff --git a/src/modules/commander/gyro_calibration_linux.cpp b/src/modules/commander/gyro_calibration_linux.cpp deleted file mode 100644 index f490487f3b..0000000000 --- a/src/modules/commander/gyro_calibration_linux.cpp +++ /dev/null @@ -1,274 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file gyro_calibration.cpp - * - * Gyroscope calibration routine - */ - -#include "gyro_calibration.h" -#include "calibration_messages.h" -#include "commander_helper.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -static const char *sensor_name = "gyro"; - -int do_gyro_calibration(int mavlink_fd) -{ - const unsigned max_gyros = 3; - - int32_t device_id[3]; - mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "HOLD STILL"); - - /* wait for the user to respond */ - sleep(2); - - struct gyro_scale gyro_scale_zero = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - struct gyro_scale gyro_scale[max_gyros] = {}; - - int res = OK; - - /* store board ID */ - uint32_t mcu_id[3]; - mcu_unique_id(&mcu_id[0]); - - /* store last 32bit number - not unique, but unique in a given set */ - (void)param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]); - - char str[30]; - - for (unsigned s = 0; s < max_gyros; s++) { - - /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); - - sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - /* reset all offsets to zero and all scales to one */ - int fd = px4_open(str, 0); - - if (fd < 0) { - continue; - } - - device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - px4_close(fd); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s); - } - } - - unsigned calibration_counter[max_gyros] = { 0 }; - const unsigned calibration_count = 5000; - - struct gyro_report gyro_report_0 = {}; - - if (res == OK) { - /* determine gyro mean values */ - unsigned poll_errcount = 0; - - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro[max_gyros]; - px4_pollfd_struct_t fds[max_gyros]; - - for (unsigned s = 0; s < max_gyros; s++) { - sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); - fds[s].fd = sub_sensor_gyro[s]; - fds[s].events = POLLIN; - } - - struct gyro_report gyro_report; - - /* use first gyro to pace, but count correctly per-gyro for statistics */ - while (calibration_counter[0] < calibration_count) { - /* wait blocking for new data */ - - int poll_ret = px4_poll(&fds[0], max_gyros, 1000); - - if (poll_ret > 0) { - - for (unsigned s = 0; s < max_gyros; s++) { - bool changed; - orb_check(sub_sensor_gyro[s], &changed); - - if (changed) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); - - if (s == 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); - } - - gyro_scale[s].x_offset += gyro_report.x; - gyro_scale[s].y_offset += gyro_report.y; - gyro_scale[s].z_offset += gyro_report.z; - calibration_counter[s]++; - } - - if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count); - } - } - - } else { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } - } - - for (unsigned s = 0; s < max_gyros; s++) { - px4_close(sub_sensor_gyro[s]); - - gyro_scale[s].x_offset /= calibration_counter[s]; - gyro_scale[s].y_offset /= calibration_counter[s]; - gyro_scale[s].z_offset /= calibration_counter[s]; - } - } - - if (res == OK) { - /* check offsets */ - float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; - float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; - float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; - - /* maximum allowable calibration error in radians */ - const float maxoff = 0.002f; - - if (!isfinite(gyro_scale[0].x_offset) || - !isfinite(gyro_scale[0].y_offset) || - !isfinite(gyro_scale[0].z_offset) || - fabsf(xdiff) > maxoff || - fabsf(ydiff) > maxoff || - fabsf(zdiff) > maxoff) { - mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration"); - res = ERROR; - } - } - - if (res == OK) { - /* set offset parameters to new values */ - bool failed = false; - - for (unsigned s = 0; s < max_gyros; s++) { - - /* if any reasonable amount of data is missing, skip */ - if (calibration_counter[s] < calibration_count / 2) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].x_offset))); - (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].y_offset))); - (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - failed |= (OK != param_set(param_find(str), &(gyro_scale[s].z_offset))); - (void)sprintf(str, "CAL_GYRO%u_ID", s); - failed |= (OK != param_set(param_find(str), &(device_id[s]))); - - /* apply new scaling and offsets */ - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); - - if (fd < 0) { - failed = true; - continue; - } - - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); - px4_close(fd); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); - } - } - - if (failed) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); - res = ERROR; - } - } - - if (res == OK) { - /* auto-save to EEPROM */ - res = param_save_default(); - - if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); - } - } - - if (res == OK) { - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - - } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - } - - return res; -} diff --git a/src/modules/commander/mag_calibration_linux.cpp b/src/modules/commander/mag_calibration_linux.cpp deleted file mode 100644 index 49ccbf3ef1..0000000000 --- a/src/modules/commander/mag_calibration_linux.cpp +++ /dev/null @@ -1,470 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file mag_calibration.cpp - * - * Magnetometer calibration routine - */ - -#include "mag_calibration.h" -#include "commander_helper.h" -#include "calibration_routines.h" -#include "calibration_messages.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -static const char *sensor_name = "mag"; -static const unsigned max_mags = 3; - -int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); -int mag_calibration_worker(detect_orientation_return orientation, void* worker_data); - -/// Data passed to calibration worker routine -typedef struct { - int mavlink_fd; - unsigned done_count; - int sub_mag[max_mags]; - unsigned int calibration_points_perside; - unsigned int calibration_interval_perside_seconds; - uint64_t calibration_interval_perside_useconds; - unsigned int calibration_counter_total; - bool side_data_collected[detect_orientation_side_count]; - float* x[max_mags]; - float* y[max_mags]; - float* z[max_mags]; -} mag_worker_data_t; - - -int do_mag_calibration(int mavlink_fd) -{ - mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - int result = OK; - - // Determine which mags are available and reset each - - int32_t device_ids[max_mags]; - char str[30]; - - for (size_t i=0; imavlink_fd, "Rotate vehicle around the detected orientation"); - mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds); - sleep(2); - - uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; - unsigned poll_errcount = 0; - - calibration_counter_side = 0; - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter_side < worker_data->calibration_points_perside) { - - // Wait clocking for new data on all mags - px4_pollfd_struct_t fds[max_mags]; - size_t fd_count = 0; - for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { - fds[fd_count].fd = worker_data->sub_mag[cur_mag]; - fds[fd_count].events = POLLIN; - fd_count++; - } - } - int poll_ret = px4_poll(fds, fd_count, 1000); - - if (poll_ret > 0) { - for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { - struct mag_report mag; - - orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); - - worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; - worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; - worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; - - } - } - - worker_data->calibration_counter_total++; - calibration_counter_side++; - - // Progress indicator for side - mavlink_and_console_log_info(worker_data->mavlink_fd, - "%s %s side calibration: progress <%u>", - sensor_name, - detect_orientation_str(orientation), - (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); - } else { - poll_errcount++; - } - - if (poll_errcount > worker_data->calibration_points_perside * 3) { - result = ERROR; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); - break; - } - } - - // Mark the opposite side as collected as well. No need to collect opposite side since it - // would generate similar points. - detect_orientation_return alternateOrientation = orientation; - switch (orientation) { - case DETECT_ORIENTATION_TAIL_DOWN: - alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN; - break; - case DETECT_ORIENTATION_NOSE_DOWN: - alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN; - break; - case DETECT_ORIENTATION_LEFT: - alternateOrientation = DETECT_ORIENTATION_RIGHT; - break; - case DETECT_ORIENTATION_RIGHT: - alternateOrientation = DETECT_ORIENTATION_LEFT; - break; - case DETECT_ORIENTATION_UPSIDE_DOWN: - alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP; - break; - case DETECT_ORIENTATION_RIGHTSIDE_UP: - alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN; - break; - case DETECT_ORIENTATION_ERROR: - warnx("Invalid orientation in mag_calibration_worker"); - break; - } - worker_data->side_data_collected[alternateOrientation] = true; - mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation)); - - worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count); - - return result; -} - -int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) -{ - int result = OK; - - mag_worker_data_t worker_data; - - worker_data.mavlink_fd = mavlink_fd; - worker_data.done_count = 0; - worker_data.calibration_counter_total = 0; - worker_data.calibration_points_perside = 80; - worker_data.calibration_interval_perside_seconds = 20; - worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; - - // Initialize to collect all sides - for (size_t cur_side=0; cur_side<6; cur_side++) { - worker_data.side_data_collected[cur_side] = false; - } - - for (size_t cur_mag=0; cur_mag(malloc(sizeof(float) * calibration_points_maxcount)); - worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); - worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); - if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); - result = ERROR; - } - } - - - // Setup subscriptions to mag sensors - if (result == OK) { - for (unsigned cur_mag=0; cur_mag= 0) { - px4_close(worker_data.sub_mag[cur_mag]); - } - } - - // Calculate calibration values for each mag - - - float sphere_x[max_mags]; - float sphere_y[max_mags]; - float sphere_z[max_mags]; - float sphere_radius[max_mags]; - - // Sphere fit the data to get calibration values - if (result == OK) { - for (unsigned cur_mag=0; cur_mag= 0) { - px4_close(fd_mag); - } - - if (result == OK) { - bool failed = false; - - /* set parameters */ - (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); - (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); - (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); - (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); - (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); - (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); - (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); - - if (failed) { - mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag); - result = ERROR; - } else { - mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga", - cur_mag, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f", - cur_mag, - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); - } - } - } - } - } - - return result; -} diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 2eff37f88e..ed4edbb24e 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -84,6 +84,8 @@ __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __EXPORT void px4_show_devices(void); +__EXPORT void px4_show_topics(void); __EXPORT const char * px4_get_device_names(unsigned int *handle); +__EXPORT const char * px4_get_topic_names(unsigned int *handle); __END_DECLS From 89a1799e618a548421b6fe744e307f4f833596b2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 20:37:37 -0700 Subject: [PATCH 113/258] Linux: Changed /vdev/... back to to /dev/ Some virtual devices were mapped to /vdev. Putting them back to /dev. Signed-off-by: Mark Charlebois --- src/drivers/ms5611/ms5611_i2c.cpp | 2 +- src/drivers/ms5611/ms5611_sim.cpp | 2 +- src/platforms/linux/drivers/accelsim/accelsim.cpp | 6 +++--- src/platforms/linux/drivers/barosim/baro.cpp | 2 +- src/platforms/linux/drivers/barosim/baro_sim.cpp | 2 +- src/platforms/linux/drivers/gyrosim/gyrosim.cpp | 3 ++- 6 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 257e42a2de..730fd9b968 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -120,7 +120,7 @@ MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : #ifdef __PX4_NUTTX nullptr, bus, 0, 400000 #else -"/vdev/MS5611_I2C", bus, 0 +"/dev/MS5611_I2C", bus, 0 #endif ), _prom(prom) diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp index b99e35b676..ab7656b9c3 100644 --- a/src/drivers/ms5611/ms5611_sim.cpp +++ b/src/drivers/ms5611/ms5611_sim.cpp @@ -100,7 +100,7 @@ MS5611_sim_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_SIM::MS5611_SIM(uint8_t bus, ms5611::prom_u &prom) : - SIM("MS5611_SIM", "/vdev/MS5611_SIM", bus, 0), + SIM("MS5611_SIM", "/dev/MS5611_SIM", bus, 0), _prom(prom) { } diff --git a/src/platforms/linux/drivers/accelsim/accelsim.cpp b/src/platforms/linux/drivers/accelsim/accelsim.cpp index f2d30a9e06..97314ab846 100644 --- a/src/platforms/linux/drivers/accelsim/accelsim.cpp +++ b/src/platforms/linux/drivers/accelsim/accelsim.cpp @@ -74,9 +74,9 @@ #endif static const int ERROR = -1; -#define ACCELSIM_DEVICE_PATH_ACCEL "/vdev/sim_accel" -#define ACCELSIM_DEVICE_PATH_ACCEL_EXT "/vdev/sim_accel_ext" -#define ACCELSIM_DEVICE_PATH_MAG "/vdev/sim_mag" +#define ACCELSIM_DEVICE_PATH_ACCEL "/dev/sim_accel" +#define ACCELSIM_DEVICE_PATH_ACCEL_EXT "/dev/sim_accel_ext" +#define ACCELSIM_DEVICE_PATH_MAG "/dev/sim_mag" #define ADDR_WHO_AM_I 0x0F diff --git a/src/platforms/linux/drivers/barosim/baro.cpp b/src/platforms/linux/drivers/barosim/baro.cpp index 05040c0909..8398d18c58 100644 --- a/src/platforms/linux/drivers/barosim/baro.cpp +++ b/src/platforms/linux/drivers/barosim/baro.cpp @@ -788,7 +788,7 @@ struct barosim_bus_option { uint8_t busnum; BAROSIM *dev; } bus_options[] = { - { BAROSIM_BUS_SIM_EXTERNAL, "/vdev/baro_sim", &BAROSIM_sim_interface, PX4_SIM_BUS_TEST, NULL }, + { BAROSIM_BUS_SIM_EXTERNAL, "/dev/baro_sim", &BAROSIM_sim_interface, PX4_SIM_BUS_TEST, NULL }, }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) diff --git a/src/platforms/linux/drivers/barosim/baro_sim.cpp b/src/platforms/linux/drivers/barosim/baro_sim.cpp index a9374137fe..2e628f571c 100644 --- a/src/platforms/linux/drivers/barosim/baro_sim.cpp +++ b/src/platforms/linux/drivers/barosim/baro_sim.cpp @@ -101,7 +101,7 @@ BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) } BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : - SIM("BARO_SIM", "/vdev/BARO_SIM", bus, 0), + SIM("BARO_SIM", "/dev/BARO_SIM", bus, 0), _prom(prom) { } diff --git a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp index d117cab9a1..c39ebeac0f 100644 --- a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/linux/drivers/gyrosim/gyrosim.cpp @@ -675,7 +675,8 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } else { printf("Writing %u bytes to register %u\n", len-1, reg); - memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + if (recv) + memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); } return PX4_OK; } From dc52bb77039171a5539662660f4ad2c64385d58c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 15 Apr 2015 20:39:31 -0700 Subject: [PATCH 114/258] Linux: Added support for drivers/rgbled Signed-off-by: Mark Charlebois --- linux-configs/linuxtest/init/rc.S | 4 + makefiles/linux/config_linux_default.mk | 1 + src/drivers/rgbled/module.mk | 5 + src/drivers/rgbled/rgbled_linux.cpp | 739 +++++++++++++++++++++ src/platforms/linux/include/board_config.h | 2 + 5 files changed, 751 insertions(+) create mode 100644 src/drivers/rgbled/rgbled_linux.cpp diff --git a/linux-configs/linuxtest/init/rc.S b/linux-configs/linuxtest/init/rc.S index dc8d3c3a0f..f10f57ec37 100644 --- a/linux-configs/linuxtest/init/rc.S +++ b/linux-configs/linuxtest/init/rc.S @@ -1,8 +1,12 @@ +simulator start uorb start barosim start adcsim start accelsim start gyrosim start +rgbled start mavlink start sensors start +hil start +commander start list_devices diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index e40a58d46a..5dfd487e77 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -13,6 +13,7 @@ MODULES += drivers/device MODULES += drivers/blinkm MODULES += drivers/hil +MODULES += drivers/rgbled MODULES += modules/sensors #MODULES += drivers/ms5611 diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index c287e35f31..5ffa628c0d 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -3,6 +3,11 @@ # MODULE_COMMAND = rgbled + +ifdef ($(PX4_TARGET_OS),nuttx) SRCS = rgbled.cpp +else +SRCS = rgbled_linux.cpp +endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/rgbled_linux.cpp b/src/drivers/rgbled/rgbled_linux.cpp new file mode 100644 index 0000000000..4344b67929 --- /dev/null +++ b/src/drivers/rgbled/rgbled_linux.cpp @@ -0,0 +1,739 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file rgbled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +#include +#include +#include + +#include + +#include + +#define RGBLED_ONTIME 120 +#define RGBLED_OFFTIME 120 + +#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +class RGBLED : public device::I2C +{ +public: + RGBLED(int bus, int rgbled); + virtual ~RGBLED(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + +private: + work_s _work; + + rgbled_mode_t _mode; + rgbled_pattern_t _pattern; + + uint8_t _r; + uint8_t _g; + uint8_t _b; + float _brightness; + + bool _running; + int _led_interval; + bool _should_run; + int _counter; + + void set_color(rgbled_color_t ledcolor); + void set_mode(rgbled_mode_t mode); + void set_pattern(rgbled_pattern_t *pattern); + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_rgb(); + int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); +}; + +/* for now, we only support one RGBLED */ +namespace +{ +RGBLED *g_rgbled = nullptr; +} + +void rgbled_usage(); + +extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); + +RGBLED::RGBLED(int bus, int rgbled) : + I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled +#ifdef __PX4_NUTTX +,100000 /* maximum speed supported */ +#endif +), + _mode(RGBLED_MODE_OFF), + _r(0), + _g(0), + _b(0), + _brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); + memset(&_pattern, 0, sizeof(_pattern)); +} + +RGBLED::~RGBLED() +{ +} + +int +RGBLED::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + /* switch off LED on start */ + send_led_enable(false); + send_led_rgb(); + + return OK; +} + +int +RGBLED::probe() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLED is on the bus. + */ + + unsigned prevretries = _retries; + _retries = 4; + + if ((ret=get(on, powersave, r, g, b)) != OK || + (ret=send_led_enable(false) != OK) || + (ret=send_led_enable(false) != OK)) { + return ret; + } + + _retries = prevretries; + + return ret; +} + +int +RGBLED::info() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + ret = get(on, powersave, r, g, b); + + if (ret == OK) { + /* we don't care about power-save mode */ + log("state: %s", on ? "ON" : "OFF"); + log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + + } else { + warnx("failed to read led"); + } + + return ret; +} + +int +RGBLED::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case RGBLED_SET_RGB: + /* set the specified color */ + _r = ((rgbled_rgbset_t *) arg)->red; + _g = ((rgbled_rgbset_t *) arg)->green; + _b = ((rgbled_rgbset_t *) arg)->blue; + send_led_rgb(); + return OK; + + case RGBLED_SET_COLOR: + /* set the specified color name */ + set_color((rgbled_color_t)arg); + send_led_rgb(); + return OK; + + case RGBLED_SET_MODE: + /* set the specified mode */ + set_mode((rgbled_mode_t)arg); + return OK; + + case RGBLED_SET_PATTERN: + /* set a special pattern */ + set_pattern((rgbled_pattern_t *)arg); + return OK; + + default: + /* see if the parent class can make any use of it */ + ret = VDev::ioctl(handlep, cmd, arg); + break; + } + + return ret; +} + + +void +RGBLED::led_trampoline(void *arg) +{ + RGBLED *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +RGBLED::led() +{ + if (!_should_run) { + _running = false; + return; + } + + switch (_mode) { + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if (_counter >= 2) + _counter = 0; + + send_led_enable(_counter == 0); + + break; + + case RGBLED_MODE_BREATHE: + + if (_counter >= 62) + _counter = 0; + + int n; + + if (_counter < 32) { + n = _counter; + + } else { + n = 62 - _counter; + } + + _brightness = n * n / (31.0f * 31.0f); + send_led_rgb(); + break; + + case RGBLED_MODE_PATTERN: + + /* don't run out of the pattern array and stop if the next frame is 0 */ + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + _counter = 0; + + set_color(_pattern.color[_counter]); + send_led_rgb(); + _led_interval = _pattern.duration[_counter]; + break; + + default: + break; + } + + _counter++; + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); +} + +/** + * Parse color constant and set _r _g _b values + */ +void +RGBLED::set_color(rgbled_color_t color) +{ + switch (color) { + case RGBLED_COLOR_OFF: + _r = 0; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_RED: + _r = 255; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_YELLOW: + _r = 255; + _g = 200; + _b = 0; + break; + + case RGBLED_COLOR_PURPLE: + _r = 255; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_GREEN: + _r = 0; + _g = 255; + _b = 0; + break; + + case RGBLED_COLOR_BLUE: + _r = 0; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_WHITE: + _r = 255; + _g = 255; + _b = 255; + break; + + case RGBLED_COLOR_AMBER: + _r = 255; + _g = 80; + _b = 0; + break; + + case RGBLED_COLOR_DIM_RED: + _r = 90; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_DIM_YELLOW: + _r = 80; + _g = 30; + _b = 0; + break; + + case RGBLED_COLOR_DIM_PURPLE: + _r = 45; + _g = 0; + _b = 45; + break; + + case RGBLED_COLOR_DIM_GREEN: + _r = 0; + _g = 90; + _b = 0; + break; + + case RGBLED_COLOR_DIM_BLUE: + _r = 0; + _g = 0; + _b = 90; + break; + + case RGBLED_COLOR_DIM_WHITE: + _r = 30; + _g = 30; + _b = 30; + break; + + case RGBLED_COLOR_DIM_AMBER: + _r = 80; + _g = 20; + _b = 0; + break; + + default: + warnx("color unknown"); + break; + } +} + +/** + * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) + */ +void +RGBLED::set_mode(rgbled_mode_t mode) +{ + if (mode != _mode) { + _mode = mode; + + switch (mode) { + case RGBLED_MODE_OFF: + _should_run = false; + send_led_enable(false); + break; + + case RGBLED_MODE_ON: + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); + break; + + case RGBLED_MODE_BLINK_SLOW: + _should_run = true; + _counter = 0; + _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_NORMAL: + _should_run = true; + _counter = 0; + _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_FAST: + _should_run = true; + _counter = 0; + _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BREATHE: + _should_run = true; + _counter = 0; + _led_interval = 25; + send_led_enable(true); + break; + + case RGBLED_MODE_PATTERN: + _should_run = true; + _counter = 0; + _brightness = 1.0f; + send_led_enable(true); + break; + + default: + warnx("mode unknown"); + break; + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } + + } +} + +/** + * Set pattern for PATTERN mode, but don't change current mode + */ +void +RGBLED::set_pattern(rgbled_pattern_t *pattern) +{ + memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); +} + +/** + * Sent ENABLE flag to LED driver + */ +int +RGBLED::send_led_enable(bool enable) +{ + uint8_t settings_byte = 0; + + if (enable) + settings_byte |= SETTING_ENABLE; + + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLED::send_led_rgb() +{ + /* To scale from 0..255 -> 0..15 shift right by 4 bits */ + const uint8_t msg[6] = { + SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), + SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), + SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) + }; + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + uint8_t result[2]; + int ret; + + ret = transfer(nullptr, 0, &result[0], 2); + + if (ret == OK) { + on = result[0] & SETTING_ENABLE; + powersave = !(result[0] & SETTING_NOT_POWERSAVE); + /* XXX check, looks wrong */ + r = (result[0] & 0x0f) << 4; + g = (result[1] & 0xf0); + b = (result[1] & 0x0f) << 4; + } + + return ret; +} + +void +rgbled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", ADDR); +} + +int +rgbled_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int rgbledadr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(myoptarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(myoptarg, NULL, 0); + break; + + default: + rgbled_usage(); + return 1; + } + } + + if (myoptind >= argc) { + rgbled_usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) { + warnx("already started"); + return 1; + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); + + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + + if (g_rgbled == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + warnx("init failed"); + return 1; + } + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_rgbled == nullptr) { + g_rgbled = new RGBLED(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) { + warnx("new failed"); + return 1; + } + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + warnx("init failed"); + return 1; + } + } + + return 0; + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + warnx("not started"); + rgbled_usage(); + return 1; + } + + if (!strcmp(verb, "test")) { + fd = px4_open(RGBLED0_DEVICE_PATH, 0); + + if (fd == -1) { + warnx( "Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, + {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern + }; + + ret = px4_ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); + + px4_close(fd); + return ret; + } + + if (!strcmp(verb, "info")) { + g_rgbled->info(); + return 0; + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + fd = px4_open(RGBLED0_DEVICE_PATH, 0); + + if (fd == -1) { + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + px4_close(fd); + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + return 0; + } + return ret; + } + + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + warnx("Usage: rgbled rgb "); + return 1; + } + + fd = px4_open(RGBLED0_DEVICE_PATH, 0); + + if (fd == -1) { + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; + } + + rgbled_rgbset_t v; + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); + ret = px4_ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + px4_close(fd); + return ret; + } + + rgbled_usage(); + return 1; +} diff --git a/src/platforms/linux/include/board_config.h b/src/platforms/linux/include/board_config.h index afbdd98cd2..a03b8c3e20 100644 --- a/src/platforms/linux/include/board_config.h +++ b/src/platforms/linux/include/board_config.h @@ -7,3 +7,5 @@ #define PX4_SIM_BUS_TEST 2 #define PX4_I2C_BUS_EXPANSION 3 #define PX4_I2C_BUS_LED 3 + +#define PX4_I2C_OBDEV_LED 0x55 From e9c2e0877096908e85edea70bdbc0dd7b0e54c86 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 11:21:18 -0700 Subject: [PATCH 115/258] Added initial QuRT support Added the basic files to start building for QuRT Signed-off-by: Mark Charlebois --- makefiles/firmware.mk | 3 + makefiles/firmware_qurt.mk | 56 ++++ makefiles/qurt.mk | 40 +++ makefiles/qurt_elf.mk | 73 +++++ makefiles/setup.mk | 8 +- makefiles/toolchain_hexagon.mk | 304 ++++++++++++++++++ src/platforms/px4_defines.h | 4 +- src/platforms/px4_includes.h | 4 +- src/platforms/px4_tasks.h | 2 +- src/platforms/qurt/include/arch/board/board.h | 0 src/platforms/qurt/include/board_config.h | 11 + src/platforms/qurt/include/crc32.h | 83 +++++ src/platforms/qurt/include/queue.h | 135 ++++++++ src/platforms/qurt/main.cpp | 104 ++++++ .../qurt/tests/hello/hello_example.cpp | 61 ++++ .../qurt/tests/hello/hello_example.h | 53 +++ src/platforms/qurt/tests/hello/hello_main.cpp | 55 ++++ .../qurt/tests/hello/hello_start_linux.cpp | 100 ++++++ src/platforms/qurt/tests/hello/module.mk | 43 +++ 19 files changed, 1129 insertions(+), 10 deletions(-) create mode 100644 makefiles/firmware_qurt.mk create mode 100644 makefiles/qurt.mk create mode 100644 makefiles/qurt_elf.mk create mode 100644 makefiles/toolchain_hexagon.mk create mode 100644 src/platforms/qurt/include/arch/board/board.h create mode 100644 src/platforms/qurt/include/board_config.h create mode 100644 src/platforms/qurt/include/crc32.h create mode 100644 src/platforms/qurt/include/queue.h create mode 100644 src/platforms/qurt/main.cpp create mode 100644 src/platforms/qurt/tests/hello/hello_example.cpp create mode 100644 src/platforms/qurt/tests/hello/hello_example.h create mode 100644 src/platforms/qurt/tests/hello/hello_main.cpp create mode 100644 src/platforms/qurt/tests/hello/hello_start_linux.cpp create mode 100644 src/platforms/qurt/tests/hello/module.mk diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 814bf8b8d6..5f5447da6b 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -363,6 +363,9 @@ endif ifeq ($(PX4_TARGET_OS),linux) include $(MK_DIR)/linux_elf.mk endif +ifeq ($(PX4_TARGET_OS),qurt) +include $(MK_DIR)/qurt_elf.mk +endif # # DEP_INCLUDES is defined by the toolchain include in terms of $(OBJS) diff --git a/makefiles/firmware_qurt.mk b/makefiles/firmware_qurt.mk new file mode 100644 index 0000000000..6acec4e733 --- /dev/null +++ b/makefiles/firmware_qurt.mk @@ -0,0 +1,56 @@ +# +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Built products +# +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) + +all: $(FIRMWARES) + +# +# Generate FIRMWARES. +# +.PHONY: $(FIRMWARES) +$(BUILD_DIR)%.build/firmware.a: config = $(patsubst $(BUILD_DIR)%.build/firmware.a,%,$@) +$(BUILD_DIR)%.build/firmware.a: work_dir = $(BUILD_DIR)$(config).build/ +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: generateuorbtopicheaders + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ + -f $(PX4_MK_DIR)firmware.mk \ + CONFIG=$(config) \ + WORK_DIR=$(work_dir) \ + $(FIRMWARE_GOAL) + + diff --git a/makefiles/qurt.mk b/makefiles/qurt.mk new file mode 100644 index 0000000000..919dcc4aba --- /dev/null +++ b/makefiles/qurt.mk @@ -0,0 +1,40 @@ +# +# Copyright (C) 2015 Mark Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Rules and definitions related to handling the Linux specific impl when +# building firmware. +# + +MODULES += \ + platforms/common \ + platforms/linux/px4_layer + diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk new file mode 100644 index 0000000000..701d731ef3 --- /dev/null +++ b/makefiles/qurt_elf.mk @@ -0,0 +1,73 @@ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Makefile for PX4 Linux based firmware images. +# + +################################################################################ +# Build rules +################################################################################ + +# +# What we're going to build. +# +PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a +PRODUCT_SHARED_PRELINK = $(WORK_DIR)firmware.o + +.PHONY: firmware +firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)mainapp + +# +# Built product rules +# + +$(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) + $(call PRELINKF,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS)) + +$(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) + $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) + +MAIN = $(PX4_BASE)/src/platforms/linux/main.cpp +$(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) + $(PX4_BASE)/Tools/linux_apps.py > apps.h + $(call LINK,$@, -I. $(MAIN) $(PRODUCT_SHARED_LIB)) + +# +# Utility rules +# + +.PHONY: clean +clean: $(MODULE_CLEANS) + @$(ECHO) %% cleaning + $(Q) $(REMOVE) $(PRODUCT_ELF) + $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) + diff --git a/makefiles/setup.mk b/makefiles/setup.mk index d8ba1304c9..944b2018d8 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,11 +33,9 @@ # Path and tool setup # -ifdef ($(PX4_TARGET_OS),nuttx) -export PX4_TARGET_OS = nuttx -else -export PX4_TARGET_OS = linux -endif +#export PX4_TARGET_OS = nuttx +#export PX4_TARGET_OS = linux +export PX4_TARGET_OS ?= qurt # # Some useful paths. diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk new file mode 100644 index 0000000000..6b3027684e --- /dev/null +++ b/makefiles/toolchain_hexagon.mk @@ -0,0 +1,304 @@ +# +# Copyright (C) 2015 Mark Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# + +# +# Definitions for a generic GNU ARM-EABI toolchain +# + +#$(info TOOLCHAIN gnu-arm-eabi) + +# Toolchain commands. Normally only used inside this file. +# +HEXAGON_TOOLS_ROOT = /opt/6.4.05 +#V_ARCH = v4 # Set for APQ8064 +V_ARCH = v5 # Set for APQ8074 +CROSSDEV = hexagon- +HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT)) +HEXAGON_CLANG_BIN = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT)) +HEXAGON_LIB_DIR = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib +HEXAGON_ISS_DIR = $(HEXAGON_TOOLS_ROOT)/qc/lib/iss + + +CC = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang +CXX = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang++ +CPP = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang -E +LD = $(HEXAGON_BIN)/$(CROSSDEV)ld +AR = $(HEXAGON_BIN)/$(CROSSDEV)ar rcs +NM = $(HEXAGON_BIN)/$(CROSSDEV)nm +OBJCOPY = $(HEXAGON_BIN)/$(CROSSDEV)objcopy +OBJDUMP = $(HEXAGON_BIN)/$(CROSSDEV)objdump + +# Check if the right version of the toolchain is available +# +CROSSDEV_VER_SUPPORTED = 6.4.05 +CROSSDEV_VER_FOUND = $(shell $(CC) --version | sed -n 's/^.*version \([\. 0-9]*\),.*$$/\1/p') + +ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) +endif + + +# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup + +MAXOPTIMIZATION ?= -O2 + +# Base CPU flags for each of the supported architectures. +# +_CODE = $(addprefix -G,$(V_G_THRESHOLD)) +ARCHCPUFLAGS = -m$(V_ARCH) \ + -c \ + -G0 \ + $(_CODE) + + +# Set the board flags +# +ifeq ($(CONFIG_BOARD),) +$(error Board config does not define CONFIG_BOARD) +endif +ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ + -D__PX4_QURT \ + -I$(PX4_BASE)/src/lib/eigen \ + -I$(PX4_BASE)/src/platforms/qurt/include \ + -Wno-error=shadow + +# optimisation flags +# +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -g3 \ + -fno-strict-aliasing \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -ffunction-sections \ + -fdata-sections + +# enable precise stack overflow tracking +# note - requires corresponding support in NuttX +INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) + +# Language-specific flags +# +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ + +# Generic warnings +# +ARCHWARNINGS = -Wall \ + -Wextra \ + -Werror \ + -Wno-cast-align \ + -Wno-missing-braces \ + -Wno-strict-aliasing +# -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+ +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +# C-specific warnings +# +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wstrict-prototypes \ + -Wnested-externs + +# C++-specific warnings +# +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-missing-field-initializers + +# pull in *just* libm from the toolchain ... this is grody +LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) +EXTRA_LIBS += $(LIBM) + +# Flags we pass to the C compiler +# +CFLAGS = $(ARCHCFLAGS) \ + $(ARCHCWARNINGS) \ + $(ARCHOPTIMIZATION) \ + $(ARCHCPUFLAGS) \ + $(ARCHINCLUDES) \ + $(INSTRUMENTATIONDEFINES) \ + $(ARCHDEFINES) \ + $(EXTRADEFINES) \ + $(EXTRACFLAGS) \ + -fno-common \ + $(addprefix -I,$(INCLUDE_DIRS)) + +# Flags we pass to the C++ compiler +# +CXXFLAGS = $(ARCHCXXFLAGS) \ + $(ARCHWARNINGSXX) \ + $(ARCHOPTIMIZATION) \ + $(ARCHCPUFLAGS) \ + $(ARCHXXINCLUDES) \ + $(INSTRUMENTATIONDEFINES) \ + $(ARCHDEFINES) \ + -DCONFIG_WCHAR_BUILTIN \ + $(EXTRADEFINES) \ + $(EXTRACXXFLAGS) \ + $(addprefix -I,$(INCLUDE_DIRS)) + +ifeq (1,$(V_dynamic)) +CXX_FLAGS += -fpic -D__V_DYNAMIC__ +endif + +HEXAGON_LIB_PATH=$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0 +LIB_HEXAGON =$(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a + +# Flags we pass to the assembler +# +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ + $(EXTRADEFINES) \ + $(EXTRAAFLAGS) + +# Flags we pass to the linker +# +LDFLAGS += \ + -Wl,--start-group -Wl,--whole-archive -lc -lgcc -lstdc++ \ + $(LIB_HEXAGON) -Wl,--no-whole-archive -Wl,--end-group -Wl,--dynamic-linker= \ + -Wl,-E -Wl,--force-dynamic \ + $(EXTRALDFLAGS) \ + $(addprefix -T,$(LDSCRIPT)) \ + $(addprefix -L,$(LIB_DIRS)) + +# Compiler support library +# +LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name) + +# Files that the final link depends on +# +LINK_DEPS += $(LDSCRIPT) + +# Files to include to get automated dependencies +# +DEP_INCLUDES = $(subst .o,.d,$(OBJS)) + +# Compile C source $1 to object $2 +# as a side-effect, generate a dependency file +# +define COMPILE + @$(ECHO) "CC: $1" + @$(MKDIR) -p $(dir $2) + $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 +endef + +# Compile C++ source $1 to $2 +# as a side-effect, generate a dependency file +# +define COMPILEXX + @$(ECHO) "CXX: $1" + @$(MKDIR) -p $(dir $2) + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 +endef + +# Assemble $1 into $2 +# +define ASSEMBLE + @$(ECHO) "AS: $1" + @$(MKDIR) -p $(dir $2) + $(Q) $(CC) -c $(AFLAGS) $(abspath $1) -o $2 +endef + +# Produce partially-linked $1 from files in $2 +# +define PRELINK + @$(ECHO) "PRELINK: $1" + @$(MKDIR) -p $(dir $1) + $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +# Update the archive $1 with the files in $2 +# +define ARCHIVE + @$(ECHO) "AR: $2" + @$(MKDIR) -p $(dir $1) + $(Q) $(AR) $1 $2 +endef + +# Link the objects in $2 into the binary $1 +# +define LINK + @$(ECHO) "LINK: $1" + @$(MKDIR) -p $(dir $1) + $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group +endef + +# Convert $1 from a linked object to a raw binary in $2 +# +define SYM_TO_BIN + @$(ECHO) "BIN: $2" + @$(MKDIR) -p $(dir $2) + $(Q) $(OBJCOPY) -O binary $1 $2 +endef + +# Take the raw binary $1 and make it into an object file $2. +# The symbol $3 points to the beginning of the file, and $3_len +# gives its length. +# +# - compile an empty file to generate a suitable object file +# - relink the object and insert the binary file +# - extract the length +# - create const unsigned $3_len with the extracted length as its value and compile it to an object file +# - link the two generated object files together +# - edit symbol names to suit +# +# NOTE: exercise caution using this with absolute pathnames; it looks +# like the MinGW tools insert an extra _ in the binary symbol name; e.g. +# the path: +# +# /d/px4/firmware/Build/px4fmu_default.build/romfs.img +# +# is assigned symbols like: +# +# _binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size +# +# when we would expect +# +# _binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size +# +define BIN_SYM_PREFIX + _binary_$(subst /,_,$(subst .,_,$1)) +endef +define BIN_TO_OBJ + @$(ECHO) "OBJ: $2" + @$(MKDIR) -p $(dir $2) + $(Q) $(ECHO) > $2.c + $(call COMPILE,$2.c,$2.c.o) + $(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1 + $(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) -p --radix=x $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9a-fA-F]*`;" > $2.c + $(call COMPILE,$2.c,$2.c.o) + $(Q) $(LD) -r -o $2 $2.c.o $2.bin.o + $(Q) $(OBJCOPY) $2 \ + --redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \ + --strip-symbol $(call BIN_SYM_PREFIX,$1)_size \ + --strip-symbol $(call BIN_SYM_PREFIX,$1)_end \ + --rename-section .data=.rodata + $(Q) $(REMOVE) $2.c $2.c.o $2.bin.o +endef diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 1cca8e55de..ba7ae4c3c2 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -67,7 +67,7 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) -#elif defined(__PX4_NUTTX) || defined(__PX4_LINUX) +#elif defined(__PX4_NUTTX) || defined(__PX4_LINUX) || defined(__PX4_QURT) /* * Building for NuttX or Linux */ @@ -134,7 +134,7 @@ __END_DECLS /* Defines for ROS and Linux */ -#if defined(__PX4_ROS) || defined(__PX4_LINUX) +#if defined(__PX4_ROS) || defined(__PX4_LINUX) || defined(__PX4_QURT) #define OK 0 #define ERROR -1 diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 8af93b2b2a..97f5bc7d6e 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -102,7 +102,7 @@ #include #include -#elif defined(__PX4_LINUX) +#elif defined(__PX4_LINUX) || defined(__PX4_QURT) #include #include #include @@ -133,5 +133,5 @@ #include #include #else -#define "No target platform defined" +#error "No target platform defined" #endif diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 28289f4e55..b81f7db6a7 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -49,7 +49,7 @@ typedef int px4_task_t; #define px4_task_exit(x) _exit(x) -#elif defined(__PX4_LINUX) +#elif defined(__PX4_LINUX) || defined(__PX4_QURT) #include #include diff --git a/src/platforms/qurt/include/arch/board/board.h b/src/platforms/qurt/include/arch/board/board.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/platforms/qurt/include/board_config.h b/src/platforms/qurt/include/board_config.h new file mode 100644 index 0000000000..a03b8c3e20 --- /dev/null +++ b/src/platforms/qurt/include/board_config.h @@ -0,0 +1,11 @@ +#define UDID_START 0x1FFF7A10 + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_SIM_BUS_TEST 2 +#define PX4_I2C_BUS_EXPANSION 3 +#define PX4_I2C_BUS_LED 3 + +#define PX4_I2C_OBDEV_LED 0x55 diff --git a/src/platforms/qurt/include/crc32.h b/src/platforms/qurt/include/crc32.h new file mode 100644 index 0000000000..bf828e3e0e --- /dev/null +++ b/src/platforms/qurt/include/crc32.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * include/crc.h + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +#ifndef __INCLUDE_CRC32_H +#define __INCLUDE_CRC32_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: crc32part + * + * Description: + * Continue CRC calculation on a part of the buffer. + * + ****************************************************************************/ + +EXTERN uint32_t crc32part(const uint8_t *src, size_t len, + uint32_t crc32val); + +/**************************************************************************** + * Name: crc32 + * + * Description: + * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' + * + ****************************************************************************/ + +EXTERN uint32_t crc32(const uint8_t *src, size_t len); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_CRC32_H */ diff --git a/src/platforms/qurt/include/queue.h b/src/platforms/qurt/include/queue.h new file mode 100644 index 0000000000..4d95541e02 --- /dev/null +++ b/src/platforms/qurt/include/queue.h @@ -0,0 +1,135 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +// Required for Linux +#define FAR +#ifndef NULL +#define NULL (void *)0 +#endif + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s +{ + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s +{ + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s +{ + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s +{ + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ + diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp new file mode 100644 index 0000000000..8ae5ee58d5 --- /dev/null +++ b/src/platforms/qurt/main.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ +/** + * @file main.cpp + * Basic shell to execute builtin "apps" + * + * @author Mark Charlebois + */ + +#include +#include +#include +#include +#include + +using namespace std; + +typedef int (*px4_main_t)(int argc, char *argv[]); + +#include "apps.h" +#include "px4_middleware.h" + +void run_cmd(const vector &appargs); +void run_cmd(const vector &appargs) { + // command is appargs[0] + string command = appargs[0]; + if (apps.find(command) != apps.end()) { + const char *arg[appargs.size()+2]; + + unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { + arg[i] = (char *)appargs[i].c_str(); + ++i; + } + arg[i] = (char *)0; + apps[command](i,(char **)arg); + } + else + { + cout << "Invalid command" << endl; + list_builtins(); + } +} + +static void process_line(string &line) +{ + vector appargs(5); + + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; + run_cmd(appargs); +} + +int main(int argc, char **argv) +{ + // Execute a command list of provided + if (argc == 2) { + ifstream infile(argv[1]); + + for (string line; getline(infile, line, '\n'); ) { + process_line(line); + } + } + + string mystr; + + px4::init(argc, argv, "mainapp"); + + while(1) { + cout << "Enter a command and its args:" << endl; + getline (cin,mystr); + process_line(mystr); + mystr = ""; + } +} diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp new file mode 100644 index 0000000000..a30aeb57bc --- /dev/null +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -0,0 +1,61 @@ + +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_example.cpp + * Example for Linux + * + * @author Mark Charlebois + */ + +#include "hello_example.h" +#include +#include + +px4::AppState HelloExample::appState; + +int HelloExample::main() +{ + appState.setRunning(true); + + int i=0; + while (!appState.exitRequested() && i<5) { + sleep(2); + + printf(" Doing work...\n"); + ++i; + } + + return 0; +} diff --git a/src/platforms/qurt/tests/hello/hello_example.h b/src/platforms/qurt/tests/hello/hello_example.h new file mode 100644 index 0000000000..a4ae517056 --- /dev/null +++ b/src/platforms/qurt/tests/hello/hello_example.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include + +class HelloExample { +public: + HelloExample() {}; + + ~HelloExample() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +}; diff --git a/src/platforms/qurt/tests/hello/hello_main.cpp b/src/platforms/qurt/tests/hello/hello_main.cpp new file mode 100644 index 0000000000..69e8c21ec0 --- /dev/null +++ b/src/platforms/qurt/tests/hello/hello_main.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include "hello_example.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "hello"); + + printf("hello\n"); + HelloExample hello; + hello.main(); + + printf("goodbye\n"); + return 0; +} diff --git a/src/platforms/qurt/tests/hello/hello_start_linux.cpp b/src/platforms/qurt/tests/hello/hello_start_linux.cpp new file mode 100644 index 0000000000..240c5d845e --- /dev/null +++ b/src/platforms/qurt/tests/hello/hello_start_linux.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file hello_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "hello_example.h" +#include +#include +#include +#include +#include + +#define SCHED_DEFAULT SCHED_FIFO +#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) +#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int hello_main(int argc, char *argv[]); +int hello_main(int argc, char *argv[]) +{ + + if (argc < 2) { + printf("usage: hello {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (HelloExample::appState.isRunning()) { + printf("already running\n"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("hello", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + HelloExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (HelloExample::appState.isRunning()) { + printf("is running\n"); + + } else { + printf("not started\n"); + } + + return 0; + } + + printf("usage: hello_main {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/qurt/tests/hello/module.mk b/src/platforms/qurt/tests/hello/module.mk new file mode 100644 index 0000000000..730f9189e7 --- /dev/null +++ b/src/platforms/qurt/tests/hello/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Publisher Example Application +# + +MODULE_COMMAND = hello + +SRCS = hello_main.cpp \ + hello_start_linux.cpp \ + hello_example.cpp + From 1126e7ed52d470bb42ea766276133f26f206e707 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 11:25:54 -0700 Subject: [PATCH 116/258] Added config files for QuRT Signed-off-by: Mark Charlebois --- makefiles/qurt/board_qurt.mk | 11 ++++ makefiles/qurt/config_qurt_default.mk | 77 +++++++++++++++++++++++++++ 2 files changed, 88 insertions(+) create mode 100644 makefiles/qurt/board_qurt.mk create mode 100644 makefiles/qurt/config_qurt_default.mk diff --git a/makefiles/qurt/board_qurt.mk b/makefiles/qurt/board_qurt.mk new file mode 100644 index 0000000000..6d145f1c33 --- /dev/null +++ b/makefiles/qurt/board_qurt.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the Linux port of PX4 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = HEXAGON +CONFIG_BOARD = QURTTEST + +include $(PX4_MK_DIR)/toolchain_hexagon.mk diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk new file mode 100644 index 0000000000..2a856ad3eb --- /dev/null +++ b/makefiles/qurt/config_qurt_default.mk @@ -0,0 +1,77 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Use the configuration's ROMFS. +# +#ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common + +# +# Board support modules +# +#MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/rgbled +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +#MODULES += systemcmds/param + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/mc_att_control + +# +# Library modules +# +#MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +#MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# Linux port +# +#MODULES += platforms/linux/px4_layer +#MODULES += platforms/linux/drivers/accelsim +#MODULES += platforms/linux/drivers/gyrosim +#MODULES += platforms/linux/drivers/adcsim +#MODULES += platforms/linux/drivers/barosim + +# +# Unit tests +# +MODULES += platforms/linux/tests/hello +#MODULES += platforms/linux/tests/vcdev_test +#MODULES += platforms/linux/tests/hrt_test +#MODULES += platforms/linux/tests/wqueue + From 47beddc88f9eb391840c5336c76e900a5c1c432c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 17:56:11 -0700 Subject: [PATCH 117/258] Linux: Fixed hil crash with no args passed The hil module did not check for argc < 2. Signed-off-by: Mark Charlebois --- makefiles/setup.mk | 4 ++-- src/drivers/hil/hil_linux.cpp | 16 +++++++++++++--- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 944b2018d8..8540718d4f 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -34,8 +34,8 @@ # #export PX4_TARGET_OS = nuttx -#export PX4_TARGET_OS = linux -export PX4_TARGET_OS ?= qurt +export PX4_TARGET_OS = linux +#export PX4_TARGET_OS ?= qurt # # Some useful paths. diff --git a/src/drivers/hil/hil_linux.cpp b/src/drivers/hil/hil_linux.cpp index 274a4369f5..93b3af7811 100644 --- a/src/drivers/hil/hil_linux.cpp +++ b/src/drivers/hil/hil_linux.cpp @@ -796,11 +796,17 @@ fake(int argc, char *argv[]) extern "C" __EXPORT int hil_main(int argc, char *argv[]); +void +usage() { + fprintf(stderr, "HIL: unrecognized command, try:\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); +} + int hil_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb = argv[1]; + const char *verb; int ret = PX4_OK; if (hil_start() != PX4_OK) { @@ -808,6 +814,11 @@ hil_main(int argc, char *argv[]) return 1; } + if (argc < 2) { + usage(); + return -EINVAL; + } + verb = argv[1]; /* * Mode switches. */ @@ -852,8 +863,7 @@ hil_main(int argc, char *argv[]) } else { - fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + usage(); ret = -EINVAL; } return ret; From 5d60437164156886a989bc8d4f49193b4c6f50d4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 18:15:18 -0700 Subject: [PATCH 118/258] Qurt: Added more support for the QuRT target Signed-off-by: Mark Charlebois --- makefiles/qurt.mk | 2 +- makefiles/qurt/config_qurt_default.mk | 6 +- makefiles/qurt_elf.mk | 4 +- makefiles/toolchain_hexagon.mk | 121 ++++--- src/drivers/drv_hrt.h | 1 + src/platforms/px4_config.h | 2 +- src/platforms/px4_includes.h | 32 +- src/platforms/px4_middleware.h | 3 + src/platforms/px4_workqueue.h | 2 +- src/platforms/qurt/include/poll.h | 0 src/platforms/qurt/main.cpp | 59 +--- src/platforms/qurt/px4_layer/dq_addlast.c | 74 +++++ src/platforms/qurt/px4_layer/dq_rem.c | 84 +++++ src/platforms/qurt/px4_layer/dq_remfirst.c | 82 +++++ src/platforms/qurt/px4_layer/drv_hrt.c | 294 +++++++++++++++++ src/platforms/qurt/px4_layer/lib_crc32.c | 126 ++++++++ src/platforms/qurt/px4_layer/module.mk | 54 ++++ .../qurt/px4_layer/px4_qurt_impl.cpp | 85 +++++ .../qurt/px4_layer/px4_qurt_tasks.cpp | 284 +++++++++++++++++ src/platforms/qurt/px4_layer/queue.c | 102 ++++++ src/platforms/qurt/px4_layer/sq_addafter.c | 71 +++++ src/platforms/qurt/px4_layer/sq_addlast.c | 72 +++++ src/platforms/qurt/px4_layer/sq_remfirst.c | 76 +++++ src/platforms/qurt/px4_layer/work_cancel.c | 120 +++++++ src/platforms/qurt/px4_layer/work_queue.c | 130 ++++++++ src/platforms/qurt/px4_layer/work_thread.c | 298 ++++++++++++++++++ 26 files changed, 2056 insertions(+), 128 deletions(-) create mode 100644 src/platforms/qurt/include/poll.h create mode 100644 src/platforms/qurt/px4_layer/dq_addlast.c create mode 100644 src/platforms/qurt/px4_layer/dq_rem.c create mode 100644 src/platforms/qurt/px4_layer/dq_remfirst.c create mode 100644 src/platforms/qurt/px4_layer/drv_hrt.c create mode 100644 src/platforms/qurt/px4_layer/lib_crc32.c create mode 100644 src/platforms/qurt/px4_layer/module.mk create mode 100644 src/platforms/qurt/px4_layer/px4_qurt_impl.cpp create mode 100644 src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp create mode 100644 src/platforms/qurt/px4_layer/queue.c create mode 100644 src/platforms/qurt/px4_layer/sq_addafter.c create mode 100644 src/platforms/qurt/px4_layer/sq_addlast.c create mode 100644 src/platforms/qurt/px4_layer/sq_remfirst.c create mode 100644 src/platforms/qurt/px4_layer/work_cancel.c create mode 100644 src/platforms/qurt/px4_layer/work_queue.c create mode 100644 src/platforms/qurt/px4_layer/work_thread.c diff --git a/makefiles/qurt.mk b/makefiles/qurt.mk index 919dcc4aba..f34392a210 100644 --- a/makefiles/qurt.mk +++ b/makefiles/qurt.mk @@ -36,5 +36,5 @@ MODULES += \ platforms/common \ - platforms/linux/px4_layer + platforms/qurt/px4_layer diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 2a856ad3eb..c076cd5841 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -59,9 +59,9 @@ #MODULES += lib/conversion # -# Linux port +# QuRT port # -#MODULES += platforms/linux/px4_layer +MODULES += platforms/qurt/px4_layer #MODULES += platforms/linux/drivers/accelsim #MODULES += platforms/linux/drivers/gyrosim #MODULES += platforms/linux/drivers/adcsim @@ -70,7 +70,7 @@ # # Unit tests # -MODULES += platforms/linux/tests/hello +MODULES += platforms/qurt/tests/hello #MODULES += platforms/linux/tests/vcdev_test #MODULES += platforms/linux/tests/hrt_test #MODULES += platforms/linux/tests/wqueue diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 701d731ef3..57c6434e1d 100644 --- a/makefiles/qurt_elf.mk +++ b/makefiles/qurt_elf.mk @@ -56,9 +56,9 @@ $(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) -MAIN = $(PX4_BASE)/src/platforms/linux/main.cpp +MAIN = $(PX4_BASE)/src/platforms/qurt/main.cpp $(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) - $(PX4_BASE)/Tools/linux_apps.py > apps.h + $(PX4_BASE)/Tools/qurt_apps.py > apps.h $(call LINK,$@, -I. $(MAIN) $(PRODUCT_SHARED_LIB)) # diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 6b3027684e..dacb60bfb1 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -38,8 +38,8 @@ # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT = /opt/6.4.05 -#V_ARCH = v4 # Set for APQ8064 -V_ARCH = v5 # Set for APQ8074 +#V_ARCH = v4 +V_ARCH = v5 CROSSDEV = hexagon- HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT)) HEXAGON_CLANG_BIN = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT)) @@ -72,11 +72,7 @@ MAXOPTIMIZATION ?= -O2 # Base CPU flags for each of the supported architectures. # -_CODE = $(addprefix -G,$(V_G_THRESHOLD)) -ARCHCPUFLAGS = -m$(V_ARCH) \ - -c \ - -G0 \ - $(_CODE) +ARCHCPUFLAGS = -m$(V_ARCH) # Set the board flags @@ -86,8 +82,13 @@ $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -D__PX4_QURT \ + -D__EXPORT= \ + -Dnoreturn_function= \ + -Drestrict= \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ + -I$(PX4_BASE)/../dspalmc/include \ + -I$(PX4_BASE)/../dspalmc/sys \ -Wno-error=shadow # optimisation flags @@ -114,6 +115,8 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics ARCHWARNINGS = -Wall \ -Wextra \ -Werror \ + -Wno-unused-parameter \ + -Wno-unused-variable \ -Wno-cast-align \ -Wno-missing-braces \ -Wno-strict-aliasing @@ -169,8 +172,8 @@ ifeq (1,$(V_dynamic)) CXX_FLAGS += -fpic -D__V_DYNAMIC__ endif -HEXAGON_LIB_PATH=$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0 -LIB_HEXAGON =$(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a +HEXAGON_LIB_PATH = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0 +LIB_HEXAGON = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a # Flags we pass to the assembler # @@ -178,14 +181,11 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ $(EXTRADEFINES) \ $(EXTRAAFLAGS) +LDSCRIPT = $(PX4_BASE)/linux-configs/linuxtest/scripts/ld.script # Flags we pass to the linker # LDFLAGS += \ - -Wl,--start-group -Wl,--whole-archive -lc -lgcc -lstdc++ \ - $(LIB_HEXAGON) -Wl,--no-whole-archive -Wl,--end-group -Wl,--dynamic-linker= \ - -Wl,-E -Wl,--force-dynamic \ $(EXTRALDFLAGS) \ - $(addprefix -T,$(LDSCRIPT)) \ $(addprefix -L,$(LIB_DIRS)) # Compiler support library @@ -215,6 +215,7 @@ endef define COMPILEXX @$(ECHO) "CXX: $1" @$(MKDIR) -p $(dir $2) + @echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 endef @@ -228,11 +229,25 @@ endef # Produce partially-linked $1 from files in $2 # +#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + echo $(Q) $(LD) -Ur -o $1 $2 + $(Q) $(LD) -Ur -o $1 $2 + endef +# Produce partially-linked $1 from files in $2 +# +#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold +define PRELINKF + @$(ECHO) "PRELINKF: $1" + @$(MKDIR) -p $(dir $1) + echo $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 + $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 + +endef +# $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 # Update the archive $1 with the files in $2 # @@ -242,63 +257,33 @@ define ARCHIVE $(Q) $(AR) $1 $2 endef -# Link the objects in $2 into the binary $1 +# Link the objects in $2 into the shared library $1 +# +define LINK_A + @$(ECHO) "LINK_A: $1" + @$(MKDIR) -p $(dir $1) + echo "$(Q) $(AR) $1 $2" + $(Q) $(AR) $1 $2 +endef + +# Link the objects in $2 into the shared library $1 +# +define LINK_SO + @$(ECHO) "LINK_SO: $1" + @$(MKDIR) -p $(dir $1) + echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)" + $(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc +endef + +# Link the objects in $2 into the application $1 # define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group + echo $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) + $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 + +# $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) + endef -# Convert $1 from a linked object to a raw binary in $2 -# -define SYM_TO_BIN - @$(ECHO) "BIN: $2" - @$(MKDIR) -p $(dir $2) - $(Q) $(OBJCOPY) -O binary $1 $2 -endef - -# Take the raw binary $1 and make it into an object file $2. -# The symbol $3 points to the beginning of the file, and $3_len -# gives its length. -# -# - compile an empty file to generate a suitable object file -# - relink the object and insert the binary file -# - extract the length -# - create const unsigned $3_len with the extracted length as its value and compile it to an object file -# - link the two generated object files together -# - edit symbol names to suit -# -# NOTE: exercise caution using this with absolute pathnames; it looks -# like the MinGW tools insert an extra _ in the binary symbol name; e.g. -# the path: -# -# /d/px4/firmware/Build/px4fmu_default.build/romfs.img -# -# is assigned symbols like: -# -# _binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size -# -# when we would expect -# -# _binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size -# -define BIN_SYM_PREFIX - _binary_$(subst /,_,$(subst .,_,$1)) -endef -define BIN_TO_OBJ - @$(ECHO) "OBJ: $2" - @$(MKDIR) -p $(dir $2) - $(Q) $(ECHO) > $2.c - $(call COMPILE,$2.c,$2.c.o) - $(Q) $(LD) -r -o $2.bin.o $2.c.o -b binary $1 - $(Q) $(ECHO) "const unsigned int $3_len = 0x`$(NM) -p --radix=x $2.bin.o | $(GREP) $(call BIN_SYM_PREFIX,$1)_size$$ | $(GREP) -o ^[0-9a-fA-F]*`;" > $2.c - $(call COMPILE,$2.c,$2.c.o) - $(Q) $(LD) -r -o $2 $2.c.o $2.bin.o - $(Q) $(OBJCOPY) $2 \ - --redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \ - --strip-symbol $(call BIN_SYM_PREFIX,$1)_size \ - --strip-symbol $(call BIN_SYM_PREFIX,$1)_end \ - --rename-section .data=.rodata - $(Q) $(REMOVE) $2.c $2.c.o $2.bin.o -endef diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index a40943d3f2..3ef3dbce94 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 5388f08f1d..caeeb09a8b 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -42,7 +42,7 @@ #if defined(__PX4_NUTTX) #include -#elif defined (__PX4_LINUX) +#elif defined (__PX4_LINUX) || defined (__PX4_QURT) #define CONFIG_NFILE_STREAMS 1 #define CONFIG_SCHED_WORKQUEUE 1 #define CONFIG_SCHED_HPWORK 1 diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 97f5bc7d6e..f2bd2f969d 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -102,7 +102,7 @@ #include #include -#elif defined(__PX4_LINUX) || defined(__PX4_QURT) +#elif defined(__PX4_LINUX) #include #include #include @@ -132,6 +132,36 @@ #include #include #include +#elif defined(__PX4_QURT) +#include +#include +#include + +#define ASSERT(x) assert(x) + +#ifdef __cplusplus +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif +#include +#include +#include #else #error "No target platform defined" #endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index d86ddd6645..786614fc9d 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -60,6 +60,9 @@ extern bool task_should_exit; * Returns true if the app/task should continue to run */ __EXPORT inline bool ok() { return !task_should_exit; } +#elif defined(__PX4_QURT) +// FIXME - usleep not supported by DSPAL +inline void usleep(uint64_t sleep_interval) { } #else /** * Linux needs to have globally unique checks for thread/task status diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 686bfdf258..a275faebe9 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -41,7 +41,7 @@ #include #include #include -#elif defined(__PX4_LINUX) +#elif defined(__PX4_LINUX) || defined(__PX4_QURT) #include #include diff --git a/src/platforms/qurt/include/poll.h b/src/platforms/qurt/include/poll.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 8ae5ee58d5..aab84cd016 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -42,63 +42,20 @@ #include #include #include +#include -using namespace std; +//using namespace std; -typedef int (*px4_main_t)(int argc, char *argv[]); +//typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" -#include "px4_middleware.h" +//#include "px4_middleware.h" -void run_cmd(const vector &appargs); -void run_cmd(const vector &appargs) { - // command is appargs[0] - string command = appargs[0]; - if (apps.find(command) != apps.end()) { - const char *arg[appargs.size()+2]; - - unsigned int i = 0; - while (i < appargs.size() && appargs[i] != "") { - arg[i] = (char *)appargs[i].c_str(); - ++i; - } - arg[i] = (char *)0; - apps[command](i,(char **)arg); - } - else - { - cout << "Invalid command" << endl; - list_builtins(); - } -} - -static void process_line(string &line) -{ - vector appargs(5); - - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; - run_cmd(appargs); -} +//static command = "list_builtins"; int main(int argc, char **argv) { - // Execute a command list of provided - if (argc == 2) { - ifstream infile(argv[1]); - - for (string line; getline(infile, line, '\n'); ) { - process_line(line); - } - } - - string mystr; - - px4::init(argc, argv, "mainapp"); - - while(1) { - cout << "Enter a command and its args:" << endl; - getline (cin,mystr); - process_line(mystr); - mystr = ""; - } + printf("hello\n"); + list_builtins(); + //apps["hello"](i,(char **)arg);; } diff --git a/src/platforms/qurt/px4_layer/dq_addlast.c b/src/platforms/qurt/px4_layer/dq_addlast.c new file mode 100644 index 0000000000..3ef08abd05 --- /dev/null +++ b/src/platforms/qurt/px4_layer/dq_addlast.c @@ -0,0 +1,74 @@ +/************************************************************ + * libc/queue/dq_addlast.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_addlast + * + * Description + * dq_addlast adds 'node' to the end of 'queue' + * + ************************************************************/ + +void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) +{ + node->flink = NULL; + node->blink = queue->tail; + + if (!queue->head) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/qurt/px4_layer/dq_rem.c b/src/platforms/qurt/px4_layer/dq_rem.c new file mode 100644 index 0000000000..db20762c75 --- /dev/null +++ b/src/platforms/qurt/px4_layer/dq_rem.c @@ -0,0 +1,84 @@ +/************************************************************ + * libc/queue/dq_rem.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_rem + * + * Descripton: + * dq_rem removes 'node' from 'queue' + * + ************************************************************/ + +void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue) +{ + FAR dq_entry_t *prev = node->blink; + FAR dq_entry_t *next = node->flink; + + if (!prev) + { + queue->head = next; + } + else + { + prev->flink = next; + } + + if (!next) + { + queue->tail = prev; + } + else + { + next->blink = prev; + } + + node->flink = NULL; + node->blink = NULL; +} + diff --git a/src/platforms/qurt/px4_layer/dq_remfirst.c b/src/platforms/qurt/px4_layer/dq_remfirst.c new file mode 100644 index 0000000000..e87acc3382 --- /dev/null +++ b/src/platforms/qurt/px4_layer/dq_remfirst.c @@ -0,0 +1,82 @@ +/************************************************************ + * libc/queue/dq_remfirst.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_remfirst + * + * Description: + * dq_remfirst removes 'node' from the head of 'queue' + * + ************************************************************/ + +FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) +{ + FAR dq_entry_t *ret = queue->head; + + if (ret) + { + FAR dq_entry_t *next = ret->flink; + if (!next) + { + queue->head = NULL; + queue->tail = NULL; + } + else + { + queue->head = next; + next->blink = NULL; + } + + ret->flink = NULL; + ret->blink = NULL; + } + + return ret; +} + diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c new file mode 100644 index 0000000000..39e0f73b37 --- /dev/null +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -0,0 +1,294 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file drv_hrt.c + * + * High-resolution timer with callouts and timekeeping. + */ + +#include +#include +#include + +static struct sq_queue_s callout_queue; + +/* latency histogram */ +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +static void hrt_call_reschedule(void); + +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* + * Get absolute time. + */ +hrt_abstime hrt_absolute_time(void) +{ + struct timespec ts; + + // FIXME - not supported in QURT + //clock_gettime(CLOCK_MONOTONIC, &ts); + return ts_to_abstime(&ts); +} + +/* + * Convert a timespec to absolute time. + */ +hrt_abstime ts_to_abstime(struct timespec *ts) +{ + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; +} + +/* + * Compute the delta between a timestamp taken in the past + * and now. + * + * This function is safe to use even if the timestamp is updated + * by an interrupt during execution. + */ +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) +{ + hrt_abstime delta = hrt_absolute_time() - *then; + return delta; +} + +/* + * Store the absolute time in an interrupt-safe fashion. + * + * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. + */ +hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) +{ + hrt_abstime ts = hrt_absolute_time(); + return ts; +} + + +/* + * If this returns true, the entry has been invoked and removed from the callout list, + * or it has never been entered. + * + * Always returns false for repeating callouts. + */ +bool hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/* + * Remove the entry from the callout list. + */ +void hrt_cancel(struct hrt_call *entry) +{ + // FIXME - need a lock + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + // endif +} + +/* + * initialise a hrt_call structure + */ +void hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} + +/* + * Initialise the HRT. + */ +void hrt_init(void) +{ + sq_init(&callout_queue); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + //lldbg("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + //lldbg("scheduled\n"); +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + //lldbg("entry in queue\n"); + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + //lldbg("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + deadline = next->deadline; + } + } +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) + sq_rem(&entry->link, &callout_queue); + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); +} + +/* + * Call callout(arg) after delay has elapsed. + * + * If callout is NULL, this can be used to implement a timeout by testing the call + * with hrt_called(). + */ +void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/* + * Call callout(arg) after delay, and then after every interval. + * + * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may + * jitter but should not drift. + */ +void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +/* + * Call callout(arg) at absolute time calltime. + */ +void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +#if 0 +/* + * Convert absolute time to a timespec. + */ +void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); +#endif + diff --git a/src/platforms/qurt/px4_layer/lib_crc32.c b/src/platforms/qurt/px4_layer/lib_crc32.c new file mode 100644 index 0000000000..4ba6fbf6df --- /dev/null +++ b/src/platforms/qurt/px4_layer/lib_crc32.c @@ -0,0 +1,126 @@ +/************************************************************************************************ + * libc/misc/lib_crc32.c + * + * This file is a part of NuttX: + * + * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. + * + * The logic in this file was developed by Gary S. Brown: + * + * COPYRIGHT (C) 1986 Gary S. Brown. You may use this program, or code or tables + * extracted from it, as desired without restriction. + * + * First, the polynomial itself and its table of feedback terms. The polynomial is: + * + * X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0 + * + * Note that we take it "backwards" and put the highest-order term in the lowest-order bit. + * The X^32 term is "implied"; the LSB is the X^31 term, etc. The X^0 term (usually shown + * as "+1") results in the MSB being 1 + * + * Note that the usual hardware shift register implementation, which is what we're using + * (we're merely optimizing it by doing eight-bit chunks at a time) shifts bits into the + * lowest-order term. In our implementation, that means shifting towards the right. Why + * do we do it this way? Because the calculated CRC must be transmitted in order from + * highest-order term to lowest-order term. UARTs transmit characters in order from LSB + * to MSB. By storing the CRC this way we hand it to the UART in the order low-byte to + * high-byte; the UART sends each low-bit to hight-bit; and the result is transmission bit + * by bit from highest- to lowest-order term without requiring any bit shuffling on our + * part. Reception works similarly + * + * The feedback terms table consists of 256, 32-bit entries. Notes + * + * - The table can be generated at runtime if desired; code to do so is shown later. It + * might not be obvious, but the feedback terms simply represent the results of eight + * shift/xor operations for all combinations of data and CRC register values + * + * - The values must be right-shifted by eight bits by the updcrc logic; the shift must + * be u_(bring in zeroes). On some hardware you could probably optimize the shift in + * assembler by using byte-swap instructions polynomial $edb88320 + ************************************************************************************************/ + +/************************************************************************************************ + * Included Files + ************************************************************************************************/ + +#include +#include +#include + +// Needed for Linux +#define FAR + +/************************************************************************************************ + * Private Data + ************************************************************************************************/ + +static const uint32_t crc32_tab[] = +{ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; + +/************************************************************************************************ + * Public Functions + ************************************************************************************************/ +/************************************************************************************************ + * Name: crc32part + * + * Description: + * Continue CRC calculation on a part of the buffer. + * + ************************************************************************************************/ + +uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) +{ + size_t i; + + for (i = 0; i < len; i++) + { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } + return crc32val; +} + +/************************************************************************************************ + * Name: crc32 + * + * Description: + * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' + * + ************************************************************************************************/ + +uint32_t crc32(FAR const uint8_t *src, size_t len) +{ + return crc32part(src, len, 0); +} diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk new file mode 100644 index 0000000000..7d377e8bdd --- /dev/null +++ b/src/platforms/qurt/px4_layer/module.mk @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# NuttX / uORB adapter library +# + +SRCS = \ + px4_qurt_impl.cpp \ + px4_qurt_tasks.cpp \ + work_thread.c \ + work_queue.c \ + work_cancel.c \ + lib_crc32.c \ + drv_hrt.c \ + queue.c \ + dq_addlast.c \ + dq_remfirst.c \ + sq_addlast.c \ + sq_remfirst.c \ + sq_addafter.c \ + dq_rem.c + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp new file mode 100644 index 0000000000..36c795a213 --- /dev/null +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_linux_impl.cpp + * + * PX4 Middleware Wrapper Linux Implementation + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "systemlib/param/param.h" + +__BEGIN_DECLS + +// FIXME - sysconf(_SC_CLK_TCK) not supported +long PX4_TICKS_PER_SEC = 1000; + +__END_DECLS + +extern struct wqueue_s gwork[NWORKERS]; + +namespace px4 +{ + +void init(int argc, char *argv[], const char *app_name) +{ + printf("App name: %s\n", app_name); + + // Create high priority worker thread + g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + work_hpthread, + (char* const*)NULL); + + // Create low priority worker thread + g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2000, + work_lpthread, + (char* const*)NULL); + +} + +} + diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp new file mode 100644 index 0000000000..f402bde549 --- /dev/null +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -0,0 +1,284 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Author: @author Mark Charlebois + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_linux_tasks.c + * Implementation of existing task API for Linux + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#define MAX_CMD_LEN 100 + +#define PX4_MAX_TASKS 100 +struct task_entry +{ + pthread_t pid; + std::string name; + bool isused; + task_entry() : isused(false) {} +}; + +static task_entry taskmap[PX4_MAX_TASKS]; + +typedef struct +{ + px4_main_t entry; + int argc; + char *argv[]; + // strings are allocated after the +} pthdata_t; + +static void *entry_adapter ( void *ptr ) +{ + pthdata_t *data; + data = (pthdata_t *) ptr; + + data->entry(data->argc, data->argv); + free(ptr); + printf("Before px4_task_exit\n"); + px4_task_exit(0); + printf("After px4_task_exit\n"); + + return NULL; +} + +void +px4_systemreset(bool to_bootloader) +{ + printf("Called px4_system_reset\n"); +} + +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +{ + int rv; + int argc = 0; + int i; + unsigned int len = 0; + unsigned long offset; + unsigned long structsize; + char * p = (char *)argv; + + pthread_t task; + pthread_attr_t attr; + struct sched_param param; + + // Calculate argc + while (p != (char *)0) { + p = argv[argc]; + if (p == (char *)0) + break; + ++argc; + len += strlen(p)+1; + } + structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); + pthdata_t *taskdata; + + // not safe to pass stack data to the thread creation + taskdata = (pthdata_t *)malloc(structsize+len); + offset = ((unsigned long)taskdata)+structsize; + + taskdata->entry = entry; + taskdata->argc = argc; + + for (i=0; iargv[i] = (char *)offset; + strcpy((char *)offset, argv[i]); + offset+=strlen(argv[i])+1; + } + // Must add NULL at end of argv + taskdata->argv[argc] = (char *)0; + +#if 0 + rv = pthread_attr_init(&attr); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to init thread attrs\n"); + return (rv < 0) ? rv : -rv; + } + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to set inherit sched\n"); + return (rv < 0) ? rv : -rv; + } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to set sched policy\n"); + return (rv < 0) ? rv : -rv; + } + + param.sched_priority = priority; + + rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to set sched param\n"); + return (rv < 0) ? rv : -rv; + } +#endif + + //rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { + + if (rv == EPERM) { + //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); + rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { + printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + return (rv < 0) ? rv : -rv; + } + } + else { + return (rv < 0) ? rv : -rv; + } + } + + for (i=0; i=PX4_MAX_TASKS) { + return -ENOSPC; + } + return i; +} + +int px4_task_delete(px4_task_t id) +{ + int rv = 0; + pthread_t pid; + printf("Called px4_task_delete\n"); + + if (id < PX4_MAX_TASKS && taskmap[id].isused) + pid = taskmap[id].pid; + else + return -EINVAL; + + // If current thread then exit, otherwise cancel + if (pthread_self() == pid) { + taskmap[id].isused = false; + pthread_exit(0); + } else { + rv = pthread_cancel(pid); + } + + taskmap[id].isused = false; + + return rv; +} + +void px4_task_exit(int ret) +{ + int i; + pthread_t pid = pthread_self(); + + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) + printf("px4_task_exit: self task not found!\n"); + else + printf("px4_task_exit: %s\n", taskmap[i].name.c_str()); + + pthread_exit((void *)(unsigned long)ret); +} + +void px4_killall(void) +{ + //printf("Called px4_killall\n"); + for (int i=0; i + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + * Modified for use in Linux by Mark Charlebois + * + ************************************************************************/ + +// FIXME - need px4_queue +#include +#include + +__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue); +void sq_rem(sq_entry_t *node, sq_queue_t *queue) +{ + if (queue->head && node) + { + if (node == queue->head) + { + queue->head = node->flink; + if (node == queue->tail) + { + queue->tail = NULL; + } + } + else + { + sq_entry_t *prev; + for(prev = (sq_entry_t*)queue->head; + prev && prev->flink != node; + prev = prev->flink); + + if (prev) + { + sq_remafter(prev, queue); + } + } + } +} + +sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue) +{ + sq_entry_t *ret = node->flink; + if (queue->head && ret) + { + if (queue->tail == ret) + { + queue->tail = node; + node->flink = NULL; + } + else + { + node->flink = ret->flink; + } + + ret->flink = NULL; + } + + return ret; +} + +__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue); +void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = queue->head; + if (!queue->head) + { + queue->tail = node; + } + queue->head = node; +} + + diff --git a/src/platforms/qurt/px4_layer/sq_addafter.c b/src/platforms/qurt/px4_layer/sq_addafter.c new file mode 100644 index 0000000000..5d47feba0f --- /dev/null +++ b/src/platforms/qurt/px4_layer/sq_addafter.c @@ -0,0 +1,71 @@ +/************************************************************ + * libc/queue/sq_addafter.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addafter.c + * + * Description: + * The sq_addafter function adds 'node' after 'prev' in the + * 'queue.' + * + ************************************************************/ + +void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue) +{ + if (!queue->head || prev == queue->tail) + { + sq_addlast(node, queue); + } + else + { + node->flink = prev->flink; + prev->flink = node; + } +} diff --git a/src/platforms/qurt/px4_layer/sq_addlast.c b/src/platforms/qurt/px4_layer/sq_addlast.c new file mode 100644 index 0000000000..faa07efb5c --- /dev/null +++ b/src/platforms/qurt/px4_layer/sq_addlast.c @@ -0,0 +1,72 @@ +/************************************************************ + * libc/queue/sq_addlast.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addlast + * + * Description: + * The sq_addlast function places the 'node' at the tail of + * the 'queue' + ************************************************************/ + +void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue) +{ + node->flink = NULL; + if (!queue->head) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/qurt/px4_layer/sq_remfirst.c b/src/platforms/qurt/px4_layer/sq_remfirst.c new file mode 100644 index 0000000000..f81c18dc2e --- /dev/null +++ b/src/platforms/qurt/px4_layer/sq_remfirst.c @@ -0,0 +1,76 @@ +/************************************************************ + * libc/queue/sq_remfirst.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_remfirst + * + * Description: + * sq_remfirst function removes the first entry from + * 'queue' + * + ************************************************************/ + +FAR sq_entry_t *sq_remfirst(sq_queue_t *queue) +{ + FAR sq_entry_t *ret = queue->head; + + if (ret) + { + queue->head = ret->flink; + if (!queue->head) + { + queue->tail = NULL; + } + + ret->flink = NULL; + } + + return ret; +} + diff --git a/src/platforms/qurt/px4_layer/work_cancel.c b/src/platforms/qurt/px4_layer/work_cancel.c new file mode 100644 index 0000000000..6f737877d9 --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_cancel.c @@ -0,0 +1,120 @@ +/**************************************************************************** + * libc/wqueue/work_cancel.c + * + * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling work_queue() + * again. + * + * Input parameters: + * qid - The work queue ID + * work - The previously queue work structure to cancel + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_cancel(int qid, struct work_s *work) +{ + struct wqueue_s *wqueue = &g_work[qid]; + //irqstate_t flags; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* Cancelling the work is simply a matter of removing the work structure + * from the work queue. This must be done with interrupts disabled because + * new work is typically added to the work queue from interrupt handlers. + */ + + //flags = irqsave(); + if (work->worker != NULL) + { + /* A little test of the integrity of the work queue */ + + //DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail); + //DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head); + + /* Remove the entry from the work queue and make sure that it is + * mark as availalbe (i.e., the worker field is nullified). + */ + + dq_rem((FAR dq_entry_t *)work, &wqueue->q); + work->worker = NULL; + } + + //irqrestore(flags); + return PX4_OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c new file mode 100644 index 0000000000..cd96aacd2f --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_queue.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * libc/wqueue/work_queue.c + * + * Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * qid - The work queue ID (index) + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in clock ticks) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + struct wqueue_s *wqueue = &g_work[qid]; + + //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS); + + /* First, initialize the work structure */ + + work->worker = worker; /* Work callback */ + work->arg = arg; /* Callback argument */ + work->delay = delay; /* Delay until work performed */ + + /* Now, time-tag that entry and put it in the work queue. This must be + * done with interrupts disabled. This permits this function to be called + * from with task logic or interrupt handlers. + */ + + //flags = irqsave(); + work->qtime = clock_systimer(); /* Time work queued */ + + dq_addlast((dq_entry_t *)work, &wqueue->q); + px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ + + //irqrestore(flags); + return PX4_OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c new file mode 100644 index 0000000000..cb8cbe9766 --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_thread.c @@ -0,0 +1,298 @@ +/**************************************************************************** + * libc/wqueue/work_thread.c + * + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef CONFIG_SCHED_WORKQUEUE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* The state of each work queue. */ +struct wqueue_s g_work[NWORKERS]; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: work_process + * + * Description: + * This is the logic that performs actions placed on any work list. + * + * Input parameters: + * wqueue - Describes the work queue to be processed + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void work_process(FAR struct wqueue_s *wqueue) +{ + volatile FAR struct work_s *work; + worker_t worker; + //irqstate_t flags; + FAR void *arg; + uint32_t elapsed; + uint32_t remaining; + uint32_t next; + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + next = CONFIG_SCHED_WORKPERIOD; + //flags = irqsave(); + work = (FAR struct work_s *)wqueue->q.head; + while (work) + { + /* Is this work ready? It is ready if there is no delay or if + * the delay has elapsed. qtime is the time that the work was added + * to the work queue. It will always be greater than or equal to + * zero. Therefore a delay of zero will always execute immediately. + */ + + elapsed = clock_systimer() - work->qtime; + //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay); + if (elapsed >= work->delay) + { + /* Remove the ready-to-execute work from the list */ + + (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + + /* Extract the work description from the entry (in case the work + * instance by the re-used after it has been de-queued). + */ + + worker = work->worker; + arg = work->arg; + + /* Mark the work as no longer being queued */ + + work->worker = NULL; + + /* Do the work. Re-enable interrupts while the work is being + * performed... we don't have any idea how long that will take! + */ + + //irqrestore(flags); + if (!worker) { + printf("MESSED UP: worker = 0\n"); + } + else + worker(arg); + + /* Now, unfortunately, since we re-enabled interrupts we don't + * know the state of the work list and we will have to start + * back at the head of the list. + */ + + //flags = irqsave(); + work = (FAR struct work_s *)wqueue->q.head; + } + else + { + /* This one is not ready.. will it be ready before the next + * scheduled wakeup interval? + */ + + /* Here: elapsed < work->delay */ + remaining = work->delay - elapsed; + if (remaining < next) + { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (FAR struct work_s *)work->dq.flink; + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + + //FIXME - DSPAL doesn't support usleep + //usleep(next); + //irqrestore(flags); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/**************************************************************************** + * Name: work_hpthread, work_lpthread, and work_usrthread + * + * Description: + * These are the worker threads that performs actions placed on the work + * lists. + * + * work_hpthread and work_lpthread: These are the kernel mode work queues + * (also build in the flat build). One of these threads also performs + * periodic garbage collection (that is otherwise performed by the idle + * thread if CONFIG_SCHED_WORKQUEUE is not defined). + * + * These worker threads are started by the OS during normal bringup. + * + * work_usrthread: This is a user mode work queue. It must be built into + * the applicatino blob during the user phase of a kernel build. The + * user work thread will then automatically be started when the system + * boots by calling through the pointer found in the header on the user + * space blob. + * + * All of these entrypoints are referenced by OS internally and should not + * not be accessed by application logic. + * + * Input parameters: + * argc, argv (not used) + * + * Returned Value: + * Does not return + * + ****************************************************************************/ + +#ifdef CONFIG_SCHED_HPWORK + +int work_hpthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + +#ifndef CONFIG_SCHED_LPWORK + sched_garbagecollection(); +#endif + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[HPWORK]); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#ifdef CONFIG_SCHED_LPWORK + +int work_lpthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* First, perform garbage collection. This cleans-up memory de-allocations + * that were queued because they could not be freed in that execution + * context (for example, if the memory was freed from an interrupt handler). + * NOTE: If the work thread is disabled, this clean-up is performed by + * the IDLE thread (at a very, very low priority). + */ + + //sched_garbagecollection(); + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[LPWORK]); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_LPWORK */ +#endif /* CONFIG_SCHED_HPWORK */ + +#ifdef CONFIG_SCHED_USRWORK + +int work_usrthread(int argc, char *argv[]) +{ + /* Loop forever */ + + for (;;) + { + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + work_process(&g_work[USRWORK]); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_USRWORK */ + +uint32_t clock_systimer() +{ + return (0x00000000ffffffff & hrt_absolute_time()); +} +#endif /* CONFIG_SCHED_WORKQUEUE */ From 0d523d57af06019e115f538bd71cd12ffe5e6ae2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 18:20:33 -0700 Subject: [PATCH 119/258] QuRT: Added nfds_t type DSPAL does not yet provide poll.h and all we need is the defintion of nfds_t. Signed-off-by: Mark Charlebois --- src/platforms/qurt/include/poll.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/qurt/include/poll.h b/src/platforms/qurt/include/poll.h index e69de29bb2..324036aa5c 100644 --- a/src/platforms/qurt/include/poll.h +++ b/src/platforms/qurt/include/poll.h @@ -0,0 +1,3 @@ +#pragma once + +typedef unsigned int nfds_t; From 6ce0b7b753eb75fda226d0bb9823705dd1467f23 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 18:38:30 -0700 Subject: [PATCH 120/258] QuRT: added missing make pieces Signed-off-by: Mark Charlebois --- Makefile | 3 ++ Tools/qurt_apps.py | 105 +++++++++++++++++++++++++++++++++++++ makefiles/firmware_qurt.mk | 9 +++- 3 files changed, 116 insertions(+), 1 deletion(-) create mode 100755 Tools/qurt_apps.py diff --git a/Makefile b/Makefile index 4c009af00d..a079e4fc47 100644 --- a/Makefile +++ b/Makefile @@ -103,6 +103,9 @@ endif ifeq ($(PX4_TARGET_OS),linux) include $(PX4_BASE)makefiles/firmware_linux.mk endif +ifeq ($(PX4_TARGET_OS),qurt) +include $(PX4_BASE)makefiles/firmware_qurt.mk +endif MSG_DIR = $(PX4_BASE)msg UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py new file mode 100755 index 0000000000..ef2972de3d --- /dev/null +++ b/Tools/qurt_apps.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2015 Mark Charlebois. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +import glob +#builtins = glob.glob("../Build/linux_default.build/builtin_commands/COMMAND*") +builtins = glob.glob("builtin_commands/COMMAND*") + +apps = [] +for f in builtins: + apps.append(f.split(".")[-1].split("_main")[0]) + +print +print """ +#include +#include + +#define __EXPORT + +#include +#include + +using namespace std; + +extern void px4_show_devices(void); + +extern "C" { +""" +for app in apps: + print "extern int "+app+"_main(int argc, char *argv[]);" + +print """ +static int shutdown_main(int argc, char *argv[]); +static int list_tasks_main(int argc, char *argv[]); +} + + +static map app_map(void); + +static map app_map(void) +{ + map apps; +""" +for app in apps: + print '\tapps["'+app+'"] = '+app+'_main;' + +print '\tapps["shutdown"] = shutdown_main;' +print '\tapps["list_tasks"] = list_tasks_main;' +print """ + return apps; +} + +map apps = app_map(); + +static void list_builtins(void) +{ + cout << "Builtin Commands:" << endl; + for (map::iterator it=apps.begin(); it!=apps.end(); ++it) + cout << '\t' << it->first << endl; +} + +static int shutdown_main(int argc, char *argv[]) +{ + cout << "Shutting down" << endl; + exit(0); +} + +static int list_tasks_main(int argc, char *argv[]) +{ + px4_show_tasks(); + return 0; +} + +""" + diff --git a/makefiles/firmware_qurt.mk b/makefiles/firmware_qurt.mk index 6acec4e733..6385517214 100644 --- a/makefiles/firmware_qurt.mk +++ b/makefiles/firmware_qurt.mk @@ -53,4 +53,11 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: generateuorbtopicheaders WORK_DIR=$(work_dir) \ $(FIRMWARE_GOAL) - +HEXAGON_TOOLS_ROOT = /opt/6.4.05 +#V_ARCH = v4 +V_ARCH = v5 +HEXAGON_CLANG_BIN = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT)) +SIM = $(HEXAGON_CLANG_BIN)/hexagon-sim +SIMFLAGS+= -m$(V_ARCH) --timing +sim: + $(SIM) $(SIMFLAGS) Build/qurt_default.build/mainapp From c9d4f02541975a0b2a073196410c13965b8c435c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 18:44:39 -0700 Subject: [PATCH 121/258] QuRT: added sched.h Added the pieces required from sched.h Signed-off-by: Mark Charlebois --- src/platforms/qurt/include/sched.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 src/platforms/qurt/include/sched.h diff --git a/src/platforms/qurt/include/sched.h b/src/platforms/qurt/include/sched.h new file mode 100644 index 0000000000..7d7724c5fa --- /dev/null +++ b/src/platforms/qurt/include/sched.h @@ -0,0 +1,13 @@ +#pragma once + +#define SCHED_FIFO 1 +#define SCHED_RR 2 + +struct sched_param +{ + int sched_priority; +}; + +int sched_get_priority_max(int policy); +int sched_get_priority_min(int policy); + From f44a23bc2648890a59358894a62a6e9361dbddfb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 17 Apr 2015 22:18:41 -0700 Subject: [PATCH 122/258] Check stack for commander only for NuttX Turn off check of stack if not a NuttX build Signed-off-by: Mark Charlebois --- src/modules/commander/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4ef361a826..4176f212d1 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -59,5 +59,7 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os +ifdef ($(PX4_TARGET_OS),nuttx) EXTRACXXFLAGS = -Wframe-larger-than=2200 +endif From b7a5e4df58246ebbfc9aae1585cc34ec75d4fcae Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 08:43:15 -0700 Subject: [PATCH 123/258] Linux: Fixed px4_ioctl calls that should be ::ioctl If simulate is not true, then a read I2C device is present. The global scope ioctl should be called on _fd, not px4_ioctl. Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_linux.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 3929c79794..2de830a521 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -67,7 +67,7 @@ I2C::I2C(const char *name, _address(address), _fd(-1) { - printf("I2C::I2C name = %s devname = %s\n", name, devname); + warnx("I2C::I2C name = %s devname = %s", name, devname); // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; _device_id.devid_s.bus = bus; @@ -163,11 +163,11 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re packets.nmsgs = msgs; if (simulate) { - printf("I2C SIM: transfer_4 on %s\n", get_devname()); + warnx("I2C SIM: transfer_4 on %s", get_devname()); ret = PX4_OK; } else { - ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); if (ret < 0) { warnx("I2C transfer failed"); return 1; @@ -199,11 +199,11 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) packets.nmsgs = msgs; if (simulate) { - printf("I2C SIM: transfer_2 on %s\n", get_devname()); + warnx("I2C SIM: transfer_2 on %s", get_devname()); ret = PX4_OK; } else { - ret = px4_ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); } if (ret < 0) { warnx("I2C transfer failed"); @@ -237,7 +237,7 @@ ssize_t I2C::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) { if (simulate) { // FIXME no idea what this should be - printf ("2C SIM I2C::read\n"); + warnx ("2C SIM I2C::read"); return 0; } @@ -247,6 +247,7 @@ ssize_t I2C::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) ssize_t I2C::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) { if (simulate) { + warnx ("2C SIM I2C::write"); return buflen; } From 94b622998ac28c078cc111562a20a0cd793c44b1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 12:02:23 -0700 Subject: [PATCH 124/258] Silence use of gnu extension gnu-array-member-paren-init Added -Wno-gnu-array-member-paren-init to toolchain_native.mk Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index f26678486a..cd3bbeaff6 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -156,6 +156,7 @@ ARCHWARNINGS = -Wall \ -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter \ + -Wno-gnu-array-member-paren-init \ -Werror=format-security \ -Werror=array-bounds \ -Wfatal-errors \ From 455b0dcaff7929f69a382fc07efd94691c6fc794 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 12:04:46 -0700 Subject: [PATCH 125/258] Fixed parenthesis bug Clang found the following: if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) which is doing fsbsf( bool ) Fixed to be: if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) Signed-off-by: Mark Charlebois --- src/modules/commander/PreflightCheck.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 6bb7e5c245..2c05fb33d1 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -40,7 +40,7 @@ * @author Johan Jansen */ -#include +#include #include #include #include @@ -259,7 +259,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional) goto out; } - if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { + if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } From 612579c809255a289842b4e565b486492c330ae0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 12:08:06 -0700 Subject: [PATCH 126/258] Removed check for isfinite as no longer needed PX4_ISFINITE resolves the definition of isfinite. Signed-off-by: Mark Charlebois --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index d3200047ed..5d56dbaae3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -49,10 +49,6 @@ #define M_PI_F static_cast(M_PI) #endif -#ifndef isfinite -#define isfinite(__x) std::isfinite(__x) -#endif - constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f; AttPosEKF::AttPosEKF() : From 36d17a061e24993ca033c44058e6873ba043336c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 12:09:54 -0700 Subject: [PATCH 127/258] Linux: Update mavlink files to track nuttx upstream Modified LInux impl to track changes to nuttx impl. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main_linux.cpp | 4 +-- src/modules/mavlink/mavlink_main_linux.h | 31 +++++++++++----------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp index e9bb7b1308..784118916d 100644 --- a/src/modules/mavlink/mavlink_main_linux.cpp +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -751,7 +751,7 @@ Mavlink::get_free_tx_buf() } void -Mavlink::send_message(const uint8_t msgid, const void *msg) +Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ @@ -785,7 +785,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) /* use mavlink's internal counter for the TX seq */ buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; + buf[4] = (component_ID == 0) ? mavlink_system.compid : component_ID; buf[5] = msgid; /* payload */ diff --git a/src/modules/mavlink/mavlink_main_linux.h b/src/modules/mavlink/mavlink_main_linux.h index 693d8451ca..1edb5c720f 100644 --- a/src/modules/mavlink/mavlink_main_linux.h +++ b/src/modules/mavlink/mavlink_main_linux.h @@ -144,6 +144,13 @@ public: bool get_forwarding_on() { return _forwarding_on; } + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + static int start_helper(int argc, char *argv[]); /** @@ -163,12 +170,12 @@ public: */ int set_hil_enabled(bool hil_enabled); - void send_message(const uint8_t msgid, const void *msg); + void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); /** * Resend message as is, don't change sequence number and CRC. */ - void resend_message(mavlink_message_t *msg); + void resend_message(mavlink_message_t *msg); void handle_message(const mavlink_message_t *msg); @@ -310,7 +317,7 @@ private: int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages - float _rate_mult; + float _rate_mult; /** * If the queue index is not at 0, the queue sending @@ -332,9 +339,9 @@ private: unsigned _bytes_txerr; unsigned _bytes_rx; uint64_t _bytes_timestamp; - float _rate_tx; - float _rate_txerr; - float _rate_rx; + float _rate_tx; + float _rate_txerr; + float _rate_rx; struct telemetry_status_s _rstatus; ///< receive status @@ -364,16 +371,9 @@ private: void mavlink_update_system(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - - static unsigned int interval_from_rate(float rate); + static unsigned int interval_from_rate(float rate); int configure_stream(const char *stream_name, const float rate); @@ -404,7 +404,6 @@ private: void update_rate_mult(); virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** * Main mavlink task. From 710fe76cdf8b0c39932200f41e230dcf1694f7d6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 12:11:27 -0700 Subject: [PATCH 128/258] Linux: minor fixups for rebase to master These changes were required after the rebase to master. Signed-off-by: Mark Charlebois --- src/drivers/drv_gpio.h | 2 +- src/drivers/hil/hil_linux.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index e11a035c20..c023ae7420 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -120,7 +120,7 @@ #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ - !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) \ + !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ !defined(CONFIG_ARCH_BOARD_LINUXTEST) # error No CONFIG_ARCH_BOARD_xxxx set #endif diff --git a/src/drivers/hil/hil_linux.cpp b/src/drivers/hil/hil_linux.cpp index 93b3af7811..eca2ec8ba5 100644 --- a/src/drivers/hil/hil_linux.cpp +++ b/src/drivers/hil/hil_linux.cpp @@ -401,7 +401,7 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ From 260bbcb64a2135e1e7ee969ffb031f9d233fcbe0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 12:57:02 -0700 Subject: [PATCH 129/258] Nuttx: fixups after rebase on Linux Seems that mavlink_receiver_linux.cpp inherited the history from mavlink_receiver.cpp so updates went to it vs mavlink_receiver_nuttx.cpp Two module.mk files used ifdef instead of ifeq. Signed-off-by: Mark Charlebois --- src/drivers/drv_led.h | 1 + src/drivers/hil/module.mk | 2 +- .../commander/calibration_routines.cpp | 8 ++-- src/modules/commander/module.mk | 2 +- .../mavlink/mavlink_receiver_nuttx.cpp | 41 +++++++++++++++++-- 5 files changed, 43 insertions(+), 11 deletions(-) diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 3f5897bd00..dbcfde91fa 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index d9137e99b1..0fe193e5c8 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -37,7 +37,7 @@ MODULE_COMMAND = hil -ifdef ($(PX4_TARGET_OS),nuttx) +ifeq ($(PX4_TARGET_OS),nuttx) SRCS = hil.cpp MAXOPTIMIZATION = -Os else diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 98a354e0cd..02d1d8a86b 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -38,6 +38,7 @@ * @author Lorenz Meier */ +#include #include #include #include @@ -53,9 +54,6 @@ #include "calibration_messages.h" #include "commander_helper.h" -// FIXME: Fix return codes -//static const int ERROR = -1; - int sphere_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) @@ -414,7 +412,7 @@ int calibrate_from_orientation(int mavlink_fd, int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0); if (sub_accel < 0) { mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort"); - return ERROR; + return PX4_ERROR; } unsigned orientation_failures = 0; @@ -422,7 +420,7 @@ int calibrate_from_orientation(int mavlink_fd, // Rotate through all three main positions while (true) { if (orientation_failures > 10) { - result = ERROR; + result = PX4_ERROR; mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT); break; } diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4176f212d1..5fb11ca165 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -59,7 +59,7 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -ifdef ($(PX4_TARGET_OS),nuttx) +ifeq ($(PX4_TARGET_OS),nuttx) EXTRACXXFLAGS = -Wframe-larger-than=2200 endif diff --git a/src/modules/mavlink/mavlink_receiver_nuttx.cpp b/src/modules/mavlink/mavlink_receiver_nuttx.cpp index 51ff07d0ee..81c76ef3f0 100644 --- a/src/modules/mavlink/mavlink_receiver_nuttx.cpp +++ b/src/modules/mavlink/mavlink_receiver_nuttx.cpp @@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rc_pub(-1), _manual_pub(-1), _land_detector_pub(-1), + _time_offset_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -157,6 +158,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_optical_flow_rad(msg); break; + case MAVLINK_MSG_ID_PING: + handle_message_ping(msg); + break; + case MAVLINK_MSG_ID_SET_MODE: handle_message_set_mode(msg); break; @@ -725,8 +730,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) // Use the component ID to identify the vision sensor vision_position.id = msg->compid; - vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time - vision_position.timestamp_computer = pos.usec; + vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time + vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time vision_position.x = pos.x; vision_position.y = pos.y; vision_position.z = pos.z; @@ -946,6 +951,19 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) +{ + mavlink_ping_t ping; + mavlink_msg_ping_decode( msg, &ping); + if ((mavlink_system.sysid == ping.target_system) && + (mavlink_system.compid == ping.target_component)) { + mavlink_message_t msg_out; + mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping); + _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out); + } +} + void MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) { @@ -996,6 +1014,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); + struct time_offset_s tsync_offset; + memset(&tsync_offset, 0, sizeof(tsync_offset)); + uint64_t now_ns = hrt_absolute_time() * 1000LL ; if (tsync.tc1 == 0) { @@ -1022,6 +1043,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } } + tsync_offset.offset_ns = _time_offset ; + + if (_time_offset_pub < 0) { + _time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset); + + } else { + orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset); + } + } void @@ -1505,9 +1535,12 @@ void MavlinkReceiver::print_status() } -uint64_t MavlinkReceiver::to_hrt(uint64_t usec) +uint64_t MavlinkReceiver::sync_stamp(uint64_t usec) { - return usec - (_time_offset / 1000) ; + if(_time_offset > 0) + return usec - (_time_offset / 1000) ; + else + return hrt_absolute_time(); } From 4749974d5cf709929b0f454d130f6dfefd2627b4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 14:22:51 -0700 Subject: [PATCH 130/258] Made nuttx the default PX4_TARGET_OS The CI builder should work when nuttx is set to be the default Signed-off-by: Mark Charlebois --- makefiles/setup.mk | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 8540718d4f..e9da766c45 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,9 +33,10 @@ # Path and tool setup # -#export PX4_TARGET_OS = nuttx -export PX4_TARGET_OS = linux -#export PX4_TARGET_OS ?= qurt +# PX4_TARGET_OS can be nuttx, linux, or qurt +ifeq ($(PX4_TARGET_OS),) +$(error Use: make PX4_TARGET_OS= where is nuttx, linux, or qurt) +endif # # Some useful paths. From 0b649204b02ade0dd0e2c139a559bbee84f87f0a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 20 Apr 2015 14:35:56 -0700 Subject: [PATCH 131/258] Make nuttx the default PX4_TARGET_OS This should make the CI builder happy again. Also fixed another itdef that should have been ifeq Signed-off-by: Mark Charlebois --- makefiles/setup.mk | 2 ++ src/drivers/rgbled/module.mk | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index e9da766c45..646846b253 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,6 +33,8 @@ # Path and tool setup # +export PX4_TARGET_OS ?= nuttx + # PX4_TARGET_OS can be nuttx, linux, or qurt ifeq ($(PX4_TARGET_OS),) $(error Use: make PX4_TARGET_OS= where is nuttx, linux, or qurt) diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index 5ffa628c0d..a96480b38d 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,7 +4,7 @@ MODULE_COMMAND = rgbled -ifdef ($(PX4_TARGET_OS),nuttx) +ifeq ($(PX4_TARGET_OS),nuttx) SRCS = rgbled.cpp else SRCS = rgbled_linux.cpp From 31818b30b6e7402ee6a6ebac9ef8381478f4ee20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 09:23:26 +0200 Subject: [PATCH 132/258] Linux: Ignore generated messages --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index bd835e271f..c453432f78 100644 --- a/.gitignore +++ b/.gitignore @@ -48,3 +48,4 @@ unittests/build .vagrant *.pretty xcode +src/platforms/linux/px4_messages/ From 40faa98416f641503446c09e179e30e58e028e73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Apr 2015 09:23:36 +0200 Subject: [PATCH 133/258] Linux: Fix unit tests --- unittests/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 84078c3e5d..96c2f995f7 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -24,6 +24,7 @@ include_directories(${PX_SRC}) include_directories(${PX_SRC}/modules) include_directories(${PX_SRC}/lib) include_directories(${PX_SRC}/drivers) +include_directories(${PX_SRC}/platforms) add_definitions(-D__EXPORT=) add_definitions(-D__PX4_TESTS) @@ -32,6 +33,7 @@ add_definitions(-Dmain_t=int) add_definitions(-DERROR=-1) add_definitions(-DOK=0) add_definitions(-D_UNIT_TEST=) +add_definitions(-D__PX4_LINUX) # check add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) From f3b5076d70f2641b43ec7b5b64d65db7937464bc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 09:53:09 -0700 Subject: [PATCH 134/258] Linux to posix conversion Changed "linux" target to "posix". Most of the changes are shared with QuRT and with OSX. The Linux specific parts are in for i2c which uses and . There is also a check for __PX4_LINUX in mavlink for a tty ioctl that is not supported. Signed-off-by: Mark Charlebois --- Makefile | 8 ++-- Tools/{linux_apps.py => posix_apps.py} | 0 Tools/{linux_run.sh => posix_run.sh} | 0 makefiles/README.txt | 10 ++--- makefiles/firmware.mk | 4 +- .../{firmware_linux.mk => firmware_posix.mk} | 0 .../{ => nuttx}/board_px4-stm32f4discovery.mk | 0 .../config_px4-stm32f4discovery_default.mk | 0 makefiles/{ => nuttx}/gumstix-aerocore.cfg | 0 makefiles/{linux.mk => posix.mk} | 2 +- .../board_linux.mk => posix/board_posix.mk} | 4 +- .../config_posix_default.mk} | 25 +++++------- makefiles/{linux_elf.mk => posix_elf.mk} | 6 +-- makefiles/setup.mk | 4 +- makefiles/toolchain_hexagon.mk | 2 +- makefiles/toolchain_native.mk | 8 ++-- makefiles/upload.mk | 2 +- .../posixtest}/init/rc.S | 0 .../posixtest}/scripts/ld.script | 0 .../{blinkm_linux.cpp => blinkm_posix.cpp} | 0 src/drivers/blinkm/module.mk | 2 +- src/drivers/device/device.h | 2 +- src/drivers/device/i2c.h | 2 +- .../device/{i2c_linux.cpp => i2c_posix.cpp} | 2 + .../device/{i2c_linux.h => i2c_posix.h} | 2 + src/drivers/device/module.mk | 4 +- src/drivers/drv_gpio.h | 6 +-- .../hil/{hil_linux.cpp => hil_posix.cpp} | 0 src/drivers/hil/module.mk | 2 +- src/drivers/ms5611/module.mk | 2 +- .../{ms5611_linux.cpp => ms5611_posix.cpp} | 0 src/drivers/rgbled/module.mk | 2 +- .../{rgbled_linux.cpp => rgbled_posix.cpp} | 0 src/lib/mathlib/math/Limits.cpp | 2 +- src/lib/version/version.h | 2 +- src/modules/commander/module.mk | 2 +- ...nux.cpp => state_machine_helper_posix.cpp} | 0 ...nk_ftp_linux.cpp => mavlink_ftp_posix.cpp} | 0 src/modules/mavlink/mavlink_main.h | 2 +- ..._main_linux.cpp => mavlink_main_posix.cpp} | 0 ...link_main_linux.h => mavlink_main_posix.h} | 2 +- .../mavlink/mavlink_receiver_nuttx.cpp | 2 +- ...r_linux.cpp => mavlink_receiver_posix.cpp} | 0 src/modules/mavlink/mavlink_tests/module.mk | 2 +- src/modules/mavlink/module.mk | 6 +-- src/modules/sensors/module.mk | 2 +- src/modules/sensors/sensors_nuttx.cpp | 2 +- .../{sensors_linux.cpp => sensors_posix.cpp} | 0 src/modules/systemlib/param/param.h | 2 +- .../drivers/accelsim/accelsim.cpp | 0 .../drivers/accelsim/module.mk | 0 .../drivers/adcsim/adcsim.cpp | 0 .../{linux => posix}/drivers/adcsim/module.mk | 0 .../{linux => posix}/drivers/barosim/baro.cpp | 0 .../drivers/barosim/baro_sim.cpp | 0 .../drivers/barosim/barosim.h | 0 .../drivers/barosim/module.mk | 0 .../drivers/gyrosim/gyrosim.cpp | 0 .../drivers/gyrosim/module.mk | 0 .../include/arch/board/board.h | 0 .../{linux => posix}/include/board_config.h | 0 .../{linux => posix}/include/crc32.h | 0 .../{linux => posix}/include/queue.h | 0 src/platforms/{linux => posix}/main.cpp | 0 .../{linux => posix}/px4_layer/dq_addlast.c | 0 .../{linux => posix}/px4_layer/dq_rem.c | 0 .../{linux => posix}/px4_layer/dq_remfirst.c | 0 .../{linux => posix}/px4_layer/drv_hrt.c | 0 .../{linux => posix}/px4_layer/lib_crc32.c | 0 .../{linux => posix}/px4_layer/module.mk | 4 +- .../px4_layer/px4_posix_impl.cpp} | 2 +- .../px4_layer/px4_posix_tasks.cpp} | 2 +- .../{linux => posix}/px4_layer/queue.c | 2 +- .../{linux => posix}/px4_layer/sq_addafter.c | 0 .../{linux => posix}/px4_layer/sq_addlast.c | 0 .../{linux => posix}/px4_layer/sq_remfirst.c | 0 .../{linux => posix}/px4_layer/work_cancel.c | 0 .../{linux => posix}/px4_layer/work_queue.c | 0 .../{linux => posix}/px4_layer/work_thread.c | 0 .../tests/hello/hello_example.cpp | 0 .../tests/hello/hello_example.h | 0 .../tests/hello/hello_main.cpp | 0 .../tests/hello/hello_start_posix.cpp} | 2 +- .../{linux => posix}/tests/hello/module.mk | 2 +- .../tests/hrt_test/hrt_test.cpp | 0 .../tests/hrt_test/hrt_test.h | 0 .../tests/hrt_test/hrt_test_main.cpp | 0 .../tests/hrt_test/hrt_test_start_posix.cpp} | 2 +- .../{linux => posix}/tests/hrt_test/module.mk | 2 +- .../tests/vcdev_test/module.mk | 2 +- .../tests/vcdev_test/vcdevtest_example.cpp | 0 .../tests/vcdev_test/vcdevtest_example.h | 0 .../tests/vcdev_test/vcdevtest_main.cpp | 0 .../vcdev_test/vcdevtest_start_posix.cpp} | 2 +- .../{linux => posix}/tests/wqueue/module.mk | 2 +- .../tests/wqueue/wqueue_main.cpp | 0 .../tests/wqueue/wqueue_start_poosix.cpp} | 2 +- .../tests/wqueue/wqueue_test.cpp | 0 .../tests/wqueue/wqueue_test.h | 0 src/platforms/px4_adc.h | 2 +- src/platforms/px4_config.h | 4 +- src/platforms/px4_defines.h | 6 +-- src/platforms/px4_i2c.h | 2 +- src/platforms/px4_includes.h | 38 +++++++++---------- src/platforms/px4_spi.h | 2 +- src/platforms/px4_tasks.h | 2 +- src/platforms/px4_workqueue.h | 2 +- .../tests/hello/hello_start_posix.cpp} | 0 src/platforms/qurt/tests/hello/module.mk | 2 +- 109 files changed, 106 insertions(+), 107 deletions(-) rename Tools/{linux_apps.py => posix_apps.py} (100%) rename Tools/{linux_run.sh => posix_run.sh} (100%) rename makefiles/{firmware_linux.mk => firmware_posix.mk} (100%) rename makefiles/{ => nuttx}/board_px4-stm32f4discovery.mk (100%) rename makefiles/{ => nuttx}/config_px4-stm32f4discovery_default.mk (100%) rename makefiles/{ => nuttx}/gumstix-aerocore.cfg (100%) rename makefiles/{linux.mk => posix.mk} (98%) rename makefiles/{linux/board_linux.mk => posix/board_posix.mk} (55%) rename makefiles/{linux/config_linux_default.mk => posix/config_posix_default.mk} (63%) rename makefiles/{linux_elf.mk => posix_elf.mk} (94%) rename {linux-configs/linuxtest => posix-configs/posixtest}/init/rc.S (100%) rename {linux-configs/linuxtest => posix-configs/posixtest}/scripts/ld.script (100%) rename src/drivers/blinkm/{blinkm_linux.cpp => blinkm_posix.cpp} (100%) rename src/drivers/device/{i2c_linux.cpp => i2c_posix.cpp} (99%) rename src/drivers/device/{i2c_linux.h => i2c_posix.h} (99%) rename src/drivers/hil/{hil_linux.cpp => hil_posix.cpp} (100%) rename src/drivers/ms5611/{ms5611_linux.cpp => ms5611_posix.cpp} (100%) rename src/drivers/rgbled/{rgbled_linux.cpp => rgbled_posix.cpp} (100%) rename src/modules/commander/{state_machine_helper_linux.cpp => state_machine_helper_posix.cpp} (100%) rename src/modules/mavlink/{mavlink_ftp_linux.cpp => mavlink_ftp_posix.cpp} (100%) rename src/modules/mavlink/{mavlink_main_linux.cpp => mavlink_main_posix.cpp} (100%) rename src/modules/mavlink/{mavlink_main_linux.h => mavlink_main_posix.h} (99%) rename src/modules/mavlink/{mavlink_receiver_linux.cpp => mavlink_receiver_posix.cpp} (100%) rename src/modules/sensors/{sensors_linux.cpp => sensors_posix.cpp} (100%) rename src/platforms/{linux => posix}/drivers/accelsim/accelsim.cpp (100%) rename src/platforms/{linux => posix}/drivers/accelsim/module.mk (100%) rename src/platforms/{linux => posix}/drivers/adcsim/adcsim.cpp (100%) rename src/platforms/{linux => posix}/drivers/adcsim/module.mk (100%) rename src/platforms/{linux => posix}/drivers/barosim/baro.cpp (100%) rename src/platforms/{linux => posix}/drivers/barosim/baro_sim.cpp (100%) rename src/platforms/{linux => posix}/drivers/barosim/barosim.h (100%) rename src/platforms/{linux => posix}/drivers/barosim/module.mk (100%) rename src/platforms/{linux => posix}/drivers/gyrosim/gyrosim.cpp (100%) rename src/platforms/{linux => posix}/drivers/gyrosim/module.mk (100%) rename src/platforms/{linux => posix}/include/arch/board/board.h (100%) rename src/platforms/{linux => posix}/include/board_config.h (100%) rename src/platforms/{linux => posix}/include/crc32.h (100%) rename src/platforms/{linux => posix}/include/queue.h (100%) rename src/platforms/{linux => posix}/main.cpp (100%) rename src/platforms/{linux => posix}/px4_layer/dq_addlast.c (100%) rename src/platforms/{linux => posix}/px4_layer/dq_rem.c (100%) rename src/platforms/{linux => posix}/px4_layer/dq_remfirst.c (100%) rename src/platforms/{linux => posix}/px4_layer/drv_hrt.c (100%) rename src/platforms/{linux => posix}/px4_layer/lib_crc32.c (100%) rename src/platforms/{linux => posix}/px4_layer/module.mk (97%) rename src/platforms/{linux/px4_layer/px4_linux_impl.cpp => posix/px4_layer/px4_posix_impl.cpp} (98%) rename src/platforms/{linux/px4_layer/px4_linux_tasks.cpp => posix/px4_layer/px4_posix_tasks.cpp} (99%) rename src/platforms/{linux => posix}/px4_layer/queue.c (98%) rename src/platforms/{linux => posix}/px4_layer/sq_addafter.c (100%) rename src/platforms/{linux => posix}/px4_layer/sq_addlast.c (100%) rename src/platforms/{linux => posix}/px4_layer/sq_remfirst.c (100%) rename src/platforms/{linux => posix}/px4_layer/work_cancel.c (100%) rename src/platforms/{linux => posix}/px4_layer/work_queue.c (100%) rename src/platforms/{linux => posix}/px4_layer/work_thread.c (100%) rename src/platforms/{linux => posix}/tests/hello/hello_example.cpp (100%) rename src/platforms/{linux => posix}/tests/hello/hello_example.h (100%) rename src/platforms/{linux => posix}/tests/hello/hello_main.cpp (100%) rename src/platforms/{qurt/tests/hello/hello_start_linux.cpp => posix/tests/hello/hello_start_posix.cpp} (99%) rename src/platforms/{linux => posix}/tests/hello/module.mk (98%) rename src/platforms/{linux => posix}/tests/hrt_test/hrt_test.cpp (100%) rename src/platforms/{linux => posix}/tests/hrt_test/hrt_test.h (100%) rename src/platforms/{linux => posix}/tests/hrt_test/hrt_test_main.cpp (100%) rename src/platforms/{linux/tests/hrt_test/hrt_test_start_linux.cpp => posix/tests/hrt_test/hrt_test_start_posix.cpp} (98%) rename src/platforms/{linux => posix}/tests/hrt_test/module.mk (98%) rename src/platforms/{linux => posix}/tests/vcdev_test/module.mk (98%) rename src/platforms/{linux => posix}/tests/vcdev_test/vcdevtest_example.cpp (100%) rename src/platforms/{linux => posix}/tests/vcdev_test/vcdevtest_example.h (100%) rename src/platforms/{linux => posix}/tests/vcdev_test/vcdevtest_main.cpp (100%) rename src/platforms/{linux/tests/vcdev_test/vcdevtest_start_linux.cpp => posix/tests/vcdev_test/vcdevtest_start_posix.cpp} (98%) rename src/platforms/{linux => posix}/tests/wqueue/module.mk (98%) rename src/platforms/{linux => posix}/tests/wqueue/wqueue_main.cpp (100%) rename src/platforms/{linux/tests/wqueue/wqueue_start_linux.cpp => posix/tests/wqueue/wqueue_start_poosix.cpp} (98%) rename src/platforms/{linux => posix}/tests/wqueue/wqueue_test.cpp (100%) rename src/platforms/{linux => posix}/tests/wqueue/wqueue_test.h (100%) rename src/platforms/{linux/tests/hello/hello_start_linux.cpp => qurt/tests/hello/hello_start_posix.cpp} (100%) diff --git a/Makefile b/Makefile index a079e4fc47..88ce8e5de6 100644 --- a/Makefile +++ b/Makefile @@ -100,8 +100,8 @@ endif ifeq ($(PX4_TARGET_OS),nuttx) include $(PX4_BASE)makefiles/firmware_nuttx.mk endif -ifeq ($(PX4_TARGET_OS),linux) -include $(PX4_BASE)makefiles/firmware_linux.mk +ifeq ($(PX4_TARGET_OS),posix) +include $(PX4_BASE)makefiles/firmware_posix.mk endif ifeq ($(PX4_TARGET_OS),qurt) include $(PX4_BASE)makefiles/firmware_qurt.mk @@ -146,8 +146,8 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -linuxrun: - Tools/linux_run.sh +posixrun: + Tools/posix_run.sh # # Unittest targets. Builds and runs the host-level diff --git a/Tools/linux_apps.py b/Tools/posix_apps.py similarity index 100% rename from Tools/linux_apps.py rename to Tools/posix_apps.py diff --git a/Tools/linux_run.sh b/Tools/posix_run.sh similarity index 100% rename from Tools/linux_run.sh rename to Tools/posix_run.sh diff --git a/makefiles/README.txt b/makefiles/README.txt index 36edf6a47b..9578793957 100644 --- a/makefiles/README.txt +++ b/makefiles/README.txt @@ -30,9 +30,9 @@ firmware_nuttx.mk Called by firmware.mk to build NuttX based firmware. -firmware_linux.mk +firmware_posix.mk - Called by firmware.mk to build Linux (non-ROS) based firmware. + Called by firmware.mk to build POSIX (non-ROS) based firmware. module.mk @@ -46,10 +46,10 @@ nuttx.mk Called by ../Makefile to build or download the NuttX archives if PX4_TARGET_OS is set to "nuttx". -linux.mk +posix.mk - Called by ../Makefile to set Linux specific parameters if - PX4_TARGET_OS is set to "linux". + Called by ../Makefile to set POSIX specific parameters if + PX4_TARGET_OS is set to "posix". upload.mk diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 5f5447da6b..b58f23cbc2 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -360,8 +360,8 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) ifeq ($(PX4_TARGET_OS),nuttx) include $(MK_DIR)/nuttx_romfs.mk endif -ifeq ($(PX4_TARGET_OS),linux) -include $(MK_DIR)/linux_elf.mk +ifeq ($(PX4_TARGET_OS),posix) +include $(MK_DIR)/posix_elf.mk endif ifeq ($(PX4_TARGET_OS),qurt) include $(MK_DIR)/qurt_elf.mk diff --git a/makefiles/firmware_linux.mk b/makefiles/firmware_posix.mk similarity index 100% rename from makefiles/firmware_linux.mk rename to makefiles/firmware_posix.mk diff --git a/makefiles/board_px4-stm32f4discovery.mk b/makefiles/nuttx/board_px4-stm32f4discovery.mk similarity index 100% rename from makefiles/board_px4-stm32f4discovery.mk rename to makefiles/nuttx/board_px4-stm32f4discovery.mk diff --git a/makefiles/config_px4-stm32f4discovery_default.mk b/makefiles/nuttx/config_px4-stm32f4discovery_default.mk similarity index 100% rename from makefiles/config_px4-stm32f4discovery_default.mk rename to makefiles/nuttx/config_px4-stm32f4discovery_default.mk diff --git a/makefiles/gumstix-aerocore.cfg b/makefiles/nuttx/gumstix-aerocore.cfg similarity index 100% rename from makefiles/gumstix-aerocore.cfg rename to makefiles/nuttx/gumstix-aerocore.cfg diff --git a/makefiles/linux.mk b/makefiles/posix.mk similarity index 98% rename from makefiles/linux.mk rename to makefiles/posix.mk index d050259c57..13cb97f0db 100644 --- a/makefiles/linux.mk +++ b/makefiles/posix.mk @@ -36,5 +36,5 @@ MODULES += \ platforms/common \ - platforms/linux/px4_layer + platforms/posix/px4_layer diff --git a/makefiles/linux/board_linux.mk b/makefiles/posix/board_posix.mk similarity index 55% rename from makefiles/linux/board_linux.mk rename to makefiles/posix/board_posix.mk index cf5d42ff4c..93146b6a2e 100644 --- a/makefiles/linux/board_linux.mk +++ b/makefiles/posix/board_posix.mk @@ -1,11 +1,11 @@ # -# Board-specific definitions for the Linux port of PX4 +# Board-specific definitions for the POSIX port of PX4 # # # Configure the toolchain # CONFIG_ARCH = NATIVE -CONFIG_BOARD = LINUXTEST +CONFIG_BOARD = POSIXTEST include $(PX4_MK_DIR)/toolchain_native.mk diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/posix/config_posix_default.mk similarity index 63% rename from makefiles/linux/config_linux_default.mk rename to makefiles/posix/config_posix_default.mk index 5dfd487e77..53b3153468 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -1,12 +1,7 @@ # -# Makefile for the Foo *default* configuration +# Makefile for the POSIXTEST *default* configuration # -# -# Use the configuration's ROMFS. -# -#ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common - # # Board support modules # @@ -61,17 +56,17 @@ MODULES += lib/conversion # # Linux port # -MODULES += platforms/linux/px4_layer -MODULES += platforms/linux/drivers/accelsim -MODULES += platforms/linux/drivers/gyrosim -MODULES += platforms/linux/drivers/adcsim -MODULES += platforms/linux/drivers/barosim +MODULES += platforms/posix/px4_layer +MODULES += platforms/posix/drivers/accelsim +MODULES += platforms/posix/drivers/gyrosim +MODULES += platforms/posix/drivers/adcsim +MODULES += platforms/posix/drivers/barosim # # Unit tests # -#MODULES += platforms/linux/tests/hello -#MODULES += platforms/linux/tests/vcdev_test -#MODULES += platforms/linux/tests/hrt_test -#MODULES += platforms/linux/tests/wqueue +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue diff --git a/makefiles/linux_elf.mk b/makefiles/posix_elf.mk similarity index 94% rename from makefiles/linux_elf.mk rename to makefiles/posix_elf.mk index 701d731ef3..edb1e32d76 100644 --- a/makefiles/linux_elf.mk +++ b/makefiles/posix_elf.mk @@ -30,7 +30,7 @@ # # -# Makefile for PX4 Linux based firmware images. +# Makefile for PX4 POSIX based firmware images. # ################################################################################ @@ -56,9 +56,9 @@ $(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) -MAIN = $(PX4_BASE)/src/platforms/linux/main.cpp +MAIN = $(PX4_BASE)/src/platforms/posix/main.cpp $(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) - $(PX4_BASE)/Tools/linux_apps.py > apps.h + $(PX4_BASE)/Tools/posix_apps.py > apps.h $(call LINK,$@, -I. $(MAIN) $(PRODUCT_SHARED_LIB)) # diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 646846b253..0cdaa1e835 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -35,9 +35,9 @@ export PX4_TARGET_OS ?= nuttx -# PX4_TARGET_OS can be nuttx, linux, or qurt +# PX4_TARGET_OS can be nuttx, posix, or qurt ifeq ($(PX4_TARGET_OS),) -$(error Use: make PX4_TARGET_OS= where is nuttx, linux, or qurt) +$(error Use: make PX4_TARGET_OS= where is nuttx, posix, or qurt) endif # diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index dacb60bfb1..aab9fada1c 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -181,7 +181,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ $(EXTRADEFINES) \ $(EXTRAAFLAGS) -LDSCRIPT = $(PX4_BASE)/linux-configs/linuxtest/scripts/ld.script +LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script # Flags we pass to the linker # LDFLAGS += \ diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index cd3bbeaff6..763b25ce86 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -1,7 +1,7 @@ # # Copyright (C) 2012-2014 PX4 Development Team. All rights reuint32_tserved. # -# 2005 Modified for clang and GCC on Linux: +# 2005 Modified for clang and GCC on POSIX: # Author: Mark Charlebois # # Redistribution and use in source and binary forms, with or without @@ -120,10 +120,10 @@ ifeq ($(CONFIG_BOARD),) $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ - -D__PX4_LINUX \ + -D__PX4_LINUX -D__PX4_POSIX \ -Dnoreturn_function= \ -I$(PX4_BASE)/src/lib/eigen \ - -I$(PX4_BASE)/src/platforms/linux/include \ + -I$(PX4_BASE)/src/platforms/posix/include \ -Wno-error=shadow # optimisation flags @@ -250,7 +250,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ $(EXTRADEFINES) \ $(EXTRAAFLAGS) -LDSCRIPT = $(PX4_BASE)/linux-configs/linuxtest/scripts/ld.script +LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script # Flags we pass to the linker # LDFLAGS += $(EXTRALDFLAGS) \ diff --git a/makefiles/upload.mk b/makefiles/upload.mk index dd7710bf72..c590f17d10 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -31,7 +31,7 @@ upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) upload-serial-aerocore: - openocd -f $(PX4_BASE)/makefiles/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit' + openocd -f $(PX4_BASE)/makefiles/nuttx/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit' upload-serial-px4-stm32f4discovery: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) diff --git a/linux-configs/linuxtest/init/rc.S b/posix-configs/posixtest/init/rc.S similarity index 100% rename from linux-configs/linuxtest/init/rc.S rename to posix-configs/posixtest/init/rc.S diff --git a/linux-configs/linuxtest/scripts/ld.script b/posix-configs/posixtest/scripts/ld.script similarity index 100% rename from linux-configs/linuxtest/scripts/ld.script rename to posix-configs/posixtest/scripts/ld.script diff --git a/src/drivers/blinkm/blinkm_linux.cpp b/src/drivers/blinkm/blinkm_posix.cpp similarity index 100% rename from src/drivers/blinkm/blinkm_linux.cpp rename to src/drivers/blinkm/blinkm_posix.cpp diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk index 712f266317..1547a17e92 100644 --- a/src/drivers/blinkm/module.mk +++ b/src/drivers/blinkm/module.mk @@ -40,7 +40,7 @@ MODULE_COMMAND = blinkm ifeq ($(PX4_TARGET_OS),nuttx) SRCS = blinkm_nuttx.cpp else -SRCS = blinkm_linux.cpp +SRCS = blinkm_posix.cpp endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 4d941b752d..e145635b37 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -35,7 +35,7 @@ #ifdef __PX4_NUTTX #include "device_nuttx.h" -#elif defined (__PX4_LINUX) +#elif defined (__PX4_POSIX) #include "vdev.h" #endif diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index fb0be9bd5d..e6a4dffc83 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -35,5 +35,5 @@ #ifdef __PX4_NUTTX #include "i2c_nuttx.h" #else -#include "i2c_linux.h" +#include "i2c_posix.h" #endif diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_posix.cpp similarity index 99% rename from src/drivers/device/i2c_linux.cpp rename to src/drivers/device/i2c_posix.cpp index 2de830a521..51e8e2083e 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -41,8 +41,10 @@ */ #include "i2c.h" +#ifdef __PX4_LINUX #include #include +#endif #include #include #include diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_posix.h similarity index 99% rename from src/drivers/device/i2c_linux.h rename to src/drivers/device/i2c_posix.h index c0a94aff01..e01e3c242d 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_posix.h @@ -43,8 +43,10 @@ #include "vdev.h" #include +#ifdef __PX4_LINUX #include #include +#endif #include namespace device __EXPORT diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 4e0b563e54..85826bbc40 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -44,11 +44,11 @@ SRCS = \ spi.cpp \ ringbuffer.cpp endif -ifeq ($(PX4_TARGET_OS),linux) +ifeq ($(PX4_TARGET_OS),posix) SRCS = vdev.cpp \ device.cpp \ vdev_posix.cpp \ - i2c_linux.cpp \ + i2c_posix.cpp \ sim.cpp \ ringbuffer.cpp endif diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index c023ae7420..a0faab8098 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -114,14 +114,14 @@ /* no GPIO driver on the PX4_STM32F4DISCOVERY board */ #endif -#ifdef CONFIG_ARCH_BOARD_LINUXTEST -/* no GPIO driver on the LINUXTEST board */ +#ifdef CONFIG_ARCH_BOARD_POSIXTEST +/* no GPIO driver on the POSIXTEST board */ #endif #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) && \ !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \ !defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ - !defined(CONFIG_ARCH_BOARD_LINUXTEST) + !defined(CONFIG_ARCH_BOARD_POSIXTEST) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* diff --git a/src/drivers/hil/hil_linux.cpp b/src/drivers/hil/hil_posix.cpp similarity index 100% rename from src/drivers/hil/hil_linux.cpp rename to src/drivers/hil/hil_posix.cpp diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index 0fe193e5c8..b168108b3e 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -41,6 +41,6 @@ ifeq ($(PX4_TARGET_OS),nuttx) SRCS = hil.cpp MAXOPTIMIZATION = -Os else -SRCS = hil_linux.cpp +SRCS = hil_posix.cpp endif diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 59316f4bfe..7ff49d7354 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -40,7 +40,7 @@ MODULE_COMMAND = ms5611 ifeq ($(PX4_TARGET_OS),nuttx) SRCS = ms5611_nuttx.cpp ms5611_spi.cpp ms5611_i2c.cpp else -SRCS = ms5611_linux.cpp ms5611_spi.cpp ms5611_i2c.cpp ms5611_sim.cpp +SRCS = ms5611_posix.cpp ms5611_spi.cpp ms5611_i2c.cpp ms5611_sim.cpp endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_posix.cpp similarity index 100% rename from src/drivers/ms5611/ms5611_linux.cpp rename to src/drivers/ms5611/ms5611_posix.cpp diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index a96480b38d..f756250071 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -7,7 +7,7 @@ MODULE_COMMAND = rgbled ifeq ($(PX4_TARGET_OS),nuttx) SRCS = rgbled.cpp else -SRCS = rgbled_linux.cpp +SRCS = rgbled_posix.cpp endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/rgbled_linux.cpp b/src/drivers/rgbled/rgbled_posix.cpp similarity index 100% rename from src/drivers/rgbled/rgbled_linux.cpp rename to src/drivers/rgbled/rgbled_posix.cpp diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index dffa213e05..4b1273c7a1 100644 --- a/src/lib/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp @@ -47,7 +47,7 @@ namespace math { -#if !defined(CONFIG_ARCH_ARM) && !defined(__PX4_LINUX) +#if !defined(CONFIG_ARCH_ARM) && !defined(__PX4_POSIX) #define M_PI_F 3.14159265358979323846f #endif diff --git a/src/lib/version/version.h b/src/lib/version/version.h index f0071f3b58..ddf13822df 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -59,7 +59,7 @@ #define HW_ARCH "PX4_STM32F4DISCOVERY" #endif -#ifdef CONFIG_ARCH_BOARD_LINUXTEST +#ifdef CONFIG_ARCH_BOARD_POSIXTEST #define HW_ARCH "LINUXTEST" #endif #endif /* VERSION_H_ */ diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 5fb11ca165..851ac9020d 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,7 +52,7 @@ ifdef ($(PX4_TARGET_OS),nuttx) SRCS += state_machine_helper.cpp else -SRCS += state_machine_helper_linux.cpp +SRCS += state_machine_helper_posix.cpp endif MODULE_STACKSIZE = 5000 diff --git a/src/modules/commander/state_machine_helper_linux.cpp b/src/modules/commander/state_machine_helper_posix.cpp similarity index 100% rename from src/modules/commander/state_machine_helper_linux.cpp rename to src/modules/commander/state_machine_helper_posix.cpp diff --git a/src/modules/mavlink/mavlink_ftp_linux.cpp b/src/modules/mavlink/mavlink_ftp_posix.cpp similarity index 100% rename from src/modules/mavlink/mavlink_ftp_linux.cpp rename to src/modules/mavlink/mavlink_ftp_posix.cpp diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 3153255b33..9b00d72c01 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -35,5 +35,5 @@ #ifdef __PX4_NUTTX #include "mavlink_main_nuttx.h" #else -#include "mavlink_main_linux.h" +#include "mavlink_main_posix.h" #endif diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_posix.cpp similarity index 100% rename from src/modules/mavlink/mavlink_main_linux.cpp rename to src/modules/mavlink/mavlink_main_posix.cpp diff --git a/src/modules/mavlink/mavlink_main_linux.h b/src/modules/mavlink/mavlink_main_posix.h similarity index 99% rename from src/modules/mavlink/mavlink_main_linux.h rename to src/modules/mavlink/mavlink_main_posix.h index 1edb5c720f..67f2c8aa6a 100644 --- a/src/modules/mavlink/mavlink_main_linux.h +++ b/src/modules/mavlink/mavlink_main_posix.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_main_linux.h + * @file mavlink_main_posix.h * MAVLink 1.0 protocol interface definition. * * @author Lorenz Meier diff --git a/src/modules/mavlink/mavlink_receiver_nuttx.cpp b/src/modules/mavlink/mavlink_receiver_nuttx.cpp index 81c76ef3f0..faede15cb9 100644 --- a/src/modules/mavlink/mavlink_receiver_nuttx.cpp +++ b/src/modules/mavlink/mavlink_receiver_nuttx.cpp @@ -41,7 +41,7 @@ */ /* XXX trim includes */ -#include +#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver_linux.cpp b/src/modules/mavlink/mavlink_receiver_posix.cpp similarity index 100% rename from src/modules/mavlink/mavlink_receiver_linux.cpp rename to src/modules/mavlink/mavlink_receiver_posix.cpp diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 030706cc47..bc52677412 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -42,7 +42,7 @@ SRCS = mavlink_tests.cpp \ ifeq ($(PX4_TARGET_NUTTX),nuttx) SRCS += ../mavlink_ftp_nuttx.cpp else -SRCS += ../mavlink_ftp_linux.cpp +SRCS += ../mavlink_ftp_posix.cpp endif INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 1740c6b776..20bf945b10 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -41,9 +41,9 @@ SRCS += mavlink_main_nuttx.cpp \ mavlink_ftp_nuttx.cpp \ mavlink_receiver_nuttx.cpp else -SRCS += mavlink_main_linux.cpp \ - mavlink_ftp_linux.cpp \ - mavlink_receiver_linux.cpp +SRCS += mavlink_main_posix.cpp \ + mavlink_ftp_posix.cpp \ + mavlink_receiver_posix.cpp endif SRCS += mavlink.c \ diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index d49f6fdfbf..7237600ff3 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -41,7 +41,7 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" ifeq ($(PX$_TARGET_OS),nuttx) SRCS = sensors_nuttx.cpp else -SRCS = sensors_linux.cpp +SRCS = sensors_posix.cpp endif SRCS += sensor_params.c diff --git a/src/modules/sensors/sensors_nuttx.cpp b/src/modules/sensors/sensors_nuttx.cpp index 86041165b3..3fc8627c15 100644 --- a/src/modules/sensors/sensors_nuttx.cpp +++ b/src/modules/sensors/sensors_nuttx.cpp @@ -46,7 +46,7 @@ * @author Anton Babushkin */ -#include +#include #include #include diff --git a/src/modules/sensors/sensors_linux.cpp b/src/modules/sensors/sensors_posix.cpp similarity index 100% rename from src/modules/sensors/sensors_linux.cpp rename to src/modules/sensors/sensors_posix.cpp diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 8f53a5d730..da810abea5 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -396,7 +396,7 @@ struct param_info_s { // See https://llvm.org/bugs/show_bug.cgi?format=multiple&id=18006 // The following hack is for GCC >=4.8 only. Clang works fine without // this. -#ifdef __PX4_LINUX +#ifdef __PX4_POSIX __attribute__((aligned(16))); #else ; diff --git a/src/platforms/linux/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp similarity index 100% rename from src/platforms/linux/drivers/accelsim/accelsim.cpp rename to src/platforms/posix/drivers/accelsim/accelsim.cpp diff --git a/src/platforms/linux/drivers/accelsim/module.mk b/src/platforms/posix/drivers/accelsim/module.mk similarity index 100% rename from src/platforms/linux/drivers/accelsim/module.mk rename to src/platforms/posix/drivers/accelsim/module.mk diff --git a/src/platforms/linux/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp similarity index 100% rename from src/platforms/linux/drivers/adcsim/adcsim.cpp rename to src/platforms/posix/drivers/adcsim/adcsim.cpp diff --git a/src/platforms/linux/drivers/adcsim/module.mk b/src/platforms/posix/drivers/adcsim/module.mk similarity index 100% rename from src/platforms/linux/drivers/adcsim/module.mk rename to src/platforms/posix/drivers/adcsim/module.mk diff --git a/src/platforms/linux/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp similarity index 100% rename from src/platforms/linux/drivers/barosim/baro.cpp rename to src/platforms/posix/drivers/barosim/baro.cpp diff --git a/src/platforms/linux/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp similarity index 100% rename from src/platforms/linux/drivers/barosim/baro_sim.cpp rename to src/platforms/posix/drivers/barosim/baro_sim.cpp diff --git a/src/platforms/linux/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h similarity index 100% rename from src/platforms/linux/drivers/barosim/barosim.h rename to src/platforms/posix/drivers/barosim/barosim.h diff --git a/src/platforms/linux/drivers/barosim/module.mk b/src/platforms/posix/drivers/barosim/module.mk similarity index 100% rename from src/platforms/linux/drivers/barosim/module.mk rename to src/platforms/posix/drivers/barosim/module.mk diff --git a/src/platforms/linux/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp similarity index 100% rename from src/platforms/linux/drivers/gyrosim/gyrosim.cpp rename to src/platforms/posix/drivers/gyrosim/gyrosim.cpp diff --git a/src/platforms/linux/drivers/gyrosim/module.mk b/src/platforms/posix/drivers/gyrosim/module.mk similarity index 100% rename from src/platforms/linux/drivers/gyrosim/module.mk rename to src/platforms/posix/drivers/gyrosim/module.mk diff --git a/src/platforms/linux/include/arch/board/board.h b/src/platforms/posix/include/arch/board/board.h similarity index 100% rename from src/platforms/linux/include/arch/board/board.h rename to src/platforms/posix/include/arch/board/board.h diff --git a/src/platforms/linux/include/board_config.h b/src/platforms/posix/include/board_config.h similarity index 100% rename from src/platforms/linux/include/board_config.h rename to src/platforms/posix/include/board_config.h diff --git a/src/platforms/linux/include/crc32.h b/src/platforms/posix/include/crc32.h similarity index 100% rename from src/platforms/linux/include/crc32.h rename to src/platforms/posix/include/crc32.h diff --git a/src/platforms/linux/include/queue.h b/src/platforms/posix/include/queue.h similarity index 100% rename from src/platforms/linux/include/queue.h rename to src/platforms/posix/include/queue.h diff --git a/src/platforms/linux/main.cpp b/src/platforms/posix/main.cpp similarity index 100% rename from src/platforms/linux/main.cpp rename to src/platforms/posix/main.cpp diff --git a/src/platforms/linux/px4_layer/dq_addlast.c b/src/platforms/posix/px4_layer/dq_addlast.c similarity index 100% rename from src/platforms/linux/px4_layer/dq_addlast.c rename to src/platforms/posix/px4_layer/dq_addlast.c diff --git a/src/platforms/linux/px4_layer/dq_rem.c b/src/platforms/posix/px4_layer/dq_rem.c similarity index 100% rename from src/platforms/linux/px4_layer/dq_rem.c rename to src/platforms/posix/px4_layer/dq_rem.c diff --git a/src/platforms/linux/px4_layer/dq_remfirst.c b/src/platforms/posix/px4_layer/dq_remfirst.c similarity index 100% rename from src/platforms/linux/px4_layer/dq_remfirst.c rename to src/platforms/posix/px4_layer/dq_remfirst.c diff --git a/src/platforms/linux/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c similarity index 100% rename from src/platforms/linux/px4_layer/drv_hrt.c rename to src/platforms/posix/px4_layer/drv_hrt.c diff --git a/src/platforms/linux/px4_layer/lib_crc32.c b/src/platforms/posix/px4_layer/lib_crc32.c similarity index 100% rename from src/platforms/linux/px4_layer/lib_crc32.c rename to src/platforms/posix/px4_layer/lib_crc32.c diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk similarity index 97% rename from src/platforms/linux/px4_layer/module.mk rename to src/platforms/posix/px4_layer/module.mk index de6678eb92..2df90461da 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -36,8 +36,8 @@ # SRCS = \ - px4_linux_impl.cpp \ - px4_linux_tasks.cpp \ + px4_posix_impl.cpp \ + px4_posix_tasks.cpp \ work_thread.c \ work_queue.c \ work_cancel.c \ diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp similarity index 98% rename from src/platforms/linux/px4_layer/px4_linux_impl.cpp rename to src/platforms/posix/px4_layer/px4_posix_impl.cpp index 973b85abda..3b1415fd72 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4_linux_impl.cpp + * @file px4_posix_impl.cpp * * PX4 Middleware Wrapper Linux Implementation */ diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp similarity index 99% rename from src/platforms/linux/px4_layer/px4_linux_tasks.cpp rename to src/platforms/posix/px4_layer/px4_posix_tasks.cpp index a210f430ea..fd58637273 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file px4_linux_tasks.c + * @file px4_posix_tasks.c * Implementation of existing task API for Linux */ diff --git a/src/platforms/linux/px4_layer/queue.c b/src/platforms/posix/px4_layer/queue.c similarity index 98% rename from src/platforms/linux/px4_layer/queue.c rename to src/platforms/posix/px4_layer/queue.c index 2543782b87..826cc2d163 100644 --- a/src/platforms/linux/px4_layer/queue.c +++ b/src/platforms/posix/px4_layer/queue.c @@ -35,7 +35,7 @@ ************************************************************************/ // FIXME - need px4_queue -#include +#include #include __EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/linux/px4_layer/sq_addafter.c b/src/platforms/posix/px4_layer/sq_addafter.c similarity index 100% rename from src/platforms/linux/px4_layer/sq_addafter.c rename to src/platforms/posix/px4_layer/sq_addafter.c diff --git a/src/platforms/linux/px4_layer/sq_addlast.c b/src/platforms/posix/px4_layer/sq_addlast.c similarity index 100% rename from src/platforms/linux/px4_layer/sq_addlast.c rename to src/platforms/posix/px4_layer/sq_addlast.c diff --git a/src/platforms/linux/px4_layer/sq_remfirst.c b/src/platforms/posix/px4_layer/sq_remfirst.c similarity index 100% rename from src/platforms/linux/px4_layer/sq_remfirst.c rename to src/platforms/posix/px4_layer/sq_remfirst.c diff --git a/src/platforms/linux/px4_layer/work_cancel.c b/src/platforms/posix/px4_layer/work_cancel.c similarity index 100% rename from src/platforms/linux/px4_layer/work_cancel.c rename to src/platforms/posix/px4_layer/work_cancel.c diff --git a/src/platforms/linux/px4_layer/work_queue.c b/src/platforms/posix/px4_layer/work_queue.c similarity index 100% rename from src/platforms/linux/px4_layer/work_queue.c rename to src/platforms/posix/px4_layer/work_queue.c diff --git a/src/platforms/linux/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c similarity index 100% rename from src/platforms/linux/px4_layer/work_thread.c rename to src/platforms/posix/px4_layer/work_thread.c diff --git a/src/platforms/linux/tests/hello/hello_example.cpp b/src/platforms/posix/tests/hello/hello_example.cpp similarity index 100% rename from src/platforms/linux/tests/hello/hello_example.cpp rename to src/platforms/posix/tests/hello/hello_example.cpp diff --git a/src/platforms/linux/tests/hello/hello_example.h b/src/platforms/posix/tests/hello/hello_example.h similarity index 100% rename from src/platforms/linux/tests/hello/hello_example.h rename to src/platforms/posix/tests/hello/hello_example.h diff --git a/src/platforms/linux/tests/hello/hello_main.cpp b/src/platforms/posix/tests/hello/hello_main.cpp similarity index 100% rename from src/platforms/linux/tests/hello/hello_main.cpp rename to src/platforms/posix/tests/hello/hello_main.cpp diff --git a/src/platforms/qurt/tests/hello/hello_start_linux.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp similarity index 99% rename from src/platforms/qurt/tests/hello/hello_start_linux.cpp rename to src/platforms/posix/tests/hello/hello_start_posix.cpp index 240c5d845e..962645fabe 100644 --- a/src/platforms/qurt/tests/hello/hello_start_linux.cpp +++ b/src/platforms/posix/tests/hello/hello_start_posix.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file hello_start_linux.cpp + * @file hello_start_posix.cpp * * @author Thomas Gubler * @author Mark Charlebois diff --git a/src/platforms/linux/tests/hello/module.mk b/src/platforms/posix/tests/hello/module.mk similarity index 98% rename from src/platforms/linux/tests/hello/module.mk rename to src/platforms/posix/tests/hello/module.mk index 730f9189e7..294c0ad7fc 100644 --- a/src/platforms/linux/tests/hello/module.mk +++ b/src/platforms/posix/tests/hello/module.mk @@ -38,6 +38,6 @@ MODULE_COMMAND = hello SRCS = hello_main.cpp \ - hello_start_linux.cpp \ + hello_start_posix.cpp \ hello_example.cpp diff --git a/src/platforms/linux/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp similarity index 100% rename from src/platforms/linux/tests/hrt_test/hrt_test.cpp rename to src/platforms/posix/tests/hrt_test/hrt_test.cpp diff --git a/src/platforms/linux/tests/hrt_test/hrt_test.h b/src/platforms/posix/tests/hrt_test/hrt_test.h similarity index 100% rename from src/platforms/linux/tests/hrt_test/hrt_test.h rename to src/platforms/posix/tests/hrt_test/hrt_test.h diff --git a/src/platforms/linux/tests/hrt_test/hrt_test_main.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp similarity index 100% rename from src/platforms/linux/tests/hrt_test/hrt_test_main.cpp rename to src/platforms/posix/tests/hrt_test/hrt_test_main.cpp diff --git a/src/platforms/linux/tests/hrt_test/hrt_test_start_linux.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp similarity index 98% rename from src/platforms/linux/tests/hrt_test/hrt_test_start_linux.cpp rename to src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp index bc53f4e52b..9d521364a8 100644 --- a/src/platforms/linux/tests/hrt_test/hrt_test_start_linux.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file hrt_test_start_linux.cpp + * @file hrt_test_start_posix.cpp * * @author Mark Charlebois */ diff --git a/src/platforms/linux/tests/hrt_test/module.mk b/src/platforms/posix/tests/hrt_test/module.mk similarity index 98% rename from src/platforms/linux/tests/hrt_test/module.mk rename to src/platforms/posix/tests/hrt_test/module.mk index 559427cd47..e1bd89ef7f 100644 --- a/src/platforms/linux/tests/hrt_test/module.mk +++ b/src/platforms/posix/tests/hrt_test/module.mk @@ -38,6 +38,6 @@ MODULE_COMMAND = hrttest SRCS = hrt_test_main.cpp \ - hrt_test_start_linux.cpp \ + hrt_test_start_posix.cpp \ hrt_test.cpp diff --git a/src/platforms/linux/tests/vcdev_test/module.mk b/src/platforms/posix/tests/vcdev_test/module.mk similarity index 98% rename from src/platforms/linux/tests/vcdev_test/module.mk rename to src/platforms/posix/tests/vcdev_test/module.mk index 72cde9e2e3..81920c860c 100644 --- a/src/platforms/linux/tests/vcdev_test/module.mk +++ b/src/platforms/posix/tests/vcdev_test/module.mk @@ -38,6 +38,6 @@ MODULE_COMMAND = vcdevtest SRCS = vcdevtest_main.cpp \ - vcdevtest_start_linux.cpp \ + vcdevtest_start_posix.cpp \ vcdevtest_example.cpp diff --git a/src/platforms/linux/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp similarity index 100% rename from src/platforms/linux/tests/vcdev_test/vcdevtest_example.cpp rename to src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp diff --git a/src/platforms/linux/tests/vcdev_test/vcdevtest_example.h b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h similarity index 100% rename from src/platforms/linux/tests/vcdev_test/vcdevtest_example.h rename to src/platforms/posix/tests/vcdev_test/vcdevtest_example.h diff --git a/src/platforms/linux/tests/vcdev_test/vcdevtest_main.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_main.cpp similarity index 100% rename from src/platforms/linux/tests/vcdev_test/vcdevtest_main.cpp rename to src/platforms/posix/tests/vcdev_test/vcdevtest_main.cpp diff --git a/src/platforms/linux/tests/vcdev_test/vcdevtest_start_linux.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp similarity index 98% rename from src/platforms/linux/tests/vcdev_test/vcdevtest_start_linux.cpp rename to src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp index 9adb650850..5ed9269b2d 100644 --- a/src/platforms/linux/tests/vcdev_test/vcdevtest_start_linux.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file vcdevtest_start_linux.cpp + * @file vcdevtest_start_posix.cpp * * @author Thomas Gubler * @author Mark Charlebois diff --git a/src/platforms/linux/tests/wqueue/module.mk b/src/platforms/posix/tests/wqueue/module.mk similarity index 98% rename from src/platforms/linux/tests/wqueue/module.mk rename to src/platforms/posix/tests/wqueue/module.mk index 29f8f4cf71..4c3b6550c6 100644 --- a/src/platforms/linux/tests/wqueue/module.mk +++ b/src/platforms/posix/tests/wqueue/module.mk @@ -38,6 +38,6 @@ MODULE_COMMAND = wqueue_test SRCS = wqueue_main.cpp \ - wqueue_start_linux.cpp \ + wqueue_start_posix.cpp \ wqueue_test.cpp diff --git a/src/platforms/linux/tests/wqueue/wqueue_main.cpp b/src/platforms/posix/tests/wqueue/wqueue_main.cpp similarity index 100% rename from src/platforms/linux/tests/wqueue/wqueue_main.cpp rename to src/platforms/posix/tests/wqueue/wqueue_main.cpp diff --git a/src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_poosix.cpp similarity index 98% rename from src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp rename to src/platforms/posix/tests/wqueue/wqueue_start_poosix.cpp index 7ac29a0d35..20c9975578 100644 --- a/src/platforms/linux/tests/wqueue/wqueue_start_linux.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_start_poosix.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file wqueue_start_linux.cpp + * @file wqueue_start_posix.cpp * * @author Thomas Gubler * @author Mark Charlebois diff --git a/src/platforms/linux/tests/wqueue/wqueue_test.cpp b/src/platforms/posix/tests/wqueue/wqueue_test.cpp similarity index 100% rename from src/platforms/linux/tests/wqueue/wqueue_test.cpp rename to src/platforms/posix/tests/wqueue/wqueue_test.cpp diff --git a/src/platforms/linux/tests/wqueue/wqueue_test.h b/src/platforms/posix/tests/wqueue/wqueue_test.h similarity index 100% rename from src/platforms/linux/tests/wqueue/wqueue_test.h rename to src/platforms/posix/tests/wqueue/wqueue_test.h diff --git a/src/platforms/px4_adc.h b/src/platforms/px4_adc.h index bda39d0369..42893ef885 100644 --- a/src/platforms/px4_adc.h +++ b/src/platforms/px4_adc.h @@ -48,7 +48,7 @@ * Building for NuttX */ #include -#elif defined(__PX4_LINUX) +#elif defined(__PX4_POSIX) // FIXME - this needs to be a px4_adc_msg_s type // Curently copied from NuttX diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index caeeb09a8b..5a187afd6c 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -42,12 +42,12 @@ #if defined(__PX4_NUTTX) #include -#elif defined (__PX4_LINUX) || defined (__PX4_QURT) +#elif defined (__PX4_POSIX) || defined (__PX4_QURT) #define CONFIG_NFILE_STREAMS 1 #define CONFIG_SCHED_WORKQUEUE 1 #define CONFIG_SCHED_HPWORK 1 #define CONFIG_SCHED_LPWORK 1 -#define CONFIG_ARCH_BOARD_LINUXTEST 1 +#define CONFIG_ARCH_BOARD_POSIXTEST 1 /** time in ms between checks for work in work queues **/ #define CONFIG_SCHED_WORKPERIOD 10 diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ba7ae4c3c2..e212e63540 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -67,7 +67,7 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) -#elif defined(__PX4_NUTTX) || defined(__PX4_LINUX) || defined(__PX4_QURT) +#elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) || defined(__PX4_QURT) /* * Building for NuttX or Linux */ @@ -107,7 +107,7 @@ typedef param_t px4_param_t; #define PX4_ISFINITE(x) isfinite(x) /* Linux Specific defines */ -#elif defined(__PX4_LINUX) +#elif defined(__PX4_POSIX) // Flag is meaningless on Linux #define O_BINARY 0 @@ -134,7 +134,7 @@ __END_DECLS /* Defines for ROS and Linux */ -#if defined(__PX4_ROS) || defined(__PX4_LINUX) || defined(__PX4_QURT) +#if defined(__PX4_ROS) || defined(__PX4_POSIX) || defined(__PX4_QURT) #define OK 0 #define ERROR -1 diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h index 9270cb203b..3e8d4ea888 100644 --- a/src/platforms/px4_i2c.h +++ b/src/platforms/px4_i2c.h @@ -70,7 +70,7 @@ typedef struct i2c_dev_s px4_i2c_dev_t; #define px4_interrupt_context() up_interrupt_context() -#elif defined(__PX4_LINUX) +#elif defined(__PX4_POSIX) #include #define I2C_M_READ 0x0001 /* read data, from slave to master */ diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index f2bd2f969d..d71eeea350 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -102,7 +102,7 @@ #include #include -#elif defined(__PX4_LINUX) +#elif defined(__PX4_POSIX) #include #include #include @@ -110,24 +110,24 @@ #define ASSERT(x) assert(x) #ifdef __cplusplus -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #endif #include #include diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h index 41289aba03..17397ee7ac 100644 --- a/src/platforms/px4_spi.h +++ b/src/platforms/px4_spi.h @@ -2,7 +2,7 @@ #ifdef __PX4_NUTTX #include -#elif defined(__PX4_LINUX) +#elif defined(__PX4_POSIX) enum spi_dev_e { SPIDEV_NONE = 0, /* Not a valid value */ diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index b81f7db6a7..0f47e53c3a 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -49,7 +49,7 @@ typedef int px4_task_t; #define px4_task_exit(x) _exit(x) -#elif defined(__PX4_LINUX) || defined(__PX4_QURT) +#elif defined(__PX4_POSIX) || defined(__PX4_QURT) #include #include diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index a275faebe9..67c2fcdfa7 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -41,7 +41,7 @@ #include #include #include -#elif defined(__PX4_LINUX) || defined(__PX4_QURT) +#elif defined(__PX4_POSIX) || defined(__PX4_QURT) #include #include diff --git a/src/platforms/linux/tests/hello/hello_start_linux.cpp b/src/platforms/qurt/tests/hello/hello_start_posix.cpp similarity index 100% rename from src/platforms/linux/tests/hello/hello_start_linux.cpp rename to src/platforms/qurt/tests/hello/hello_start_posix.cpp diff --git a/src/platforms/qurt/tests/hello/module.mk b/src/platforms/qurt/tests/hello/module.mk index 730f9189e7..6bed2aea67 100644 --- a/src/platforms/qurt/tests/hello/module.mk +++ b/src/platforms/qurt/tests/hello/module.mk @@ -38,6 +38,6 @@ MODULE_COMMAND = hello SRCS = hello_main.cpp \ - hello_start_linux.cpp \ + hello_start_qurt.cpp \ hello_example.cpp From 19162ba5bee94b8a45e5fc398bb4e7fa8e0bfbc7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 17:14:52 -0700 Subject: [PATCH 135/258] Posix: Changed PreflightCheck to read Vdev PreflightCheck was failing because it was trying to read actual devices instad of virtual devices. ADCSIM had a LINUXTEST ifdef that was removed. posix_run.sh was using the wrong path Signed-off-by: Mark Charlebois --- Makefile | 3 + Tools/posix_run.sh | 2 +- posix-configs/posixtest/init/rc.S | 2 +- .../commander/PreflightCheck_posix.cpp | 344 ++++++++++++++++++ src/modules/commander/module.mk | 9 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 4 +- ...o_start_posix.cpp => hello_start_qurt.cpp} | 0 7 files changed, 355 insertions(+), 9 deletions(-) create mode 100644 src/modules/commander/PreflightCheck_posix.cpp rename src/platforms/qurt/tests/hello/{hello_start_posix.cpp => hello_start_qurt.cpp} (100%) diff --git a/Makefile b/Makefile index 88ce8e5de6..73ff8c1bf1 100644 --- a/Makefile +++ b/Makefile @@ -146,6 +146,9 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) +posix: + make PX4_TARGET_OS=posix + posixrun: Tools/posix_run.sh diff --git a/Tools/posix_run.sh b/Tools/posix_run.sh index ebe7a77745..500261fce8 100755 --- a/Tools/posix_run.sh +++ b/Tools/posix_run.sh @@ -23,4 +23,4 @@ if [ ! -d /eeprom ] && [ ! -w /eeprom ] exit 1 fi -Build/linux_default.build/mainapp linux-configs/linuxtest/init/rc.S +Build/posix_default.build/mainapp posix-configs/posixtest/init/rc.S diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index f10f57ec37..bc4b6a4b61 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -7,6 +7,6 @@ gyrosim start rgbled start mavlink start sensors start -hil start +hil mode_pwm commander start list_devices diff --git a/src/modules/commander/PreflightCheck_posix.cpp b/src/modules/commander/PreflightCheck_posix.cpp new file mode 100644 index 0000000000..1f8791fe33 --- /dev/null +++ b/src/modules/commander/PreflightCheck_posix.cpp @@ -0,0 +1,344 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 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. +* +****************************************************************************/ + +/** +* @file PreflightCheck.cpp +* +* Preflight check for main system components +* +* @author Lorenz Meier +* @author Johan Jansen +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "PreflightCheck.h" + +namespace Commander +{ +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); + int fd = px4_open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_MAG%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + px4_close(fd); + return success; +} + +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); + int fd = px4_open(s, O_RDONLY); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_ACC%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + + if (dynamic) { + /* check measurement result range */ + struct accel_report acc; + ret = px4_read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still"); + /* this is frickin' fatal */ + success = false; + goto out; + } + } else { + mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + /* this is frickin' fatal */ + success = false; + goto out; + } + } + +out: + px4_close(fd); + return success; +} + +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); + int fd = px4_open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_GYRO%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + px4_close(fd); + return success; +} + +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); + int fd = px4_open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } + + return false; + } + + px4_close(fd); + return success; +} + +static bool airspeedCheck(int mavlink_fd, bool optional) +{ + bool success = true; + int ret; + int fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { + mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + success = false; + goto out; + } + + if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + // XXX do not make this fatal yet + } + +out: + close(fd); + return success; +} + +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic) +{ + bool failed = false; + + /* ---- MAG ---- */ + if (checkMag) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_mag_count; i++) { + bool required = (i < max_mandatory_mag_count); + + if (!magnometerCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + + /* ---- ACCEL ---- */ + if (checkAcc) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_accel_count; i++) { + bool required = (i < max_mandatory_accel_count); + + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { + failed = true; + } + } + } + + /* ---- GYRO ---- */ + if (checkGyro) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_gyro_count; i++) { + bool required = (i < max_mandatory_gyro_count); + + if (!gyroCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + + /* ---- BARO ---- */ + if (checkBaro) { + /* check all sensors, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_baro_count; i++) { + bool required = (i < max_mandatory_baro_count); + + if (!baroCheck(mavlink_fd, i, !required) && required) { + failed = true; + } + } + } + + /* ---- AIRSPEED ---- */ + if (checkAirspeed) { + if (!airspeedCheck(mavlink_fd, true)) { + failed = true; + } + } + + /* ---- RC CALIBRATION ---- */ + if (checkRC) { + if (rc_calibration_check(mavlink_fd) != OK) { + failed = true; + } + } + + /* Report status */ + return !failed; +} + +} diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 851ac9020d..7b72dafd95 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -45,14 +45,15 @@ SRCS = commander.cpp \ baro_calibration.cpp \ accelerometer_calibration.cpp \ rc_calibration.cpp \ - airspeed_calibration.cpp \ - PreflightCheck.cpp + airspeed_calibration.cpp ifdef ($(PX4_TARGET_OS),nuttx) SRCS += - state_machine_helper.cpp + state_machine_helper.cpp \ + PreflightCheck.cpp else -SRCS += state_machine_helper_posix.cpp +SRCS += state_machine_helper_posix.cpp \ + PreflightCheck_posix.cpp endif MODULE_STACKSIZE = 5000 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 78ffbf1db7..db167331f9 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -277,10 +277,8 @@ adcsim_main(int argc, char *argv[]) int ret = 0; if (g_adc == nullptr) { -#ifdef CONFIG_ARCH_BOARD_LINUXTEST - /* XXX this hardcodes the default channel set for LINUXTEST - should be configurable */ + /* XXX this hardcodes the default channel set for POSIXTEST - should be configurable */ g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); -#endif if (g_adc == nullptr) { warnx("couldn't allocate the ADCSIM driver"); diff --git a/src/platforms/qurt/tests/hello/hello_start_posix.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp similarity index 100% rename from src/platforms/qurt/tests/hello/hello_start_posix.cpp rename to src/platforms/qurt/tests/hello/hello_start_qurt.cpp From 3f7d4de74a5ca206f614b6ec2642409be7ea3493 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 17:26:07 -0700 Subject: [PATCH 136/258] Posix: fixed ioctl calls to be px4_ioctl Signed-off-by: Mark Charlebois --- src/modules/commander/PreflightCheck_posix.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/PreflightCheck_posix.cpp b/src/modules/commander/PreflightCheck_posix.cpp index 1f8791fe33..5567b43529 100644 --- a/src/modules/commander/PreflightCheck_posix.cpp +++ b/src/modules/commander/PreflightCheck_posix.cpp @@ -88,7 +88,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_MAG%u_ID", instance); param_get(param_find(s), &(calibration_devid)); @@ -132,7 +132,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_ACC%u_ID", instance); param_get(param_find(s), &(calibration_devid)); From da29004a26cf3ee4337553ba846d0f74cc0af7f4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 22:04:01 -0700 Subject: [PATCH 137/258] Sync state_machine_helper_posix to state_machine_helper state_machine_helper_posix.cpp was out of sync with state_machine_helper_posix.cpp. Added debug to detect when sensors is started before uorb. Signed-off-by: Mark Charlebois --- .../commander/state_machine_helper_posix.cpp | 89 +++++-------------- src/modules/sensors/sensors_posix.cpp | 4 + src/modules/uORB/MuORB.cpp | 7 ++ 3 files changed, 35 insertions(+), 65 deletions(-) diff --git a/src/modules/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp index 97c2f11043..2d3b78ddaf 100644 --- a/src/modules/commander/state_machine_helper_posix.cpp +++ b/src/modules/commander/state_machine_helper_posix.cpp @@ -53,18 +53,17 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #include "state_machine_helper.h" #include "commander_helper.h" +#include "PreflightCheck.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -78,12 +77,12 @@ static const int ERROR = -1; // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, true, false, false }, { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; @@ -208,14 +207,28 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // HIL can always go to standby if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + warnx("HIL STATE ON -> STANDBY"); valid_transition = true; } /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + warnx("NOT ARMING: Sensors not operational."); mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); feedback_provided = true; valid_transition = false; + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; + } + + /* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */ + if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR && + new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + } else { + mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); + } + feedback_provided = true; } // Finish up the state transition @@ -607,69 +620,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { - int ret; - bool failed = false; - - int fd = px4_open(ACCEL0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); - failed = true; - goto system_eval; - } - - ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret != OK) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); - failed = true; - goto system_eval; - } - - /* check measurement result range */ - struct accel_report acc; - ret = px4_read(fd, &acc, sizeof(acc)); - - if (ret == sizeof(acc)) { - /* evaluate values */ - float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - - if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); - /* this is frickin' fatal */ - failed = true; - goto system_eval; - } - } else { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); - /* this is frickin' fatal */ - failed = true; - goto system_eval; - } - + /* at this point this equals the preflight check, but might add additional + * quantities later. + */ + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { - /* accel done, close it */ - px4_close(fd); - fd = orb_subscribe(ORB_ID(airspeed)); - - struct airspeed_s airspeed; - - if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || - (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); - failed = true; - goto system_eval; - } - - if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); - // XXX do not make this fatal yet - } + checkAirspeed = true; } -system_eval: - px4_close(fd); - return (failed); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); } diff --git a/src/modules/sensors/sensors_posix.cpp b/src/modules/sensors/sensors_posix.cpp index 01a0c0dee1..e082ed4756 100644 --- a/src/modules/sensors/sensors_posix.cpp +++ b/src/modules/sensors/sensors_posix.cpp @@ -2152,6 +2152,10 @@ Sensors::task_main() /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + if (_sensor_pub < 0) { + warnx("Sensors::task_main ERROR - uORB not started"); + } + /* wakeup source(s) */ struct pollfd fds[1]; diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index 791a639e59..0f2a79822b 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -420,6 +420,13 @@ ssize_t ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) { PX4_DEBUG("ORBDevNode::publish meta = %p\n", meta); + + if (handle < 0) { + PX4_DEBUG("ORBDevNode::publish called with invalid handle\n", meta); + errno = EINVAL; + return ERROR; + } + ORBDevNode *devnode = (ORBDevNode *)handle; int ret; From 02aaa403f1c44743f7d60c113c4654dec4c77788 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 23:20:59 -0700 Subject: [PATCH 138/258] Posix: removed PX4_DEVIOC* definitions The following should not have been defined: PX4_DIOC_GETPRIV PX4_DEVIOCSPUBBLOCK PX4_DEVIOCGPUBBLOCK PX4_DEVIOCGDEVICEID The actual defines are in drv_device.h and are: DEVIOCSPUBBLOCK DEVIOCGPUBBLOCK DEVIOCGDEVICEID DIOC_GETPRIV is defined by Nuttx, so mapped to SIOCDEVPRIVATE for POSIX Signed-off-by: Mark Charlebois --- src/drivers/device/device.cpp | 3 ++- src/drivers/device/vdev.cpp | 9 +++++---- src/drivers/drv_device.h | 3 +++ src/platforms/px4_posix.h | 6 ------ 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index b4976250d4..34382263ac 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -121,7 +122,7 @@ int Device::dev_ioctl(unsigned operation, unsigned &arg) { switch (operation) { - case PX4_DEVIOCGDEVICEID: + case DEVIOCGDEVICEID: return (int)_device_id.devid; } return -ENODEV; diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 0d02d2b420..0c1dc3abe9 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -298,19 +298,20 @@ VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) switch (cmd) { /* fetch a pointer to the driver's private data */ - case PX4_DIOC_GETPRIV: + case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; ret = PX4_OK; break; - case PX4_DEVIOCSPUBBLOCK: + case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); ret = PX4_OK; break; - case PX4_DEVIOCGPUBBLOCK: + case DEVIOCGPUBBLOCK: ret = _pub_blocked; break; - case PX4_DEVIOCGDEVICEID: + case DEVIOCGDEVICEID: ret = (int)_device_id.devid; + printf("IOCTL DEVIOCGDEVICEID %d\n", ret); default: break; } diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 650b18bed5..3477dde9fd 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -65,5 +65,8 @@ */ #define DEVIOCGDEVICEID _DEVICEIOC(2) +#ifdef __PX4_POSIX +#define DIOC_GETPRIV SIOCDEVPRIVATE +#endif #endif /* _DRV_DEVICE_H */ diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index ed4edbb24e..18f4878878 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -49,12 +49,6 @@ #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 -#define PX4_DEVIOCGDEVICEID 1 - -#define PX4_DIOC_GETPRIV 2 -#define PX4_DEVIOCSPUBBLOCK 3 -#define PX4_DEVIOCGPUBBLOCK 4 - #define PX4_DEBUG(...) //#define PX4_DEBUG(...) printf(__VA_ARGS__) From 36a9f7a8182790c40e89b3e8aea0fd1076de1000 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 23:26:14 -0700 Subject: [PATCH 139/258] Posix: fixed calls to open that should be px4_open Signed-off-by: Mark Charlebois --- src/modules/sensors/sensors_posix.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors_posix.cpp b/src/modules/sensors/sensors_posix.cpp index e082ed4756..755d4f1678 100644 --- a/src/modules/sensors/sensors_posix.cpp +++ b/src/modules/sensors/sensors_posix.cpp @@ -1340,7 +1340,7 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { continue; @@ -1407,7 +1407,7 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { continue; From 93dea668dc0c0ca14c311020518de087b7abae24 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 21 Apr 2015 23:29:07 -0700 Subject: [PATCH 140/258] Posix: make simulated devices always pass self tests To facilitate testing, the simulated devices always return OK for self tests. rc.S was also upated to set CAL_XXXY_ID to the devid so tests pass the calibration check. Signed-off-by: Mark Charlebois --- posix-configs/posixtest/init/rc.S | 4 ++++ src/platforms/posix/drivers/accelsim/accelsim.cpp | 5 +++++ src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 4 ++++ 3 files changed, 13 insertions(+) diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index bc4b6a4b61..5e408cd544 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -4,6 +4,10 @@ barosim start adcsim start accelsim start gyrosim start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1310720 +param set CAL_ACC1_ID 1376256 +param set CAL_MAG0_ID 196608 rgbled start mavlink start sensors start diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 97314ab846..704c7fc088 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -707,6 +707,9 @@ ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; + case ACCELIOCSELFTEST: + return OK; + default: /* give it to the superclass */ return VDev::ioctl(handlep, cmd, arg); @@ -824,6 +827,8 @@ ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar */ return 0; + case MAGIOCSELFTEST: + return OK; default: /* give it to the superclass */ return VDev::ioctl(handlep, cmd, arg); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index c39ebeac0f..a984a310a2 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -782,6 +782,8 @@ GYROSIM::self_test() int GYROSIM::accel_self_test() { + return OK; + if (self_test()) return 1; @@ -807,6 +809,8 @@ GYROSIM::accel_self_test() int GYROSIM::gyro_self_test() { + return OK; + if (self_test()) return 1; From 7aac0e94db50a8be6f03e12f15c76907be0ad3b2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 07:50:45 -0700 Subject: [PATCH 141/258] Posix: disable stack size check Signed-off-by: Mark Charlebois --- src/modules/attitude_estimator_ekf/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index d158d7a49c..20eda27690 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -45,4 +45,6 @@ MODULE_STACKSIZE = 1200 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 +ifeq ($(PX4_TARGET_OS),nuttx) EXTRACXXFLAGS = -Wframe-larger-than=2400 +endif From 9ec7020e025b8cd3af6cb923229264f27de5acd6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 08:31:41 -0700 Subject: [PATCH 142/258] Make a local function static platforms/posix/main.cpp had a local function that wasn't static. Signed-off-by: Mark Charlebois --- src/platforms/posix/main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 8ae5ee58d5..b340ebb6d1 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -50,8 +50,7 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" #include "px4_middleware.h" -void run_cmd(const vector &appargs); -void run_cmd(const vector &appargs) { +static void run_cmd(const vector &appargs) { // command is appargs[0] string command = appargs[0]; if (apps.find(command) != apps.end()) { From 6a439f7ddc3593d3617da51c619a01f7d4f18514 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 08:48:48 -0700 Subject: [PATCH 143/258] QuRT: Hello world app for QuRT DSPAL for QuRT is still missing the pthreads exports and there is no exported sleep function. These functions are stubbed out for the time being. This is based on the 6.4.05 version of the Hexagon tools. The Hexagon tools and DSPAL are needed to build the qurt target. Signed-off-by: Mark Charlebois --- Makefile | 9 ++ makefiles/toolchain_hexagon.mk | 4 +- src/platforms/px4_tasks.h | 6 + src/platforms/qurt/main.cpp | 81 ++++++++++- src/platforms/qurt/px4_layer/drv_hrt.c | 8 ++ .../qurt/px4_layer/px4_qurt_tasks.cpp | 127 +++--------------- src/platforms/qurt/px4_layer/queue.c | 2 +- .../qurt/tests/hello/hello_example.cpp | 8 +- .../qurt/tests/hello/hello_start_qurt.cpp | 17 +-- 9 files changed, 139 insertions(+), 123 deletions(-) diff --git a/Makefile b/Makefile index 73ff8c1bf1..63bb6cec92 100644 --- a/Makefile +++ b/Makefile @@ -149,9 +149,18 @@ testbuild: posix: make PX4_TARGET_OS=posix +nuttx: + make PX4_TARGET_OS=nuttx + +qurt: + make PX4_TARGET_OS=qurt + posixrun: Tools/posix_run.sh +qurtrun: + make PX4_TARGET_OS=qurt sim + # # Unittest targets. Builds and runs the host-level # unit tests. diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index aab9fada1c..c409136c5b 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -87,8 +87,8 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Drestrict= \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ - -I$(PX4_BASE)/../dspalmc/include \ - -I$(PX4_BASE)/../dspalmc/sys \ + -I$(PX4_BASE)/../dspal/include \ + -I$(PX4_BASE)/../dspal/sys \ -Wno-error=shadow # optimisation flags diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 0f47e53c3a..817fbbbb61 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -54,9 +54,15 @@ typedef int px4_task_t; #include #define SCHED_DEFAULT SCHED_FIFO +#ifdef __PX4_LINUX #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) #define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO) #define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) +#elif defined(__PX4_QURT) +#define SCHED_PRIORITY_MAX 0 +#define SCHED_PRIORITY_MIN 0 +#define SCHED_PRIORITY_DEFAULT 0 +#endif typedef int px4_task_t; diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index aab84cd016..3697fed0df 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -51,11 +51,84 @@ #include "apps.h" //#include "px4_middleware.h" -//static command = "list_builtins"; +static const char *commands = "hello start\n"; + +static void run_cmd(const vector &appargs) { + // command is appargs[0] + string command = appargs[0]; + printf("Looking for %s\n", command.c_str()); + if (apps.find(command) != apps.end()) { + const char *arg[appargs.size()+2]; + + unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { + arg[i] = (char *)appargs[i].c_str(); + printf(" arg = '%s'\n", arg[i]); + ++i; + } + arg[i] = (char *)0; + apps[command](i,(char **)arg); + } + else + { + cout << "Invalid command" << endl; + list_builtins(); + } +} + +static void process_commands(const char *cmds) +{ + vector appargs(5); + int i=0; + int j=0; + const char *b = cmds; + bool found_first_char = false; + char arg[20]; + + // Eat leading whitespace + while (b[i] == ' ') { ++i; }; + b = &b[i]; + + for(;;) { + // End of command line + if (b[i] == '\n' || b[i] == '\0') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs[j] = arg; + + // If we have a command to run + if (i > 0 || j > 0) + run_cmd(appargs); + j=0; + if (b[i] == '\n') { + // Eat whitespace + while (b[++i] == ' '); + b = &b[i]; + i=0; + continue; + } + else { + break; + } + } + // End of arg + else if (b[i] == ' ') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs[j] = arg; + j++; + // Eat whitespace + while (b[++i] == ' '); + b = &b[i]; + i=0; + continue; + } + ++i; + } +} int main(int argc, char **argv) { - printf("hello\n"); - list_builtins(); - //apps["hello"](i,(char **)arg);; + process_commands(commands); + for (;;) {} } diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 39e0f73b37..0b42cfa0e3 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -285,6 +285,14 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo hrt_call_internal(entry, calltime, 0, callout, arg); } +void hrt_sleep(uint32_t seconds) +{ +} + +void hrt_usleep(uint32_t useconds) +{ +} + #if 0 /* * Convert absolute time to a timespec. diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index f402bde549..cd4b9527f9 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -53,16 +53,18 @@ #include #include +#include #define MAX_CMD_LEN 100 #define PX4_MAX_TASKS 100 struct task_entry { - pthread_t pid; + int pid; std::string name; bool isused; task_entry() : isused(false) {} + void *sp; }; static task_entry taskmap[PX4_MAX_TASKS]; @@ -75,7 +77,7 @@ typedef struct // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void entry_adapter ( void *ptr ) { pthdata_t *data; data = (pthdata_t *) ptr; @@ -85,8 +87,6 @@ static void *entry_adapter ( void *ptr ) printf("Before px4_task_exit\n"); px4_task_exit(0); printf("After px4_task_exit\n"); - - return NULL; } void @@ -103,15 +103,12 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; - - pthread_t task; - pthread_attr_t attr; - struct sched_param param; + char * p; // Calculate argc - while (p != (char *)0) { + for(;;) { p = argv[argc]; + printf("arg %d %s\n", argc, argv[argc]); if (p == (char *)0) break; ++argc; @@ -136,105 +133,29 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; -#if 0 - rv = pthread_attr_init(&attr); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to init thread attrs\n"); - return (rv < 0) ? rv : -rv; - } - rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set inherit sched\n"); - return (rv < 0) ? rv : -rv; - } - rv = pthread_attr_setschedpolicy(&attr, scheduler); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set sched policy\n"); - return (rv < 0) ? rv : -rv; - } - - param.sched_priority = priority; - - rv = pthread_attr_setschedparam(&attr, ¶m); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set sched param\n"); - return (rv < 0) ? rv : -rv; - } -#endif - - //rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); - if (rv != 0) { - - if (rv == EPERM) { - //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); - if (rv != 0) { - printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); - return (rv < 0) ? rv : -rv; - } - } - else { - return (rv < 0) ? rv : -rv; - } - } - for (i=0; i=PX4_MAX_TASKS) { - return -ENOSPC; - } - return i; + thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); + + return i+1; } int px4_task_delete(px4_task_t id) { - int rv = 0; - pthread_t pid; printf("Called px4_task_delete\n"); - - if (id < PX4_MAX_TASKS && taskmap[id].isused) - pid = taskmap[id].pid; - else - return -EINVAL; - - // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { - taskmap[id].isused = false; - pthread_exit(0); - } else { - rv = pthread_cancel(pid); - } - - taskmap[id].isused = false; - - return rv; + return -EINVAL; } void px4_task_exit(int ret) { - int i; - pthread_t pid = pthread_self(); - - // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) - printf("px4_task_exit: self task not found!\n"); - else - printf("px4_task_exit: %s\n", taskmap[i].name.c_str()); - - pthread_exit((void *)(unsigned long)ret); + thread_stop(); } void px4_killall(void) @@ -250,19 +171,8 @@ void px4_killall(void) int px4_task_kill(px4_task_t id, int sig) { - int rv = 0; - pthread_t pid; - //printf("Called px4_task_delete\n"); - - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) - pid = taskmap[id].pid; - else - return -EINVAL; - - // If current thread then exit, otherwise cancel - rv = pthread_kill(pid, sig); - - return rv; + printf("Called px4_task_kill\n"); + return -EINVAL; } void px4_show_tasks() @@ -274,7 +184,7 @@ void px4_show_tasks() for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %p\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } @@ -282,3 +192,6 @@ void px4_show_tasks() printf(" No running tasks\n"); } + +// STUBS + diff --git a/src/platforms/qurt/px4_layer/queue.c b/src/platforms/qurt/px4_layer/queue.c index 2543782b87..eecbd98830 100644 --- a/src/platforms/qurt/px4_layer/queue.c +++ b/src/platforms/qurt/px4_layer/queue.c @@ -35,7 +35,7 @@ ************************************************************************/ // FIXME - need px4_queue -#include +#include #include __EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp index a30aeb57bc..1975029a3f 100644 --- a/src/platforms/qurt/tests/hello/hello_example.cpp +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -40,9 +40,15 @@ */ #include "hello_example.h" +#include #include #include +/* + * Wrap the sleep call. + */ +__EXPORT extern void hrt_sleep(uint32_t seconds); + px4::AppState HelloExample::appState; int HelloExample::main() @@ -51,7 +57,7 @@ int HelloExample::main() int i=0; while (!appState.exitRequested() && i<5) { - sleep(2); + hrt_sleep(2); printf(" Doing work...\n"); ++i; diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index 240c5d845e..987e4807dd 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -44,20 +44,21 @@ #include #include -#define SCHED_DEFAULT SCHED_FIFO -#define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) -#define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO) - static int daemon_task; /* Handle of deamon task / thread */ //using namespace px4; extern "C" __EXPORT int hello_main(int argc, char *argv[]); + +static void usage() +{ + printf("usage: hello {start|stop|status}\n"); +} int hello_main(int argc, char *argv[]) { - + printf("argc = %d %s %s\n", argc, argv[0], argv[1]); if (argc < 2) { - printf("usage: hello {start|stop|status}\n"); + usage(); return 1; } @@ -74,7 +75,7 @@ int hello_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 2000, PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + (char* const*)argv); return 0; } @@ -95,6 +96,6 @@ int hello_main(int argc, char *argv[]) return 0; } - printf("usage: hello_main {start|stop|status}\n"); + usage(); return 1; } From 8e500f543ee1353baf9166d81f0718141d525257 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 09:39:02 -0700 Subject: [PATCH 144/258] Combined nuttx and posix mavlink_main headers Removed the separate implementations of mavlink_main_X.h Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main.h | 394 ++++++++++++++++++++- src/modules/mavlink/mavlink_main_nuttx.h | 415 ----------------------- 2 files changed, 391 insertions(+), 418 deletions(-) delete mode 100644 src/modules/mavlink/mavlink_main_nuttx.h diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9b00d72c01..1e346088aa 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,10 +30,398 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + +/** + * @file mavlink_main.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + #pragma once +#include #ifdef __PX4_NUTTX -#include "mavlink_main_nuttx.h" +#include #else -#include "mavlink_main_posix.h" +#include #endif +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" +#include "mavlink_messages.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" + +#ifdef __PX4_NUTTX +class Mavlink +#else +class Mavlink : public device::VDev +#endif +{ + +public: + /** + * Constructor + */ + Mavlink(); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + static int start(int argc, char *argv[]); + + /** + * Display the mavlink status. + */ + void display_status(); + + static int stream_command(int argc, char *argv[]); + + static int instance_count(); + + static Mavlink *new_instance(); + + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); + + static int destroy_all_instances(); + + static int get_status_all_instances(); + + static bool instance_exists(const char *device_name, Mavlink *self); + + static void forward_message(const mavlink_message_t *msg, Mavlink *self); + + static int get_uart_fd(unsigned index); + + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); + + const char *_device_name; + + enum MAVLINK_MODE { + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_ONBOARD + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } + + bool get_hil_enabled() { return _hil_enabled; } + + bool get_use_hil_gps() { return _use_hil_gps; } + + bool get_forward_externalsp() { return _forward_externalsp; } + + bool get_flow_control_enabled() { return _flow_control_enabled; } + + bool get_forwarding_on() { return _forwarding_on; } + + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + + static int start_helper(int argc, char *argv[]); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_enabled(bool hil_enabled); + + void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); + + int get_instance_id(); + + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + + mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, float rate); + + bool _task_should_exit; /**< if true, mavlink task should exit */ + + int get_mavlink_fd() { return _mavlink_fd; } + + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level + */ + void send_statustext(unsigned char severity, const char *string); + + MavlinkStream * get_streams() const { return _streams; } + + float get_rate_mult(); + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(const void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); + + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + + unsigned get_system_type() { return _system_type; } + +protected: + Mavlink *next; + +private: + int _instance_id; + + int _mavlink_fd; + bool _task_running; + + /* states */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; + + MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; + + MAVLINK_MODE _mode; + + mavlink_channel_t _channel; + + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; + + pthread_t _receive_thread; + + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; + + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int _mavlink_param_queue_index; + + bool mavlink_link_termination_allowed; + + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; + + bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; + + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + + struct telemetry_status_s _rstatus; ///< receive status + + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; + + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; + param_t _param_forward_externalsp; + + unsigned _system_type; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + + void mavlink_update_system(); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); + + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(const mavlink_message_t *msg); + + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + +#ifdef __PX4_NUTTX + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); +#else + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); +#endif + + /** + * Main mavlink task. + */ + int task_main(int argc, char *argv[]); + + /* do not allow copying this class */ + Mavlink(const Mavlink&); + Mavlink operator=(const Mavlink&); +}; diff --git a/src/modules/mavlink/mavlink_main_nuttx.h b/src/modules/mavlink/mavlink_main_nuttx.h deleted file mode 100644 index f7f0ad4b26..0000000000 --- a/src/modules/mavlink/mavlink_main_nuttx.h +++ /dev/null @@ -1,415 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file mavlink_main_nuttx.h - * MAVLink 1.0 protocol interface definition. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "mavlink_bridge_header.h" -#include "mavlink_orb_subscription.h" -#include "mavlink_stream.h" -#include "mavlink_messages.h" -#include "mavlink_mission.h" -#include "mavlink_parameters.h" - -class Mavlink -{ - -public: - /** - * Constructor - */ - Mavlink(); - - /** - * Destructor, also kills the mavlinks task. - */ - ~Mavlink(); - - /** - * Start the mavlink task. - * - * @return OK on success. - */ - static int start(int argc, char *argv[]); - - /** - * Display the mavlink status. - */ - void display_status(); - - static int stream_command(int argc, char *argv[]); - - static int instance_count(); - - static Mavlink *new_instance(); - - static Mavlink *get_instance(unsigned instance); - - static Mavlink *get_instance_for_device(const char *device_name); - - static int destroy_all_instances(); - - static int get_status_all_instances(); - - static bool instance_exists(const char *device_name, Mavlink *self); - - static void forward_message(const mavlink_message_t *msg, Mavlink *self); - - static int get_uart_fd(unsigned index); - - int get_uart_fd(); - - /** - * Get the MAVLink system id. - * - * @return The system ID of this vehicle - */ - int get_system_id(); - - /** - * Get the MAVLink component id. - * - * @return The component ID of this vehicle - */ - int get_component_id(); - - const char *_device_name; - - enum MAVLINK_MODE { - MAVLINK_MODE_NORMAL = 0, - MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD - }; - - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } - - bool get_hil_enabled() { return _hil_enabled; } - - bool get_use_hil_gps() { return _use_hil_gps; } - - bool get_forward_externalsp() { return _forward_externalsp; } - - bool get_flow_control_enabled() { return _flow_control_enabled; } - - bool get_forwarding_on() { return _forwarding_on; } - - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - - static int start_helper(int argc, char *argv[]); - - /** - * Handle parameter related messages. - */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); - - /** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ - int set_hil_enabled(bool hil_enabled); - - void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); - - /** - * Resend message as is, don't change sequence number and CRC. - */ - void resend_message(mavlink_message_t *msg); - - void handle_message(const mavlink_message_t *msg); - - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); - - int get_instance_id(); - - /** - * Enable / disable hardware flow control. - * - * @param enabled True if hardware flow control should be enabled - */ - int enable_flow_control(bool enabled); - - mavlink_channel_t get_channel(); - - void configure_stream_threadsafe(const char *stream_name, float rate); - - bool _task_should_exit; /**< if true, mavlink task should exit */ - - int get_mavlink_fd() { return _mavlink_fd; } - - /** - * Send a status text with loglevel INFO - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_info(const char *string); - - /** - * Send a status text with loglevel CRITICAL - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_critical(const char *string); - - /** - * Send a status text with loglevel EMERGENCY - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_emergency(const char *string); - - /** - * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent - * only on this mavlink connection. Useful for reporting communication specific, not system-wide info - * only to client interested in it. Message will be not sent immediately but queued in buffer as - * for mavlink_log_xxx(). - * - * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level - */ - void send_statustext(unsigned char severity, const char *string); - - MavlinkStream * get_streams() const { return _streams; } - - float get_rate_mult(); - - /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - - bool message_buffer_write(const void *ptr, int size); - - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } - - /** - * Count a transmision error - */ - void count_txerr(); - - /** - * Count transmitted bytes - */ - void count_txbytes(unsigned n) { _bytes_tx += n; }; - - /** - * Count bytes not transmitted because of errors - */ - void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; - - /** - * Count received bytes - */ - void count_rxbytes(unsigned n) { _bytes_rx += n; }; - - /** - * Get the receive status of this MAVLink link - */ - struct telemetry_status_s& get_rx_status() { return _rstatus; } - - struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } - - unsigned get_system_type() { return _system_type; } - -protected: - Mavlink *next; - -private: - int _instance_id; - - int _mavlink_fd; - bool _task_running; - - /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ - bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ - bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ - - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; - - MavlinkMissionManager *_mission_manager; - MavlinkParametersManager *_parameters_manager; - - MAVLINK_MODE _mode; - - mavlink_channel_t _channel; - - struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; - - pthread_t _receive_thread; - - bool _verbose; - bool _forwarding_on; - bool _passing_on; - bool _ftp_on; - int _uart_fd; - int _baudrate; - int _datarate; ///< data rate for normal streams (attitude, position, etc.) - int _datarate_events; ///< data rate for params, waypoints, text messages - float _rate_mult; - - /** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ - unsigned int _mavlink_param_queue_index; - - bool mavlink_link_termination_allowed; - - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; - - bool _flow_control_enabled; - uint64_t _last_write_success_time; - uint64_t _last_write_try_time; - - unsigned _bytes_tx; - unsigned _bytes_txerr; - unsigned _bytes_rx; - uint64_t _bytes_timestamp; - float _rate_tx; - float _rate_txerr; - float _rate_rx; - - struct telemetry_status_s _rstatus; ///< receive status - - struct mavlink_message_buffer { - int write_ptr; - int read_ptr; - int size; - char *data; - }; - - mavlink_message_buffer _message_buffer; - - pthread_mutex_t _message_buffer_mutex; - pthread_mutex_t _send_mutex; - - bool _param_initialized; - param_t _param_system_id; - param_t _param_component_id; - param_t _param_system_type; - param_t _param_use_hil_gps; - param_t _param_forward_externalsp; - - unsigned _system_type; - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _txerr_perf; /**< TX error counter */ - - void mavlink_update_system(); - - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - static unsigned int interval_from_rate(float rate); - - int configure_stream(const char *stream_name, const float rate); - - /** - * Adjust the stream rates based on the current rate - * - * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease - */ - void adjust_stream_rates(const float multiplier); - - int message_buffer_init(int size); - - void message_buffer_destroy(); - - int message_buffer_count(); - - int message_buffer_is_empty(); - - int message_buffer_get_ptr(void **ptr, bool *is_part); - - void message_buffer_mark_read(int n); - - void pass_message(const mavlink_message_t *msg); - - /** - * Update rate mult so total bitrate will be equal to _datarate. - */ - void update_rate_mult(); - - static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - - /** - * Main mavlink task. - */ - int task_main(int argc, char *argv[]); - - /* do not allow copying this class */ - Mavlink(const Mavlink&); - Mavlink operator=(const Mavlink&); -}; From d63d2f7a6164a5be2d19ccb4d172f09612a55d64 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 10:41:32 -0700 Subject: [PATCH 145/258] Posix: removed obsolete file mavlink_main_posix.h The changes for __PX4_POSIX are in mavlink_main.h Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main_posix.h | 416 ----------------------- 1 file changed, 416 deletions(-) delete mode 100644 src/modules/mavlink/mavlink_main_posix.h diff --git a/src/modules/mavlink/mavlink_main_posix.h b/src/modules/mavlink/mavlink_main_posix.h deleted file mode 100644 index 67f2c8aa6a..0000000000 --- a/src/modules/mavlink/mavlink_main_posix.h +++ /dev/null @@ -1,416 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file mavlink_main_posix.h - * MAVLink 1.0 protocol interface definition. - * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Mark Charlebois - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "mavlink_bridge_header.h" -#include "mavlink_orb_subscription.h" -#include "mavlink_stream.h" -#include "mavlink_messages.h" -#include "mavlink_mission.h" -#include "mavlink_parameters.h" - -class Mavlink : public device::VDev -{ - -public: - /** - * Constructor - */ - Mavlink(); - - /** - * Destructor, also kills the mavlinks task. - */ - ~Mavlink(); - - /** - * Start the mavlink task. - * - * @return OK on success. - */ - static int start(int argc, char *argv[]); - - /** - * Display the mavlink status. - */ - void display_status(); - - static int stream_command(int argc, char *argv[]); - - static int instance_count(); - - static Mavlink *new_instance(); - - static Mavlink *get_instance(unsigned instance); - - static Mavlink *get_instance_for_device(const char *device_name); - - static int destroy_all_instances(); - - static int get_status_all_instances(); - - static bool instance_exists(const char *device_name, Mavlink *self); - - static void forward_message(const mavlink_message_t *msg, Mavlink *self); - - static int get_uart_fd(unsigned index); - - int get_uart_fd(); - - /** - * Get the MAVLink system id. - * - * @return The system ID of this vehicle - */ - int get_system_id(); - - /** - * Get the MAVLink component id. - * - * @return The component ID of this vehicle - */ - int get_component_id(); - - const char *_device_name; - - enum MAVLINK_MODE { - MAVLINK_MODE_NORMAL = 0, - MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_ONBOARD - }; - - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } - - bool get_hil_enabled() { return _hil_enabled; } - - bool get_use_hil_gps() { return _use_hil_gps; } - - bool get_forward_externalsp() { return _forward_externalsp; } - - bool get_flow_control_enabled() { return _flow_control_enabled; } - - bool get_forwarding_on() { return _forwarding_on; } - - /** - * Get the free space in the transmit buffer - * - * @return free space in the UART TX buffer - */ - unsigned get_free_tx_buf(); - - static int start_helper(int argc, char *argv[]); - - /** - * Handle parameter related messages. - */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); - - /** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ - int set_hil_enabled(bool hil_enabled); - - void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); - - /** - * Resend message as is, don't change sequence number and CRC. - */ - void resend_message(mavlink_message_t *msg); - - void handle_message(const mavlink_message_t *msg); - - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); - - int get_instance_id(); - - /** - * Enable / disable hardware flow control. - * - * @param enabled True if hardware flow control should be enabled - */ - int enable_flow_control(bool enabled); - - mavlink_channel_t get_channel(); - - void configure_stream_threadsafe(const char *stream_name, float rate); - - bool _task_should_exit; /**< if true, mavlink task should exit */ - - int get_mavlink_fd() { return _mavlink_fd; } - - /** - * Send a status text with loglevel INFO - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_info(const char *string); - - /** - * Send a status text with loglevel CRITICAL - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_critical(const char *string); - - /** - * Send a status text with loglevel EMERGENCY - * - * @param string the message to send (will be capped by mavlink max string length) - */ - void send_statustext_emergency(const char *string); - - /** - * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent - * only on this mavlink connection. Useful for reporting communication specific, not system-wide info - * only to client interested in it. Message will be not sent immediately but queued in buffer as - * for mavlink_log_xxx(). - * - * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level - */ - void send_statustext(unsigned char severity, const char *string); - - MavlinkStream * get_streams() const { return _streams; } - - float get_rate_mult(); - - /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - - bool message_buffer_write(const void *ptr, int size); - - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } - - /** - * Count a transmision error - */ - void count_txerr(); - - /** - * Count transmitted bytes - */ - void count_txbytes(unsigned n) { _bytes_tx += n; }; - - /** - * Count bytes not transmitted because of errors - */ - void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; - - /** - * Count received bytes - */ - void count_rxbytes(unsigned n) { _bytes_rx += n; }; - - /** - * Get the receive status of this MAVLink link - */ - struct telemetry_status_s& get_rx_status() { return _rstatus; } - - struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } - - unsigned get_system_type() { return _system_type; } - -protected: - Mavlink *next; - -private: - int _instance_id; - - int _mavlink_fd; - bool _task_running; - - /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ - bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ - bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ - - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; - - MavlinkMissionManager *_mission_manager; - MavlinkParametersManager *_parameters_manager; - - MAVLINK_MODE _mode; - - mavlink_channel_t _channel; - - struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; - - pthread_t _receive_thread; - - bool _verbose; - bool _forwarding_on; - bool _passing_on; - bool _ftp_on; - int _uart_fd; - int _baudrate; - int _datarate; ///< data rate for normal streams (attitude, position, etc.) - int _datarate_events; ///< data rate for params, waypoints, text messages - float _rate_mult; - - /** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ - unsigned int _mavlink_param_queue_index; - - bool mavlink_link_termination_allowed; - - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; - - bool _flow_control_enabled; - uint64_t _last_write_success_time; - uint64_t _last_write_try_time; - - unsigned _bytes_tx; - unsigned _bytes_txerr; - unsigned _bytes_rx; - uint64_t _bytes_timestamp; - float _rate_tx; - float _rate_txerr; - float _rate_rx; - - struct telemetry_status_s _rstatus; ///< receive status - - struct mavlink_message_buffer { - int write_ptr; - int read_ptr; - int size; - char *data; - }; - - mavlink_message_buffer _message_buffer; - - pthread_mutex_t _message_buffer_mutex; - pthread_mutex_t _send_mutex; - - bool _param_initialized; - param_t _param_system_id; - param_t _param_component_id; - param_t _param_system_type; - param_t _param_use_hil_gps; - param_t _param_forward_externalsp; - - unsigned _system_type; - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _txerr_perf; /**< TX error counter */ - - void mavlink_update_system(); - - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - - static unsigned int interval_from_rate(float rate); - - int configure_stream(const char *stream_name, const float rate); - - /** - * Adjust the stream rates based on the current rate - * - * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease - */ - void adjust_stream_rates(const float multiplier); - - int message_buffer_init(int size); - - void message_buffer_destroy(); - - int message_buffer_count(); - - int message_buffer_is_empty(); - - int message_buffer_get_ptr(void **ptr, bool *is_part); - - void message_buffer_mark_read(int n); - - void pass_message(const mavlink_message_t *msg); - - /** - * Update rate mult so total bitrate will be equal to _datarate. - */ - void update_rate_mult(); - - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - - /** - * Main mavlink task. - */ - int task_main(int argc, char *argv[]); - - /* do not allow copying this class */ - Mavlink(const Mavlink&); - Mavlink operator=(const Mavlink&); -}; From e764c68d0aec7818e9f14384f325a79068b44d38 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 11:08:19 -0700 Subject: [PATCH 146/258] mavlink: consolidated nuttx and posix changes Removed nuttx and posix specific files for mavlink_ftp and mavlink_receiver. Signed-off-by: Mark Charlebois --- ...{mavlink_ftp_posix.cpp => mavlink_ftp.cpp} | 7 +- src/modules/mavlink/mavlink_ftp_nuttx.cpp | 853 --------- ...eceiver_posix.cpp => mavlink_receiver.cpp} | 9 +- .../mavlink/mavlink_receiver_nuttx.cpp | 1590 ----------------- src/modules/mavlink/mavlink_tests/module.mk | 6 +- src/modules/mavlink/module.mk | 12 +- 6 files changed, 19 insertions(+), 2458 deletions(-) rename src/modules/mavlink/{mavlink_ftp_posix.cpp => mavlink_ftp.cpp} (99%) delete mode 100644 src/modules/mavlink/mavlink_ftp_nuttx.cpp rename src/modules/mavlink/{mavlink_receiver_posix.cpp => mavlink_receiver.cpp} (99%) delete mode 100644 src/modules/mavlink/mavlink_receiver_nuttx.cpp diff --git a/src/modules/mavlink/mavlink_ftp_posix.cpp b/src/modules/mavlink/mavlink_ftp.cpp similarity index 99% rename from src/modules/mavlink/mavlink_ftp_posix.cpp rename to src/modules/mavlink/mavlink_ftp.cpp index 1ff7dce735..4d644a8627 100644 --- a/src/modules/mavlink/mavlink_ftp_posix.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -820,7 +820,12 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length return -1; } - dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY, 0x0777); + dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY +// POSIX requires the permissions to be supplied if O_CREAT passed +#ifdef __PX4_POSIX + , 0x0777 +#endif + ); if (dst_fd < 0) { op_errno = errno; ::close(src_fd); diff --git a/src/modules/mavlink/mavlink_ftp_nuttx.cpp b/src/modules/mavlink/mavlink_ftp_nuttx.cpp deleted file mode 100644 index 4ba595a87b..0000000000 --- a/src/modules/mavlink/mavlink_ftp_nuttx.cpp +++ /dev/null @@ -1,853 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/// @file mavlink_ftp.cpp -/// @author px4dev, Don Gagne - -#include -#include -#include -#include -#include -#include - -#include "mavlink_ftp.h" -#include "mavlink_tests/mavlink_ftp_test.h" - -// Uncomment the line below to get better debug output. Never commit with this left on. -//#define MAVLINK_FTP_DEBUG - -MavlinkFTP * -MavlinkFTP::get_server(void) -{ - static MavlinkFTP server; - return &server; -} - -MavlinkFTP::MavlinkFTP() : - _request_bufs{}, - _request_queue{}, - _request_queue_sem{}, - _utRcvMsgFunc{}, - _ftp_test{} -{ - // initialise the request freelist - dq_init(&_request_queue); - sem_init(&_request_queue_sem, 0, 1); - - // initialize session list - for (size_t i=0; imsgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { - mavlink_msg_file_transfer_protocol_decode(msg, &req->message); - -#ifdef MAVLINK_FTP_UNIT_TEST - if (!_utRcvMsgFunc) { - warnx("Incorrectly written unit test\n"); - return; - } - // We use fake ids when unit testing - req->serverSystemId = MavlinkFtpTest::serverSystemId; - req->serverComponentId = MavlinkFtpTest::serverComponentId; - req->serverChannel = MavlinkFtpTest::serverChannel; -#else - // Not unit testing, use the real thing - req->serverSystemId = mavlink->get_system_id(); - req->serverComponentId = mavlink->get_component_id(); - req->serverChannel = mavlink->get_channel(); -#endif - - // This is the system id we want to target when sending - req->targetSystemId = msg->sysid; - - if (req->message.target_system == req->serverSystemId) { - req->mavlink = mavlink; -#ifdef MAVLINK_FTP_UNIT_TEST - // We are running in Unit Test mode. Don't queue, just call _worket directly. - _process_request(req); -#else - // We are running in normal mode. Queue the request to the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); -#endif - return; - } - } - - _return_request(req); -} - -/// @brief Queued static work queue routine to handle mavlink messages -void -MavlinkFTP::_worker_trampoline(void *arg) -{ - Request* req = reinterpret_cast(arg); - MavlinkFTP* server = MavlinkFTP::get_server(); - - // call the server worker with the work item - server->_process_request(req); -} - -/// @brief Processes an FTP message -void -MavlinkFTP::_process_request(Request *req) -{ - PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); - - ErrorCode errorCode = kErrNone; - - // basic sanity checks; must validate length before use - if (payload->size > kMaxDataLength) { - errorCode = kErrInvalidDataSize; - goto out; - } - -#ifdef MAVLINK_FTP_DEBUG - printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); -#endif - - switch (payload->opcode) { - case kCmdNone: - break; - - case kCmdTerminateSession: - errorCode = _workTerminate(payload); - break; - - case kCmdResetSessions: - errorCode = _workReset(payload); - break; - - case kCmdListDirectory: - errorCode = _workList(payload); - break; - - case kCmdOpenFileRO: - errorCode = _workOpen(payload, O_RDONLY); - break; - - case kCmdCreateFile: - errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY); - break; - - case kCmdOpenFileWO: - errorCode = _workOpen(payload, O_CREAT | O_WRONLY); - break; - - case kCmdReadFile: - errorCode = _workRead(payload); - break; - - case kCmdWriteFile: - errorCode = _workWrite(payload); - break; - - case kCmdRemoveFile: - errorCode = _workRemoveFile(payload); - break; - - case kCmdRename: - errorCode = _workRename(payload); - break; - - case kCmdTruncateFile: - errorCode = _workTruncateFile(payload); - break; - - case kCmdCreateDirectory: - errorCode = _workCreateDirectory(payload); - break; - - case kCmdRemoveDirectory: - errorCode = _workRemoveDirectory(payload); - break; - - - case kCmdCalcFileCRC32: - errorCode = _workCalcFileCRC32(payload); - break; - - default: - errorCode = kErrUnknownCommand; - break; - } - -out: - // handle success vs. error - if (errorCode == kErrNone) { - payload->req_opcode = payload->opcode; - payload->opcode = kRspAck; -#ifdef MAVLINK_FTP_DEBUG - warnx("FTP: ack\n"); -#endif - } else { - int r_errno = errno; - warnx("FTP: nak %u", errorCode); - payload->req_opcode = payload->opcode; - payload->opcode = kRspNak; - payload->size = 1; - payload->data[0] = errorCode; - if (errorCode == kErrFailErrno) { - payload->size = 2; - payload->data[1] = r_errno; - } - } - - - // respond to the request - _reply(req); - - _return_request(req); -} - -/// @brief Sends the specified FTP reponse message out through mavlink -void -MavlinkFTP::_reply(Request *req) -{ - PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); - - payload->seqNumber = payload->seqNumber + 1; - - mavlink_message_t msg; - msg.checksum = 0; -#ifndef MAVLINK_FTP_UNIT_TEST - uint16_t len = -#endif - mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id - req->serverComponentId, // Sender component id - req->serverChannel, // Channel to send on - &msg, // Message to pack payload into - 0, // Target network - req->targetSystemId, // Target system id - 0, // Target component id - (const uint8_t*)payload); // Payload to pack into message - - bool success = true; -#ifdef MAVLINK_FTP_UNIT_TEST - // Unit test hook is set, call that instead - _utRcvMsgFunc(&msg, _ftp_test); -#else - Mavlink *mavlink = req->mavlink; - - mavlink->lockMessageBufferMutex(); - success = mavlink->message_buffer_write(&msg, len); - mavlink->unlockMessageBufferMutex(); - -#endif - - if (!success) { - warnx("FTP TX ERR"); - } -#ifdef MAVLINK_FTP_DEBUG - else { - warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d", - req->serverSystemId, - req->serverComponentId, - req->serverChannel, - msg.checksum); - } -#endif -} - -/// @brief Responds to a List command -MavlinkFTP::ErrorCode -MavlinkFTP::_workList(PayloadHeader* payload) -{ - char dirPath[kMaxDataLength]; - strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); - - DIR *dp = opendir(dirPath); - - if (dp == nullptr) { - warnx("FTP: can't open path '%s'", dirPath); - return kErrFailErrno; - } - -#ifdef MAVLINK_FTP_DEBUG - warnx("FTP: list %s offset %d", dirPath, payload->offset); -#endif - - ErrorCode errorCode = kErrNone; - struct dirent entry, *result = nullptr; - unsigned offset = 0; - - // move to the requested offset - seekdir(dp, payload->offset); - - for (;;) { - // read the directory entry - if (readdir_r(dp, &entry, &result)) { - warnx("FTP: list %s readdir_r failure\n", dirPath); - errorCode = kErrFailErrno; - break; - } - - // no more entries? - if (result == nullptr) { - if (payload->offset != 0 && offset == 0) { - // User is requesting subsequent dir entries but there were none. This means the user asked - // to seek past EOF. - errorCode = kErrEOF; - } - // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that - break; - } - - uint32_t fileSize = 0; - char buf[256]; - char direntType; - - // Determine the directory entry type - switch (entry.d_type) { - case DTYPE_FILE: - // For files we get the file size as well - direntType = kDirentFile; - snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); - struct stat st; - if (stat(buf, &st) == 0) { - fileSize = st.st_size; - } - break; - case DTYPE_DIRECTORY: - if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { - // Don't bother sending these back - direntType = kDirentSkip; - } else { - direntType = kDirentDir; - } - break; - default: - // We only send back file and diretory entries, skip everything else - direntType = kDirentSkip; - } - - if (direntType == kDirentSkip) { - // Skip send only dirent identifier - buf[0] = '\0'; - } else if (direntType == kDirentFile) { - // Files send filename and file length - snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); - } else { - // Everything else just sends name - strncpy(buf, entry.d_name, sizeof(buf)); - buf[sizeof(buf)-1] = 0; - } - size_t nameLen = strlen(buf); - - // Do we have room for the name, the one char directory identifier and the null terminator? - if ((offset + nameLen + 2) > kMaxDataLength) { - break; - } - - // Move the data into the buffer - payload->data[offset++] = direntType; - strcpy((char *)&payload->data[offset], buf); -#ifdef MAVLINK_FTP_DEBUG - printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); -#endif - offset += nameLen + 1; - } - - closedir(dp); - payload->size = offset; - - return errorCode; -} - -/// @brief Responds to an Open command -MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) -{ - int session_index = _find_unused_session(); - if (session_index < 0) { - warnx("FTP: Open failed - out of sessions\n"); - return kErrNoSessionsAvailable; - } - - char *filename = _data_as_cstring(payload); - - uint32_t fileSize = 0; - struct stat st; - if (stat(filename, &st) != 0) { - // fail only if requested open for read - if (oflag & O_RDONLY) - return kErrFailErrno; - else - st.st_size = 0; - } - fileSize = st.st_size; - - int fd = ::open(filename, oflag); - if (fd < 0) { - return kErrFailErrno; - } - _session_fds[session_index] = fd; - - payload->session = session_index; - payload->size = sizeof(uint32_t); - *((uint32_t*)payload->data) = fileSize; - - return kErrNone; -} - -/// @brief Responds to a Read command -MavlinkFTP::ErrorCode -MavlinkFTP::_workRead(PayloadHeader* payload) -{ - int session_index = payload->session; - - if (!_valid_session(session_index)) { - return kErrInvalidSession; - } - - // Seek to the specified position -#ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", payload->offset); -#endif - if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { - // Unable to see to the specified location - warnx("seek fail"); - return kErrEOF; - } - - int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); - if (bytes_read < 0) { - // Negative return indicates error other than eof - warnx("read fail %d", bytes_read); - return kErrFailErrno; - } - - payload->size = bytes_read; - - return kErrNone; -} - -/// @brief Responds to a Write command -MavlinkFTP::ErrorCode -MavlinkFTP::_workWrite(PayloadHeader* payload) -{ - int session_index = payload->session; - - if (!_valid_session(session_index)) { - return kErrInvalidSession; - } - - // Seek to the specified position -#ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", payload->offset); -#endif - if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { - // Unable to see to the specified location - warnx("seek fail"); - return kErrFailErrno; - } - - int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size); - if (bytes_written < 0) { - // Negative return indicates error other than eof - warnx("write fail %d", bytes_written); - return kErrFailErrno; - } - - payload->size = sizeof(uint32_t); - *((uint32_t*)payload->data) = bytes_written; - - return kErrNone; -} - -/// @brief Responds to a RemoveFile command -MavlinkFTP::ErrorCode -MavlinkFTP::_workRemoveFile(PayloadHeader* payload) -{ - char file[kMaxDataLength]; - strncpy(file, _data_as_cstring(payload), kMaxDataLength); - - if (unlink(file) == 0) { - payload->size = 0; - return kErrNone; - } else { - return kErrFailErrno; - } -} - -/// @brief Responds to a TruncateFile command -MavlinkFTP::ErrorCode -MavlinkFTP::_workTruncateFile(PayloadHeader* payload) -{ - char file[kMaxDataLength]; - const char temp_file[] = "/fs/microsd/.trunc.tmp"; - strncpy(file, _data_as_cstring(payload), kMaxDataLength); - payload->size = 0; - - // emulate truncate(file, payload->offset) by - // copying to temp and overwrite with O_TRUNC flag. - - struct stat st; - if (stat(file, &st) != 0) { - return kErrFailErrno; - } - - if (!S_ISREG(st.st_mode)) { - errno = EISDIR; - return kErrFailErrno; - } - - // check perms allow us to write (not romfs) - if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) { - errno = EROFS; - return kErrFailErrno; - } - - if (payload->offset == (unsigned)st.st_size) { - // nothing to do - return kErrNone; - } - else if (payload->offset == 0) { - // 1: truncate all data - int fd = ::open(file, O_TRUNC | O_WRONLY); - if (fd < 0) { - return kErrFailErrno; - } - - ::close(fd); - return kErrNone; - } - else if (payload->offset > (unsigned)st.st_size) { - // 2: extend file - int fd = ::open(file, O_WRONLY); - if (fd < 0) { - return kErrFailErrno; - } - - if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) { - ::close(fd); - return kErrFailErrno; - } - - bool ok = 1 == ::write(fd, "", 1); - ::close(fd); - - return (ok)? kErrNone : kErrFailErrno; - } - else { - // 3: truncate - if (_copy_file(file, temp_file, payload->offset) != 0) { - return kErrFailErrno; - } - if (_copy_file(temp_file, file, payload->offset) != 0) { - return kErrFailErrno; - } - if (::unlink(temp_file) != 0) { - return kErrFailErrno; - } - - return kErrNone; - } -} - -/// @brief Responds to a Terminate command -MavlinkFTP::ErrorCode -MavlinkFTP::_workTerminate(PayloadHeader* payload) -{ - if (!_valid_session(payload->session)) { - return kErrInvalidSession; - } - - ::close(_session_fds[payload->session]); - _session_fds[payload->session] = -1; - - payload->size = 0; - - return kErrNone; -} - -/// @brief Responds to a Reset command -MavlinkFTP::ErrorCode -MavlinkFTP::_workReset(PayloadHeader* payload) -{ - for (size_t i=0; isize = 0; - - return kErrNone; -} - -/// @brief Responds to a Rename command -MavlinkFTP::ErrorCode -MavlinkFTP::_workRename(PayloadHeader* payload) -{ - char oldpath[kMaxDataLength]; - char newpath[kMaxDataLength]; - - char *ptr = _data_as_cstring(payload); - size_t oldpath_sz = strlen(ptr); - - if (oldpath_sz == payload->size) { - // no newpath - errno = EINVAL; - return kErrFailErrno; - } - - strncpy(oldpath, ptr, kMaxDataLength); - strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength); - - if (rename(oldpath, newpath) == 0) { - payload->size = 0; - return kErrNone; - } else { - return kErrFailErrno; - } -} - -/// @brief Responds to a RemoveDirectory command -MavlinkFTP::ErrorCode -MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) -{ - char dir[kMaxDataLength]; - strncpy(dir, _data_as_cstring(payload), kMaxDataLength); - - if (rmdir(dir) == 0) { - payload->size = 0; - return kErrNone; - } else { - return kErrFailErrno; - } -} - -/// @brief Responds to a CreateDirectory command -MavlinkFTP::ErrorCode -MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) -{ - char dir[kMaxDataLength]; - strncpy(dir, _data_as_cstring(payload), kMaxDataLength); - - if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { - payload->size = 0; - return kErrNone; - } else { - return kErrFailErrno; - } -} - -/// @brief Responds to a CalcFileCRC32 command -MavlinkFTP::ErrorCode -MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) -{ - char file_buf[256]; - uint32_t checksum = 0; - ssize_t bytes_read; - strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength); - - int fd = ::open(file_buf, O_RDONLY); - if (fd < 0) { - return kErrFailErrno; - } - - do { - bytes_read = ::read(fd, file_buf, sizeof(file_buf)); - if (bytes_read < 0) { - int r_errno = errno; - ::close(fd); - errno = r_errno; - return kErrFailErrno; - } - - checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum); - } while (bytes_read == sizeof(file_buf)); - - ::close(fd); - - payload->size = sizeof(uint32_t); - *((uint32_t*)payload->data) = checksum; - return kErrNone; -} - -/// @brief Returns true if the specified session is a valid open session -bool -MavlinkFTP::_valid_session(unsigned index) -{ - if ((index >= kMaxSession) || (_session_fds[index] < 0)) { - return false; - } - return true; -} - -/// @brief Returns an unused session index -int -MavlinkFTP::_find_unused_session(void) -{ - for (size_t i=0; isize < kMaxDataLength) { - payload->data[payload->size] = '\0'; - } else { - payload->data[kMaxDataLength - 1] = '\0'; - } - - // and return data - return (char *)&(payload->data[0]); -} - -/// @brief Returns a unused Request entry. NULL if none available. -MavlinkFTP::Request * -MavlinkFTP::_get_request(void) -{ - _lock_request_queue(); - Request* req = reinterpret_cast(dq_remfirst(&_request_queue)); - _unlock_request_queue(); - return req; -} - -/// @brief Locks a semaphore to provide exclusive access to the request queue -void -MavlinkFTP::_lock_request_queue(void) -{ - do {} - while (sem_wait(&_request_queue_sem) != 0); -} - -/// @brief Unlocks the semaphore providing exclusive access to the request queue -void -MavlinkFTP::_unlock_request_queue(void) -{ - sem_post(&_request_queue_sem); -} - -/// @brief Returns a no longer needed request to the queue -void -MavlinkFTP::_return_request(Request *req) -{ - _lock_request_queue(); - dq_addlast(&req->work.dq, &_request_queue); - _unlock_request_queue(); -} - -/// @brief Copy file (with limited space) -int -MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) -{ - char buff[512]; - int src_fd = -1, dst_fd = -1; - int op_errno = 0; - - src_fd = ::open(src_path, O_RDONLY); - if (src_fd < 0) { - return -1; - } - - dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY); - if (dst_fd < 0) { - op_errno = errno; - ::close(src_fd); - errno = op_errno; - return -1; - } - - while (length > 0) { - ssize_t bytes_read, bytes_written; - size_t blen = (length > sizeof(buff))? sizeof(buff) : length; - - bytes_read = ::read(src_fd, buff, blen); - if (bytes_read == 0) { - // EOF - break; - } - else if (bytes_read < 0) { - warnx("cp: read"); - op_errno = errno; - break; - } - - bytes_written = ::write(dst_fd, buff, bytes_read); - if (bytes_written != bytes_read) { - warnx("cp: short write"); - op_errno = errno; - break; - } - - length -= bytes_written; - } - - ::close(src_fd); - ::close(dst_fd); - - errno = op_errno; - return (length > 0)? -1 : 0; -} diff --git a/src/modules/mavlink/mavlink_receiver_posix.cpp b/src/modules/mavlink/mavlink_receiver.cpp similarity index 99% rename from src/modules/mavlink/mavlink_receiver_posix.cpp rename to src/modules/mavlink/mavlink_receiver.cpp index bfe7a4552c..03ad2eca15 100644 --- a/src/modules/mavlink/mavlink_receiver_posix.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -41,7 +41,12 @@ */ /* XXX trim includes */ +#ifdef __PX4_NUTTX +#include +#include +#else #include +#endif #include #include #include @@ -1495,14 +1500,14 @@ MavlinkReceiver::receive_thread(void *arg) sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); - px4_pollfd_struct_t fds[1]; + struct pollfd fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; ssize_t nread = 0; while (!_mavlink->_task_should_exit) { - if (px4_poll(fds, 1, timeout) > 0) { + if (poll(fds, 1, timeout) > 0) { /* non-blocking read. read may return negative values */ if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { diff --git a/src/modules/mavlink/mavlink_receiver_nuttx.cpp b/src/modules/mavlink/mavlink_receiver_nuttx.cpp deleted file mode 100644 index faede15cb9..0000000000 --- a/src/modules/mavlink/mavlink_receiver_nuttx.cpp +++ /dev/null @@ -1,1590 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file mavlink_receiver.cpp - * MAVLink protocol message receive and dispatch - * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler - */ - -/* XXX trim includes */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -__BEGIN_DECLS - -#include "mavlink_bridge_header.h" -#include "mavlink_receiver.h" -#include "mavlink_main.h" - -__END_DECLS - -static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; - -MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : - _mavlink(parent), - status{}, - hil_local_pos{}, - hil_land_detector{}, - _control_mode{}, - _global_pos_pub(-1), - _local_pos_pub(-1), - _attitude_pub(-1), - _gps_pub(-1), - _sensors_pub(-1), - _gyro_pub(-1), - _accel_pub(-1), - _mag_pub(-1), - _baro_pub(-1), - _airspeed_pub(-1), - _battery_pub(-1), - _cmd_pub(-1), - _flow_pub(-1), - _range_pub(-1), - _offboard_control_mode_pub(-1), - _actuator_controls_pub(-1), - _global_vel_sp_pub(-1), - _att_sp_pub(-1), - _rates_sp_pub(-1), - _force_sp_pub(-1), - _pos_sp_triplet_pub(-1), - _vicon_position_pub(-1), - _vision_position_pub(-1), - _telemetry_status_pub(-1), - _rc_pub(-1), - _manual_pub(-1), - _land_detector_pub(-1), - _time_offset_pub(-1), - _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), - _hil_frames(0), - _old_timestamp(0), - _hil_local_proj_inited(0), - _hil_local_alt0(0.0f), - _hil_local_proj_ref{}, - _offboard_control_mode{}, - _rates_sp{}, - _time_offset_avg_alpha(0.6), - _time_offset(0) -{ - - // make sure the FTP server is started - (void)MavlinkFTP::get_server(); -} - -MavlinkReceiver::~MavlinkReceiver() -{ -} - -void -MavlinkReceiver::handle_message(mavlink_message_t *msg) -{ - switch (msg->msgid) { - case MAVLINK_MSG_ID_COMMAND_LONG: - handle_message_command_long(msg); - break; - - case MAVLINK_MSG_ID_COMMAND_INT: - handle_message_command_int(msg); - break; - - case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: - handle_message_optical_flow_rad(msg); - break; - - case MAVLINK_MSG_ID_PING: - handle_message_ping(msg); - break; - - case MAVLINK_MSG_ID_SET_MODE: - handle_message_set_mode(msg); - break; - - case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: - handle_message_vicon_position_estimate(msg); - break; - - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - handle_message_set_position_target_local_ned(msg); - break; - - case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: - handle_message_set_attitude_target(msg); - break; - - case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET: - handle_message_set_actuator_control_target(msg); - break; - - case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: - handle_message_vision_position_estimate(msg); - break; - - case MAVLINK_MSG_ID_RADIO_STATUS: - handle_message_radio_status(msg); - break; - - case MAVLINK_MSG_ID_MANUAL_CONTROL: - handle_message_manual_control(msg); - break; - - case MAVLINK_MSG_ID_HEARTBEAT: - handle_message_heartbeat(msg); - break; - - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - handle_message_request_data_stream(msg); - break; - - case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: - MavlinkFTP::get_server()->handle_message(_mavlink, msg); - break; - - case MAVLINK_MSG_ID_SYSTEM_TIME: - handle_message_system_time(msg); - break; - - case MAVLINK_MSG_ID_TIMESYNC: - handle_message_timesync(msg); - break; - - default: - break; - } - - /* - * Only decode hil messages in HIL mode. - * - * The HIL mode is enabled by the HIL bit flag - * in the system mode. Either send a set mode - * COMMAND_LONG message or a SET_MODE message - * - * Accept HIL GPS messages if use_hil_gps flag is true. - * This allows to provide fake gps measurements to the system. - */ - if (_mavlink->get_hil_enabled()) { - switch (msg->msgid) { - case MAVLINK_MSG_ID_HIL_SENSOR: - handle_message_hil_sensor(msg); - break; - - case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: - handle_message_hil_state_quaternion(msg); - break; - - case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: - handle_message_hil_optical_flow(msg); - break; - - default: - break; - } - } - - - if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { - switch (msg->msgid) { - case MAVLINK_MSG_ID_HIL_GPS: - handle_message_hil_gps(msg); - break; - - default: - break; - } - - } - - /* If we've received a valid message, mark the flag indicating so. - This is used in the '-w' command-line flag. */ - _mavlink->set_has_received_messages(true); -} - -void -MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) -{ - /* command */ - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - //check for MAVLINK terminate command - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { - /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote"); - fflush(stdout); - usleep(50000); - - /* terminate other threads and this thread */ - _mavlink->_task_should_exit = true; - - } else { - - if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", - mavlink_system.sysid, mavlink_system.compid); - return; - } - - struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - // XXX do proper translation - vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; - - if (_cmd_pub < 0) { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } - } - } -} - -void -MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) -{ - /* command */ - mavlink_command_int_t cmd_mavlink; - mavlink_msg_command_int_decode(msg, &cmd_mavlink); - - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - //check for MAVLINK terminate command - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { - /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote"); - fflush(stdout); - usleep(50000); - - /* terminate other threads and this thread */ - _mavlink->_task_should_exit = true; - - } else { - - if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", - mavlink_system.sysid, mavlink_system.compid); - return; - } - - struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); - - /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ - vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; - vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; - vcmd.param7 = cmd_mavlink.z; - // XXX do proper translation - vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - - if (_cmd_pub < 0) { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } - } - } -} - -void -MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) -{ - /* optical flow */ - mavlink_optical_flow_rad_t flow; - mavlink_msg_optical_flow_rad_decode(msg, &flow); - - enum Rotation flow_rot; - param_get(param_find("SENS_FLOW_ROT"),&flow_rot); - - struct optical_flow_s f; - memset(&f, 0, sizeof(f)); - - f.timestamp = flow.time_usec; - f.integration_timespan = flow.integration_time_us; - f.pixel_flow_x_integral = flow.integrated_x; - f.pixel_flow_y_integral = flow.integrated_y; - f.gyro_x_rate_integral = flow.integrated_xgyro; - f.gyro_y_rate_integral = flow.integrated_ygyro; - f.gyro_z_rate_integral = flow.integrated_zgyro; - f.time_since_last_sonar_update = flow.time_delta_distance_us; - f.ground_distance_m = flow.distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - f.gyro_temperature = flow.temperature; - - /* rotate measurements according to parameter */ - float zeroval = 0.0f; - rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); - - if (_flow_pub < 0) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &f); - } -} - -void -MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) -{ - /* optical flow */ - mavlink_hil_optical_flow_t flow; - mavlink_msg_hil_optical_flow_decode(msg, &flow); - - struct optical_flow_s f; - memset(&f, 0, sizeof(f)); - - f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; - f.integration_timespan = flow.integration_time_us; - f.pixel_flow_x_integral = flow.integrated_x; - f.pixel_flow_y_integral = flow.integrated_y; - f.gyro_x_rate_integral = flow.integrated_xgyro; - f.gyro_y_rate_integral = flow.integrated_ygyro; - f.gyro_z_rate_integral = flow.integrated_zgyro; - f.time_since_last_sonar_update = flow.time_delta_distance_us; - f.ground_distance_m = flow.distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - f.gyro_temperature = flow.temperature; - - if (_flow_pub < 0) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &f); - } - - /* Use distance value for range finder report */ - struct range_finder_report r; - memset(&r, 0, sizeof(r)); - - r.timestamp = hrt_absolute_time(); - r.error_count = 0; - r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.distance; - r.minimum_distance = 0.0f; - r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable - r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); - - if (_range_pub < 0) { - _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); - } else { - orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); - } -} - -void -MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) -{ - mavlink_set_mode_t new_mode; - mavlink_msg_set_mode_decode(msg, &new_mode); - - struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); - - union px4_custom_mode custom_mode; - custom_mode.data = new_mode.custom_mode; - /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = new_mode.base_mode; - vcmd.param2 = custom_mode.main_mode; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = VEHICLE_CMD_DO_SET_MODE; - vcmd.target_system = new_mode.target_system; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - if (_cmd_pub < 0) { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } -} - -void -MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) -{ - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); - - struct vehicle_vicon_position_s vicon_position; - memset(&vicon_position, 0, sizeof(vicon_position)); - - vicon_position.timestamp = hrt_absolute_time(); - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - vicon_position.roll = pos.roll; - vicon_position.pitch = pos.pitch; - vicon_position.yaw = pos.yaw; - - if (_vicon_position_pub < 0) { - _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - - } else { - orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); - } -} - -void -MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg) -{ - mavlink_set_position_target_local_ned_t set_position_target_local_ned; - mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); - - struct offboard_control_mode_s offboard_control_mode; - memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints - - /* Only accept messages which are intended for this system */ - if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - set_position_target_local_ned.target_system == 0) && - (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0)) { - - /* convert mavlink type (local, NED) to uORB offboard control struct */ - offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); - offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); - offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); - bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); - /* yaw ignore flag mapps to ignore_attitude */ - offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); - /* yawrate ignore flag mapps to ignore_bodyrate */ - offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); - - offboard_control_mode.timestamp = hrt_absolute_time(); - - if (_offboard_control_mode_pub < 0) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); - - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); - } - - /* If we are in offboard control mode and offboard control loop through is enabled - * also publish the setpoint topic which is read by the controller */ - if (_mavlink->get_forward_externalsp()) { - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - } - if (_control_mode.flag_control_offboard_enabled) { - if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { - /* The offboard setpoint is a force setpoint only, directly writing to the force - * setpoint topic and not publishing the setpoint triplet topic */ - struct vehicle_force_setpoint_s force_sp; - force_sp.x = set_position_target_local_ned.afx; - force_sp.y = set_position_target_local_ned.afy; - force_sp.z = set_position_target_local_ned.afz; - //XXX: yaw - if (_force_sp_pub < 0) { - _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); - } else { - orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); - } - } else { - /* It's not a pure force setpoint: publish to setpoint triplet topic */ - struct position_setpoint_triplet_s pos_sp_triplet; - pos_sp_triplet.previous.valid = false; - pos_sp_triplet.next.valid = false; - pos_sp_triplet.current.valid = true; - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others - - /* set the local pos values */ - if (!offboard_control_mode.ignore_position) { - pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = set_position_target_local_ned.x; - pos_sp_triplet.current.y = set_position_target_local_ned.y; - pos_sp_triplet.current.z = set_position_target_local_ned.z; - } else { - pos_sp_triplet.current.position_valid = false; - } - - /* set the local vel values */ - if (!offboard_control_mode.ignore_velocity) { - pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = set_position_target_local_ned.vx; - pos_sp_triplet.current.vy = set_position_target_local_ned.vy; - pos_sp_triplet.current.vz = set_position_target_local_ned.vz; - } else { - pos_sp_triplet.current.velocity_valid = false; - } - - /* set the local acceleration values if the setpoint type is 'local pos' and none - * of the accelerations fields is set to 'ignore' */ - if (!offboard_control_mode.ignore_acceleration_force) { - pos_sp_triplet.current.acceleration_valid = true; - pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; - pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; - pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; - pos_sp_triplet.current.acceleration_is_force = - is_force_sp; - - } else { - pos_sp_triplet.current.acceleration_valid = false; - } - - /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude) { - pos_sp_triplet.current.yaw_valid = true; - pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; - - } else { - pos_sp_triplet.current.yaw_valid = false; - } - - /* set the yawrate sp value */ - if (!offboard_control_mode.ignore_bodyrate) { - pos_sp_triplet.current.yawspeed_valid = true; - pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; - - } else { - pos_sp_triplet.current.yawspeed_valid = false; - } - //XXX handle global pos setpoints (different MAV frames) - - - if (_pos_sp_triplet_pub < 0) { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); - } else { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); - } - - } - - } - - } - } -} - -void -MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) -{ - mavlink_set_actuator_control_target_t set_actuator_control_target; - mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); - - struct offboard_control_mode_s offboard_control_mode; - memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints - - struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints - - if ((mavlink_system.sysid == set_actuator_control_target.target_system || - set_actuator_control_target.target_system == 0) && - (mavlink_system.compid == set_actuator_control_target.target_component || - set_actuator_control_target.target_component == 0)) { - - /* ignore all since we are setting raw actuators here */ - offboard_control_mode.ignore_thrust = true; - offboard_control_mode.ignore_attitude = true; - offboard_control_mode.ignore_bodyrate = true; - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; - - offboard_control_mode.timestamp = hrt_absolute_time(); - - if (_offboard_control_mode_pub < 0) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); - } - - - /* If we are in offboard control mode, publish the actuator controls */ - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - } - - if (_control_mode.flag_control_offboard_enabled) { - - actuator_controls.timestamp = hrt_absolute_time(); - - /* Set duty cycles for the servos in actuator_controls_0 */ - for(size_t i = 0; i < 8; i++) { - actuator_controls.control[i] = set_actuator_control_target.controls[i]; - } - - if (_actuator_controls_pub < 0) { - _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); - } else { - orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); - } - } - } - -} - -void -MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) -{ - mavlink_vision_position_estimate_t pos; - mavlink_msg_vision_position_estimate_decode(msg, &pos); - - struct vision_position_estimate vision_position; - memset(&vision_position, 0, sizeof(vision_position)); - - // Use the component ID to identify the vision sensor - vision_position.id = msg->compid; - - vision_position.timestamp_boot = hrt_absolute_time(); // Monotonic time - vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time - vision_position.x = pos.x; - vision_position.y = pos.y; - vision_position.z = pos.z; - - // XXX fix this - vision_position.vx = 0.0f; - vision_position.vy = 0.0f; - vision_position.vz = 0.0f; - - math::Quaternion q; - q.from_euler(pos.roll, pos.pitch, pos.yaw); - - vision_position.q[0] = q(0); - vision_position.q[1] = q(1); - vision_position.q[2] = q(2); - vision_position.q[3] = q(3); - - if (_vision_position_pub < 0) { - _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); - - } else { - orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); - } -} - -void -MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) -{ - mavlink_set_attitude_target_t set_attitude_target; - mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); - - /* Only accept messages which are intended for this system */ - if ((mavlink_system.sysid == set_attitude_target.target_system || - set_attitude_target.target_system == 0) && - (mavlink_system.compid == set_attitude_target.target_component || - set_attitude_target.target_component == 0)) { - - /* set correct ignore flags for thrust field: copy from mavlink message */ - _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); - - /* - * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust - * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore - * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits - * set for everything else. - */ - - /* - * if attitude or body rate have been used (not ignored) previously and this message only sends - * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and - * body rates to keep the controllers running - */ - bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); - bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); - - if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { - /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; - _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; - } else { - _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - _offboard_control_mode.ignore_attitude = ignore_attitude; - } - - _offboard_control_mode.ignore_position = true; - _offboard_control_mode.ignore_velocity = true; - _offboard_control_mode.ignore_acceleration_force = true; - - _offboard_control_mode.timestamp = hrt_absolute_time(); - - if (_offboard_control_mode_pub < 0) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode); - - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode); - } - - /* If we are in offboard control mode and offboard control loop through is enabled - * also publish the setpoint topic which is read by the controller */ - if (_mavlink->get_forward_externalsp()) { - bool updated; - orb_check(_control_mode_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); - } - - if (_control_mode.flag_control_offboard_enabled) { - - /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(_offboard_control_mode.ignore_attitude)) { - static struct vehicle_attitude_setpoint_s att_sp = {}; - att_sp.timestamp = hrt_absolute_time(); - mavlink_quaternion_to_euler(set_attitude_target.q, - &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); - att_sp.R_valid = true; - if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - att_sp.thrust = set_attitude_target.thrust; - } - att_sp.yaw_sp_move_rate = 0.0; - memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); - att_sp.q_d_valid = true; - if (_att_sp_pub < 0) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } - } - - /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ - ///XXX add support for ignoring individual axes - if (!(_offboard_control_mode.ignore_bodyrate)) { - _rates_sp.timestamp = hrt_absolute_time(); - _rates_sp.roll = set_attitude_target.body_roll_rate; - _rates_sp.pitch = set_attitude_target.body_pitch_rate; - _rates_sp.yaw = set_attitude_target.body_yaw_rate; - if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - _rates_sp.thrust = set_attitude_target.thrust; - } - - if (_att_sp_pub < 0) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); - } - } - } - - } - } -} - -void -MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) -{ - /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ - if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - - tstatus.timestamp = hrt_absolute_time(); - tstatus.telem_time = tstatus.timestamp; - /* tstatus.heartbeat_time is set by system heartbeats */ - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; - tstatus.system_id = msg->sysid; - tstatus.component_id = msg->compid; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } - } -} - -void -MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) -{ - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - manual.timestamp = hrt_absolute_time(); - manual.x = man.x / 1000.0f; - manual.y = man.y / 1000.0f; - manual.r = man.r / 1000.0f; - manual.z = man.z / 1000.0f; - - // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); - - if (_manual_pub < 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } -} - -void -MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) -{ - /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ - if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { - mavlink_heartbeat_t hb; - mavlink_msg_heartbeat_decode(msg, &hb); - - /* ignore own heartbeats, accept only heartbeats from GCS */ - if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - - struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - - /* set heartbeat time and topic time and publish - - * the telem status also gets updated on telemetry events - */ - tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = tstatus.timestamp; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } - } - } -} - -void -MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) -{ - mavlink_ping_t ping; - mavlink_msg_ping_decode( msg, &ping); - if ((mavlink_system.sysid == ping.target_system) && - (mavlink_system.compid == ping.target_component)) { - mavlink_message_t msg_out; - mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping); - _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out); - } -} - -void -MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) -{ - mavlink_request_data_stream_t req; - mavlink_msg_request_data_stream_decode(msg, &req); - - if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid && req.req_message_rate != 0) { - float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; - - MavlinkStream *stream; - LL_FOREACH(_mavlink->get_streams(), stream) { - if (req.req_stream_id == stream->get_id()) { - _mavlink->configure_stream_threadsafe(stream->get_name(), rate); - } - } - } -} - -void -MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) -{ - mavlink_system_time_t time; - mavlink_msg_system_time_decode(msg, &time); - - timespec tv; - clock_gettime(CLOCK_REALTIME, &tv); - - // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; - bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; - - if (!onb_unix_valid && ofb_unix_valid) { - tv.tv_sec = time.time_unix_usec / 1000000ULL; - tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; - if(clock_settime(CLOCK_REALTIME, &tv)) { - warn("failed setting clock"); - } - else { - warnx("[timesync] UTC time synced."); - } - } - -} - -void -MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) -{ - mavlink_timesync_t tsync; - mavlink_msg_timesync_decode(msg, &tsync); - - struct time_offset_s tsync_offset; - memset(&tsync_offset, 0, sizeof(tsync_offset)); - - uint64_t now_ns = hrt_absolute_time() * 1000LL ; - - if (tsync.tc1 == 0) { - - mavlink_timesync_t rsync; // return timestamped sync message - - rsync.tc1 = now_ns; - rsync.ts1 = tsync.ts1; - - _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); - - return; - - } else if (tsync.tc1 > 0) { - - int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; - int64_t dt = _time_offset - offset_ns; - - if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew - _time_offset = offset_ns; - warnx("[timesync] Hard setting offset."); - } else { - smooth_time_offset(offset_ns); - } - } - - tsync_offset.offset_ns = _time_offset ; - - if (_time_offset_pub < 0) { - _time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset); - - } else { - orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset); - } - -} - -void -MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) -{ - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); - - uint64_t timestamp = hrt_absolute_time(); - - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); - - float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - // XXX need to fix this - float tas = ias; - - airspeed.timestamp = timestamp; - airspeed.indicated_airspeed_m_s = ias; - airspeed.true_airspeed_m_s = tas; - - if (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } - - /* gyro */ - { - struct gyro_report gyro; - memset(&gyro, 0, sizeof(gyro)); - - gyro.timestamp = timestamp; - gyro.x_raw = imu.xgyro * 1000.0f; - gyro.y_raw = imu.ygyro * 1000.0f; - gyro.z_raw = imu.zgyro * 1000.0f; - gyro.x = imu.xgyro; - gyro.y = imu.ygyro; - gyro.z = imu.zgyro; - gyro.temperature = imu.temperature; - - if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); - } - } - - /* accelerometer */ - { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); - - accel.timestamp = timestamp; - accel.x_raw = imu.xacc / mg2ms2; - accel.y_raw = imu.yacc / mg2ms2; - accel.z_raw = imu.zacc / mg2ms2; - accel.x = imu.xacc; - accel.y = imu.yacc; - accel.z = imu.zacc; - accel.temperature = imu.temperature; - - if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } - } - - /* magnetometer */ - { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); - - mag.timestamp = timestamp; - mag.x_raw = imu.xmag * 1000.0f; - mag.y_raw = imu.ymag * 1000.0f; - mag.z_raw = imu.zmag * 1000.0f; - mag.x = imu.xmag; - mag.y = imu.ymag; - mag.z = imu.zmag; - - if (_mag_pub < 0) { - /* publish to the first mag topic */ - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - } - - /* baro */ - { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); - - baro.timestamp = timestamp; - baro.pressure = imu.abs_pressure; - baro.altitude = imu.pressure_alt; - baro.temperature = imu.temperature; - - if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - } - } - - /* sensor combined */ - { - struct sensor_combined_s hil_sensors; - memset(&hil_sensors, 0, sizeof(hil_sensors)); - - hil_sensors.timestamp = timestamp; - - hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; - hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; - hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - hil_sensors.gyro_rad_s[0] = imu.xgyro; - hil_sensors.gyro_rad_s[1] = imu.ygyro; - hil_sensors.gyro_rad_s[2] = imu.zgyro; - - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_timestamp = timestamp; - - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; - - hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; - hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; - hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; - hil_sensors.magnetometer_ga[0] = imu.xmag; - hil_sensors.magnetometer_ga[1] = imu.ymag; - hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_timestamp = timestamp; - - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_timestamp = timestamp; - - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_timestamp = timestamp; - - /* publish combined sensor topic */ - if (_sensors_pub < 0) { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - - } else { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); - } - } - - /* battery status */ - { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - - hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; - hil_battery_status.current_a = 10.0f; - hil_battery_status.discharged_mah = -1.0f; - - if (_battery_pub < 0) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - } - } - - /* increment counters */ - _hil_frames++; - - /* print HIL sensors rate */ - if ((timestamp - _old_timestamp) > 10000000) { - // printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); - _old_timestamp = timestamp; - _hil_frames = 0; - } -} - -void -MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) -{ - mavlink_hil_gps_t gps; - mavlink_msg_hil_gps_decode(msg, &gps); - - uint64_t timestamp = hrt_absolute_time(); - - struct vehicle_gps_position_s hil_gps; - memset(&hil_gps, 0, sizeof(hil_gps)); - - hil_gps.timestamp_time = timestamp; - hil_gps.time_utc_usec = gps.time_usec; - - hil_gps.timestamp_position = timestamp; - hil_gps.lat = gps.lat; - hil_gps.lon = gps.lon; - hil_gps.alt = gps.alt; - hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m - hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m - - hil_gps.timestamp_variance = timestamp; - hil_gps.s_variance_m_s = 5.0f; - - hil_gps.timestamp_velocity = timestamp; - hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m - hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m - hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m - hil_gps.vel_ned_valid = true; - hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - - hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? - - if (_gps_pub < 0) { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - - } else { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); - } -} - -void -MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) -{ - mavlink_hil_state_quaternion_t hil_state; - mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - - uint64_t timestamp = hrt_absolute_time(); - - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); - - airspeed.timestamp = timestamp; - airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; - airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - - if (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } - - /* attitude */ - struct vehicle_attitude_s hil_attitude; - { - memset(&hil_attitude, 0, sizeof(hil_attitude)); - math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3, 3> C_nb = q.to_dcm(); - math::Vector<3> euler = C_nb.to_euler(); - - hil_attitude.timestamp = timestamp; - memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); - hil_attitude.R_valid = true; - - hil_attitude.q[0] = q(0); - hil_attitude.q[1] = q(1); - hil_attitude.q[2] = q(2); - hil_attitude.q[3] = q(3); - hil_attitude.q_valid = true; - - hil_attitude.roll = euler(0); - hil_attitude.pitch = euler(1); - hil_attitude.yaw = euler(2); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - - if (_attitude_pub < 0) { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - - } else { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); - } - } - - /* global position */ - { - struct vehicle_global_position_s hil_global_pos; - memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - - hil_global_pos.timestamp = timestamp; - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vel_n = hil_state.vx / 100.0f; - hil_global_pos.vel_e = hil_state.vy / 100.0f; - hil_global_pos.vel_d = hil_state.vz / 100.0f; - hil_global_pos.yaw = hil_attitude.yaw; - hil_global_pos.eph = 2.0f; - hil_global_pos.epv = 4.0f; - - if (_global_pos_pub < 0) { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - - } else { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); - } - } - - /* local position */ - { - double lat = hil_state.lat * 1e-7; - double lon = hil_state.lon * 1e-7; - - if (!_hil_local_proj_inited) { - _hil_local_proj_inited = true; - _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); - hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = lat; - hil_local_pos.ref_lon = lon; - hil_local_pos.ref_alt = _hil_local_alt0; - } - - float x; - float y; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); - hil_local_pos.timestamp = timestamp; - hil_local_pos.xy_valid = true; - hil_local_pos.z_valid = true; - hil_local_pos.v_xy_valid = true; - hil_local_pos.v_z_valid = true; - hil_local_pos.x = x; - hil_local_pos.y = y; - hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; - hil_local_pos.vx = hil_state.vx / 100.0f; - hil_local_pos.vy = hil_state.vy / 100.0f; - hil_local_pos.vz = hil_state.vz / 100.0f; - hil_local_pos.yaw = hil_attitude.yaw; - hil_local_pos.xy_global = true; - hil_local_pos.z_global = true; - - if (_local_pos_pub < 0) { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); - - } else { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); - } - } - - /* land detector */ - { - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - if(hil_land_detector.landed != landed) { - hil_land_detector.landed = landed; - hil_land_detector.timestamp = hrt_absolute_time(); - - if (_land_detector_pub < 0) { - _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector); - - } else { - orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector); - } - } - } - - /* accelerometer */ - { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); - - accel.timestamp = timestamp; - accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; - accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f; - accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f; - accel.x = hil_state.xacc; - accel.y = hil_state.yacc; - accel.z = hil_state.zacc; - accel.temperature = 25.0f; - - if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } - } - - /* battery status */ - { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - - hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; - hil_battery_status.current_a = 10.0f; - hil_battery_status.discharged_mah = -1.0f; - - if (_battery_pub < 0) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - } - } -} - - -/** - * Receive data from UART. - */ -void * -MavlinkReceiver::receive_thread(void *arg) -{ - int uart_fd = _mavlink->get_uart_fd(); - - const int timeout = 500; - uint8_t buf[32]; - - mavlink_message_t msg; - - /* set thread name */ - char thread_name[24]; - sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); - prctl(PR_SET_NAME, thread_name, getpid()); - - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; - - ssize_t nread = 0; - - while (!_mavlink->_task_should_exit) { - if (poll(fds, 1, timeout) > 0) { - - /* non-blocking read. read may return negative values */ - if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { - /* to avoid reading very small chunks wait for data before reading */ - usleep(1000); - } - - /* if read failed, this loop won't execute */ - for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { - /* handle generic messages and commands */ - handle_message(&msg); - - /* handle packet with parent object */ - _mavlink->handle_message(&msg); - } - } - - /* count received bytes */ - _mavlink->count_rxbytes(nread); - } - } - - return NULL; -} - -void MavlinkReceiver::print_status() -{ - -} - -uint64_t MavlinkReceiver::sync_stamp(uint64_t usec) -{ - if(_time_offset > 0) - return usec - (_time_offset / 1000) ; - else - return hrt_absolute_time(); -} - - -void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) -{ - /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, - * the faster the moving average updates in response to - * new offset samples. - */ - - _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; -} - - -void *MavlinkReceiver::start_helper(void *context) -{ - MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); - - rcv->receive_thread(NULL); - - delete rcv; - - return nullptr; -} - -pthread_t -MavlinkReceiver::receive_start(Mavlink *parent) -{ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - - // set to non-blocking read - int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); - fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); - - struct sched_param param; - (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 80; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - - pthread_attr_setstacksize(&receiveloop_attr, 1800); - pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); - - pthread_attr_destroy(&receiveloop_attr); - return thread; -} diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index bc52677412..1a9936015e 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -38,12 +38,8 @@ MODULE_COMMAND = mavlink_tests SRCS = mavlink_tests.cpp \ mavlink_ftp_test.cpp \ +SRCS += ../mavlink_ftp.cpp \ ../mavlink.c -ifeq ($(PX4_TARGET_NUTTX),nuttx) -SRCS += ../mavlink_ftp_nuttx.cpp -else -SRCS += ../mavlink_ftp_posix.cpp -endif INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 20bf945b10..41873e9d43 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -37,13 +37,9 @@ MODULE_COMMAND = mavlink ifeq ($(PX4_TARGET_OS),nuttx) -SRCS += mavlink_main_nuttx.cpp \ - mavlink_ftp_nuttx.cpp \ - mavlink_receiver_nuttx.cpp +SRCS += mavlink_main_nuttx.cpp else -SRCS += mavlink_main_posix.cpp \ - mavlink_ftp_posix.cpp \ - mavlink_receiver_posix.cpp +SRCS += mavlink_main_posix.cpp endif SRCS += mavlink.c \ @@ -52,7 +48,9 @@ SRCS += mavlink.c \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp + mavlink_rate_limiter.cpp \ + mavlink_receiver.cpp \ + mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From a4c33f51734ce7bd6a36f68a904b7078c3e623a0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 22 Apr 2015 11:16:58 -0700 Subject: [PATCH 147/258] QuRT: removed calls to sleep A stub for hrt_sleep was removed. Will add back when DSPAL supports sleep. Signed-off-by: Mark Charlebois --- src/platforms/qurt/px4_layer/drv_hrt.c | 8 -------- src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 5 +++++ src/platforms/qurt/tests/hello/hello_example.cpp | 7 ------- 3 files changed, 5 insertions(+), 15 deletions(-) diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 0b42cfa0e3..39e0f73b37 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -285,14 +285,6 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo hrt_call_internal(entry, calltime, 0, callout, arg); } -void hrt_sleep(uint32_t seconds) -{ -} - -void hrt_usleep(uint32_t useconds) -{ -} - #if 0 /* * Convert absolute time to a timespec. diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index cd4b9527f9..92f25e5827 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -195,3 +195,8 @@ void px4_show_tasks() // STUBS +extern "C" { +void hrt_sleep(unsigned long) +{ +} +} diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp index 1975029a3f..6827220888 100644 --- a/src/platforms/qurt/tests/hello/hello_example.cpp +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -40,15 +40,9 @@ */ #include "hello_example.h" -#include #include #include -/* - * Wrap the sleep call. - */ -__EXPORT extern void hrt_sleep(uint32_t seconds); - px4::AppState HelloExample::appState; int HelloExample::main() @@ -57,7 +51,6 @@ int HelloExample::main() int i=0; while (!appState.exitRequested() && i<5) { - hrt_sleep(2); printf(" Doing work...\n"); ++i; From 3336fce1f48550d4390c5f863cbe692549dcdb73 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 00:52:44 -0700 Subject: [PATCH 148/258] QuRT and POSIX changes Partial commit of the changes for QuRT and related changes for POSIX Signed-off-by: Mark Charlebois --- makefiles/qurt.mk | 3 +- makefiles/qurt/config_qurt_default.mk | 50 +- makefiles/toolchain_hexagon.mk | 8 +- src/drivers/device/vdev_posix.cpp | 3 +- src/drivers/hil/hil_posix.cpp | 7 +- .../{sensors_posix.cpp => sensors.cpp} | 91 +- src/modules/sensors/sensors_nuttx.cpp | 2305 ----------------- src/modules/simulator/simulator.cpp | 11 +- src/modules/simulator/simulator.h | 2 + src/modules/uORB/MuORB.cpp | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 8 +- .../posix/drivers/airspeedsim/airspeedsim.cpp | 407 +++ .../posix/drivers/airspeedsim/airspeedsim.h | 187 ++ .../posix/drivers/airspeedsim/module.mk | 40 + src/platforms/posix/drivers/barosim/baro.cpp | 17 +- .../posix/drivers/barosim/baro_sim.cpp | 5 +- .../posix/drivers/gyrosim/gyrosim.cpp | 1 - src/platforms/px4_common.h | 8 + src/platforms/px4_time.h | 31 + src/platforms/qurt/include/i2c.h | 43 + src/platforms/qurt/include/poll.h | 37 + src/platforms/qurt/include/sys/ioctl.h | 3 + src/platforms/qurt/main.cpp | 47 +- .../qurt/px4_layer/px4_qurt_tasks.cpp | 37 +- .../qurt/tests/hello/hello_start_qurt.cpp | 2 +- 25 files changed, 919 insertions(+), 2435 deletions(-) rename src/modules/sensors/{sensors_posix.cpp => sensors.cpp} (97%) delete mode 100644 src/modules/sensors/sensors_nuttx.cpp create mode 100644 src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp create mode 100644 src/platforms/posix/drivers/airspeedsim/airspeedsim.h create mode 100644 src/platforms/posix/drivers/airspeedsim/module.mk create mode 100644 src/platforms/px4_common.h create mode 100644 src/platforms/px4_time.h create mode 100644 src/platforms/qurt/include/i2c.h create mode 100644 src/platforms/qurt/include/sys/ioctl.h diff --git a/makefiles/qurt.mk b/makefiles/qurt.mk index f34392a210..612dd0bd16 100644 --- a/makefiles/qurt.mk +++ b/makefiles/qurt.mk @@ -35,6 +35,5 @@ # MODULES += \ - platforms/common \ - platforms/qurt/px4_layer + platforms/common diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index c076cd5841..ba7ca3e143 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -10,17 +10,17 @@ # # Board support modules # -#MODULES += drivers/device -#MODULES += drivers/blinkm -#MODULES += drivers/hil -#MODULES += drivers/rgbled -#MODULES += modules/sensors +MODULES += drivers/device +MODULES += drivers/blinkm +MODULES += drivers/hil +MODULES += drivers/rgbled +MODULES += modules/sensors #MODULES += drivers/ms5611 # # System commands # -#MODULES += systemcmds/param +MODULES += systemcmds/param # # General system control @@ -30,8 +30,8 @@ # # Estimation modules (EKF/ SO3 / other filters) # -#MODULES += modules/attitude_estimator_ekf -#MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator # # Vehicle Control @@ -41,37 +41,37 @@ # # Library modules # -#MODULES += modules/systemlib -#MODULES += modules/systemlib/mixer -#MODULES += modules/uORB +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB #MODULES += modules/dataman #MODULES += modules/sdlog2 -#MODULES += modules/simulator -#MODULES += modules/commander +MODULES += modules/simulator +MODULES += modules/commander # # Libraries # -#MODULES += lib/mathlib -#MODULES += lib/mathlib/math/filter -#MODULES += lib/geo -#MODULES += lib/geo_lookup -#MODULES += lib/conversion +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion # # QuRT port # MODULES += platforms/qurt/px4_layer -#MODULES += platforms/linux/drivers/accelsim -#MODULES += platforms/linux/drivers/gyrosim -#MODULES += platforms/linux/drivers/adcsim -#MODULES += platforms/linux/drivers/barosim +MODULES += platforms/posix/drivers/accelsim +MODULES += platforms/posix/drivers/gyrosim +MODULES += platforms/posix/drivers/adcsim +MODULES += platforms/posix/drivers/barosim # # Unit tests # MODULES += platforms/qurt/tests/hello -#MODULES += platforms/linux/tests/vcdev_test -#MODULES += platforms/linux/tests/hrt_test -#MODULES += platforms/linux/tests/wqueue +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index c409136c5b..75bcec41ba 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -68,7 +68,7 @@ endif # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup -MAXOPTIMIZATION ?= -O2 +MAXOPTIMIZATION ?= -Os # Base CPU flags for each of the supported architectures. # @@ -81,10 +81,11 @@ ifeq ($(CONFIG_BOARD),) $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ - -D__PX4_QURT \ + -D__PX4_QURT -D__PX4_POSIX \ -D__EXPORT= \ -Dnoreturn_function= \ -Drestrict= \ + -I/opt/6.4.05/gnu/hexagon/include \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ -I$(PX4_BASE)/../dspal/include \ @@ -116,7 +117,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Werror \ -Wno-unused-parameter \ + -Wno-unused-function \ -Wno-unused-variable \ + -Wno-gnu-array-member-paren-init \ -Wno-cast-align \ -Wno-missing-braces \ -Wno-strict-aliasing @@ -206,6 +209,7 @@ DEP_INCLUDES = $(subst .o,.d,$(OBJS)) define COMPILE @$(ECHO) "CC: $1" @$(MKDIR) -p $(dir $2) + @echo $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 endef diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 31f141267f..58b445d607 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -37,7 +37,8 @@ * POSIX-like API for virtual character device */ -#include "px4_posix.h" +#include +#include #include "device.h" #include diff --git a/src/drivers/hil/hil_posix.cpp b/src/drivers/hil/hil_posix.cpp index eca2ec8ba5..eb86a49742 100644 --- a/src/drivers/hil/hil_posix.cpp +++ b/src/drivers/hil/hil_posix.cpp @@ -50,6 +50,8 @@ #include #include +#include +#include #include #include @@ -61,7 +63,8 @@ #include #include #include -#include +#include +#include #include #include @@ -409,7 +412,7 @@ HIL::task_main() /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && - isfinite(outputs.output[i]) && + PX4_ISFINITE(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { /* scale for PWM output 900 - 2100us */ diff --git a/src/modules/sensors/sensors_posix.cpp b/src/modules/sensors/sensors.cpp similarity index 97% rename from src/modules/sensors/sensors_posix.cpp rename to src/modules/sensors/sensors.cpp index 755d4f1678..adf231840c 100644 --- a/src/modules/sensors/sensors_posix.cpp +++ b/src/modules/sensors/sensors.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -62,6 +63,7 @@ #include #include +//#include #include #include @@ -113,7 +115,7 @@ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 #endif @@ -126,8 +128,8 @@ #ifdef CONFIG_ARCH_BOARD_AEROCORE #define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL -1 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) +#define ADC_AIRSPEED_VOLTAGE_CHANNEL ((uint8_t)(-1)) #endif #define BATT_V_LOWPASS 0.001f @@ -147,7 +149,7 @@ #endif static const int ERROR = -1; -#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL" +#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" /** * Sensor app start / stop handling function @@ -225,7 +227,7 @@ private: int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _baro1_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ + //int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ @@ -671,11 +673,7 @@ Sensors::parameters_update() tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; /* handle blowup in the scaling factor calculation */ -#ifdef __PX4_NUTTX - if (!isfinite(tmpScaleFactor) || -#else - if (!std::isfinite(tmpScaleFactor) || -#endif + if (!PX4_ISFINITE(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || (tmpRevFactor > 0.2f)) { warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); @@ -855,7 +853,7 @@ Sensors::parameters_update() barofd = px4_open(BARO0_DEVICE_PATH, 0); if (barofd < 0) { - warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); + warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH); return ERROR; } else { @@ -1021,6 +1019,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_timestamp = accel_report.timestamp; raw.accelerometer_errcount = accel_report.error_count; + raw.accelerometer_temp = accel_report.temperature; } orb_check(_accel1_sub, &accel_updated); @@ -1043,6 +1042,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer1_timestamp = accel_report.timestamp; raw.accelerometer1_errcount = accel_report.error_count; + raw.accelerometer1_temp = accel_report.temperature; } orb_check(_accel2_sub, &accel_updated); @@ -1065,6 +1065,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer2_timestamp = accel_report.timestamp; raw.accelerometer2_errcount = accel_report.error_count; + raw.accelerometer2_temp = accel_report.temperature; } } @@ -1092,6 +1093,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.timestamp = gyro_report.timestamp; raw.gyro_errcount = gyro_report.error_count; + raw.gyro_temp = gyro_report.temperature; } orb_check(_gyro1_sub, &gyro_updated); @@ -1114,6 +1116,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro1_timestamp = gyro_report.timestamp; raw.gyro1_errcount = gyro_report.error_count; + raw.gyro1_temp = gyro_report.temperature; } orb_check(_gyro2_sub, &gyro_updated); @@ -1136,6 +1139,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro2_timestamp = gyro_report.timestamp; raw.gyro2_errcount = gyro_report.error_count; + raw.gyro2_temp = gyro_report.temperature; } } @@ -1164,6 +1168,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_timestamp = mag_report.timestamp; raw.magnetometer_errcount = mag_report.error_count; + raw.magnetometer_temp = mag_report.temperature; } orb_check(_mag1_sub, &mag_updated); @@ -1187,6 +1192,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer1_timestamp = mag_report.timestamp; raw.magnetometer1_errcount = mag_report.error_count; + raw.magnetometer1_temp = mag_report.temperature; } orb_check(_mag2_sub, &mag_updated); @@ -1210,6 +1216,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer2_timestamp = mag_report.timestamp; raw.magnetometer2_errcount = mag_report.error_count; + raw.magnetometer2_temp = mag_report.temperature; } } @@ -1379,14 +1386,13 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx("%s: gyro #%u", CAL_FAILED_APPLY_CAL_MSG, gyro_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); } else { - gyro_count++; config_ok = true; } } @@ -1394,11 +1400,11 @@ Sensors::parameter_update_poll(bool forced) } } - px4_close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR GYRO #%u", s); + if (config_ok) { + gyro_count++; } + + px4_close(fd); } /* run through all accel sensors */ @@ -1446,14 +1452,13 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); if (failed) { - warnx("%s: acc #%u", CAL_FAILED_APPLY_CAL_MSG, accel_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); } else { - accel_count++; config_ok = true; } } @@ -1461,11 +1466,11 @@ Sensors::parameter_update_poll(bool forced) } } - px4_close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR ACCEL #%u", s); + if (config_ok) { + accel_count++; } + + px4_close(fd); } /* run through all mag sensors */ @@ -1477,9 +1482,16 @@ Sensors::parameter_update_poll(bool forced) int fd = px4_open(str, 0); if (fd < 0) { + /* the driver is not running, abort */ continue; } + /* set a valid default rotation (same as board). + * if the mag is configured, this might be replaced + * in the section below. + */ + _mag_rotation[s] = _board_rotation; + bool config_ok = false; /* run through all stored calibrations */ @@ -1563,14 +1575,13 @@ Sensors::parameter_update_poll(bool forced) } if (failed) { - warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count); + warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG); + warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); } else { - mag_count++; config_ok = true; } } @@ -1578,11 +1589,11 @@ Sensors::parameter_update_poll(bool forced) } } - px4_close(fd); - - if (!config_ok) { - warnx("NO CONFIG FOR MAG #%u", s); + if (config_ok) { + mag_count++; } + + px4_close(fd); } int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); @@ -1602,7 +1613,7 @@ Sensors::parameter_update_poll(bool forced) px4_close(fd); } - warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); + warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); } } @@ -1947,11 +1958,7 @@ Sensors::rc_poll() _rc.channels[i] *= _parameters.rev[i]; /* handle any parameter-induced blowups */ -#ifdef __PX4_NUTTX - if (!isfinite(_rc.channels[i])) { -#else - if (!std::isfinite(_rc.channels[i])) { -#endif + if (!PX4_ISFINITE(_rc.channels[i])) { _rc.channels[i] = 0.0f; } } @@ -2152,12 +2159,8 @@ Sensors::task_main() /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - if (_sensor_pub < 0) { - warnx("Sensors::task_main ERROR - uORB not started"); - } - /* wakeup source(s) */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ fds[0].fd = _gyro_sub; @@ -2168,7 +2171,7 @@ Sensors::task_main() while (!_task_should_exit) { /* wait for up to 50ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ diff --git a/src/modules/sensors/sensors_nuttx.cpp b/src/modules/sensors/sensors_nuttx.cpp deleted file mode 100644 index 3fc8627c15..0000000000 --- a/src/modules/sensors/sensors_nuttx.cpp +++ /dev/null @@ -1,2305 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file sensors.cpp - * - * PX4 Flight Core transitional mapping layer. - * - * This app / class mapps the PX4 middleware layer / drivers to the application - * layer of the PX4 Flight Core. Individual sensors can be accessed directly as - * well instead of relying on the sensor_combined topic. - * - * @author Lorenz Meier - * @author Julian Oes - * @author Thomas Gubler - * @author Anton Babushkin - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * Analog layout: - * FMU: - * IN2 - battery voltage - * IN3 - battery current - * IN4 - 5V sense - * IN10 - spare (we could actually trim these from the set) - * IN11 - spare (we could actually trim these from the set) - * IN12 - spare (we could actually trim these from the set) - * IN13 - aux1 - * IN14 - aux2 - * IN15 - pressure sensor - * - * IO: - * IN4 - servo supply rail - * IN5 - analog RSSI - */ - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL -1 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#define ADC_BATTERY_VOLTAGE_CHANNEL 2 -#define ADC_BATTERY_CURRENT_CHANNEL 3 -#define ADC_5V_RAIL_SENSE 4 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -#endif - -#ifdef CONFIG_ARCH_BOARD_AEROCORE -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL -1 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 -#endif - -#define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 4.8f - -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f -#define STICK_ON_OFF_LIMIT 0.75f -#define MAG_ROT_VAL_INTERNAL -1 - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" - -/** - * Sensor app start / stop handling function - * - * @ingroup apps - */ -extern "C" __EXPORT int sensors_main(int argc, char *argv[]); - -class Sensors -{ -public: - /** - * Constructor - */ - Sensors(); - - /** - * Destructor, also kills the sensors task. - */ - ~Sensors(); - - /** - * Start the sensors task. - * - * @return OK on success. - */ - int start(); - -private: - static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ - - /** - * Get and limit value for specified RC function. Returns NAN if not mapped. - */ - float get_rc_value(uint8_t func, float min_value, float max_value); - - /** - * Get switch position for specified function. - */ - switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); - switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); - - /** - * Update paramters from RC channels if the functionality is activated and the - * input has changed since the last update - * - * @param - */ - void set_params_from_rc(); - - /** - * Gather and publish RC input data. - */ - void rc_poll(); - - /* XXX should not be here - should be own driver */ - int _fd_adc; /**< ADC driver handle */ - hrt_abstime _last_adc; /**< last time we took input from the ADC */ - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _sensors_task; /**< task handle for sensor task */ - - bool _hil_enabled; /**< if true, HIL is active */ - bool _publishing; /**< if true, we are publishing sensor data */ - - int _gyro_sub; /**< raw gyro0 data subscription */ - int _accel_sub; /**< raw accel0 data subscription */ - int _mag_sub; /**< raw mag0 data subscription */ - int _gyro1_sub; /**< raw gyro1 data subscription */ - int _accel1_sub; /**< raw accel1 data subscription */ - int _mag1_sub; /**< raw mag1 data subscription */ - int _gyro2_sub; /**< raw gyro2 data subscription */ - int _accel2_sub; /**< raw accel2 data subscription */ - int _mag2_sub; /**< raw mag2 data subscription */ - int _rc_sub; /**< raw rc channels data subscription */ - int _baro_sub; /**< raw baro data subscription */ - int _baro1_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vcontrol_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _rc_parameter_map_sub; /**< rc parameter map subscription */ - int _manual_control_sub; /**< notification of manual control updates */ - - orb_advert_t _sensor_pub; /**< combined sensor data topic */ - orb_advert_t _manual_control_pub; /**< manual control signal topic */ - orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ - orb_advert_t _rc_pub; /**< raw r/c control topic */ - orb_advert_t _battery_pub; /**< battery status */ - orb_advert_t _airspeed_pub; /**< airspeed */ - orb_advert_t _diff_pres_pub; /**< differential_pressure */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - struct rc_channels_s _rc; /**< r/c channel data */ - struct battery_status_s _battery_status; /**< battery status */ - struct baro_report _barometer; /**< barometer data */ - struct differential_pressure_s _diff_pres; - struct airspeed_s _airspeed; - struct rc_parameter_map_s _rc_parameter_map; - - math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ - - uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ - hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ - - struct { - float min[_rc_max_chan_count]; - float trim[_rc_max_chan_count]; - float max[_rc_max_chan_count]; - float rev[_rc_max_chan_count]; - float dz[_rc_max_chan_count]; - float scaling_factor[_rc_max_chan_count]; - - float diff_pres_offset_pa; - float diff_pres_analog_scale; - - int board_rotation; - int flow_rotation; - - float board_offset[3]; - - int rc_map_roll; - int rc_map_pitch; - int rc_map_yaw; - int rc_map_throttle; - int rc_map_failsafe; - - int rc_map_mode_sw; - int rc_map_return_sw; - int rc_map_posctl_sw; - int rc_map_loiter_sw; - int rc_map_acro_sw; - int rc_map_offboard_sw; - - int rc_map_flaps; - - int rc_map_aux1; - int rc_map_aux2; - int rc_map_aux3; - int rc_map_aux4; - int rc_map_aux5; - - int rc_map_param[RC_PARAM_MAP_NCHAN]; - - int32_t rc_fails_thr; - float rc_assist_th; - float rc_auto_th; - float rc_posctl_th; - float rc_return_th; - float rc_loiter_th; - float rc_acro_th; - float rc_offboard_th; - bool rc_assist_inv; - bool rc_auto_inv; - bool rc_posctl_inv; - bool rc_return_inv; - bool rc_loiter_inv; - bool rc_acro_inv; - bool rc_offboard_inv; - - float battery_voltage_scaling; - float battery_current_scaling; - - float baro_qnh; - - } _parameters; /**< local copies of interesting parameters */ - - struct { - param_t min[_rc_max_chan_count]; - param_t trim[_rc_max_chan_count]; - param_t max[_rc_max_chan_count]; - param_t rev[_rc_max_chan_count]; - param_t dz[_rc_max_chan_count]; - - param_t diff_pres_offset_pa; - param_t diff_pres_analog_scale; - - param_t rc_map_roll; - param_t rc_map_pitch; - param_t rc_map_yaw; - param_t rc_map_throttle; - param_t rc_map_failsafe; - - param_t rc_map_mode_sw; - param_t rc_map_return_sw; - param_t rc_map_posctl_sw; - param_t rc_map_loiter_sw; - param_t rc_map_acro_sw; - param_t rc_map_offboard_sw; - - param_t rc_map_flaps; - - param_t rc_map_aux1; - param_t rc_map_aux2; - param_t rc_map_aux3; - param_t rc_map_aux4; - param_t rc_map_aux5; - - param_t rc_map_param[RC_PARAM_MAP_NCHAN]; - param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound - to a RC channel, equivalent float values in the - _parameters struct are not existing - because these parameters are never read. */ - - param_t rc_fails_thr; - param_t rc_assist_th; - param_t rc_auto_th; - param_t rc_posctl_th; - param_t rc_return_th; - param_t rc_loiter_th; - param_t rc_acro_th; - param_t rc_offboard_th; - - param_t battery_voltage_scaling; - param_t battery_current_scaling; - - param_t board_rotation; - param_t flow_rotation; - - param_t board_offset[3]; - - param_t baro_qnh; - - } _parameter_handles; /**< handles for interesting parameters */ - - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - /** - * Do accel-related initialisation. - */ - int accel_init(); - - /** - * Do gyro-related initialisation. - */ - int gyro_init(); - - /** - * Do mag-related initialisation. - */ - int mag_init(); - - /** - * Do baro-related initialisation. - */ - int baro_init(); - - /** - * Do adc-related initialisation. - */ - int adc_init(); - - /** - * Poll the accelerometer for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void accel_poll(struct sensor_combined_s &raw); - - /** - * Poll the gyro for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void gyro_poll(struct sensor_combined_s &raw); - - /** - * Poll the magnetometer for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void mag_poll(struct sensor_combined_s &raw); - - /** - * Poll the barometer for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void baro_poll(struct sensor_combined_s &raw); - - /** - * Poll the differential pressure sensor for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void diff_pres_poll(struct sensor_combined_s &raw); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in parameters. - */ - void parameter_update_poll(bool forced = false); - - /** - * Check for changes in rc_parameter_map - */ - void rc_parameter_map_poll(bool forced = false); - - /** - * Poll the ADC and update readings to suit. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void adc_poll(struct sensor_combined_s &raw); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main sensor collection task. - */ - void task_main(); -}; - -namespace sensors -{ - -Sensors *g_sensors = nullptr; -} - -Sensors::Sensors() : - _fd_adc(-1), - _last_adc(0), - - _task_should_exit(true), - _sensors_task(-1), - _hil_enabled(false), - _publishing(true), - - /* subscriptions */ - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), - _gyro1_sub(-1), - _accel1_sub(-1), - _mag1_sub(-1), - _gyro2_sub(-1), - _accel2_sub(-1), - _mag2_sub(-1), - _rc_sub(-1), - _baro_sub(-1), - _baro1_sub(-1), - _vcontrol_mode_sub(-1), - _params_sub(-1), - _rc_parameter_map_sub(-1), - _manual_control_sub(-1), - - /* publications */ - _sensor_pub(-1), - _manual_control_pub(-1), - _actuator_group_3_pub(-1), - _rc_pub(-1), - _battery_pub(-1), - _airspeed_pub(-1), - _diff_pres_pub(-1), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - - _board_rotation{}, - _mag_rotation{}, - - _battery_discharged(0), - _battery_current_timestamp(0) -{ - memset(&_rc, 0, sizeof(_rc)); - memset(&_diff_pres, 0, sizeof(_diff_pres)); - memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); - - /* basic r/c parameters */ - for (unsigned i = 0; i < _rc_max_chan_count; i++) { - char nbuf[16]; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles.min[i] = param_find(nbuf); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles.trim[i] = param_find(nbuf); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles.max[i] = param_find(nbuf); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles.rev[i] = param_find(nbuf); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles.dz[i] = param_find(nbuf); - - } - - /* mandatory input switched, mapped to channels 1-4 per default */ - _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); - _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); - _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); - _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); - _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); - - /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); - _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); - - _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); - - /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); - _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); - _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); - _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); - - _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); - _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); - _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); - _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); - _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - - /* RC to parameter mapping for changing parameters with RC */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - char name[PARAM_ID_LEN]; - snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 - _parameter_handles.rc_map_param[i] = param_find(name); - } - - /* RC thresholds */ - _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); - _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); - _parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); - _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); - _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); - _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); - _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); - _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); - - /* Differential pressure offset */ - _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); - _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); - - _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); - _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); - - /* rotations */ - _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); - _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); - - /* rotation offsets */ - _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); - _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); - _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); - - /* Barometer QNH */ - _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); - - /* fetch initial parameter values */ - parameters_update(); -} - -Sensors::~Sensors() -{ - if (_sensors_task != -1) { - - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_sensors_task); - break; - } - } while (_sensors_task != -1); - } - - sensors::g_sensors = nullptr; -} - -int -Sensors::parameters_update() -{ - bool rc_valid = true; - float tmpScaleFactor = 0.0f; - float tmpRevFactor = 0.0f; - - /* rc values */ - for (unsigned int i = 0; i < _rc_max_chan_count; i++) { - - param_get(_parameter_handles.min[i], &(_parameters.min[i])); - param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); - param_get(_parameter_handles.max[i], &(_parameters.max[i])); - param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); - param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); - - tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); - tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; - - /* handle blowup in the scaling factor calculation */ - if (!isfinite(tmpScaleFactor) || - (tmpRevFactor < 0.000001f) || - (tmpRevFactor > 0.2f)) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); - /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0.0f; - rc_valid = false; - - } else { - _parameters.scaling_factor[i] = tmpScaleFactor; - } - } - - /* handle wrong values */ - if (!rc_valid) { - warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - } - - const char *paramerr = "FAIL PARM LOAD"; - - /* channel mapping */ - if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { - warnx("%s", paramerr); - } - - if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("%s", paramerr); - } - - param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); - param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); - param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); - param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); - param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); - } - - param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); - param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); - _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); - _parameters.rc_assist_th = fabs(_parameters.rc_assist_th); - param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); - _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); - _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); - param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); - _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); - _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); - param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th)); - _parameters.rc_return_inv = (_parameters.rc_return_th < 0); - _parameters.rc_return_th = fabs(_parameters.rc_return_th); - param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); - _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); - _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); - param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th)); - _parameters.rc_acro_inv = (_parameters.rc_acro_th < 0); - _parameters.rc_acro_th = fabs(_parameters.rc_acro_th); - param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th)); - _parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0); - _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); - - /* update RC function mappings */ - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; - - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; - - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; - - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; - } - - /* Airspeed offset */ - param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); - param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); - - /* scaling of ADC ticks to battery voltage */ - if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx("%s", paramerr); - } - - /* scaling of ADC ticks to battery current */ - if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx("%s", paramerr); - } - - param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); - param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); - - /* set px4flow rotation */ - int flowfd; - flowfd = open(PX4FLOW0_DEVICE_PATH, 0); - - if (flowfd >= 0) { - int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); - - if (flowret) { - warnx("flow rotation could not be set"); - close(flowfd); - return ERROR; - } - - close(flowfd); - } - - get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); - - param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); - param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); - param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - - /** fine tune board offset on parameter update **/ - math::Matrix<3, 3> board_rotation_offset; - board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], - M_DEG_TO_RAD_F * _parameters.board_offset[1], - M_DEG_TO_RAD_F * _parameters.board_offset[2]); - - _board_rotation = _board_rotation * board_rotation_offset; - - /* update barometer qnh setting */ - param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int barofd; - barofd = open(BARO0_DEVICE_PATH, 0); - - if (barofd < 0) { - warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH); - return ERROR; - - } else { - int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); - - if (baroret) { - warnx("qnh could not be set"); - close(barofd); - return ERROR; - } - - close(barofd); - } - - return OK; -} - -int -Sensors::accel_init() -{ - int fd; - - fd = open(ACCEL0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); - return ERROR; - - } else { - - /* set the accel internal sampling rate to default rate */ - ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); - - /* set the driver to poll at default rate */ - ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - - close(fd); - } - - return OK; -} - -int -Sensors::gyro_init() -{ - int fd; - - fd = open(GYRO0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); - return ERROR; - - } else { - - /* set the gyro internal sampling rate to default rate */ - ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); - - /* set the driver to poll at default rate */ - ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - - } - - return OK; -} - -int -Sensors::mag_init() -{ - int fd; - int ret; - - fd = open(MAG0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); - return ERROR; - } - - /* try different mag sampling rates */ - - - ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); - - if (ret == OK) { - /* set the pollrate accordingly */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); - - } else { - ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); - - /* if the slower sampling rate still fails, something is wrong */ - if (ret == OK) { - /* set the driver to poll also at the slower rate */ - ioctl(fd, SENSORIOCSPOLLRATE, 100); - - } else { - warnx("FATAL: mag sampling rate could not be set"); - return ERROR; - } - } - - close(fd); - - return OK; -} - -int -Sensors::baro_init() -{ - int fd; - - fd = open(BARO0_DEVICE_PATH, 0); - - if (fd < 0) { - warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); - return ERROR; - } - - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); - - close(fd); - - return OK; -} - -int -Sensors::adc_init() -{ - - _fd_adc = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); - - if (_fd_adc < 0) { - warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); - return ERROR; - } - - return OK; -} - -void -Sensors::accel_poll(struct sensor_combined_s &raw) -{ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer_m_s2[0] = vect(0); - raw.accelerometer_m_s2[1] = vect(1); - raw.accelerometer_m_s2[2] = vect(2); - - raw.accelerometer_raw[0] = accel_report.x_raw; - raw.accelerometer_raw[1] = accel_report.y_raw; - raw.accelerometer_raw[2] = accel_report.z_raw; - - raw.accelerometer_timestamp = accel_report.timestamp; - raw.accelerometer_errcount = accel_report.error_count; - raw.accelerometer_temp = accel_report.temperature; - } - - orb_check(_accel1_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer1_m_s2[0] = vect(0); - raw.accelerometer1_m_s2[1] = vect(1); - raw.accelerometer1_m_s2[2] = vect(2); - - raw.accelerometer1_raw[0] = accel_report.x_raw; - raw.accelerometer1_raw[1] = accel_report.y_raw; - raw.accelerometer1_raw[2] = accel_report.z_raw; - - raw.accelerometer1_timestamp = accel_report.timestamp; - raw.accelerometer1_errcount = accel_report.error_count; - raw.accelerometer1_temp = accel_report.temperature; - } - - orb_check(_accel2_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer2_m_s2[0] = vect(0); - raw.accelerometer2_m_s2[1] = vect(1); - raw.accelerometer2_m_s2[2] = vect(2); - - raw.accelerometer2_raw[0] = accel_report.x_raw; - raw.accelerometer2_raw[1] = accel_report.y_raw; - raw.accelerometer2_raw[2] = accel_report.z_raw; - - raw.accelerometer2_timestamp = accel_report.timestamp; - raw.accelerometer2_errcount = accel_report.error_count; - raw.accelerometer2_temp = accel_report.temperature; - } -} - -void -Sensors::gyro_poll(struct sensor_combined_s &raw) -{ - bool gyro_updated; - orb_check(_gyro_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro_rad_s[0] = vect(0); - raw.gyro_rad_s[1] = vect(1); - raw.gyro_rad_s[2] = vect(2); - - raw.gyro_raw[0] = gyro_report.x_raw; - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; - - raw.timestamp = gyro_report.timestamp; - raw.gyro_errcount = gyro_report.error_count; - raw.gyro_temp = gyro_report.temperature; - } - - orb_check(_gyro1_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro1_rad_s[0] = vect(0); - raw.gyro1_rad_s[1] = vect(1); - raw.gyro1_rad_s[2] = vect(2); - - raw.gyro1_raw[0] = gyro_report.x_raw; - raw.gyro1_raw[1] = gyro_report.y_raw; - raw.gyro1_raw[2] = gyro_report.z_raw; - - raw.gyro1_timestamp = gyro_report.timestamp; - raw.gyro1_errcount = gyro_report.error_count; - raw.gyro1_temp = gyro_report.temperature; - } - - orb_check(_gyro2_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro2_rad_s[0] = vect(0); - raw.gyro2_rad_s[1] = vect(1); - raw.gyro2_rad_s[2] = vect(2); - - raw.gyro2_raw[0] = gyro_report.x_raw; - raw.gyro2_raw[1] = gyro_report.y_raw; - raw.gyro2_raw[2] = gyro_report.z_raw; - - raw.gyro2_timestamp = gyro_report.timestamp; - raw.gyro2_errcount = gyro_report.error_count; - raw.gyro2_temp = gyro_report.temperature; - } -} - -void -Sensors::mag_poll(struct sensor_combined_s &raw) -{ - bool mag_updated; - orb_check(_mag_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[0] * vect; - - raw.magnetometer_ga[0] = vect(0); - raw.magnetometer_ga[1] = vect(1); - raw.magnetometer_ga[2] = vect(2); - - raw.magnetometer_raw[0] = mag_report.x_raw; - raw.magnetometer_raw[1] = mag_report.y_raw; - raw.magnetometer_raw[2] = mag_report.z_raw; - - raw.magnetometer_timestamp = mag_report.timestamp; - raw.magnetometer_errcount = mag_report.error_count; - raw.magnetometer_temp = mag_report.temperature; - } - - orb_check(_mag1_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[1] * vect; - - raw.magnetometer1_ga[0] = vect(0); - raw.magnetometer1_ga[1] = vect(1); - raw.magnetometer1_ga[2] = vect(2); - - raw.magnetometer1_raw[0] = mag_report.x_raw; - raw.magnetometer1_raw[1] = mag_report.y_raw; - raw.magnetometer1_raw[2] = mag_report.z_raw; - - raw.magnetometer1_timestamp = mag_report.timestamp; - raw.magnetometer1_errcount = mag_report.error_count; - raw.magnetometer1_temp = mag_report.temperature; - } - - orb_check(_mag2_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[2] * vect; - - raw.magnetometer2_ga[0] = vect(0); - raw.magnetometer2_ga[1] = vect(1); - raw.magnetometer2_ga[2] = vect(2); - - raw.magnetometer2_raw[0] = mag_report.x_raw; - raw.magnetometer2_raw[1] = mag_report.y_raw; - raw.magnetometer2_raw[2] = mag_report.z_raw; - - raw.magnetometer2_timestamp = mag_report.timestamp; - raw.magnetometer2_errcount = mag_report.error_count; - raw.magnetometer2_temp = mag_report.temperature; - } -} - -void -Sensors::baro_poll(struct sensor_combined_s &raw) -{ - bool baro_updated; - orb_check(_baro_sub, &baro_updated); - - if (baro_updated) { - - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); - - raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar - raw.baro_alt_meter = _barometer.altitude; // Altitude in meters - raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius - - raw.baro_timestamp = _barometer.timestamp; - } - - orb_check(_baro1_sub, &baro_updated); - - if (baro_updated) { - - struct baro_report baro_report; - - orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); - - raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - - raw.baro1_timestamp = baro_report.timestamp; - } -} - -void -Sensors::diff_pres_poll(struct sensor_combined_s &raw) -{ - bool updated; - orb_check(_diff_pres_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - - raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; - raw.differential_pressure_timestamp = _diff_pres.timestamp; - raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - - float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : - (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); - - _airspeed.timestamp = _diff_pres.timestamp; - - /* don't risk to feed negative airspeed into the system */ - _airspeed.indicated_airspeed_m_s = math::max(0.0f, - calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); - _airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); - _airspeed.air_temperature_celsius = air_temperature_celsius; - - /* announce the airspeed if needed, just publish else */ - if (_airspeed_pub > 0) { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); - - } else { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); - } - } -} - -void -Sensors::vehicle_control_mode_poll() -{ - struct vehicle_control_mode_s vcontrol_mode; - bool vcontrol_mode_updated; - - /* Check HIL state if vehicle control mode has changed */ - orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - - if (vcontrol_mode_updated) { - - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); - - /* switching from non-HIL to HIL mode */ - //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { - _hil_enabled = true; - _publishing = false; - - /* switching from HIL to non-HIL mode */ - - } else if (!_publishing && !_hil_enabled) { - _hil_enabled = false; - _publishing = true; - } - } -} - -void -Sensors::parameter_update_poll(bool forced) -{ - bool param_updated; - - /* Check if any parameter has changed */ - orb_check(_params_sub, ¶m_updated); - - if (param_updated || forced) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - /* update parameters */ - parameters_update(); - - /* set offset parameters to new values */ - bool failed; - int res; - char str[30]; - unsigned mag_count = 0; - unsigned gyro_count = 0; - unsigned accel_count = 0; - - /* run through all gyro sensors */ - for (unsigned s = 0; s < 3; s++) { - - res = ERROR; - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - - int fd = open(str, 0); - - if (fd < 0) { - continue; - } - - bool config_ok = false; - - /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { - /* initially status is ok per config */ - failed = false; - - (void)sprintf(str, "CAL_GYRO%u_ID", i); - int device_id; - failed = failed || (OK != param_get(param_find(str), &device_id)); - - if (failed) { - close(fd); - continue; - } - - /* if the calibration is for this device, apply it */ - if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { - struct gyro_scale gscale = {}; - (void)sprintf(str, "CAL_GYRO%u_XOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); - (void)sprintf(str, "CAL_GYRO%u_YOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); - (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); - (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); - (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); - (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); - - if (failed) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); - } else { - /* apply new scaling and offsets */ - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); - if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i); - } else { - config_ok = true; - } - } - break; - } - } - - if (config_ok) { - gyro_count++; - } - - close(fd); - } - - /* run through all accel sensors */ - for (unsigned s = 0; s < 3; s++) { - - res = ERROR; - (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - - int fd = open(str, 0); - - if (fd < 0) { - continue; - } - - bool config_ok = false; - - /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { - /* initially status is ok per config */ - failed = false; - - (void)sprintf(str, "CAL_ACC%u_ID", i); - int device_id; - failed = failed || (OK != param_get(param_find(str), &device_id)); - - if (failed) { - close(fd); - continue; - } - - /* if the calibration is for this device, apply it */ - if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { - struct accel_scale gscale = {}; - (void)sprintf(str, "CAL_ACC%u_XOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); - (void)sprintf(str, "CAL_ACC%u_YOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); - (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); - (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); - (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); - (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); - - if (failed) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); - } else { - /* apply new scaling and offsets */ - res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); - if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i); - } else { - config_ok = true; - } - } - break; - } - } - - if (config_ok) { - accel_count++; - } - - close(fd); - } - - /* run through all mag sensors */ - for (unsigned s = 0; s < 3; s++) { - - res = ERROR; - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - - int fd = open(str, 0); - - if (fd < 0) { - /* the driver is not running, abort */ - continue; - } - - /* set a valid default rotation (same as board). - * if the mag is configured, this might be replaced - * in the section below. - */ - _mag_rotation[s] = _board_rotation; - - bool config_ok = false; - - /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { - /* initially status is ok per config */ - failed = false; - - (void)sprintf(str, "CAL_MAG%u_ID", i); - int device_id; - failed = failed || (OK != param_get(param_find(str), &device_id)); - - if (failed) { - close(fd); - continue; - } - - /* if the calibration is for this device, apply it */ - if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { - struct mag_scale gscale = {}; - (void)sprintf(str, "CAL_MAG%u_XOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); - (void)sprintf(str, "CAL_MAG%u_YOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); - (void)sprintf(str, "CAL_MAG%u_ZOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); - (void)sprintf(str, "CAL_MAG%u_XSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); - (void)sprintf(str, "CAL_MAG%u_YSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); - (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); - - (void)sprintf(str, "CAL_MAG%u_ROT", i); - - if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { - /* mag is internal */ - _mag_rotation[s] = _board_rotation; - /* reset param to -1 to indicate internal mag */ - int32_t minus_one = MAG_ROT_VAL_INTERNAL; - param_set_no_notification(param_find(str), &minus_one); - } else { - - int32_t mag_rot; - param_get(param_find(str), &mag_rot); - - /* check if this mag is still set as internal */ - if (mag_rot < 0) { - /* it was marked as internal, change to external with no rotation */ - mag_rot = 0; - param_set_no_notification(param_find(str), &mag_rot); - } - - /* handling of old setups, will be removed later (noted Feb 2015) */ - int32_t deprecated_mag_rot = 0; - param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); - - /* - * If the deprecated parameter is non-default (is != 0), - * and the new parameter is default (is == 0), then this board - * was configured already and we need to copy the old value - * to the new parameter. - * The < 0 case is special: It means that this param slot was - * used previously by an internal sensor, but the the call above - * proved that it is currently occupied by an external sensor. - * In that case we consider the orientation to be default as well. - */ - if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) { - mag_rot = deprecated_mag_rot; - param_set_no_notification(param_find(str), &mag_rot); - /* clear the old param, not supported in GUI anyway */ - deprecated_mag_rot = 0; - param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); - } - - /* handling of transition from internal to external */ - if (mag_rot < 0) { - mag_rot = 0; - } - - get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[s]); - } - - if (failed) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); - } else { - /* apply new scaling and offsets */ - res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); - if (res) { - warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i); - } else { - config_ok = true; - } - } - break; - } - } - - if (config_ok) { - mag_count++; - } - - close(fd); - } - - int fd = open(AIRSPEED0_DEVICE_PATH, 0); - - /* this sensor is optional, abort without error */ - - if (fd >= 0) { - struct airspeed_scale airscale = { - _parameters.diff_pres_offset_pa, - 1.0f, - }; - - if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - } - - close(fd); - } - - warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); - } -} - -void -Sensors::rc_parameter_map_poll(bool forced) -{ - bool map_updated; - orb_check(_rc_parameter_map_sub, &map_updated); - - if (map_updated) { - orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); - - /* update paramter handles to which the RC channels are mapped */ - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { - /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) - * or no request to map this channel to a param has been sent via mavlink - */ - continue; - } - - /* Set the handle by index if the index is set, otherwise use the id */ - if (_rc_parameter_map.param_index[i] >= 0) { - _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); - - } else { - _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); - } - - } - - warnx("rc to parameter map updated"); - - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", - i, - _rc_parameter_map.param_id[i], - (double)_rc_parameter_map.scale[i], - (double)_rc_parameter_map.value0[i], - (double)_rc_parameter_map.value_min[i], - (double)_rc_parameter_map.value_max[i] - ); - } - } -} - -void -Sensors::adc_poll(struct sensor_combined_s &raw) -{ - /* only read if publishing */ - if (!_publishing) { - return; - } - - hrt_abstime t = hrt_absolute_time(); - - /* rate limit to 100 Hz */ - if (t - _last_adc >= 10000) { - /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ - struct adc_msg_s buf_adc[12]; - /* read all channels available */ - int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); - - if (ret >= (int)sizeof(buf_adc[0])) { - - /* Read add channels we got */ - for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { - /* Save raw voltage values */ - if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { - raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); - raw.adc_mapping[i] = buf_adc[i].am_channel; - } - - /* look for specific channels and process the raw voltage to measurement data */ - if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { - /* Voltage in volts */ - float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); - - if (voltage > BATT_V_IGNORE_THRESHOLD) { - _battery_status.voltage_v = voltage; - - /* one-time initialization of low-pass value to avoid long init delays */ - if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { - _battery_status.voltage_filtered_v = voltage; - } - - _battery_status.timestamp = t; - _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS; - - } else { - /* mark status as invalid */ - _battery_status.voltage_v = -1.0f; - _battery_status.voltage_filtered_v = -1.0f; - } - - } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { - /* handle current only if voltage is valid */ - if (_battery_status.voltage_v > 0.0f) { - float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); - - /* check measured current value */ - if (current >= 0.0f) { - _battery_status.timestamp = t; - _battery_status.current_a = current; - - if (_battery_current_timestamp != 0) { - /* initialize discharged value */ - if (_battery_status.discharged_mah < 0.0f) { - _battery_status.discharged_mah = 0.0f; - } - - _battery_discharged += current * (t - _battery_current_timestamp); - _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; - } - } - } - - _battery_current_timestamp = t; - - } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { - - /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) - - /** - * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor. Also assume a non- - * zero offset from the sensor if its connected. - */ - if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { - - float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - - _diff_pres.timestamp = t; - _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + - (diff_pres_pa_raw * 0.1f); - _diff_pres.temperature = -1000.0f; - - /* announce the airspeed if needed, just publish else */ - if (_diff_pres_pub > 0) { - orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres); - - } else { - _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); - } - } - } - } - - _last_adc = t; - - if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { - /* announce the battery status if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); - - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); - } - } - } - } -} - -float -Sensors::get_rc_value(uint8_t func, float min_value, float max_value) -{ - if (_rc.function[func] >= 0) { - float value = _rc.channels[_rc.function[func]]; - - if (value < min_value) { - return min_value; - - } else if (value > max_value) { - return max_value; - - } else { - return value; - } - - } else { - return 0.0f; - } -} - -switch_pos_t -Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) -{ - if (_rc.function[func] >= 0) { - float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; - - if (on_inv ? value < on_th : value > on_th) { - return manual_control_setpoint_s::SWITCH_POS_ON; - - } else if (mid_inv ? value < mid_th : value > mid_th) { - return manual_control_setpoint_s::SWITCH_POS_MIDDLE; - - } else { - return manual_control_setpoint_s::SWITCH_POS_OFF; - } - - } else { - return manual_control_setpoint_s::SWITCH_POS_NONE; - } -} - -switch_pos_t -Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) -{ - if (_rc.function[func] >= 0) { - float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; - - if (on_inv ? value < on_th : value > on_th) { - return manual_control_setpoint_s::SWITCH_POS_ON; - - } else { - return manual_control_setpoint_s::SWITCH_POS_OFF; - } - - } else { - return manual_control_setpoint_s::SWITCH_POS_NONE; - } -} - -void -Sensors::set_params_from_rc() -{ - static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; - - for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { - /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) - * or no request to map this channel to a param has been sent via mavlink - */ - continue; - } - - float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); - /* Check if the value has changed, - * maybe we need to introduce a more aggressive limit here */ - if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { - param_rc_values[i] = rc_val; - float param_val = math::constrain( - _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, - _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); - param_set(_parameter_handles.rc_param[i], ¶m_val); - } - } -} - -void -Sensors::rc_poll() -{ - bool rc_updated; - orb_check(_rc_sub, &rc_updated); - - if (rc_updated) { - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - struct rc_input_values rc_input; - - orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - - /* detect RC signal loss */ - bool signal_lost; - - /* check flags and require at least four channels to consider the signal valid */ - if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { - /* signal is lost or no enough channels */ - signal_lost = true; - - } else { - /* signal looks good */ - signal_lost = false; - - /* check failsafe */ - int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle - - if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping - fs_ch = _parameters.rc_map_failsafe - 1; - } - - if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { - /* failsafe configured */ - if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || - (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { - /* failsafe triggered, signal is lost by receiver */ - signal_lost = true; - } - } - } - - unsigned channel_limit = rc_input.channel_count; - - if (channel_limit > _rc_max_chan_count) { - channel_limit = _rc_max_chan_count; - } - - /* read out and scale values from raw message even if signal is invalid */ - for (unsigned int i = 0; i < channel_limit; i++) { - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (rc_input.values[i] < _parameters.min[i]) { - rc_input.values[i] = _parameters.min[i]; - } - - if (rc_input.values[i] > _parameters.max[i]) { - rc_input.values[i] = _parameters.max[i]; - } - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * the total range is 2 (-1..1). - * If center (trim) == min, scale to 0..1, if center (trim) == max, - * scale to -1..0. - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)( - _parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); - - } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - _rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)( - _parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); - - } else { - /* in the configured dead zone, output zero */ - _rc.channels[i] = 0.0f; - } - - _rc.channels[i] *= _parameters.rev[i]; - - /* handle any parameter-induced blowups */ - if (!isfinite(_rc.channels[i])) { - _rc.channels[i] = 0.0f; - } - } - - _rc.channel_count = rc_input.channel_count; - _rc.rssi = rc_input.rssi; - _rc.signal_lost = signal_lost; - _rc.timestamp = rc_input.timestamp_last_signal; - - /* publish rc_channels topic even if signal is invalid, for debug */ - if (_rc_pub > 0) { - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - - } else { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); - } - - if (!signal_lost) { - struct manual_control_setpoint_s manual; - memset(&manual, 0 , sizeof(manual)); - - /* fill values in manual_control_setpoint topic only if signal is valid */ - manual.timestamp = rc_input.timestamp_last_signal; - - /* limit controls */ - manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); - - /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); - - /* publish manual_control_setpoint topic */ - if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); - - } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - } - - /* copy from mapped manual control to control group 3 */ - struct actuator_controls_s actuator_group_3; - memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); - - actuator_group_3.timestamp = rc_input.timestamp_last_signal; - - actuator_group_3.control[0] = manual.y; - actuator_group_3.control[1] = manual.x; - actuator_group_3.control[2] = manual.r; - actuator_group_3.control[3] = manual.z; - actuator_group_3.control[4] = manual.flaps; - actuator_group_3.control[5] = manual.aux1; - actuator_group_3.control[6] = manual.aux2; - actuator_group_3.control[7] = manual.aux3; - - /* publish actuator_controls_3 topic */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); - - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); - } - - /* Update parameters from RC Channels (tuning with RC) if activated */ - static hrt_abstime last_rc_to_param_map_time = 0; - - if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { - set_params_from_rc(); - last_rc_to_param_map_time = hrt_absolute_time(); - } - } - } -} - -void -Sensors::task_main_trampoline(int argc, char *argv[]) -{ - sensors::g_sensors->task_main(); -} - -void -Sensors::task_main() -{ - - /* start individual sensors */ - int ret; - ret = accel_init(); - - if (ret) { - goto exit_immediate; - } - - ret = gyro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = mag_init(); - - if (ret) { - goto exit_immediate; - } - - ret = baro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = adc_init(); - - if (ret) { - goto exit_immediate; - } - - /* - * do subscriptions - */ - _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); - _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); - _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); - _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); - _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); - _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); - _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); - _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); - _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vcontrol_mode_sub, 200); - - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); - - /* - * do advertisements - */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - raw.timestamp = hrt_absolute_time(); - raw.adc_voltage_v[0] = 0.0f; - raw.adc_voltage_v[1] = 0.0f; - raw.adc_voltage_v[2] = 0.0f; - raw.adc_voltage_v[3] = 0.0f; - - /* set high initial error counts to deselect gyros */ - raw.gyro_errcount = 100000; - raw.gyro1_errcount = 100000; - raw.gyro2_errcount = 100000; - - /* set high initial error counts to deselect accels */ - raw.accelerometer_errcount = 100000; - raw.accelerometer1_errcount = 100000; - raw.accelerometer2_errcount = 100000; - - /* set high initial error counts to deselect mags */ - raw.magnetometer_errcount = 100000; - raw.magnetometer1_errcount = 100000; - raw.magnetometer2_errcount = 100000; - - memset(&_battery_status, 0, sizeof(_battery_status)); - _battery_status.voltage_v = -1.0f; - _battery_status.voltage_filtered_v = -1.0f; - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; - - /* get a set of initial values */ - accel_poll(raw); - gyro_poll(raw); - mag_poll(raw); - baro_poll(raw); - diff_pres_poll(raw); - - parameter_update_poll(true /* forced */); - rc_parameter_map_poll(true /* forced */); - - /* advertise the sensor_combined topic and make the initial publication */ - _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - - /* wakeup source(s) */ - struct pollfd fds[1]; - - /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ - fds[0].fd = _gyro_sub; - fds[0].events = POLLIN; - - _task_should_exit = false; - - while (!_task_should_exit) { - - /* wait for up to 50ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); - - /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - perf_begin(_loop_perf); - - /* check vehicle status for changes to publication state */ - vehicle_control_mode_poll(); - - /* the timestamp of the raw struct is updated by the gyro_poll() method */ - /* copy most recent sensor data */ - gyro_poll(raw); - accel_poll(raw); - mag_poll(raw); - baro_poll(raw); - - /* work out if main gyro timed out and fail over to alternate gyro */ - if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { - - /* if the secondary failed as well, go to the tertiary */ - if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { - fds[0].fd = _gyro2_sub; - } else { - fds[0].fd = _gyro1_sub; - } - } - - /* check battery voltage */ - adc_poll(raw); - - diff_pres_poll(raw); - - /* Inform other processes that new data is available to copy */ - if (_publishing) { - orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); - } - - /* check parameters for updates */ - parameter_update_poll(); - - /* check rc parameter map for updates */ - rc_parameter_map_poll(); - - /* Look for new r/c input data */ - rc_poll(); - - perf_end(_loop_perf); - } - - warnx("exiting."); - -exit_immediate: - _sensors_task = -1; - _exit(ret); -} - -int -Sensors::start() -{ - ASSERT(_sensors_task == -1); - - /* start the task */ - _sensors_task = task_spawn_cmd("sensors_task", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (main_t)&Sensors::task_main_trampoline, - nullptr); - - /* wait until the task is up and running or has failed */ - while (_sensors_task > 0 && _task_should_exit) { - usleep(100); - } - - if (_sensors_task < 0) { - return -ERROR; - } - - return OK; -} - -int sensors_main(int argc, char *argv[]) -{ - if (argc < 2) { - errx(1, "usage: sensors {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (sensors::g_sensors != nullptr) { - errx(0, "already running"); - } - - sensors::g_sensors = new Sensors; - - if (sensors::g_sensors == nullptr) { - errx(1, "alloc failed"); - } - - if (OK != sensors::g_sensors->start()) { - delete sensors::g_sensors; - sensors::g_sensors = nullptr; - err(1, "start failed"); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (sensors::g_sensors == nullptr) { - errx(1, "not running"); - } - - delete sensors::g_sensors; - sensors::g_sensors = nullptr; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (sensors::g_sensors) { - errx(0, "is running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - return 1; -} diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 9984896525..1b3b6fa2c4 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -37,13 +37,15 @@ */ #include -#include +#include #include #include #include #include +#ifndef __PX4_QURT #include #include +#endif #include "simulator.h" using namespace simulator; @@ -76,8 +78,11 @@ int Simulator::start(int argc, char *argv[]) { int ret = 0; _instance = new Simulator(); - if (_instance) + if (_instance) { +#ifndef __PX4_QURT _instance->updateSamples(); +#endif + } else { printf("Simulator creation failed\n"); ret = 1; @@ -86,6 +91,7 @@ int Simulator::start(int argc, char *argv[]) } +#ifndef __PX4_QURT void Simulator::updateSamples() { // get samples from external provider @@ -133,6 +139,7 @@ void Simulator::updateSamples() } } } +#endif static void usage() { diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index e07cc0f296..f27b2fe017 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -148,7 +148,9 @@ private: Simulator() : _accel(1), _mpu(1), _baro(1) {} ~Simulator() { _instance=NULL; } +#ifndef __PX4_QURT void updateSamples(); +#endif static Simulator *_instance; diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index 0f2a79822b..e55e137d61 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -37,6 +37,7 @@ */ #include +#include #include diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index db167331f9..1e859c386e 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -41,6 +41,7 @@ */ #include +#include #include #include #include @@ -89,8 +90,6 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ - orb_advert_t _to_system_power; - /** work trampoline */ static void _tick_trampoline(void *arg); @@ -114,8 +113,7 @@ ADCSIM::ADCSIM(uint32_t channels) : VDev("adcsim", ADCSIM0_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), - _samples(nullptr), - _to_system_power(0) + _samples(nullptr) { //_debug_enabled = true; @@ -259,7 +257,7 @@ test(void) unsigned channels = count / sizeof(data[0]); for (unsigned j = 0; j < channels; j++) { - printf ("%d: %u ", data[j].am_channel, data[j].am_data); + printf ("%d: %lu ", data[j].am_channel, (unsigned long)data[j].am_data); } printf("\n"); diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp new file mode 100644 index 0000000000..2a500af794 --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -0,0 +1,407 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file ets_airspeed.cpp + * @author Simon Wilks + * + * Driver for the Eagle Tree Airspeed V3 connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) : + I2C("Airspeed", path, bus, address), + _reports(nullptr), + _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), + _max_differential_pressure_pa(0), + _sensor_ok(false), + _last_published_sensor_ok(true), /* initialize differently to force publication */ + _measure_ticks(0), + _collect_phase(false), + _diff_pres_offset(0.0f), + _airspeed_pub(-1), + _subsys_pub(-1), + _class_instance(-1), + _conversion_interval(conversion_interval), + _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), + _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) +{ + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +AirspeedSim::~AirspeedSim() +{ + /* make sure we are truly inactive */ + stop(); + + if (_class_instance != -1) + unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + + /* free any existing reports */ + if (_reports != nullptr) + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +AirspeedSim::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(differential_pressure_s)); + if (_reports == nullptr) + goto out; + + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); + + /* publication init */ + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct differential_pressure_s arp; + measure(); + _reports->get(&arp); + + /* measurement will have generated a report, publish */ + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); + + if (_airspeed_pub < 0) + warnx("uORB started?"); + } + + ret = OK; + +out: + return ret; +} + +int +AirspeedSim::probe() +{ + /* on initial power up the device may need more than one retry + for detection. Once it is running the number of retries can + be reduced + */ + _retries = 4; + int ret = measure(); + + // drop back to 2 retries once initialised + _retries = 2; + return ret; +} + +int +AirspeedSim::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(_conversion_interval); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_conversion_interval)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case AIRSPEEDIOCSSCALE: { + struct airspeed_scale *s = (struct airspeed_scale*)arg; + _diff_pres_offset = s->offset_pa; + return OK; + } + + case AIRSPEEDIOCGSCALE: { + struct airspeed_scale *s = (struct airspeed_scale*)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; + } + + default: + /* give it to the superclass */ + return I2C::ioctl(handlep, cmd, arg); + } +} + +ssize_t +AirspeedSim::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(differential_pressure_s); + differential_pressure_s *abuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(abuf)) { + ret += sizeof(*abuf); + abuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(_conversion_interval); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(abuf)) { + ret = sizeof(*abuf); + } + + } while (0); + + return ret; +} + +void +AirspeedSim::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1); +} + +void +AirspeedSim::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +AirspeedSim::update_status() +{ + if (_sensor_ok != _last_published_sensor_ok) { + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _sensor_ok, + SUBSYSTEM_TYPE_DIFFPRESSURE + }; + + if (_subsys_pub > 0) { + orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { + _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); + } + + _last_published_sensor_ok = _sensor_ok; + } +} + +void +AirspeedSim::cycle_trampoline(void *arg) +{ + Airspeed *dev = (Airspeed *)arg; + + dev->cycle(); + // XXX we do not know if this is + // really helping - do not update the + // subsys state right now + //dev->update_status(); +} + +void +AirspeedSim::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + warnx("poll interval: %u ticks", _measure_ticks); + _reports->print_info("report queue"); +} + +void +AirspeedSim::new_report(const differential_pressure_s &report) +{ + if (!_reports->force(&report)) + perf_count(_buffer_overflows); +} diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h new file mode 100644 index 0000000000..66f4c58627 --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file airspeed.h + * @author Simon Wilks + * + * Generic driver for airspeed sensors connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +/* Default I2C bus */ +#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class __EXPORT AirspeedSim : public device::I2C +{ +public: + AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path); + virtual ~AirspeedSim(); + + virtual int init(); + + virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + virtual void print_info(); + +private: + RingBuffer *_reports; + perf_counter_t _buffer_overflows; + + /* this class has pointer data members and should not be copied */ + AirspeedSim(const AirspeedSim &); + AirspeedSim &operator=(const AirspeedSim &); + +protected: + virtual int probe(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + virtual void cycle() = 0; + virtual int measure() = 0; + virtual int collect() = 0; + + /** + * Update the subsystem status + */ + void update_status(); + + work_s _work; + float _max_differential_pressure_pa; + bool _sensor_ok; + bool _last_published_sensor_ok; + int _measure_ticks; + bool _collect_phase; + float _diff_pres_offset; + + orb_advert_t _airspeed_pub; + orb_advert_t _subsys_pub; + + int _class_instance; + + unsigned _conversion_interval; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * add a new report to the reports queue + * + * @param report differential_pressure_s report + */ + void new_report(const differential_pressure_s &report); +}; + + diff --git a/src/platforms/posix/drivers/airspeedsim/module.mk b/src/platforms/posix/drivers/airspeedsim/module.mk new file mode 100644 index 0000000000..2d971e4d28 --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Makefile to build the generic airspeed driver. +# + +SRCS = airspeedsim.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 8398d18c58..bef9c9c5aa 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -755,7 +756,7 @@ BAROSIM::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); _reports->print_info("report queue"); - printf("TEMP: %d\n", _TEMP); + printf("TEMP: %ld\n", (long)_TEMP); printf("SENS: %lld\n", (long long)_SENS); printf("OFF: %lld\n", (long long)_OFF); printf("P: %.3f\n", (double)_P); @@ -964,12 +965,12 @@ test() /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; + px4_pollfd_struct_t fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + ret = px4_poll(&fds, 1, 2000); if (ret != 1) { warnx("timed out waiting for sensor data"); @@ -990,7 +991,7 @@ test() warnx("time: %lld", (long long)report.timestamp); } - close(fd); + px4_close(fd); warnx("PASS"); return 0; } @@ -1004,7 +1005,7 @@ reset() struct barosim_bus_option &bus = bus_options[0]; int fd; - fd = open(bus.devpath, O_RDONLY); + fd = px4_open(bus.devpath, O_RDONLY); if (fd < 0) { warn("failed "); return 1; @@ -1068,14 +1069,14 @@ calibrate(unsigned altitude) pressure = 0.0f; for (unsigned i = 0; i < 20; i++) { - struct pollfd fds; + px4_pollfd_struct_t fds; int ret; ssize_t sz; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 1000); + ret = px4_poll(&fds, 1, 1000); if (ret != 1) { warnx("timed out waiting for sensor data"); @@ -1116,7 +1117,7 @@ calibrate(unsigned altitude) return 1; } - close(fd); + px4_close(fd); return 0; } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 2e628f571c..42d52b4fef 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -69,7 +69,7 @@ public: virtual int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); private: - barosim::prom_u &_prom; + //barosim::prom_u &_prom; /** * Send a reset command to the barometer simulator. @@ -101,8 +101,7 @@ BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) } BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : - SIM("BARO_SIM", "/dev/BARO_SIM", bus, 0), - _prom(prom) + SIM("BARO_SIM", "/dev/BARO_SIM", bus, 0) { } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index a984a310a2..55a34b191a 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -57,7 +57,6 @@ #include #include #include -#include #include diff --git a/src/platforms/px4_common.h b/src/platforms/px4_common.h new file mode 100644 index 0000000000..d7b7a69421 --- /dev/null +++ b/src/platforms/px4_common.h @@ -0,0 +1,8 @@ +#pragma once + +#ifdef __PX4_QURT +#include +size_t strnlen(const char *s, size_t maxlen); + +//inline bool isfinite(int x) { return true; } +#endif diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h new file mode 100644 index 0000000000..02dc1c4a7e --- /dev/null +++ b/src/platforms/px4_time.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +#ifdef __PX4_LINUX + +#define px4_clock_gettime clock_gettime +#define px4_clock_settime clock_settime + +#elif defined(__PX4_QURT) + +#define CLOCK_REALTIME 1 + +__BEGIN_DECLS + +#if !defined(__cplusplus) +struct timespec +{ + time_t tv_sec; + long tv_nsec; +}; +#endif +int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); +int px4_clock_settime(clockid_t clk_id, struct timespec *tp); + +__EXPORT int usleep(useconds_t usec); +__EXPORT unsigned int sleep(unsigned int sec); + +__END_DECLS +#endif diff --git a/src/platforms/qurt/include/i2c.h b/src/platforms/qurt/include/i2c.h new file mode 100644 index 0000000000..ee3e4196dd --- /dev/null +++ b/src/platforms/qurt/include/i2c.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * include/nuttx/i2c.h + * + * Copyright(C) 2009-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ +#pragma once + +struct i2c_msg_s +{ + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; +}; diff --git a/src/platforms/qurt/include/poll.h b/src/platforms/qurt/include/poll.h index 324036aa5c..59f1809e2a 100644 --- a/src/platforms/qurt/include/poll.h +++ b/src/platforms/qurt/include/poll.h @@ -1,3 +1,40 @@ +/**************************************************************************** + * include/poll.h + * + * Copyright (C) 2008-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + #pragma once typedef unsigned int nfds_t; + +#define POLLIN (0x01) diff --git a/src/platforms/qurt/include/sys/ioctl.h b/src/platforms/qurt/include/sys/ioctl.h new file mode 100644 index 0000000000..0d08f75231 --- /dev/null +++ b/src/platforms/qurt/include/sys/ioctl.h @@ -0,0 +1,3 @@ +#pragma once + +#define _IO(x,y) (x+y) diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 3697fed0df..55f9ad9a8d 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -49,25 +49,29 @@ //typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" -//#include "px4_middleware.h" +#include "px4_middleware.h" -static const char *commands = "hello start\n"; +static const char *commands = "hello start\n" + "list_tasks"; static void run_cmd(const vector &appargs) { // command is appargs[0] string command = appargs[0]; printf("Looking for %s\n", command.c_str()); if (apps.find(command) != apps.end()) { - const char *arg[appargs.size()+2]; + const char *arg[2+1]; unsigned int i = 0; - while (i < appargs.size() && appargs[i] != "") { + printf("size = %d\n", appargs.size()); + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); printf(" arg = '%s'\n", arg[i]); ++i; } arg[i] = (char *)0; + //printf("BEFORE argc = %d %s %s %p\n", i, arg[0], arg[1], arg[2]); apps[command](i,(char **)arg); + //printf("AFTER argc = %d %s %s %p\n", i, arg[0], arg[1], arg[2]); } else { @@ -76,35 +80,39 @@ static void run_cmd(const vector &appargs) { } } +void eat_whitespace(const char *&b, int &i) +{ + // Eat whitespace + while (b[i] == ' ' || b[i] == '\t') { ++i; } + b = &b[i]; + i=0; +} + static void process_commands(const char *cmds) { - vector appargs(5); + vector appargs; int i=0; - int j=0; const char *b = cmds; bool found_first_char = false; char arg[20]; // Eat leading whitespace - while (b[i] == ' ') { ++i; }; - b = &b[i]; + eat_whitespace(b, i); for(;;) { // End of command line if (b[i] == '\n' || b[i] == '\0') { strncpy(arg, b, i); arg[i] = '\0'; - appargs[j] = arg; + appargs.push_back(arg); // If we have a command to run - if (i > 0 || j > 0) + if (appargs.size() > 0) { run_cmd(appargs); - j=0; + } + appargs.clear(); if (b[i] == '\n') { - // Eat whitespace - while (b[++i] == ' '); - b = &b[i]; - i=0; + eat_whitespace(b, ++i); continue; } else { @@ -115,12 +123,8 @@ static void process_commands(const char *cmds) else if (b[i] == ' ') { strncpy(arg, b, i); arg[i] = '\0'; - appargs[j] = arg; - j++; - // Eat whitespace - while (b[++i] == ' '); - b = &b[i]; - i=0; + appargs.push_back(arg); + eat_whitespace(b, ++i); continue; } ++i; @@ -129,6 +133,7 @@ static void process_commands(const char *cmds) int main(int argc, char **argv) { + px4::init(argc, argv, "mainapp"); process_commands(commands); for (;;) {} } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 92f25e5827..94d52be945 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -79,14 +79,17 @@ typedef struct static void entry_adapter ( void *ptr ) { - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data = (pthdata_t *) ptr; - data->entry(data->argc, data->argv); - free(ptr); + printf("TEST3\n"); +#if 0 + //data->entry(data->argc, data->argv); + printf("TEST4\n"); printf("Before px4_task_exit\n"); px4_task_exit(0); + //free(ptr); printf("After px4_task_exit\n"); +#endif } void @@ -95,7 +98,7 @@ px4_systemreset(bool to_bootloader) printf("Called px4_system_reset\n"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) { int rv; int argc = 0; @@ -105,14 +108,18 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p; + //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); + printf("arg %d %p\n", argc, argv); // Calculate argc - for(;;) { - p = argv[argc]; - printf("arg %d %s\n", argc, argv[argc]); - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; + if (argv) { + for(;;) { + p = argv[argc]; + if (p == (char *)0) + break; + printf("arg %d %s\n", argc, argv[argc]); + ++argc; + len += strlen(p)+1; + } } structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; @@ -125,6 +132,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskdata->argc = argc; for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); @@ -138,10 +146,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskmap[i].pid = i+1; taskmap[i].name = name; taskmap[i].isused = true; - taskmap[i].sp = malloc(stack_size);; + taskmap[i].sp = malloc(stack_size); break; } } + printf("TEST2\n"); thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); return i+1; @@ -156,6 +165,8 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { thread_stop(); + + // Free stack } void px4_killall(void) diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index 987e4807dd..71f30cd8dd 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -56,7 +56,7 @@ static void usage() } int hello_main(int argc, char *argv[]) { - printf("argc = %d %s %s\n", argc, argv[0], argv[1]); + printf("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); if (argc < 2) { usage(); return 1; From 8737d7794752d9d1e4c4c7dbbd50e6a2b75c72dd Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 01:01:36 -0700 Subject: [PATCH 149/258] QuRT and POSIX changes - part 2 Second staged group of changes for QuRT and related POSIX changes Signed-off-by: Mark Charlebois --- src/drivers/device/module.mk | 3 +-- src/drivers/device/ringbuffer.cpp | 3 +++ src/platforms/px4_adc.h | 2 +- src/platforms/px4_config.h | 2 +- src/platforms/px4_i2c.h | 23 ++++++++++++++++++----- src/platforms/px4_includes.h | 2 +- src/platforms/px4_nodehandle.h | 5 +++-- src/systemcmds/param/param.c | 12 ++++++------ 8 files changed, 34 insertions(+), 18 deletions(-) diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 85826bbc40..f7883568e9 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -43,8 +43,7 @@ SRCS = \ pio.cpp \ spi.cpp \ ringbuffer.cpp -endif -ifeq ($(PX4_TARGET_OS),posix) +else SRCS = vdev.cpp \ device.cpp \ vdev_posix.cpp \ diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index 427876463f..e77402b0d1 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -234,6 +234,8 @@ RingBuffer::force(double val) return force(&val, sizeof(val)); } +// FIXME - clang crashes on this get() call +#ifndef __PX4_QURT bool RingBuffer::get(void *val, size_t val_size) { @@ -263,6 +265,7 @@ RingBuffer::get(void *val, size_t val_size) return false; } } +#endif bool RingBuffer::get(int8_t &val) diff --git a/src/platforms/px4_adc.h b/src/platforms/px4_adc.h index 42893ef885..8420f51971 100644 --- a/src/platforms/px4_adc.h +++ b/src/platforms/px4_adc.h @@ -60,7 +60,7 @@ struct adc_msg_s // Example settings #define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 #define ADCSIM0_DEVICE_PATH "/dev/adc0" #else diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 5a187afd6c..635d82c216 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -41,7 +41,7 @@ #pragma once #if defined(__PX4_NUTTX) -#include +#include #elif defined (__PX4_POSIX) || defined (__PX4_QURT) #define CONFIG_NFILE_STREAMS 1 #define CONFIG_SCHED_WORKQUEUE 1 diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h index 3e8d4ea888..423329c557 100644 --- a/src/platforms/px4_i2c.h +++ b/src/platforms/px4_i2c.h @@ -117,13 +117,26 @@ inline bool px4_interrupt_context(void) { return false; } inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count); inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { return 0; } -struct i2c_msg_s +#ifdef __PX4_QURT + +struct i2c_msg { - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buf; + int len; }; + +#define I2C_RDWR 0x0FFF + +struct i2c_rdwr_ioctl_data { + struct i2c_msg *msgs; /* pointers to i2c_msgs */ + uint32_t nmsgs; /* number of i2c_msgs */ +}; + +int ioctl(int fd, int flags, unsigned long data); +int write(int fd, const char *buffer, int buflen); +#endif #else #error "No target platform defined" #endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index d71eeea350..83bb60d796 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -102,7 +102,7 @@ #include #include -#elif defined(__PX4_POSIX) +#elif defined(__PX4_POSIX) && !defined(__PX4_QURT) #include #include #include diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index e4a483de65..259dab8005 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -42,6 +42,7 @@ #include "px4_subscriber.h" #include "px4_publisher.h" #include "px4_middleware.h" +#include "px4_posix.h" #include "px4_app.h" #if defined(__PX4_ROS) @@ -274,10 +275,10 @@ public: } /* Poll fd with smallest interval */ - struct pollfd pfd; + px4_pollfd_struct_t pfd; pfd.fd = _sub_min_interval->getUORBHandle(); pfd.events = POLLIN; - poll(&pfd, 1, timeout_ms); + px4_poll(&pfd, 1, timeout_ms); spinOnce(); } } diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 30baaeb49c..1bc3df4b7b 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -305,7 +305,7 @@ do_show_print(void *arg, param_t param) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { - printf("%d\n", i); + printf("%ld\n", (long)i); return; } @@ -320,7 +320,7 @@ do_show_print(void *arg, param_t param) break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - printf("\n", 0 + param_type(param), param_size(param)); + printf("\n", 0 + param_type(param), param_size(param)); return; default: @@ -328,7 +328,7 @@ do_show_print(void *arg, param_t param) return; } - printf("\n", param); + printf("\n", (unsigned long)param); } static int @@ -365,9 +365,9 @@ do_set(const char *name, const char *val, bool fail_on_not_found) printf("unchanged\n"); } else { - printf("curr: %d", i); + printf("curr: %ld", (long)i); param_set(param, &newval); - printf(" -> new: %d\n", newval); + printf(" -> new: %ld\n", (long)newval); } } @@ -436,7 +436,7 @@ do_compare(const char *name, char *vals[], unsigned comparisons) int j = strtol(vals[k], &end, 10); if (i == j) { - printf(" %d: ", i); + printf(" %ld: ", (long)i); ret = 0; } } From 071c4c1a9e2255f5ed963175d6412739d87909f0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 01:05:49 -0700 Subject: [PATCH 150/258] Updated module.mk for changed file names sensors_x.c were consolidated to sensors.c Signed-off-by: Mark Charlebois --- src/modules/sensors/module.mk | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index 7237600ff3..d92ebc00eb 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -38,12 +38,8 @@ MODULE_COMMAND = sensors MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" -ifeq ($(PX$_TARGET_OS),nuttx) -SRCS = sensors_nuttx.cpp -else -SRCS = sensors_posix.cpp -endif -SRCS += sensor_params.c +SRCS = sensors.cpp \ + sensor_params.c MODULE_STACKSIZE = 1200 From c802beb3d712e7c00b8882b98354a8a78dd78e20 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 01:13:08 -0700 Subject: [PATCH 151/258] QuRT and POSIX changes - part 3 More staged changes to support QuRT and related POSIX changes Signed-off-by: Mark Charlebois --- .../commander/accelerometer_calibration.cpp | 2 +- .../commander/airspeed_calibration.cpp | 5 +-- src/modules/mavlink/mavlink_main_posix.cpp | 22 +++++++++++- src/platforms/px4_defines.h | 34 ++++++++++++++++++- 4 files changed, 58 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4248ad282b..e4c1d201b2 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -132,7 +132,7 @@ #include #include #include -#include +//#include #include #include #include diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index dd78676af6..87a2998dee 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -41,11 +41,12 @@ #include "commander_helper.h" #include +#include #include #include #include #include -#include +#include #include #include #include @@ -157,7 +158,7 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset = diff_pres_offset / calibration_count; - if (isfinite(diff_pres_offset)) { + if (PX4_ISFINITE(diff_pres_offset)) { int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); airscale.offset_pa = diff_pres_offset; diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp index 784118916d..cce0f4472f 100644 --- a/src/modules/mavlink/mavlink_main_posix.cpp +++ b/src/modules/mavlink/mavlink_main_posix.cpp @@ -51,7 +51,9 @@ #include #include #include +#ifndef __PX4_QURT #include +#endif #include #include /* isinf / isnan checks */ @@ -138,7 +140,9 @@ Mavlink::Mavlink() : _forwarding_on(false), _passing_on(false), _ftp_on(false), +#ifndef __PX4_QURT _uart_fd(-1), +#endif _baudrate(57600), _datarate(1000), _datarate_events(500), @@ -395,6 +399,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) } } +#ifndef __PX4_QURT int Mavlink::get_uart_fd(unsigned index) { @@ -412,6 +417,7 @@ Mavlink::get_uart_fd() { return _uart_fd; } +#endif // __PX4_QURT int Mavlink::get_instance_id() @@ -546,6 +552,7 @@ int Mavlink::get_component_id() return mavlink_system.compid; } +#ifndef __PX4_QURT int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -697,6 +704,8 @@ Mavlink::enable_flow_control(bool enabled) return ret; } +#endif + int Mavlink::set_hil_enabled(bool hil_enabled) { @@ -729,8 +738,10 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; +#ifndef __PX4_QURT + // No FIONWRITE on Linux -#ifndef __PX4_LINUX +#if !defined(__PX4_LINUX) (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); #endif @@ -746,6 +757,7 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } +#endif return buf_free; } @@ -800,6 +812,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); +#ifndef __PX4_QURT /* send message to UART */ ssize_t ret = ::write(_uart_fd, buf, packet_len); @@ -811,6 +824,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } +#endif pthread_mutex_unlock(&_send_mutex); } @@ -850,6 +864,7 @@ Mavlink::resend_message(mavlink_message_t *msg) buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); +#ifndef __PX4_QURT /* send message to UART */ ssize_t ret = ::write(_uart_fd, buf, packet_len); @@ -861,6 +876,7 @@ Mavlink::resend_message(mavlink_message_t *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } +#endif pthread_mutex_unlock(&_send_mutex); } @@ -1299,6 +1315,7 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); +#ifndef __PX4_QURT struct termios uart_config_original; /* default values for arguments */ @@ -1308,6 +1325,7 @@ Mavlink::task_main(int argc, char *argv[]) warn("could not open %s", _device_name); return ERROR; } +#endif /* initialize send mutex */ pthread_mutex_init(&_send_mutex, NULL); @@ -1560,11 +1578,13 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(_receive_thread, NULL); +#ifndef __PX4_QURT /* reset the UART flags to original state */ tcsetattr(_uart_fd, TCSANOW, &uart_config_original); /* close UART */ ::close(_uart_fd); +#endif /* close mavlink logging device */ px4_close(_mavlink_fd); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index e212e63540..89f98942d8 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -106,7 +106,7 @@ typedef param_t px4_param_t; #define PX4_ISFINITE(x) isfinite(x) -/* Linux Specific defines */ +/* POSIX Specific defines */ #elif defined(__PX4_POSIX) // Flag is meaningless on Linux @@ -138,7 +138,16 @@ __END_DECLS #define OK 0 #define ERROR -1 +#if defined(__PX4_QURT) +#define M_PI 3.14159265358979323846 +#define M_PI_2 1.57079632679489661923 +__BEGIN_DECLS #include +__END_DECLS +#else +#include +#endif + /* Float defines of the standard double length constants */ #define M_E_F (float)M_E #define M_LOG2E_F (float)M_LOG2E @@ -167,7 +176,30 @@ __END_DECLS #define M_DEG_TO_RAD 0.01745329251994 #define M_RAD_TO_DEG 57.2957795130823 +#ifndef __PX4_QURT + +#if defined(__cplusplus) #define PX4_ISFINITE(x) std::isfinite(x) +#else +#define PX4_ISFINITE(x) isfinite(x) +#endif +#endif + +#endif + +#if defined(__PX4_QURT) +#define SIOCDEVPRIVATE 999999 + +// Missing math.h defines +#define PX4_ISFINITE(x) isfinite(x) + +// FIXME - these are missing for clang++ but not for clang +#if defined(__cplusplus) +#define isfinite(x) true +#define isnan(x) false +#define isinf(x) false +#define fminf(x, y) ((x) > (y) ? y : x) +#endif #endif From 187f13cd7059893408c38c3e6415e5ac628eb469 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 01:24:31 -0700 Subject: [PATCH 152/258] QuRT and POSIX changes - part 4 Signed-off-by: Mark Charlebois --- .../attitude_estimator_ekf_main.cpp | 7 ++--- .../commander/calibration_routines.cpp | 6 ++-- src/modules/commander/commander.cpp | 10 +++++-- .../ekf_att_pos_estimator_main.cpp | 22 +++++++------- src/modules/systemlib/module.mk | 11 ++++--- src/modules/systemlib/otp.c | 2 +- src/modules/systemlib/perf_counter.c | 6 +++- src/modules/systemlib/perf_counter.h | 2 +- src/modules/systemlib/rc_check.c | 1 + src/modules/systemlib/state_table.h | 3 +- src/platforms/px4_posix.h | 30 ++++++++++++++----- 11 files changed, 65 insertions(+), 35 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 3a09b44bb1..bb01e18f1d 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -50,8 +51,6 @@ #include #include #include -#include -#include #include #include #include @@ -299,12 +298,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* Main loop*/ while (!thread_should_exit) { - struct pollfd fds[2]; + px4_pollfd_struct_t fds[2]; fds[0].fd = sub_raw; fds[0].events = POLLIN; fds[1].fd = sub_params; fds[1].events = POLLIN; - int ret = poll(fds, 2, 1000); + int ret = px4_poll(fds, 2, 1000); if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 02d1d8a86b..fe256e60ad 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -39,6 +39,8 @@ */ #include +#include +#include #include #include #include @@ -246,7 +248,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) float accel_err_thr = 5.0f; /* still time required in us */ hrt_abstime still_time = 2000000; - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = accel_sub; fds[0].events = POLLIN; @@ -262,7 +264,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub) while (true) { /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); + int poll_ret = px4_poll(fds, 1, 1000); if (poll_ret) { orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45dc4309ba..e36bfd474a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -43,6 +43,8 @@ */ #include +#include +#include #include #include #include @@ -54,7 +56,9 @@ #include #include //#include +#ifndef __PX4_QURT #include +#endif #include #include #include @@ -2571,8 +2575,10 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul void *commander_low_prio_loop(void *arg) { +#ifndef __PX4_QURT /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); +#endif /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -2583,7 +2589,7 @@ void *commander_low_prio_loop(void *arg) hrt_abstime need_param_autosave_timeout = 0; /* wakeup source(s) */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ fds[0].fd = cmd_sub; @@ -2591,7 +2597,7 @@ void *commander_low_prio_loop(void *arg) while (!thread_should_exit) { /* wait for up to 1000ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) { diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 41c62875bb..15a663bc1e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -46,6 +46,8 @@ #include #include #include +#include +#include #include #include #include @@ -230,10 +232,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : for (unsigned s = 0; s < 3; s++) { char str[30]; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - fd = open(str, O_RDONLY); + fd = px4_open(str, O_RDONLY); if (fd >= 0) { - res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]); + res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]); close(fd); if (res) { @@ -242,11 +244,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : } (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - fd = open(str, O_RDONLY); + fd = px4_open(str, O_RDONLY); if (fd >= 0) { - res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]); - close(fd); + res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]); + px4_close(fd); if (res) { warnx("A%u SCALE FAIL", s); @@ -254,11 +256,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : } (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - fd = open(str, O_RDONLY); + fd = px4_open(str, O_RDONLY); if (fd >= 0) { - res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]); - close(fd); + res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]); + px4_close(fd); if (res) { warnx("M%u SCALE FAIL", s); @@ -520,7 +522,7 @@ void AttitudePositionEstimatorEKF::task_main() parameters_update(); /* wakeup source(s) */ - struct pollfd fds[2]; + px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -538,7 +540,7 @@ void AttitudePositionEstimatorEKF::task_main() while (!_task_should_exit) { /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 5e35f2c2bc..4d805c57c0 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -36,10 +36,8 @@ # SRCS = err.c \ - hx_stream.c \ perf_counter.c \ param/param.c \ - bson/tinybson.c \ conversions.c \ cpuload.c \ getopt_long.c \ @@ -52,14 +50,19 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.cpp \ - circuit_breaker_params.c \ mcu_version.c ifeq ($(PX4_TARGET_OS),nuttx) SRCS += up_cxxinitialize.c endif +ifneq ($(PX4_TARGET_OS),qurt) +SRCS += hx_stream.c \ + circuit_breaker.cpp \ + circuit_breaker_params.c \ + bson/tinybson.c +endif + MAXOPTIMIZATION = -Os EXTRACFLAGS = -Wno-sign-compare diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index cb8ffbd3c8..a866c843d7 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -71,7 +71,7 @@ int val_read(void *dest, volatile const void *src, int bytes) int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) { - warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid); + warnx("write_otp: PX4 / %02X / %02lX / %02lX / ... etc \n", id_type, (unsigned long)vid, (unsigned long)pid); int errors = 0; diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 909979eca3..1344accb84 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -45,6 +45,10 @@ #include #include "perf_counter.h" +#ifdef __PX4_QURT +#define dprintf(...) +#endif + /** * Header common to all counters. */ @@ -470,7 +474,7 @@ perf_print_latency(int fd) { dprintf(fd, "bucket : events\n"); for (int i = 0; i < latency_bucket_count; i++) { - printf(" %4i : %i\n", latency_buckets[i], latency_counters[i]); + printf(" %4i : %li\n", latency_buckets[i], (long int)latency_counters[i]); } // print the overflow bucket value dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 8543ba7bb6..0a5ca94cb9 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -40,7 +40,7 @@ #define _SYSTEMLIB_PERF_COUNTER_H value #include -#include +#include /** * Counter types. diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index f576736ab1..fc28d17416 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -38,6 +38,7 @@ */ #include +#include #include #include diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index e6011fdef2..100c5ac499 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -51,7 +51,7 @@ public: }; StateTable(Tran const *table, unsigned nStates, unsigned nSignals) - : myTable(table), myNsignals(nSignals), myNstates(nStates) {} + : myTable(table), myNsignals(nSignals) {} #define NO_ACTION &StateTable::doNothing #define ACTION(_target) StateTable::Action(_target) @@ -76,7 +76,6 @@ protected: private: Tran const *myTable; unsigned myNsignals; - unsigned myNstates; }; #endif diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 18f4878878..b4cb41a4fd 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -52,13 +53,18 @@ #define PX4_DEBUG(...) //#define PX4_DEBUG(...) printf(__VA_ARGS__) -__BEGIN_DECLS +#ifdef __PX4_NUTTX -extern int px4_errno; +#define px4_pollfd_struct_t struct pollfd +#define px4_open open +#define px4_close close +#define px4_ioctl ioctl +#define px4_write write +#define px4_read read +#define px4_poll poll -#ifndef __PX4_NUTTX +#else typedef short pollevent_t; -#endif typedef struct { /* This part of the struct is POSIX-like */ @@ -71,15 +77,23 @@ typedef struct { void *priv; /* For use by drivers */ } px4_pollfd_struct_t; +__BEGIN_DECLS + __EXPORT int px4_open(const char *path, int flags); __EXPORT int px4_close(int fd); __EXPORT ssize_t px4_read(int fd, void *buffer, size_t buflen); __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); -__EXPORT void px4_show_devices(void); -__EXPORT void px4_show_topics(void); -__EXPORT const char * px4_get_device_names(unsigned int *handle); -__EXPORT const char * px4_get_topic_names(unsigned int *handle); __END_DECLS +#endif +__BEGIN_DECLS +extern int px4_errno; + +__EXPORT void px4_show_devices(void); +__EXPORT const char * px4_get_device_names(unsigned int *handle); + +__EXPORT void px4_show_topics(void); +__EXPORT const char * px4_get_topic_names(unsigned int *handle); +__END_DECLS From a1332e699cc5a16c12dfbedfbe1050d9c858df9a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 01:39:25 -0700 Subject: [PATCH 153/258] QuRT and POSIX changes - part 5 Last part of the main QuRT related changes Signed-off-by: Mark Charlebois --- src/modules/commander/gyro_calibration.cpp | 9 +++++---- src/modules/commander/mag_calibration.cpp | 5 +++-- src/modules/commander/rc_calibration.cpp | 7 +++++-- src/modules/mavlink/mavlink_main.h | 8 ++++++++ src/modules/mavlink/mavlink_messages.cpp | 7 +++++-- src/modules/mavlink/mavlink_receiver.cpp | 16 +++++++++------- src/modules/mavlink/mavlink_stream.cpp | 2 ++ src/modules/mavlink/mavlink_stream.h | 2 ++ src/modules/systemlib/mavlink_log.c | 3 ++- src/platforms/px4_time.h | 2 +- 10 files changed, 42 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index b5d5bb23c7..deceed011a 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -42,11 +42,12 @@ #include "commander_helper.h" #include +#include #include #include #include #include -#include +#include #include #include #include @@ -193,9 +194,9 @@ int do_gyro_calibration(int mavlink_fd) /* maximum allowable calibration error in radians */ const float maxoff = 0.0055f; - if (!isfinite(gyro_scale[0].x_offset) || - !isfinite(gyro_scale[0].y_offset) || - !isfinite(gyro_scale[0].z_offset) || + if (!PX4_ISFINITE(gyro_scale[0].x_offset) || + !PX4_ISFINITE(gyro_scale[0].y_offset) || + !PX4_ISFINITE(gyro_scale[0].z_offset) || fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 49ccbf3ef1..82f404914a 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -43,12 +43,13 @@ #include "calibration_messages.h" #include +#include #include #include #include #include #include -#include +#include #include #include #include @@ -376,7 +377,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); - if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !isfinite(sphere_z[cur_mag])) { + if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) { mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag); result = ERROR; } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0776894fb7..8c51e95b9d 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -36,6 +36,9 @@ * Remote Control calibration routine */ +#include +#include + #include "rc_calibration.h" #include "commander_helper.h" @@ -82,11 +85,11 @@ int do_trim_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); - close(sub_man); + px4_close(sub_man); return ERROR; } mavlink_log_info(mavlink_fd, "trim cal done"); - close(sub_man); + px4_close(sub_man); return OK; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1e346088aa..86b259a91b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -112,9 +112,11 @@ public: static void forward_message(const mavlink_message_t *msg, Mavlink *self); +#ifndef __PX4_QURT static int get_uart_fd(unsigned index); int get_uart_fd(); +#endif /** * Get the MAVLink system id. @@ -190,12 +192,14 @@ public: int get_instance_id(); +#ifndef __PX4_QURT /** * Enable / disable hardware flow control. * * @param enabled True if hardware flow control should be enabled */ int enable_flow_control(bool enabled); +#endif mavlink_channel_t get_channel(); @@ -320,7 +324,9 @@ private: bool _forwarding_on; bool _passing_on; bool _ftp_on; +#ifndef __PX4_QURT int _uart_fd; +#endif int _baudrate; int _datarate; ///< data rate for normal streams (attitude, position, etc.) int _datarate_events; ///< data rate for params, waypoints, text messages @@ -378,7 +384,9 @@ private: void mavlink_update_system(); +#ifndef __PX4_QURT int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +#endif static unsigned int interval_from_rate(float rate); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5adfea36b6..648b875800 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -39,6 +39,7 @@ * @author Anton Babushkin */ +#include #include #include @@ -354,6 +355,7 @@ protected: } } +#ifndef __PX4_QURT void send(const hrt_abstime t) { if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { @@ -391,7 +393,7 @@ protected: char log_file_path[64] = ""; timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); + px4_clock_gettime(CLOCK_REALTIME, &ts); /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; @@ -410,6 +412,7 @@ protected: } } } +#endif }; class MavlinkStreamCommandLong : public MavlinkStream @@ -975,7 +978,7 @@ protected: mavlink_system_time_t msg; timespec tv; - clock_gettime(CLOCK_REALTIME, &tv); + px4_clock_gettime(CLOCK_REALTIME, &tv); msg.time_boot_ms = hrt_absolute_time() / 1000; msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 03ad2eca15..db508b6f04 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -41,12 +41,8 @@ */ /* XXX trim includes */ -#ifdef __PX4_NUTTX -#include -#include -#else #include -#endif +#include #include #include #include @@ -64,8 +60,10 @@ #include #include #include +#ifndef __PX4_QURT #include #include +#endif #include #include #include @@ -993,7 +991,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) mavlink_msg_system_time_decode(msg, &time); timespec tv; - clock_gettime(CLOCK_REALTIME, &tv); + px4_clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS; @@ -1002,7 +1000,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) if (!onb_unix_valid && ofb_unix_valid) { tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; - if(clock_settime(CLOCK_REALTIME, &tv)) { + if(px4_clock_settime(CLOCK_REALTIME, &tv)) { warn("failed setting clock"); } else { @@ -1488,6 +1486,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { +#ifndef __PX4_QURT int uart_fd = _mavlink->get_uart_fd(); const int timeout = 500; @@ -1530,6 +1529,7 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->count_rxbytes(nread); } } +#endif return NULL; } @@ -1576,9 +1576,11 @@ MavlinkReceiver::receive_start(Mavlink *parent) pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); +#ifndef __PX4_QURT // set to non-blocking read int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); +#endif struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 57a92c8b58..be50602b7d 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -79,7 +79,9 @@ MavlinkStream::update(const hrt_abstime t) if (dt > 0 && dt >= interval) { /* interval expired, send message */ +#ifndef __PX4_QURT send(t); +#endif if (const_rate()) { _last_sent = (t / _interval) * _interval; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 5e39bbbdf9..6d0d0b6775 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -92,7 +92,9 @@ protected: Mavlink *_mavlink; unsigned int _interval; +#ifndef __PX4_QURT virtual void send(const hrt_abstime t) = 0; +#endif private: hrt_abstime _last_sent; diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index f321f9ceba..56201588a2 100644 --- a/src/modules/systemlib/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -39,6 +39,7 @@ * @author Lorenz Meier */ +#include #include #include #include @@ -124,5 +125,5 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...) char text[MAVLINK_LOG_MAXLEN + 1]; vsnprintf(text, sizeof(text), fmt, ap); va_end(ap); - ioctl(_fd, severity, (unsigned long)&text[0]); + px4_ioctl(_fd, severity, (unsigned long)&text[0]); } diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 02dc1c4a7e..d72cdbb8b1 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -3,7 +3,7 @@ #include #include -#ifdef __PX4_LINUX +#if defined(__PX4_LINUX) || defined(__PX4_NUTTX) #define px4_clock_gettime clock_gettime #define px4_clock_settime clock_settime From 4cedcfc58efe80466f056be00c9d67dbdd0b0760 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 01:40:46 -0700 Subject: [PATCH 154/258] math/test/test.cpp has invalid calls The function calls ceil() and floor() but passes an int and there is obviously no implementation for that so clang fails. It seems like exp should be a float from this code. Signed-off-by: Mark Charlebois --- src/lib/mathlib/math/test/test.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index c52771ab8c..aff31bca0d 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -59,6 +59,9 @@ void __EXPORT float2SigExp( float &sig, int &exp) { +// FIXME - This code makes no sense when exp is an int +// FIXME - isnan and isinf not defined for QuRT +#ifndef __PX4_QURT if (isnan(num) || isinf(num)) { sig = 0.0f; exp = -99; @@ -79,6 +82,7 @@ void __EXPORT float2SigExp( } else { exp = floor(exp); } +#endif sig = num; From 58a73a539236c8219ab8a74dda3de51b67ee7e1a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 01:56:09 -0700 Subject: [PATCH 155/258] Fixed list of SRCS in mavlink_tests/module.mk Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_tests/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 1a9936015e..a2793c6365 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = mavlink_tests SRCS = mavlink_tests.cpp \ mavlink_ftp_test.cpp \ -SRCS += ../mavlink_ftp.cpp \ + ../mavlink_ftp.cpp \ ../mavlink.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From 16d6068bfd3579531501cde118ddcdd5ee1786b9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 10:52:07 -0700 Subject: [PATCH 156/258] QuRT: patch for eigen This patch is required for QuRT. comlpex.h defines "I" and it replaces "I" in the enum definition without this patch creating an error. Signed-off-by: Mark Charlebois --- makefiles/qurt/qurt_eigen.patch | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 makefiles/qurt/qurt_eigen.patch diff --git a/makefiles/qurt/qurt_eigen.patch b/makefiles/qurt/qurt_eigen.patch new file mode 100644 index 0000000000..d5cec3141a --- /dev/null +++ b/makefiles/qurt/qurt_eigen.patch @@ -0,0 +1,17 @@ +This patch is required for QuRT. compex.h defines "I" and it replaces "I" in the +enum definition without this patch creating an error. + +diff --git a/Eigen/src/Core/SolveTriangular.h b/Eigen/src/Core/SolveTriangular.h +index ef17f28..1116270 100644 +--- a/Eigen/src/Core/SolveTriangular.h ++++ b/Eigen/src/Core/SolveTriangular.h +@@ -112,6 +112,9 @@ template + struct triangular_solver_unroller; + ++#ifdef __PX4_QURT ++#undef I ++#endif + template + struct triangular_solver_unroller { + enum { From 20d35e33da475d55358c6d5eac6a30dfe3757a2c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 11:45:14 -0700 Subject: [PATCH 157/258] Platform header file cleanup and consolidation Removed obsolete porting cruft from px4_XXX.h files and merged the POSIX changes in PreflightCheck_posix.cpp back to PreflightCheck.cpp Signed-off-by: Mark Charlebois --- makefiles/qurt/qurt_eigen.patch | 2 +- src/modules/commander/PreflightCheck.cpp | 31 +- .../commander/PreflightCheck_posix.cpp | 344 ------------------ src/modules/commander/module.mk | 9 +- src/modules/mavlink/mavlink_main_posix.cpp | 3 +- src/platforms/px4_common.h | 1 - src/platforms/px4_config.h | 10 +- src/platforms/px4_defines.h | 23 +- src/platforms/px4_i2c.h | 23 +- 9 files changed, 47 insertions(+), 399 deletions(-) delete mode 100644 src/modules/commander/PreflightCheck_posix.cpp diff --git a/makefiles/qurt/qurt_eigen.patch b/makefiles/qurt/qurt_eigen.patch index d5cec3141a..9ea57403ba 100644 --- a/makefiles/qurt/qurt_eigen.patch +++ b/makefiles/qurt/qurt_eigen.patch @@ -1,4 +1,4 @@ -This patch is required for QuRT. compex.h defines "I" and it replaces "I" in the +This patch is required for QuRT. complex.h defines "I" and it replaces "I" in the enum definition without this patch creating an error. diff --git a/Eigen/src/Core/SolveTriangular.h b/Eigen/src/Core/SolveTriangular.h diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2c05fb33d1..5567b43529 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -41,6 +41,7 @@ */ #include +#include #include #include #include @@ -74,7 +75,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) char s[30]; sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); - int fd = open(s, 0); + int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { @@ -87,7 +88,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_MAG%u_ID", instance); param_get(param_find(s), &(calibration_devid)); @@ -98,7 +99,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) goto out; } - ret = ioctl(fd, MAGIOCSELFTEST, 0); + ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -108,7 +109,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -118,7 +119,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, char s[30]; sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); - int fd = open(s, O_RDONLY); + int fd = px4_open(s, O_RDONLY); if (fd < 0) { if (!optional) { @@ -131,7 +132,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_ACC%u_ID", instance); param_get(param_find(s), &(calibration_devid)); @@ -142,7 +143,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, goto out; } - ret = ioctl(fd, ACCELIOCSELFTEST, 0); + ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -154,7 +155,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (dynamic) { /* check measurement result range */ struct accel_report acc; - ret = read(fd, &acc, sizeof(acc)); + ret = px4_read(fd, &acc, sizeof(acc)); if (ret == sizeof(acc)) { /* evaluate values */ @@ -175,7 +176,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, } out: - close(fd); + px4_close(fd); return success; } @@ -185,7 +186,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) char s[30]; sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); - int fd = open(s, 0); + int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { @@ -198,7 +199,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) int calibration_devid; int ret; - int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); sprintf(s, "CAL_GYRO%u_ID", instance); param_get(param_find(s), &(calibration_devid)); @@ -209,7 +210,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) goto out; } - ret = ioctl(fd, GYROIOCSELFTEST, 0); + ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, @@ -219,7 +220,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -229,7 +230,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) char s[30]; sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); - int fd = open(s, 0); + int fd = px4_open(s, 0); if (fd < 0) { if (!optional) { @@ -240,7 +241,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - close(fd); + px4_close(fd); return success; } diff --git a/src/modules/commander/PreflightCheck_posix.cpp b/src/modules/commander/PreflightCheck_posix.cpp deleted file mode 100644 index 5567b43529..0000000000 --- a/src/modules/commander/PreflightCheck_posix.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in -* the documentation and/or other materials provided with the -* distribution. -* 3. Neither the name PX4 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. -* -****************************************************************************/ - -/** -* @file PreflightCheck.cpp -* -* Preflight check for main system components -* -* @author Lorenz Meier -* @author Johan Jansen -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include "PreflightCheck.h" - -namespace Commander -{ -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) -{ - bool success = true; - - char s[30]; - sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); - - if (fd < 0) { - if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); - } - - return false; - } - - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_MAG%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); - - if (devid != calibration_devid) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance); - success = false; - goto out; - } - - ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); - - if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); - success = false; - goto out; - } - -out: - px4_close(fd); - return success; -} - -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) -{ - bool success = true; - - char s[30]; - sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, O_RDONLY); - - if (fd < 0) { - if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); - } - - return false; - } - - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_ACC%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); - - if (devid != calibration_devid) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance); - success = false; - goto out; - } - - ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); - - if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); - success = false; - goto out; - } - - if (dynamic) { - /* check measurement result range */ - struct accel_report acc; - ret = px4_read(fd, &acc, sizeof(acc)); - - if (ret == sizeof(acc)) { - /* evaluate values */ - float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - - if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still"); - /* this is frickin' fatal */ - success = false; - goto out; - } - } else { - mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); - /* this is frickin' fatal */ - success = false; - goto out; - } - } - -out: - px4_close(fd); - return success; -} - -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) -{ - bool success = true; - - char s[30]; - sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); - - if (fd < 0) { - if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); - } - - return false; - } - - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_GYRO%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); - - if (devid != calibration_devid) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance); - success = false; - goto out; - } - - ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); - success = false; - goto out; - } - -out: - px4_close(fd); - return success; -} - -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) -{ - bool success = true; - - char s[30]; - sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); - int fd = px4_open(s, 0); - - if (fd < 0) { - if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, - "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); - } - - return false; - } - - px4_close(fd); - return success; -} - -static bool airspeedCheck(int mavlink_fd, bool optional) -{ - bool success = true; - int ret; - int fd = orb_subscribe(ORB_ID(airspeed)); - - struct airspeed_s airspeed; - - if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || - (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); - success = false; - goto out; - } - - if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); - // XXX do not make this fatal yet - } - -out: - close(fd); - return success; -} - -bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic) -{ - bool failed = false; - - /* ---- MAG ---- */ - if (checkMag) { - /* check all sensors, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_mag_count; i++) { - bool required = (i < max_mandatory_mag_count); - - if (!magnometerCheck(mavlink_fd, i, !required) && required) { - failed = true; - } - } - } - - /* ---- ACCEL ---- */ - if (checkAcc) { - /* check all sensors, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_accel_count; i++) { - bool required = (i < max_mandatory_accel_count); - - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { - failed = true; - } - } - } - - /* ---- GYRO ---- */ - if (checkGyro) { - /* check all sensors, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_gyro_count; i++) { - bool required = (i < max_mandatory_gyro_count); - - if (!gyroCheck(mavlink_fd, i, !required) && required) { - failed = true; - } - } - } - - /* ---- BARO ---- */ - if (checkBaro) { - /* check all sensors, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_baro_count; i++) { - bool required = (i < max_mandatory_baro_count); - - if (!baroCheck(mavlink_fd, i, !required) && required) { - failed = true; - } - } - } - - /* ---- AIRSPEED ---- */ - if (checkAirspeed) { - if (!airspeedCheck(mavlink_fd, true)) { - failed = true; - } - } - - /* ---- RC CALIBRATION ---- */ - if (checkRC) { - if (rc_calibration_check(mavlink_fd) != OK) { - failed = true; - } - } - - /* Report status */ - return !failed; -} - -} diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 7b72dafd95..851ac9020d 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -45,15 +45,14 @@ SRCS = commander.cpp \ baro_calibration.cpp \ accelerometer_calibration.cpp \ rc_calibration.cpp \ - airspeed_calibration.cpp + airspeed_calibration.cpp \ + PreflightCheck.cpp ifdef ($(PX4_TARGET_OS),nuttx) SRCS += - state_machine_helper.cpp \ - PreflightCheck.cpp + state_machine_helper.cpp else -SRCS += state_machine_helper_posix.cpp \ - PreflightCheck_posix.cpp +SRCS += state_machine_helper_posix.cpp endif MODULE_STACKSIZE = 5000 diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp index cce0f4472f..5147368f41 100644 --- a/src/modules/mavlink/mavlink_main_posix.cpp +++ b/src/modules/mavlink/mavlink_main_posix.cpp @@ -216,7 +216,8 @@ Mavlink::Mavlink() : #endif default: - px4_errx(1, "instance ID is out of range"); + warnx("instance ID is out of range"); + px4_task_exit(1); break; } diff --git a/src/platforms/px4_common.h b/src/platforms/px4_common.h index d7b7a69421..384b1bca65 100644 --- a/src/platforms/px4_common.h +++ b/src/platforms/px4_common.h @@ -4,5 +4,4 @@ #include size_t strnlen(const char *s, size_t maxlen); -//inline bool isfinite(int x) { return true; } #endif diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 635d82c216..d6fcebd703 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -34,15 +34,17 @@ /** * @file px4_config.h - * Preserve abiility to load config information that is used in subsequent - * includes or code + Configuration flags used in code. */ #pragma once #if defined(__PX4_NUTTX) + #include -#elif defined (__PX4_POSIX) || defined (__PX4_QURT) + +#elif defined (__PX4_POSIX) + #define CONFIG_NFILE_STREAMS 1 #define CONFIG_SCHED_WORKQUEUE 1 #define CONFIG_SCHED_HPWORK 1 @@ -55,6 +57,4 @@ #define CONFIG_SCHED_INSTRUMENTATION 1 #define CONFIG_MAX_TASKS 32 -#define px4_errx(x, ...) errx(x, __VA_ARGS__) - #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 89f98942d8..8fcc9695ad 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -67,9 +67,9 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) -#elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) || defined(__PX4_QURT) +#elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) /* - * Building for NuttX or Linux + * Building for NuttX or POSIX */ #include /* Main entry point */ @@ -90,7 +90,9 @@ typedef param_t px4_param_t; #error "No target OS defined" #endif -/* NuttX Specific defines */ +/* + * NuttX Specific defines + */ #if defined(__PX4_NUTTX) /* XXX this is a hack to resolve conflicts with NuttX headers */ @@ -106,7 +108,9 @@ typedef param_t px4_param_t; #define PX4_ISFINITE(x) isfinite(x) -/* POSIX Specific defines */ +/* + * POSIX Specific defines + */ #elif defined(__PX4_POSIX) // Flag is meaningless on Linux @@ -119,6 +123,7 @@ typedef param_t px4_param_t; //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) #define UNIQUE_ID 0x1FFF7A10 +/* FIXME - Used to satisfy build */ #define getreg32(a) (*(volatile uint32_t *)(a)) __BEGIN_DECLS @@ -133,8 +138,10 @@ __END_DECLS #endif -/* Defines for ROS and Linux */ -#if defined(__PX4_ROS) || defined(__PX4_POSIX) || defined(__PX4_QURT) +/* + * Defines for ROS and Linux + */ +#if defined(__PX4_ROS) || defined(__PX4_POSIX) #define OK 0 #define ERROR -1 @@ -203,7 +210,9 @@ __END_DECLS #endif -/* Defines for all platforms */ +/* + *Defines for all platforms + */ /* wrapper for 2d matrices */ #define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h index 423329c557..2b72f37c5d 100644 --- a/src/platforms/px4_i2c.h +++ b/src/platforms/px4_i2c.h @@ -42,7 +42,9 @@ #define PX4_I2C_M_READ 0x0001 /* read data, from slave to master */ #if defined(__PX4_ROS) + #error "Devices not supported in ROS" + #elif defined (__PX4_NUTTX) /* * Building for NuttX @@ -64,12 +66,6 @@ typedef struct i2c_dev_s px4_i2c_dev_t; -#define px4_i2cuninitialize(x) up_i2cuninitialize(x) -#define px4_i2cinitialize(x) up_i2cinitialize(x) -#define px4_i2creset(x) up_i2creset(x) - -#define px4_interrupt_context() up_interrupt_context() - #elif defined(__PX4_POSIX) #include @@ -85,25 +81,11 @@ typedef struct { int length; } px4_i2c_msg_t; -struct px4_i2c_ops_t; // NOTE - This is a copy of the NuttX i2c_ops_s structure typedef struct { const struct px4_i2c_ops_t *ops; /* I2C vtable */ } px4_i2c_dev_t; -// FIXME - Stub implementations -inline void px4_i2cuninitialize(px4_i2c_dev_t *dev); -inline void px4_i2cuninitialize(px4_i2c_dev_t *dev) {} - -inline px4_i2c_dev_t *px4_i2cinitialize(int bus); -inline px4_i2c_dev_t *px4_i2cinitialize(int bus) { return (px4_i2c_dev_t *)0; } - -inline void px4_i2creset(px4_i2c_dev_t *dev); -inline void px4_i2creset(px4_i2c_dev_t *dev) { } - -inline bool px4_interrupt_context(void); -inline bool px4_interrupt_context(void) { return false; } - // FIXME - Empty defines for I2C ops // Original version commented out //#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f)) @@ -134,6 +116,7 @@ struct i2c_rdwr_ioctl_data { uint32_t nmsgs; /* number of i2c_msgs */ }; +// FIXME - The functions are not implemented on QuRT/DSPAL int ioctl(int fd, int flags, unsigned long data); int write(int fd, const char *buffer, int buflen); #endif From d913ec8dc928969a349e810027cf82904970915d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 24 Apr 2015 17:49:35 -0700 Subject: [PATCH 158/258] Changed device::px4_device_handle_t to device::file_t This change allowed the _posix.cpp file changes to be merged back into the original files. Signed-off-by: Mark Charlebois --- posix-configs/posixtest/init/rc.S | 2 +- src/drivers/blinkm/blinkm_posix.cpp | 4 +- .../device/{cdev_nuttx.cpp => cdev.cpp} | 48 +- src/drivers/device/device_nuttx.h | 27 +- .../device/{device.cpp => device_posix.cpp} | 0 src/drivers/device/i2c_posix.cpp | 8 +- src/drivers/device/i2c_posix.h | 6 +- src/drivers/device/module.mk | 7 +- src/drivers/device/vdev.cpp | 30 +- src/drivers/device/vdev.h | 49 +- src/drivers/device/vdev_posix.cpp | 8 +- src/drivers/hil/hil.cpp | 187 ++-- src/drivers/hil/hil_posix.cpp | 873 ------------------ src/drivers/hil/module.mk | 4 - src/drivers/rgbled/module.mk | 4 - src/drivers/rgbled/rgbled.cpp | 101 +- src/modules/commander/module.mk | 8 +- .../commander/state_machine_helper.cpp | 192 ++-- ...avlink_main_nuttx.cpp => mavlink_main.cpp} | 112 ++- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_main_posix.cpp | 2 +- src/modules/mavlink/module.mk | 8 +- src/modules/uORB/MuORB.cpp | 102 +- .../posix/drivers/accelsim/accelsim.cpp | 40 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 16 +- src/platforms/posix/drivers/barosim/baro.cpp | 10 +- .../posix/drivers/gyrosim/gyrosim.cpp | 40 +- src/platforms/px4_posix.h | 37 +- 28 files changed, 600 insertions(+), 1327 deletions(-) rename src/drivers/device/{cdev_nuttx.cpp => cdev.cpp} (86%) rename src/drivers/device/{device.cpp => device_posix.cpp} (100%) delete mode 100644 src/drivers/hil/hil_posix.cpp rename src/modules/mavlink/{mavlink_main_nuttx.cpp => mavlink_main.cpp} (95%) diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index 5e408cd544..9fae9240d7 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -9,7 +9,7 @@ param set CAL_ACC0_ID 1310720 param set CAL_ACC1_ID 1376256 param set CAL_MAG0_ID 196608 rgbled start -mavlink start +mavlink start -d /tmp/ttyS0 sensors start hil mode_pwm commander start diff --git a/src/drivers/blinkm/blinkm_posix.cpp b/src/drivers/blinkm/blinkm_posix.cpp index 5a139a710a..04d0e5bcb5 100644 --- a/src/drivers/blinkm/blinkm_posix.cpp +++ b/src/drivers/blinkm/blinkm_posix.cpp @@ -140,7 +140,7 @@ public: virtual int init(); virtual int probe(); virtual int setMode(int mode); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); static const char *const script_names[]; @@ -358,7 +358,7 @@ BlinkM::probe() } int -BlinkM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = ENOTTY; diff --git a/src/drivers/device/cdev_nuttx.cpp b/src/drivers/device/cdev.cpp similarity index 86% rename from src/drivers/device/cdev_nuttx.cpp rename to src/drivers/device/cdev.cpp index 6db0400373..3bc05b0c73 100644 --- a/src/drivers/device/cdev_nuttx.cpp +++ b/src/drivers/device/cdev.cpp @@ -60,13 +60,13 @@ static const unsigned pollset_increment = 0; * The standard NuttX operation dispatch table can't call C++ member functions * directly, so we have to bounce them through this dispatch table. */ -static int cdev_open(struct file *filp); -static int cdev_close(struct file *filp); -static ssize_t cdev_read(struct file *filp, char *buffer, size_t buflen); -static ssize_t cdev_write(struct file *filp, const char *buffer, size_t buflen); -static off_t cdev_seek(struct file *filp, off_t offset, int whence); -static int cdev_ioctl(struct file *filp, int cmd, unsigned long arg); -static int cdev_poll(struct file *filp, struct pollfd *fds, bool setup); +static int cdev_open(file_t *filp); +static int cdev_close(file_t *filp); +static ssize_t cdev_read(file_t *filp, char *buffer, size_t buflen); +static ssize_t cdev_write(file_t *filp, const char *buffer, size_t buflen); +static off_t cdev_seek(file_t *filp, off_t offset, int whence); +static int cdev_ioctl(file_t *filp, int cmd, unsigned long arg); +static int cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup); /** * Character device indirection table. @@ -169,7 +169,7 @@ out: * Default implementations of the character device interface */ int -CDev::open(struct file *filp) +CDev::open(file_t *filp) { int ret = OK; @@ -192,13 +192,13 @@ CDev::open(struct file *filp) } int -CDev::open_first(struct file *filp) +CDev::open_first(file_t *filp) { return OK; } int -CDev::close(struct file *filp) +CDev::close(file_t *filp) { int ret = OK; @@ -222,31 +222,31 @@ CDev::close(struct file *filp) } int -CDev::close_last(struct file *filp) +CDev::close_last(file_t *filp) { return OK; } ssize_t -CDev::read(struct file *filp, char *buffer, size_t buflen) +CDev::read(file_t *filp, char *buffer, size_t buflen) { return -ENOSYS; } ssize_t -CDev::write(struct file *filp, const char *buffer, size_t buflen) +CDev::write(file_t *filp, const char *buffer, size_t buflen) { return -ENOSYS; } off_t -CDev::seek(struct file *filp, off_t offset, int whence) +CDev::seek(file_t *filp, off_t offset, int whence) { return -ENOSYS; } int -CDev::ioctl(struct file *filp, int cmd, unsigned long arg) +CDev::ioctl(file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -275,7 +275,7 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) } int -CDev::poll(struct file *filp, struct pollfd *fds, bool setup) +CDev::poll(file_t *filp, struct pollfd *fds, bool setup) { int ret = OK; @@ -347,7 +347,7 @@ CDev::poll_notify_one(struct pollfd *fds, pollevent_t events) } pollevent_t -CDev::poll_state(struct file *filp) +CDev::poll_state(file_t *filp) { /* by default, no poll events to report */ return 0; @@ -389,7 +389,7 @@ CDev::remove_poll_waiter(struct pollfd *fds) } static int -cdev_open(struct file *filp) +cdev_open(file_t *filp) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -397,7 +397,7 @@ cdev_open(struct file *filp) } static int -cdev_close(struct file *filp) +cdev_close(file_t *filp) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -405,7 +405,7 @@ cdev_close(struct file *filp) } static ssize_t -cdev_read(struct file *filp, char *buffer, size_t buflen) +cdev_read(file_t *filp, char *buffer, size_t buflen) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -413,7 +413,7 @@ cdev_read(struct file *filp, char *buffer, size_t buflen) } static ssize_t -cdev_write(struct file *filp, const char *buffer, size_t buflen) +cdev_write(file_t *filp, const char *buffer, size_t buflen) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -421,7 +421,7 @@ cdev_write(struct file *filp, const char *buffer, size_t buflen) } static off_t -cdev_seek(struct file *filp, off_t offset, int whence) +cdev_seek(file_t *filp, off_t offset, int whence) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -429,7 +429,7 @@ cdev_seek(struct file *filp, off_t offset, int whence) } static int -cdev_ioctl(struct file *filp, int cmd, unsigned long arg) +cdev_ioctl(file_t *filp, int cmd, unsigned long arg) { CDev *cdev = (CDev *)(filp->f_inode->i_private); @@ -437,7 +437,7 @@ cdev_ioctl(struct file *filp, int cmd, unsigned long arg) } static int -cdev_poll(struct file *filp, struct pollfd *fds, bool setup) +cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup) { CDev *cdev = (CDev *)(filp->f_inode->i_private); diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index 3d5737cee4..a3a0640d2d 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -44,6 +44,7 @@ * Includes here should only cover the needs of the framework definitions. */ #include +#include #include #include @@ -59,6 +60,8 @@ namespace device __EXPORT { +typedef struct file file_t; + /** * Fundamental base class for all device drivers. * @@ -275,7 +278,7 @@ public: * @param filp Pointer to the NuttX file structure. * @return OK if the open is allowed, -errno otherwise. */ - virtual int open(struct file *filp); + virtual int open(file_t *filp); /** * Handle a close of the device. @@ -286,7 +289,7 @@ public: * @param filp Pointer to the NuttX file structure. * @return OK if the close was successful, -errno otherwise. */ - virtual int close(struct file *filp); + virtual int close(file_t *filp); /** * Perform a read from the device. @@ -298,7 +301,7 @@ public: * @param buflen The number of bytes to be read. * @return The number of bytes read or -errno otherwise. */ - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t read(file_t *filp, char *buffer, size_t buflen); /** * Perform a write to the device. @@ -310,7 +313,7 @@ public: * @param buflen The number of bytes to be written. * @return The number of bytes written or -errno otherwise. */ - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual ssize_t write(file_t *filp, const char *buffer, size_t buflen); /** * Perform a logical seek operation on the device. @@ -322,7 +325,7 @@ public: * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. * @return The previous offset, or -errno otherwise. */ - virtual off_t seek(struct file *filp, off_t offset, int whence); + virtual off_t seek(file_t *filp, off_t offset, int whence); /** * Perform an ioctl operation on the device. @@ -336,7 +339,7 @@ public: * @param arg The ioctl argument value. * @return OK on success, or -errno otherwise. */ - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(file_t *filp, int cmd, unsigned long arg); /** * Perform a poll setup/teardown operation. @@ -349,7 +352,7 @@ public: * it is being torn down. * @return OK on success, or -errno otherwise. */ - virtual int poll(struct file *filp, struct pollfd *fds, bool setup); + virtual int poll(file_t *filp, struct pollfd *fds, bool setup); /** * Test whether the device is currently open. @@ -380,7 +383,7 @@ protected: * @param filp The file that's interested. * @return The current set of poll events. */ - virtual pollevent_t poll_state(struct file *filp); + virtual pollevent_t poll_state(file_t *filp); /** * Report new poll events. @@ -398,7 +401,7 @@ protected: * @param fds A poll waiter to notify. * @param events The event(s) to send to the waiter. */ - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); /** * Notification of the first open. @@ -411,7 +414,7 @@ protected: * @param filp Pointer to the NuttX file structure. * @return OK if the open should proceed, -errno otherwise. */ - virtual int open_first(struct file *filp); + virtual int open_first(file_t *filp); /** * Notification of the last close. @@ -424,7 +427,7 @@ protected: * @param filp Pointer to the NuttX file structure. * @return OK if the open should return OK, -errno otherwise. */ - virtual int close_last(struct file *filp); + virtual int close_last(file_t *filp); /** * Register a class device name, automatically adding device @@ -470,7 +473,7 @@ private: * * @return OK, or -errno on error. */ - int store_poll_waiter(struct pollfd *fds); + int store_poll_waiter(px4_pollfd_struct_t *fds); /** * Remove a poll waiter. diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device_posix.cpp similarity index 100% rename from src/drivers/device/device.cpp rename to src/drivers/device/device_posix.cpp diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index 51e8e2083e..c04cfaafb3 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -221,7 +221,7 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) return ret; } -int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) { //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; @@ -231,11 +231,11 @@ int I2C::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) return 0; default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } -ssize_t I2C::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) { if (simulate) { // FIXME no idea what this should be @@ -246,7 +246,7 @@ ssize_t I2C::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) return ::read(_fd, buffer, buflen); } -ssize_t I2C::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) { if (simulate) { warnx ("2C SIM I2C::write"); diff --git a/src/drivers/device/i2c_posix.h b/src/drivers/device/i2c_posix.h index e01e3c242d..c05100ae43 100644 --- a/src/drivers/device/i2c_posix.h +++ b/src/drivers/device/i2c_posix.h @@ -93,9 +93,9 @@ protected: virtual int init(); - virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(file_t *handlep, char *buffer, size_t buflen); + virtual ssize_t write(file_t *handlep, const char *buffer, size_t buflen); + virtual int ioctl(file_t *handlep, int cmd, unsigned long arg); /** * Perform an I2C transaction to the device. diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index f7883568e9..22e7b15622 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -38,14 +38,15 @@ ifeq ($(PX4_TARGET_OS),nuttx) SRCS = \ device_nuttx.cpp \ - cdev_nuttx.cpp \ + cdev.cpp \ i2c_nuttx.cpp \ pio.cpp \ spi.cpp \ ringbuffer.cpp else -SRCS = vdev.cpp \ - device.cpp \ +SRCS = \ + device_posix.cpp \ + vdev.cpp \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 0c1dc3abe9..699bc817b1 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -207,7 +207,7 @@ out: * Default implementations of the character device interface */ int -VDev::open(px4_dev_handle_t *handlep) +VDev::open(file_t *filep) { int ret = PX4_OK; @@ -219,7 +219,7 @@ VDev::open(px4_dev_handle_t *handlep) if (_open_count == 1) { /* first-open callback may decline the open */ - ret = open_first(handlep); + ret = open_first(filep); if (ret != PX4_OK) _open_count--; @@ -231,14 +231,14 @@ VDev::open(px4_dev_handle_t *handlep) } int -VDev::open_first(px4_dev_handle_t *handlep) +VDev::open_first(file_t *filep) { debug("VDev::open_first"); return PX4_OK; } int -VDev::close(px4_dev_handle_t *handlep) +VDev::close(file_t *filep) { debug("VDev::close"); int ret = PX4_OK; @@ -251,7 +251,7 @@ VDev::close(px4_dev_handle_t *handlep) /* callback cannot decline the close */ if (_open_count == 0) - ret = close_last(handlep); + ret = close_last(filep); } else { ret = -EBADF; @@ -263,34 +263,34 @@ VDev::close(px4_dev_handle_t *handlep) } int -VDev::close_last(px4_dev_handle_t *handlep) +VDev::close_last(file_t *filep) { debug("VDev::close_last"); return PX4_OK; } ssize_t -VDev::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +VDev::read(file_t *filep, char *buffer, size_t buflen) { debug("VDev::read"); return -ENOSYS; } ssize_t -VDev::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +VDev::write(file_t *filep, const char *buffer, size_t buflen) { debug("VDev::write"); return -ENOSYS; } off_t -VDev::seek(px4_dev_handle_t *handlep, off_t offset, int whence) +VDev::seek(file_t *filep, off_t offset, int whence) { return -ENOSYS; } int -VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) +VDev::ioctl(file_t *filep, int cmd, unsigned long arg) { int ret = -ENOTTY; @@ -320,7 +320,7 @@ VDev::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) } int -VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) +VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) { int ret = PX4_OK; debug("VDev::Poll %s", setup ? "setup" : "teardown"); @@ -335,8 +335,8 @@ VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * Save the file pointer in the pollfd for the subclass' * benefit. */ - fds->priv = (void *)handlep; - debug("VDev::poll: fds->priv = %p", handlep); + fds->priv = (void *)filep; + debug("VDev::poll: fds->priv = %p", filep); /* * Handle setup requests. @@ -349,7 +349,7 @@ VDev::poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup) * Check to see whether we should send a poll notification * immediately. */ - fds->revents |= fds->events & poll_state(handlep); + fds->revents |= fds->events & poll_state(filep); /* yes? post the notification */ if (fds->revents != 0) @@ -402,7 +402,7 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) } pollevent_t -VDev::poll_state(px4_dev_handle_t *handlep) +VDev::poll_state(file_t *filep) { debug("VDev::poll_notify"); /* by default, no poll events to report */ diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 6448b8aa98..3cf0a3f691 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -57,17 +57,14 @@ namespace device __EXPORT { -#define PX4_F_RDONLY 1 -#define PX4_F_WRONLY 2 - -struct px4_dev_handle_t { +struct file_t { int fd; int flags; void *priv; void *vdev; - px4_dev_handle_t() : fd(-1), flags(0), priv(NULL), vdev(NULL) {} - px4_dev_handle_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), vdev(c) {} + file_t() : fd(-1), flags(0), priv(NULL), vdev(NULL) {} + file_t(int f, void *c, int d) : fd(d), flags(f), priv(NULL), vdev(c) {} }; /** @@ -242,13 +239,13 @@ public: * * This is handled internally and should not normally be overridden. * - * @param handlep Pointer to the internal file structure. + * @param filep Pointer to the internal file structure. * @param fds Poll descriptor being waited on. * @param setup True if this is establishing a request, false if * it is being torn down. * @return OK on success, or -errno otherwise. */ - virtual int poll(px4_dev_handle_t *handlep, px4_pollfd_struct_t *fds, bool setup); + virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup); /** * Test whether the device is currently open. @@ -266,10 +263,10 @@ public: * This function is called for every open of the device. The default * implementation maintains _open_count and always returns OK. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the open is allowed, -errno otherwise. */ - virtual int open(px4_dev_handle_t *handlep); + virtual int open(file_t *filep); /** * Handle a close of the device. @@ -277,46 +274,46 @@ public: * This function is called for every close of the device. The default * implementation maintains _open_count and returns OK as long as it is not zero. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the close was successful, -errno otherwise. */ - virtual int close(px4_dev_handle_t *handlep); + virtual int close(file_t *filep); /** * Perform a read from the device. * * The default implementation returns -ENOSYS. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param buffer Pointer to the buffer into which data should be placed. * @param buflen The number of bytes to be read. * @return The number of bytes read or -errno otherwise. */ - virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual ssize_t read(file_t *filep, char *buffer, size_t buflen); /** * Perform a write to the device. * * The default implementation returns -ENOSYS. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param buffer Pointer to the buffer from which data should be read. * @param buflen The number of bytes to be written. * @return The number of bytes written or -errno otherwise. */ - virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); + virtual ssize_t write(file_t *filep, const char *buffer, size_t buflen); /** * Perform a logical seek operation on the device. * * The default implementation returns -ENOSYS. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param offset The new file position relative to whence. * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. * @return The previous offset, or -errno otherwise. */ - virtual off_t seek(px4_dev_handle_t *handlep, off_t offset, int whence); + virtual off_t seek(file_t *filep, off_t offset, int whence); /** * Perform an ioctl operation on the device. @@ -325,12 +322,12 @@ public: * returns -ENOTTY. Subclasses should call the default implementation * for any command they do not handle themselves. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @param cmd The ioctl command value. * @param arg The ioctl argument value. * @return OK on success, or -errno otherwise. */ - virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(file_t *filep, int cmd, unsigned long arg); static VDev *getDev(const char *path); static void showDevices(void); @@ -352,10 +349,10 @@ protected: * * The default implementation returns no events. * - * @param handlep The file that's interested. + * @param filep The file that's interested. * @return The current set of poll events. */ - virtual pollevent_t poll_state(px4_dev_handle_t *handlep); + virtual pollevent_t poll_state(file_t *filep); /** * Report new poll events. @@ -383,10 +380,10 @@ protected: * * The default implementation returns OK. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the open should proceed, -errno otherwise. */ - virtual int open_first(px4_dev_handle_t *handlep); + virtual int open_first(file_t *filep); /** * Notification of the last close. @@ -396,10 +393,10 @@ protected: * * The default implementation returns OK. * - * @param handlep Pointer to the NuttX file structure. + * @param filep Pointer to the NuttX file structure. * @return OK if the open should return OK, -errno otherwise. */ - virtual int close_last(px4_dev_handle_t *handlep); + virtual int close_last(file_t *filep); /** * Register a class device name, automatically adding device diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 58b445d607..e0e643ae2c 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file vcdev_posix.cpp + * @file vdev_posix.cpp * * POSIX-like API for virtual character device */ @@ -47,6 +47,8 @@ #include #include +#define PX4_DEBUG(...) + using namespace device; extern "C" { @@ -74,7 +76,7 @@ static void *timer_handler(void *data) } #define PX4_MAX_FD 100 -static px4_dev_handle_t *filemap[PX4_MAX_FD] = {}; +static device::file_t *filemap[PX4_MAX_FD] = {}; int px4_errno; @@ -95,7 +97,7 @@ int px4_open(const char *path, int flags) else { for (i=0; i +#include +#include +#include #include #include @@ -60,11 +63,10 @@ #include #include #include -#include +#include +#include #include -#include - #include #include #include @@ -82,21 +84,25 @@ #include +#ifdef __PX4_NUTTX class HIL : public device::CDev +#else +class HIL : public device::VDev +#endif { public: enum Mode { MODE_2PWM, MODE_4PWM, - MODE_8PWM, - MODE_12PWM, - MODE_16PWM, + MODE_8PWM, + MODE_12PWM, + MODE_16PWM, MODE_NONE }; HIL(); virtual ~HIL(); - virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int init(); @@ -131,7 +137,7 @@ private: uint8_t control_index, float &input); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); + int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); struct GPIOConfig { uint32_t input; @@ -146,7 +152,7 @@ private: void gpio_set_function(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function); uint32_t gpio_read(void); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); + int gpio_ioctl(device::file_t *filp, int cmd, unsigned long arg); }; @@ -158,7 +164,12 @@ HIL *g_hil; } // namespace HIL::HIL() : - CDev("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), +#ifdef __PX4_NUTTX + CDev( +#else + VDev( +#endif + "hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), _mode(MODE_NONE), _update_rate(50), _current_update_rate(0), @@ -188,7 +199,7 @@ HIL::~HIL() /* if we have given up, kill it */ if (--i == 0) { - task_delete(_task); + px4_task_delete(_task); break; } @@ -211,7 +222,11 @@ HIL::init() ASSERT(_task == -1); /* do regular cdev init */ +#ifdef __PX4_NUTTX ret = CDev::init(); +#else + ret = VDev::init(); +#endif if (ret != OK) return ret; @@ -228,11 +243,11 @@ HIL::init() // gpio_reset(); /* start the HIL interface task */ - _task = task_spawn_cmd("fmuhil", + _task = px4_task_spawn_cmd("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1200, - (main_t)&HIL::task_main_trampoline, + (px4_main_t)&HIL::task_main_trampoline, nullptr); if (_task < 0) { @@ -273,22 +288,22 @@ HIL::set_mode(Mode mode) break; case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + break; - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + break; - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + break; case MODE_NONE: debug("MODE_NONE"); @@ -336,7 +351,7 @@ HIL::task_main() _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &dummy, ORB_PRIO_LOW); - pollfd fds[2]; + px4_pollfd_struct_t fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_armed; @@ -383,7 +398,7 @@ HIL::task_main() } /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + int ret = px4_poll(&fds[0], 2, 1000); /* this would be bad... */ if (ret < 0) { @@ -410,7 +425,7 @@ HIL::task_main() /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && - isfinite(outputs.output[i]) && + PX4_ISFINITE(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { /* scale for PWM output 900 - 2100us */ @@ -439,8 +454,8 @@ HIL::task_main() } } - ::close(_t_actuators); - ::close(_t_armed); + px4_close(_t_actuators); + px4_close(_t_armed); /* make sure servos are off */ // up_pwm_servo_deinit(); @@ -449,7 +464,6 @@ HIL::task_main() /* tell the dtor that we are exiting */ _task = -1; - _exit(0); } int @@ -465,7 +479,7 @@ HIL::control_callback(uintptr_t handle, } int -HIL::ioctl(file *filp, int cmd, unsigned long arg) +HIL::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret; @@ -480,9 +494,9 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg) switch(_mode) { case MODE_2PWM: case MODE_4PWM: - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: ret = HIL::pwm_ioctl(filp, cmd, arg); break; default: @@ -492,14 +506,19 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg) } /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { +#ifdef __PX4_NUTTX ret = CDev::ioctl(filp, cmd, arg); +#else + ret = VDev::ioctl(filp, cmd, arg); +#endif + } return ret; } int -HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) +HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = OK; // int channel; @@ -675,8 +694,8 @@ hil_new_mode(PortMode new_mode) case PORT_MODE_UNDEFINED: case PORT1_MODE_UNSET: case PORT2_MODE_UNSET: - /* nothing more to do here */ - break; + /* nothing more to do here */ + break; case PORT1_FULL_PWM: /* select 4-pin PWM mode */ @@ -695,20 +714,20 @@ hil_new_mode(PortMode new_mode) servo_mode = HIL::MODE_2PWM; break; - case PORT2_8PWM: - /* select 8-pin PWM mode */ - servo_mode = HIL::MODE_8PWM; - break; + case PORT2_8PWM: + /* select 8-pin PWM mode */ + servo_mode = HIL::MODE_8PWM; + break; - case PORT2_12PWM: - /* select 12-pin PWM mode */ - servo_mode = HIL::MODE_12PWM; - break; + case PORT2_12PWM: + /* select 12-pin PWM mode */ + servo_mode = HIL::MODE_12PWM; + break; - case PORT2_16PWM: - /* select 16-pin PWM mode */ - servo_mode = HIL::MODE_16PWM; - break; + case PORT2_16PWM: + /* select 16-pin PWM mode */ + servo_mode = HIL::MODE_16PWM; + break; } // /* adjust GPIO config for serial mode(s) */ @@ -746,32 +765,32 @@ hil_start(void) return ret; } -void +int test(void) { int fd; - fd = open(PWM_OUTPUT0_DEVICE_PATH, 0); + fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); if (fd < 0) { puts("open fail"); - exit(1); + return -ENODEV; } - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); + px4_ioctl(fd, PWM_SERVO_ARM, 0); + px4_ioctl(fd, PWM_SERVO_SET(0), 1000); - close(fd); + px4_close(fd); - exit(0); + return OK; } -void +int fake(int argc, char *argv[]) { if (argc < 5) { puts("hil fake (values -100 .. 100)"); - exit(1); + return -EINVAL; } actuator_controls_s ac; @@ -788,24 +807,39 @@ fake(int argc, char *argv[]) if (handle < 0) { puts("advertise failed"); - exit(1); + return 1; } - exit(0); + return 0; } } // namespace extern "C" __EXPORT int hil_main(int argc, char *argv[]); +static void +usage() { + fprintf(stderr, "HIL: unrecognized command, try:\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); +} + int hil_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb = argv[1]; + const char *verb; + int ret = OK; - if (hil_start() != OK) - errx(1, "failed to start the HIL driver"); + if (hil_start() != OK) { + warnx("failed to start the HIL driver"); + return 1; + } + + if (argc < 2) { + usage(); + return -EINVAL; + } + verb = argv[1]; /* * Mode switches. @@ -842,14 +876,17 @@ hil_main(int argc, char *argv[]) return hil_new_mode(new_mode); } - if (!strcmp(verb, "test")) - test(); + if (!strcmp(verb, "test")) { + ret = test(); + } - if (!strcmp(verb, "fake")) - fake(argc - 1, argv + 1); + else if (!strcmp(verb, "fake")) { + ret = fake(argc - 1, argv + 1); + } - - fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); - return -EINVAL; + else { + usage(); + ret = -EINVAL; + } + return ret; } diff --git a/src/drivers/hil/hil_posix.cpp b/src/drivers/hil/hil_posix.cpp deleted file mode 100644 index eb86a49742..0000000000 --- a/src/drivers/hil/hil_posix.cpp +++ /dev/null @@ -1,873 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file hil_linux.cpp - * - * Driver/configurator for the virtual HIL port. - * - * This virtual driver emulates PWM / servo outputs for setups where - * the connected hardware does not provide enough or no PWM outputs. - * - * Its only function is to take actuator_control uORB messages, - * mix them with any loaded mixer and output the result to the - * actuator_output uORB topic. HIL can also be performed with normal - * PWM outputs, a special flag prevents the outputs to be operated - * during HIL mode. If HIL is not performed with a standalone FMU, - * but in a real system, it is NOT recommended to use this virtual - * driver. Use instead the normal FMU or IO driver. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include - -class HIL : public device::VDev -{ -public: - enum Mode { - MODE_2PWM, - MODE_4PWM, - MODE_8PWM, - MODE_12PWM, - MODE_16PWM, - MODE_NONE - }; - HIL(); - virtual ~HIL(); - - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - - virtual int init(); - - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); - -private: - static const unsigned _max_actuators = 4; - - Mode _mode; - int _update_rate; - int _current_update_rate; - int _task; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; - unsigned _num_outputs; - bool _primary_pwm_device; - - volatile bool _task_should_exit; - bool _armed; - - MixerGroup *_mixers; - - actuator_controls_s _controls; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main(); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int pwm_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; - - void gpio_reset(void); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - -}; - -namespace -{ - -HIL *g_hil; - -} // namespace - -HIL::HIL() : - VDev("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), - _mode(MODE_NONE), - _update_rate(50), - _current_update_rate(0), - _task(-1), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), - _num_outputs(0), - _primary_pwm_device(false), - _task_should_exit(false), - _armed(false), - _mixers(nullptr) -{ - _debug_enabled = true; -} - -HIL::~HIL() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - px4_task_delete(_task); - break; - } - - } while (_task != -1); - } - - // XXX already claimed with CDEV - // /* clean up the alternate device node */ - // if (_primary_pwm_device) - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - g_hil = nullptr; -} - -int -HIL::init() -{ - int ret; - - ASSERT(_task == -1); - - /* do regular cdev init */ - ret = VDev::init(); - - if (ret != PX4_OK) - return ret; - - // XXX already claimed with CDEV - ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); - if (ret == PX4_OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } - - /* reset GPIOs */ - // gpio_reset(); - - /* start the HIL interface task */ - _task = px4_task_spawn_cmd("fmuhil", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1200, - (px4_main_t)&HIL::task_main_trampoline, - nullptr); - - if (_task < 0) { - debug("task start failed: %d", errno); - return -errno; - } - - return PX4_OK; -} - -void -HIL::task_main_trampoline(int argc, char *argv[]) -{ - g_hil->task_main(); -} - -int -HIL::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - debug("MODE_2PWM"); - /* multi-port with flow control lines as PWM */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_4PWM: - debug("MODE_4PWM"); - /* multi-port as 4 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; - - case MODE_NONE: - debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - _update_rate = 10; - break; - - default: - return -EINVAL; - } - _mode = mode; - return PX4_OK; -} - -int -HIL::set_pwm_rate(unsigned rate) -{ - if ((rate > 500) || (rate < 10)) - return -EINVAL; - - _update_rate = rate; - return PX4_OK; -} - -void -HIL::task_main() -{ - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); - /* force a reset of the update rate */ - _current_update_rate = 0; - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - int dummy; - _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_LOW); - - px4_pollfd_struct_t fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - - unsigned num_outputs; - - /* select the number of virtual outputs */ - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - // XXX only support the lower 8 - trivial to extend - num_outputs = 8; - break; - - case MODE_NONE: - default: - num_outputs = 0; - break; - } - - log("starting"); - - /* loop until killed */ - while (!_task_should_exit) { - - /* handle update rate changes */ - if (_current_update_rate != _update_rate) { - int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) - update_rate_in_ms = 2; - orb_set_interval(_t_actuators, update_rate_in_ms); - // up_pwm_servo_set_rate(_update_rate); - _current_update_rate = _update_rate; - } - - /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&fds[0], 2, 1000); - - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - continue; - } - - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; - } - } - - /* and publish for anyone that cares to see */ - orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); - } - } - - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; - - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - } - } - - px4_close(_t_actuators); - px4_close(_t_armed); - - /* make sure servos are off */ - // up_pwm_servo_deinit(); - - /* note - someone else is responsible for restoring the GPIO config */ - - /* tell the dtor that we are exiting */ - _task = -1; -} - -int -HIL::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls->control[control_index]; - return 0; -} - -int -HIL::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) -{ - int ret; - - debug("ioctl 0x%04x 0x%08x", cmd, arg); - - // /* try it as a GPIO ioctl first */ - // ret = HIL::gpio_ioctl(handlep, cmd, arg); - // if (ret != -ENOTTY) - // return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - ret = HIL::pwm_ioctl(handlep, cmd, arg); - break; - default: - ret = -ENOTTY; - debug("not in a PWM mode"); - break; - } - - /* if nobody wants it, let VDev have it */ - if (ret == -ENOTTY) - ret = VDev::ioctl(handlep, cmd, arg); - - return ret; -} - -int -HIL::pwm_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) -{ - int ret = PX4_OK; - // int channel; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - // up_pwm_servo_arm(true); - break; - - case PWM_SERVO_DISARM: - // up_pwm_servo_arm(false); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - g_hil->set_pwm_rate(arg); - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate - break; - - case PWM_SERVO_SET(2): - case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(0): - case PWM_SERVO_SET(1): - if (arg < 2100) { - // channel = cmd - PWM_SERVO_SET(0); -// up_pwm_servo_set(channel, arg); XXX - - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(2): - case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - // channel = cmd - PWM_SERVO_SET(0); - // *(servo_position_t *)arg = up_pwm_servo_get(channel); - break; - } - - case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - - *(uint32_t *)arg = (1 << channel); - break; - } - - case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - - } else { - *(unsigned *)arg = 2; - } - - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - break; - - case MIXERIOCADDSIMPLE: { - mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - - SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); - - if (mixer->check()) { - delete mixer; - ret = -EINVAL; - - } else { - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); - - _mixers->add_mixer(mixer); - } - - break; - } - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - debug("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - break; - } - - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -namespace -{ - -enum PortMode { - PORT_MODE_UNDEFINED = 0, - PORT1_MODE_UNSET, - PORT1_FULL_PWM, - PORT1_PWM_AND_SERIAL, - PORT1_PWM_AND_GPIO, - PORT2_MODE_UNSET, - PORT2_8PWM, - PORT2_12PWM, - PORT2_16PWM, -}; - -PortMode g_port_mode; - -int -hil_new_mode(PortMode new_mode) -{ - // uint32_t gpio_bits; - - -// /* reset to all-inputs */ -// g_hil->ioctl(0, GPIO_RESET, 0); - - // gpio_bits = 0; - - HIL::Mode servo_mode = HIL::MODE_NONE; - - switch (new_mode) { - case PORT_MODE_UNDEFINED: - case PORT1_MODE_UNSET: - case PORT2_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT1_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = HIL::MODE_4PWM; - break; - - case PORT1_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; -// /* set RX/TX multi-GPIOs to serial mode */ -// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT1_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = HIL::MODE_2PWM; - break; - - case PORT2_8PWM: - /* select 8-pin PWM mode */ - servo_mode = HIL::MODE_8PWM; - break; - - case PORT2_12PWM: - /* select 12-pin PWM mode */ - servo_mode = HIL::MODE_12PWM; - break; - - case PORT2_16PWM: - /* select 16-pin PWM mode */ - servo_mode = HIL::MODE_16PWM; - break; - } - -// /* adjust GPIO config for serial mode(s) */ -// if (gpio_bits != 0) -// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - - /* (re)set the PWM output mode */ - g_hil->set_mode(servo_mode); - - return PX4_OK; -} - -int -hil_start(void) -{ - int ret = PX4_OK; - - if (g_hil == nullptr) { - - g_hil = new HIL; - - if (g_hil == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_hil->init(); - - if (ret != PX4_OK) { - delete g_hil; - g_hil = nullptr; - } - } - } - - return ret; -} - -int -test(void) -{ - int fd; - - fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); - - if (fd < 0) { - puts("open fail"); - return -ENODEV; - } - - px4_ioctl(fd, PWM_SERVO_ARM, 0); - px4_ioctl(fd, PWM_SERVO_SET(0), 1000); - - px4_close(fd); - - return PX4_OK; -} - -int -fake(int argc, char *argv[]) -{ - if (argc < 5) { - puts("hil fake (values -100 .. 100)"); - return -EINVAL; - } - - actuator_controls_s ac; - - ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; - - ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; - - ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; - - ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - - orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - - if (handle < 0) { - puts("advertise failed"); - return 1; - } - - return 0; -} - -} // namespace - -extern "C" __EXPORT int hil_main(int argc, char *argv[]); - -void -usage() { - fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); -} - -int -hil_main(int argc, char *argv[]) -{ - PortMode new_mode = PORT_MODE_UNDEFINED; - const char *verb; - int ret = PX4_OK; - - if (hil_start() != PX4_OK) { - warnx("failed to start the HIL driver"); - return 1; - } - - if (argc < 2) { - usage(); - return -EINVAL; - } - verb = argv[1]; - /* - * Mode switches. - */ - - // this was all cut-and-pasted from the FMU driver; it's junk - if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT1_FULL_PWM; - - } else if (!strcmp(verb, "mode_pwm_serial")) { - new_mode = PORT1_PWM_AND_SERIAL; - - } else if (!strcmp(verb, "mode_pwm_gpio")) { - new_mode = PORT1_PWM_AND_GPIO; - - } else if (!strcmp(verb, "mode_port2_pwm8")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm12")) { - new_mode = PORT2_8PWM; - - } else if (!strcmp(verb, "mode_port2_pwm16")) { - new_mode = PORT2_8PWM; - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNDEFINED) { - - /* yes but it's the same mode */ - if (new_mode == g_port_mode) - return PX4_OK; - - /* switch modes */ - return hil_new_mode(new_mode); - } - - if (!strcmp(verb, "test")) { - ret = test(); - } - - else if (!strcmp(verb, "fake")) { - ret = fake(argc - 1, argv + 1); - } - - else { - usage(); - ret = -EINVAL; - } - return ret; -} diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index b168108b3e..77213b4be7 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -37,10 +37,6 @@ MODULE_COMMAND = hil -ifeq ($(PX4_TARGET_OS),nuttx) SRCS = hil.cpp MAXOPTIMIZATION = -Os -else -SRCS = hil_posix.cpp -endif diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index f756250071..924624cebb 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,10 +4,6 @@ MODULE_COMMAND = rgbled -ifeq ($(PX4_TARGET_OS),nuttx) SRCS = rgbled.cpp -else -SRCS = rgbled_posix.cpp -endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 999983467b..85049abc1f 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -41,6 +41,7 @@ */ #include +#include #include @@ -54,7 +55,7 @@ #include #include -#include +#include #include #include @@ -88,7 +89,7 @@ public: virtual int init(); virtual int probe(); virtual int info(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: work_s _work; @@ -129,7 +130,11 @@ void rgbled_usage(); extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : - I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000 /* maximum speed supported */), + I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled +#ifdef __PX4_NUTTX + ,100000 /* maximum speed supported */ +#endif + ), _mode(RGBLED_MODE_OFF), _r(0), _g(0), @@ -218,7 +223,7 @@ RGBLED::info() } int -RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) +RGBLED::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = ENOTTY; @@ -249,7 +254,11 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* see if the parent class can make any use of it */ +#ifdef __PX4_NUTTX ret = CDev::ioctl(filp, cmd, arg); +#else + ret = VDev::ioctl(filp, cmd, arg); +#endif break; } @@ -582,35 +591,39 @@ rgbled_main(int argc, char *argv[]) int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': - rgbledadr = strtol(optarg, NULL, 0); + rgbledadr = strtol(myoptarg, NULL, 0); break; case 'b': - i2cdevice = strtol(optarg, NULL, 0); + i2cdevice = strtol(myoptarg, NULL, 0); break; default: rgbled_usage(); - exit(0); + return 1; } } - if (optind >= argc) { + if (myoptind >= argc) { rgbled_usage(); - exit(1); + return 1; } - const char *verb = argv[optind]; + const char *verb = argv[myoptind]; int fd; int ret; if (!strcmp(verb, "start")) { - if (g_rgbled != nullptr) - errx(1, "already started"); + if (g_rgbled != nullptr) { + warnx("already started"); + return 1; + } if (i2cdevice == -1) { // try the external bus first @@ -625,7 +638,8 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { - errx(1, "init failed"); + warnx("init failed"); + return 1; } i2cdevice = PX4_I2C_BUS_LED; } @@ -634,88 +648,95 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { g_rgbled = new RGBLED(i2cdevice, rgbledadr); - if (g_rgbled == nullptr) - errx(1, "new failed"); + if (g_rgbled == nullptr) { + warnx("new failed"); + return 1; + } if (OK != g_rgbled->init()) { delete g_rgbled; g_rgbled = nullptr; - errx(1, "init failed"); + warnx("init failed"); + return 1; } } - exit(0); + return 0; } /* need the driver past this point */ if (g_rgbled == nullptr) { warnx("not started"); rgbled_usage(); - exit(1); + return 1; } if (!strcmp(verb, "test")) { - fd = open(RGBLED0_DEVICE_PATH, 0); + fd = px4_open(RGBLED0_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; } rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern }; - ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); + ret = px4_ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); - close(fd); - exit(ret); + px4_close(fd); + return ret; } if (!strcmp(verb, "info")) { g_rgbled->info(); - exit(0); + return 0; } if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { - fd = open(RGBLED0_DEVICE_PATH, 0); + fd = px4_open(RGBLED0_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; } - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); - close(fd); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + px4_close(fd); /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ if (!strcmp(verb, "stop")) { delete g_rgbled; g_rgbled = nullptr; - exit(0); + return 0; } - exit(ret); + return ret; } if (!strcmp(verb, "rgb")) { if (argc < 5) { - errx(1, "Usage: rgbled rgb "); + warnx("Usage: rgbled rgb "); + return 1; } - fd = open(RGBLED0_DEVICE_PATH, 0); + fd = px4_open(RGBLED0_DEVICE_PATH, 0); if (fd == -1) { - errx(1, "Unable to open " RGBLED0_DEVICE_PATH); + warnx("Unable to open " RGBLED0_DEVICE_PATH); + return 1; } rgbled_rgbset_t v; v.red = strtol(argv[2], NULL, 0); v.green = strtol(argv[3], NULL, 0); v.blue = strtol(argv[4], NULL, 0); - ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); - ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); - close(fd); - exit(ret); + ret = px4_ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); + ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + px4_close(fd); + return ret; } rgbled_usage(); - exit(0); + return 1; } diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 851ac9020d..f739e4caec 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -46,14 +46,8 @@ SRCS = commander.cpp \ accelerometer_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp \ - PreflightCheck.cpp - -ifdef ($(PX4_TARGET_OS),nuttx) -SRCS += + PreflightCheck.cpp \ state_machine_helper.cpp -else -SRCS += state_machine_helper_posix.cpp -endif MODULE_STACKSIZE = 5000 diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7410baca3e..3b9d5fdc75 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -39,6 +39,7 @@ * @author Julian Oes */ +#include #include #include #include @@ -129,10 +130,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s prearm_ret = prearm_check(status, mavlink_fd); } +#ifdef __PX4_NUTTX /* * Perform an atomic state update */ irqstate_t flags = irqsave(); +#endif /* enforce lockdown in HIL */ if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { @@ -236,8 +239,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s status->arming_state = new_arming_state; } +#ifdef __PX4_NUTTX /* end of atomic state update */ irqrestore(flags); +#endif } if (ret == TRANSITION_DENIED) { @@ -337,6 +342,121 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } +#ifdef PX4_NUTTX +static transition_result_t disable_publication(const int mavlink_fd) +{ + transition_result_t ret; + + /* Disable publication of all attached sensors */ + /* list directory */ + DIR *d; + d = opendir("/dev"); + + if (d) { + struct dirent *direntry; + char devname[24]; + + while ((direntry = readdir(d)) != NULL) { + + /* skip serial ports */ + if (!strncmp("tty", direntry->d_name, 3)) { + continue; + } + + /* skip mtd devices */ + if (!strncmp("mtd", direntry->d_name, 3)) { + continue; + } + + /* skip ram devices */ + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { + continue; + } + + /* skip mavlink */ + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + close(sensfd); + + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + closedir(d); + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + + + } else { + /* failed opening dir */ + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; + } + return ret; +} + +#else + +static transition_result_t disable_publication(const int mavlink_fd) +{ + transition_result_t ret; + const char *devname; + unsigned int handle = 0; + for(;;) { + devname = px4_get_device_names(&handle); + if (devname == NULL) + break; + + /* skip mavlink */ + if (!strcmp("/dev/mavlink", devname)) { + continue; + } + + + int sensfd = px4_open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); + px4_close(sensfd); + + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); + } + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + + return ret; +} +#endif + /** * Transition from one hil state to another */ @@ -359,77 +479,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - - /* Disable publication of all attached sensors */ - /* list directory */ - DIR *d; - d = opendir("/dev"); - - if (d) { - struct dirent *direntry; - char devname[24]; - - while ((direntry = readdir(d)) != NULL) { - - /* skip serial ports */ - if (!strncmp("tty", direntry->d_name, 3)) { - continue; - } - - /* skip mtd devices */ - if (!strncmp("mtd", direntry->d_name, 3)) { - continue; - } - - /* skip ram devices */ - if (!strncmp("ram", direntry->d_name, 3)) { - continue; - } - - /* skip MMC devices */ - if (!strncmp("mmc", direntry->d_name, 3)) { - continue; - } - - /* skip mavlink */ - if (!strcmp("mavlink", direntry->d_name)) { - continue; - } - - /* skip console */ - if (!strcmp("console", direntry->d_name)) { - continue; - } - - /* skip null */ - if (!strcmp("null", direntry->d_name)) { - continue; - } - - snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); - - int sensfd = ::open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); - continue; - } - - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - close(sensfd); - - printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); - } - closedir(d); - ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - - - } else { - /* failed opening dir */ - mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); - ret = TRANSITION_DENIED; - } + ret = disable_publication(mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; diff --git a/src/modules/mavlink/mavlink_main_nuttx.cpp b/src/modules/mavlink/mavlink_main.cpp similarity index 95% rename from src/modules/mavlink/mavlink_main_nuttx.cpp rename to src/modules/mavlink/mavlink_main.cpp index ce80953e2d..5aa3a10a5d 100644 --- a/src/modules/mavlink/mavlink_main_nuttx.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -50,7 +51,9 @@ #include #include #include +#ifndef __PX4_QURT #include +#endif #include #include /* isinf / isnan checks */ @@ -68,7 +71,7 @@ #include #include #include -#include +//#include #include #include @@ -96,8 +99,10 @@ static const int ERROR = -1; static Mavlink *_mavlink_instances = nullptr; +#ifdef __PX4_NUTTX /* TODO: if this is a class member it crashes */ static struct file_operations fops; +#endif static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; @@ -114,6 +119,9 @@ extern mavlink_system_t mavlink_system; static void usage(void); Mavlink::Mavlink() : +#ifndef __PX4_NUTTX + VDev("mavlink-log", MAVLINK_LOG_DEVICE), +#endif _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), next(nullptr), @@ -140,7 +148,9 @@ Mavlink::Mavlink() : _forwarding_on(false), _passing_on(false), _ftp_on(false), +#ifndef __PX4_QURT _uart_fd(-1), +#endif _baudrate(57600), _datarate(1000), _datarate_events(500), @@ -175,7 +185,9 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { +#ifdef __PX4_NUTTX fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; +#endif _instance_id = Mavlink::instance_count(); @@ -216,7 +228,8 @@ Mavlink::Mavlink() : #endif default: - errx(1, "instance ID is out of range"); + warnx("instance ID is out of range"); + px4_task_exit(1); break; } @@ -398,6 +411,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) } } +#ifndef __PX4_QURT int Mavlink::get_uart_fd(unsigned index) { @@ -415,6 +429,7 @@ Mavlink::get_uart_fd() { return _uart_fd; } +#endif // __PX4_QURT int Mavlink::get_instance_id() @@ -433,7 +448,11 @@ Mavlink::get_channel() ****************************************************************************/ int -Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +#ifdef __PX4_NUTTX +Mavlink::mavlink_dev_ioctl(struct file *filp, int cmd, unsigned long arg) +#else +Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg) +#endif { switch (cmd) { case (int)MAVLINK_IOC_SEND_TEXT_INFO: @@ -549,6 +568,7 @@ int Mavlink::get_component_id() return mavlink_system.compid; } +#ifndef __PX4_QURT int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -604,7 +624,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } /* open uart */ - _uart_fd = open(uart_name, O_RDWR | O_NOCTTY); + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); if (_uart_fd < 0) { return _uart_fd; @@ -619,7 +639,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); - close(_uart_fd); + ::close(_uart_fd); return -1; } @@ -635,7 +655,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); - close(_uart_fd); + ::close(_uart_fd); return -1; } @@ -643,7 +663,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); - close(_uart_fd); + ::close(_uart_fd); return -1; } @@ -653,7 +673,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * * which is not an issue, but requires a separate call so we can fail silently. */ (void)tcgetattr(_uart_fd, &uart_config); +#ifdef CRTS_IFLOW uart_config.c_cflag |= CRTS_IFLOW; +#else + uart_config.c_cflag |= CRTSCTS; +#endif (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); /* setup output flow control */ @@ -696,6 +720,8 @@ Mavlink::enable_flow_control(bool enabled) return ret; } +#endif + int Mavlink::set_hil_enabled(bool hil_enabled) { @@ -727,7 +753,13 @@ Mavlink::get_free_tx_buf() * flow control if it continues to be full */ int buf_free = 0; + +#ifndef __PX4_QURT + +// No FIONWRITE on Linux +#if !defined(__PX4_LINUX) (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#endif if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { /* Disable hardware flow control: @@ -741,6 +773,7 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } +#endif return buf_free; } @@ -795,8 +828,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); +#ifndef __PX4_QURT /* send message to UART */ - ssize_t ret = write(_uart_fd, buf, packet_len); + ssize_t ret = ::write(_uart_fd, buf, packet_len); if (ret != (int) packet_len) { count_txerr(); @@ -806,6 +840,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } +#endif pthread_mutex_unlock(&_send_mutex); } @@ -845,8 +880,9 @@ Mavlink::resend_message(mavlink_message_t *msg) buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); +#ifndef __PX4_QURT /* send message to UART */ - ssize_t ret = write(_uart_fd, buf, packet_len); + ssize_t ret = ::write(_uart_fd, buf, packet_len); if (ret != (int) packet_len) { count_txerr(); @@ -856,6 +892,7 @@ Mavlink::resend_message(mavlink_message_t *msg) _last_write_success_time = _last_write_try_time; count_txbytes(packet_len); } +#endif pthread_mutex_unlock(&_send_mutex); } @@ -1195,38 +1232,42 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = 0; _mode = MAVLINK_MODE_NORMAL; +#ifdef __PX4_NUTTX /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; +#endif /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; + int myoptind=1; + const char *myoptarg = NULL; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': - _baudrate = strtoul(optarg, NULL, 10); + _baudrate = strtoul(myoptarg, NULL, 10); if (_baudrate < 9600 || _baudrate > 921600) { - warnx("invalid baud rate '%s'", optarg); + warnx("invalid baud rate '%s'", myoptarg); err_flag = true; } break; case 'r': - _datarate = strtoul(optarg, NULL, 10); + _datarate = strtoul(myoptarg, NULL, 10); if (_datarate < 10 || _datarate > MAX_DATA_RATE) { - warnx("invalid data rate '%s'", optarg); + warnx("invalid data rate '%s'", myoptarg); err_flag = true; } break; case 'd': - _device_name = optarg; + _device_name = myoptarg; break; // case 'e': @@ -1234,13 +1275,13 @@ Mavlink::task_main(int argc, char *argv[]) // break; case 'm': - if (strcmp(optarg, "custom") == 0) { + if (strcmp(myoptarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; - } else if (strcmp(optarg, "camera") == 0) { + } else if (strcmp(myoptarg, "camera") == 0) { // left in here for compatibility _mode = MAVLINK_MODE_ONBOARD; - } else if (strcmp(optarg, "onboard") == 0) { + } else if (strcmp(myoptarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; } @@ -1296,6 +1337,7 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); +#ifndef __PX4_QURT struct termios uart_config_original; /* default values for arguments */ @@ -1305,6 +1347,7 @@ Mavlink::task_main(int argc, char *argv[]) warn("could not open %s", _device_name); return ERROR; } +#endif /* initialize send mutex */ pthread_mutex_init(&_send_mutex, NULL); @@ -1319,7 +1362,8 @@ Mavlink::task_main(int argc, char *argv[]) * marker ring buffer approach. */ if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { - errx(1, "msg buf:"); + warnx("msg buf:"); + return 1; } /* initialize message buffer mutex */ @@ -1327,10 +1371,14 @@ Mavlink::task_main(int argc, char *argv[]) } /* create the device node that's used for sending text log messages, etc. */ +#ifdef __PX4_NUTTX register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); +#else + register_driver(MAVLINK_LOG_DEVICE, NULL); +#endif /* initialize logging device */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* Initialize system properties */ mavlink_update_system(); @@ -1556,14 +1604,16 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(_receive_thread, NULL); +#ifndef __PX4_QURT /* reset the UART flags to original state */ tcsetattr(_uart_fd, TCSANOW, &uart_config_original); /* close UART */ - close(_uart_fd); + ::close(_uart_fd); +#endif /* close mavlink logging device */ - close(_mavlink_fd); + px4_close(_mavlink_fd); if (_passing_on || _ftp_on) { message_buffer_destroy(); @@ -1618,11 +1668,11 @@ Mavlink::start(int argc, char *argv[]) // between the starting task and the spawned // task - start_helper() only returns // when the started task exits. - task_spawn_cmd(buf, + px4_task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2400, - (main_t)&Mavlink::start_helper, + (px4_main_t)&Mavlink::start_helper, (char * const *)argv); // Ensure that this shell command @@ -1653,7 +1703,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time)); } printf("\tmavlink chan: #%u\n", _channel); @@ -1743,11 +1793,13 @@ Mavlink::stream_command(int argc, char *argv[]) // If the link is not running we should complain, but not fall over // because this is so easy to get wrong and not fatal. Warning is sufficient. - errx(0, "mavlink for device %s is not running", device_name); + warnx("mavlink for device %s is not running", device_name); + return 0; } } else { - errx(1, "usage: mavlink stream [-d device] -s stream -r rate"); + warnx("usage: mavlink stream [-d device] -s stream -r rate"); + return 1; } return OK; @@ -1762,7 +1814,7 @@ int mavlink_main(int argc, char *argv[]) { if (argc < 2) { usage(); - exit(1); + return 1; } if (!strcmp(argv[1], "start")) { @@ -1771,7 +1823,7 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); usage(); - exit(1); + return 1; } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); @@ -1784,7 +1836,7 @@ int mavlink_main(int argc, char *argv[]) } else { usage(); - exit(1); + return 1; } return 0; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 86b259a91b..87e8be4362 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -421,7 +421,7 @@ private: #ifdef __PX4_NUTTX static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); #else - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); #endif /** diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp index 5147368f41..9ac7707cfd 100644 --- a/src/modules/mavlink/mavlink_main_posix.cpp +++ b/src/modules/mavlink/mavlink_main_posix.cpp @@ -437,7 +437,7 @@ Mavlink::get_channel() ****************************************************************************/ int -Mavlink::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +Mavlink::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case (int)MAVLINK_IOC_SEND_TEXT_INFO: diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 41873e9d43..0b035f0258 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -35,14 +35,8 @@ # MAVLink protocol to uORB interface process # -MODULE_COMMAND = mavlink -ifeq ($(PX4_TARGET_OS),nuttx) -SRCS += mavlink_main_nuttx.cpp -else -SRCS += mavlink_main_posix.cpp -endif - SRCS += mavlink.c \ + mavlink_main.cpp \ mavlink_mission.cpp \ mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index e55e137d61..528212dc12 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -121,16 +121,16 @@ public: ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); - virtual int open(device::px4_dev_handle_t *handlep); - virtual int close(device::px4_dev_handle_t *handlep); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual ssize_t write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int open(device::file_t *filp); + virtual int close(device::file_t *filp); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); protected: - virtual pollevent_t poll_state(device::px4_dev_handle_t *handlep); + virtual pollevent_t poll_state(device::file_t *filp); virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); private: @@ -150,7 +150,7 @@ private: pid_t _publisher; /**< if nonzero, current publisher */ const int _priority; /**< priority of topic */ - SubscriberData *handlep_to_sd(device::px4_dev_handle_t *handlep); + SubscriberData *filp_to_sd(device::file_t *filp); /** * Perform a deferred update for a rate-limited subscriber. @@ -173,16 +173,13 @@ private: bool appears_updated(SubscriberData *sd); }; -ORBDevNode::SubscriberData *ORBDevNode::handlep_to_sd(device::px4_dev_handle_t *handlep) +ORBDevNode::SubscriberData *ORBDevNode::filp_to_sd(device::file_t *filp) { - PX4_DEBUG("ORBDevNode::handlep_to_sd %p\n", handlep); ORBDevNode::SubscriberData *sd; - if (handlep) { - sd = (ORBDevNode::SubscriberData *)(handlep->priv); - PX4_DEBUG(" sd = %p \n", sd); + if (filp) { + sd = (ORBDevNode::SubscriberData *)(filp->priv); } else { - PX4_DEBUG("*** ERROR ORBDevNode::handlep_to_sd(0)\n"); sd = 0; } return sd; @@ -209,12 +206,12 @@ ORBDevNode::~ORBDevNode() } int -ORBDevNode::open(device::px4_dev_handle_t *handlep) +ORBDevNode::open(device::file_t *filp) { int ret; /* is this a publisher? */ - if (handlep->flags == PX4_F_WRONLY) { + if (filp->flags == PX4_F_WRONLY) { /* become the publisher if we can */ lock(); @@ -231,7 +228,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) /* now complete the open */ if (ret == PX4_OK) { - ret = VDev::open(handlep); + ret = VDev::open(filp); /* open failed - not the publisher anymore */ if (ret != PX4_OK) @@ -242,7 +239,7 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) } /* is this a new subscriber? */ - if (handlep->flags == PX4_F_RDONLY) { + if (filp->flags == PX4_F_RDONLY) { /* allocate subscriber data */ SubscriberData *sd = new SubscriberData; @@ -258,16 +255,16 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) /* set priority */ sd->priority = _priority; - handlep->priv = (void *)sd; + filp->priv = (void *)sd; - ret = VDev::open(handlep); + ret = VDev::open(filp); if (ret != PX4_OK) { - PX4_DEBUG("ERROR: VDev::open failed\n"); + warnx("ERROR: VDev::open failed\n"); delete sd; } - PX4_DEBUG("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", handlep->fd, handlep->flags, handlep->priv, handlep->cdev); + //warnx("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); return ret; } @@ -276,31 +273,30 @@ ORBDevNode::open(device::px4_dev_handle_t *handlep) } int -ORBDevNode::close(device::px4_dev_handle_t *handlep) +ORBDevNode::close(device::file_t *filp) { - PX4_DEBUG("ORBDevNode::close fd = %d\n", handlep->fd); + //warnx("ORBDevNode::close fd = %d", filp->fd); /* is this the publisher closing? */ if (getpid() == _publisher) { _publisher = 0; } else { - SubscriberData *sd = handlep_to_sd(handlep); + SubscriberData *sd = filp_to_sd(filp); if (sd != nullptr) { - PX4_DEBUG(" delete handlep->priv %p", handlep->priv); hrt_cancel(&sd->update_call); delete sd; } } - return VDev::close(handlep); + return VDev::close(filp); } ssize_t -ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ORBDevNode::read(device::file_t *filp, char *buffer, size_t buflen) { - PX4_DEBUG("ORBDevNode::read fd = %d\n", handlep->fd); - SubscriberData *sd = (SubscriberData *)handlep_to_sd(handlep); + //warnx("ORBDevNode::read fd = %d\n", filp->fd); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); /* if the object has not been written yet, return zero */ if (_data == nullptr) @@ -338,9 +334,9 @@ ORBDevNode::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) } ssize_t -ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +ORBDevNode::write(device::file_t *filp, const char *buffer, size_t buflen) { - PX4_DEBUG("ORBDevNode::write handlep = %p (null is normal)\n", handlep); + //warnx("ORBDevNode::write filp = %p (null is normal)", filp); /* * Writes are legal from interrupt context as long as the * object has already been initialised from thread context. @@ -348,7 +344,7 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t * Writes outside interrupt context will allocate the object * if it has not yet been allocated. * - * Note that handlep will usually be NULL. + * Note that filp will usually be NULL. */ if (nullptr == _data) { lock(); @@ -385,10 +381,10 @@ ORBDevNode::write(device::px4_dev_handle_t *handlep, const char *buffer, size_t } int -ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ORBDevNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - PX4_DEBUG("ORBDevNode::ioctl fd = %d cmd = %d\n", handlep->fd, cmd); - SubscriberData *sd = handlep_to_sd(handlep); + //warnx("ORBDevNode::ioctl fd = %d cmd = %d", filp->fd, cmd); + SubscriberData *sd = filp_to_sd(filp); switch (cmd) { case ORBIOCLASTUPDATE: @@ -413,17 +409,17 @@ ORBDevNode::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } ssize_t ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) { - PX4_DEBUG("ORBDevNode::publish meta = %p\n", meta); + //warnx("ORBDevNode::publish meta = %p", meta); if (handle < 0) { - PX4_DEBUG("ORBDevNode::publish called with invalid handle\n", meta); + warnx("ORBDevNode::publish called with invalid handle"); errno = EINVAL; return ERROR; } @@ -452,10 +448,10 @@ ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *d } pollevent_t -ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) +ORBDevNode::poll_state(device::file_t *filp) { - PX4_DEBUG("ORBDevNode::poll_state fd = %d\n", handlep->fd); - SubscriberData *sd = handlep_to_sd(handlep); + //warnx("ORBDevNode::poll_state fd = %d", filp->fd); + SubscriberData *sd = filp_to_sd(filp); /* * If the topic appears updated to the subscriber, say so. @@ -469,8 +465,8 @@ ORBDevNode::poll_state(device::px4_dev_handle_t *handlep) void ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - PX4_DEBUG("ORBDevNode::poll_notify_one fds = %p fds->priv = %p\n", fds, fds->priv); - SubscriberData *sd = handlep_to_sd((device::px4_dev_handle_t *)fds->priv); + //warnx("ORBDevNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); + SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); /* * If the topic looks updated to the subscriber, go ahead and notify them. @@ -482,7 +478,7 @@ ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) bool ORBDevNode::appears_updated(SubscriberData *sd) { - PX4_DEBUG("ORBDevNode::appears_updated sd = %p\n", sd); + //warnx("ORBDevNode::appears_updated sd = %p", sd); /* assume it doesn't look updated */ bool ret = false; @@ -583,7 +579,7 @@ public: ORBDevMaster(Flavor f); ~ORBDevMaster(); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: Flavor _flavor; }; @@ -603,7 +599,7 @@ ORBDevMaster::~ORBDevMaster() } int -ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ORBDevMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret; @@ -696,7 +692,7 @@ ORBDevMaster::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } @@ -1242,7 +1238,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - PX4_DEBUG("orb_advertise meta = %p\n", meta); + //warnx("orb_advertise meta = %p", meta); return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); } @@ -1252,29 +1248,27 @@ orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *inst int result, fd; orb_advert_t advertiser; - PX4_DEBUG("orb_advertise_multi meta = %p\n", meta); + //warnx("orb_advertise_multi meta = %p\n", meta); /* open the node as an advertiser */ fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) { - PX4_DEBUG(" node_open as advertiser failed.\n"); + warnx("node_open as advertiser failed."); return ERROR; } - PX4_DEBUG(" node_open as advertiser passed. fd = %d, instance = %p %d\n", fd, instance, instance != 0 ? *instance : 0); - /* get the advertiser handle and close the node */ result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); px4_close(fd); if (result == ERROR) { - PX4_DEBUG(" px4_ioctl ORBIOCGADVERTISER failed. fd = %d\n", fd); + warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); return ERROR; } /* the advertiser must perform an initial publish to initialise the object */ result = orb_publish(meta, advertiser, data); if (result == ERROR) { - PX4_DEBUG(" orb_publish failed\n"); + warnx("orb_publish failed"); return ERROR; } diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 704c7fc088..b108f4969b 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -100,8 +100,8 @@ public: virtual int init(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -116,8 +116,8 @@ public: protected: friend class ACCELSIM_mag; - virtual ssize_t mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -325,8 +325,8 @@ public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int init(); @@ -516,7 +516,7 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } ssize_t -ACCELSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); accel_report *arb = reinterpret_cast(buffer); @@ -553,7 +553,7 @@ ACCELSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) } ssize_t -ACCELSIM::mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); mag_report *mrb = reinterpret_cast(buffer); @@ -592,7 +592,7 @@ ACCELSIM::mag_read(device::px4_dev_handle_t *handlep, char *buffer, size_t bufle } int -ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -614,10 +614,10 @@ ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(handlep, SENSORIOCSPOLLRATE, 1600); + return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(handlep, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); + return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -712,12 +712,12 @@ ACCELSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } int -ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -741,7 +741,7 @@ ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ - return mag_ioctl(handlep, SENSORIOCSPOLLRATE, 100); + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ default: { @@ -831,7 +831,7 @@ ACCELSIM::mag_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar return OK; default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } @@ -1209,20 +1209,20 @@ ACCELSIM_mag::parent_poll_notify() } ssize_t -ACCELSIM_mag::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) { - return _parent->mag_read(handlep, buffer, buflen); + return _parent->mag_read(filp, buffer, buflen); } int -ACCELSIM_mag::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ACCELSIM_mag::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(handlep, cmd, arg); + return (int)VDev::ioctl(filp, cmd, arg); break; default: - return _parent->mag_ioctl(handlep, cmd, arg); + return _parent->mag_ioctl(filp, cmd, arg); } } diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 1e859c386e..893c30948c 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -74,12 +74,12 @@ public: virtual int init(); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t len); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t len); protected: - virtual int open_first(device::px4_dev_handle_t *handlep); - virtual int close_last(device::px4_dev_handle_t *handlep); + virtual int open_first(device::file_t *filp); + virtual int close_last(device::file_t *filp); private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ @@ -157,13 +157,13 @@ ADCSIM::init() } int -ADCSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +ADCSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { return -ENOTTY; } ssize_t -ADCSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t len) +ADCSIM::read(device::file_t *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; @@ -177,7 +177,7 @@ ADCSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t len) } int -ADCSIM::open_first(device::px4_dev_handle_t *handlep) +ADCSIM::open_first(device::file_t *filp) { /* get fresh data */ _tick(); @@ -189,7 +189,7 @@ ADCSIM::open_first(device::px4_dev_handle_t *handlep) } int -ADCSIM::close_last(device::px4_dev_handle_t *handlep) +ADCSIM::close_last(device::file_t *filp) { hrt_cancel(&_call); return 0; diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index bef9c9c5aa..eb6c6c29f7 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -100,8 +100,8 @@ public: virtual int init(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -327,7 +327,7 @@ out: } ssize_t -BAROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); struct baro_report *brp = reinterpret_cast(buffer); @@ -397,7 +397,7 @@ BAROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) } int -BAROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -502,7 +502,7 @@ BAROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } void diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 55a34b191a..44e85150c0 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -197,8 +197,8 @@ public: virtual int init(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** @@ -211,8 +211,8 @@ public: protected: friend class GYROSIM_gyro; - virtual ssize_t gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t gyro_read(device::file_t *filp, char *buffer, size_t buflen); + virtual int gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg); private: GYROSIM_gyro *_gyro; @@ -438,8 +438,8 @@ public: GYROSIM_gyro(GYROSIM *parent, const char *path); ~GYROSIM_gyro(); - virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int init(); @@ -733,7 +733,7 @@ GYROSIM::_set_dlpf_filter(uint16_t frequency_hz) } ssize_t -GYROSIM::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(accel_report); @@ -856,7 +856,7 @@ GYROSIM::gyro_self_test() } ssize_t -GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(gyro_report); @@ -891,7 +891,7 @@ GYROSIM::gyro_read(device::px4_dev_handle_t *handlep, char *buffer, size_t bufle } int -GYROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -916,10 +916,10 @@ GYROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(handlep, SENSORIOCSPOLLRATE, 1000); + return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: - return ioctl(handlep, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); + return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -1030,12 +1030,12 @@ GYROSIM::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } int -GYROSIM::gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -1043,7 +1043,7 @@ GYROSIM::gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: - return ioctl(handlep, cmd, arg); + return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ @@ -1101,7 +1101,7 @@ GYROSIM::gyro_ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long ar default: /* give it to the superclass */ - return VDev::ioctl(handlep, cmd, arg); + return VDev::ioctl(filp, cmd, arg); } } @@ -1578,21 +1578,21 @@ GYROSIM_gyro::parent_poll_notify() } ssize_t -GYROSIM_gyro::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +GYROSIM_gyro::read(device::file_t *filp, char *buffer, size_t buflen) { - return _parent->gyro_read(handlep, buffer, buflen); + return _parent->gyro_read(filp, buffer, buflen); } int -GYROSIM_gyro::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(handlep, cmd, arg); + return (int)VDev::ioctl(filp, cmd, arg); break; default: - return _parent->gyro_ioctl(handlep, cmd, arg); + return _parent->gyro_ioctl(filp, cmd, arg); } } diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index b4cb41a4fd..b891ab9982 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -46,24 +46,30 @@ #include #include +#define PX4_F_RDONLY 1 +#define PX4_F_WRONLY 2 + +#ifdef __PX4_NUTTX + +typedef struct pollfd px4_pollfd_struct_t; + +#if defined(__cplusplus) +#define _GLOBAL :: +#else +#define _GLOBAL +#endif +#define px4_open _GLOBAL open +#define px4_close _GLOBAL close +#define px4_ioctl _GLOBAL ioctl +#define px4_write _GLOBAL write +#define px4_read _GLOBAL read +#define px4_poll _GLOBAL poll + +#elif defined(__PX4_POSIX) #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 -#define PX4_DEBUG(...) -//#define PX4_DEBUG(...) printf(__VA_ARGS__) - -#ifdef __PX4_NUTTX - -#define px4_pollfd_struct_t struct pollfd -#define px4_open open -#define px4_close close -#define px4_ioctl ioctl -#define px4_write write -#define px4_read read -#define px4_poll poll - -#else typedef short pollevent_t; typedef struct { @@ -87,7 +93,10 @@ __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); __END_DECLS +#else +#error "No TARGET OS Provided" #endif + __BEGIN_DECLS extern int px4_errno; From 3af6e9d76e9ba6cd860a2addf0f0ab197aac2807 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 26 Apr 2015 11:03:52 +0200 Subject: [PATCH 159/258] added autogenerated code for topic listener tool --- .gitignore | 1 + Tools/generate_listener.py | 112 ++++++++++++++++++++++++ makefiles/posix/config_posix_default.mk | 1 + src/systemcmds/topic_listener/module.mk | 43 +++++++++ 4 files changed, 157 insertions(+) create mode 100644 Tools/generate_listener.py create mode 100644 src/systemcmds/topic_listener/module.mk diff --git a/.gitignore b/.gitignore index c453432f78..881396fa2f 100644 --- a/.gitignore +++ b/.gitignore @@ -49,3 +49,4 @@ unittests/build *.pretty xcode src/platforms/linux/px4_messages/ +src/systemcmds/topic_listener.cpp diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py new file mode 100644 index 0000000000..3fa4ea476f --- /dev/null +++ b/Tools/generate_listener.py @@ -0,0 +1,112 @@ +#!/usr/bin/python + +import glob + +raw_messages = glob.glob("../msg/*.msg") +messages = [] +message_floats = [] + + +for index,m in enumerate(raw_messages): + temp_list = [] + if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m): + f = open(m,'r') + for line in f.readlines(): + if(line.split(' ')[0] == "float32"): + temp_list.append(line.split(' ')[1].split('\t')[0].split('\n')[0]) + f.close() + messages.append(m.split('/')[-1].split('.')[0]) + message_floats.append(temp_list) +num_messages = len(messages); + +print +print """ + +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file topic_listener.cpp, autogenerated by Tools/generate_listener.py + * + * Tool for listening to topics when running flight stack on linux. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +""" +for m in messages: + print "#include " % m + + +print """ +extern "C" __EXPORT int listener_main(int argc, char *argv[]); + +int listener_main(int argc, char *argv[]) { + int sub = -1; + orb_id_t ID; + if(argc < 3) { + printf("need at least two arguments: topic name, number of messages to print\\n"); + return 1; + } +""" +print "\tuint32_t num_msgs = (uint32_t)std::stoi(argv[2],NULL,10);" +print "\tif(strncmp(argv[1],\"%s\",50)== 0) {" % messages[0] +print "\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0] +print "\t\tID = ORB_ID(%s);" % messages[0] +print "\t\tstruct %s_s container;" % messages[0] +print "\t\tmemset(&container, 0, sizeof(container));" +for index,m in enumerate(messages[1:]): + print "\t} else if(strncmp(argv[1],\"%s\",50) == 0) {" % m + print "\t\tsub = orb_subscribe(ORB_ID(%s));" % m + print "\t\tID = ORB_ID(%s);" % m + print "\t\tstruct %s_s container;" % m + print "\t\tmemset(&container, 0, sizeof(container));" + print "\t\tfor(uint32_t i = 0;i Date: Sun, 26 Apr 2015 20:02:58 -0700 Subject: [PATCH 160/258] topic_listener: added missing build deps The posix build would complain that toipc_listener.cpp did not exist and there was no rule to create it. The required rule was added to src/systemcmds/topic_listener/module.mk The script generate_listener.py is run from the Build tree and needs to access $(PX4_BASE)/msgs so $(PX4_BASE) is now passed as an argument to generate_listener.py Signed-off-by: Mark Charlebois --- .gitignore | 5 +++-- Tools/generate_listener.py | 7 ++++++- Tools/posix_apps.py | 1 - src/systemcmds/topic_listener/module.mk | 3 +++ 4 files changed, 12 insertions(+), 4 deletions(-) mode change 100644 => 100755 Tools/generate_listener.py diff --git a/.gitignore b/.gitignore index 881396fa2f..d3fd211d13 100644 --- a/.gitignore +++ b/.gitignore @@ -48,5 +48,6 @@ unittests/build .vagrant *.pretty xcode -src/platforms/linux/px4_messages/ -src/systemcmds/topic_listener.cpp +src/platforms/posix/px4_messages/ +src/platforms/qurt/px4_messages/ +src/systemcmds/topic_listener/topic_listener.cpp diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py old mode 100644 new mode 100755 index 3fa4ea476f..805b390901 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -1,8 +1,13 @@ #!/usr/bin/python import glob +import sys -raw_messages = glob.glob("../msg/*.msg") +# This script is run from Build/_default.build/$(PX4_BASE)/Firmware/src/systemcmds/topic_listener + +# argv[1] must be the full path of the top Firmware dir + +raw_messages = glob.glob(sys.argv[1]+"/msg/*.msg") messages = [] message_floats = [] diff --git a/Tools/posix_apps.py b/Tools/posix_apps.py index f48977c49a..fe457e778e 100755 --- a/Tools/posix_apps.py +++ b/Tools/posix_apps.py @@ -33,7 +33,6 @@ ############################################################################ import glob -#builtins = glob.glob("../Build/linux_default.build/builtin_commands/COMMAND*") builtins = glob.glob("builtin_commands/COMMAND*") apps = [] diff --git a/src/systemcmds/topic_listener/module.mk b/src/systemcmds/topic_listener/module.mk index ea54c3e9e5..14c8deb9f8 100644 --- a/src/systemcmds/topic_listener/module.mk +++ b/src/systemcmds/topic_listener/module.mk @@ -35,6 +35,9 @@ # Build the topic listener tool. # +$(PX4_BASE)/src/systemcmds/topic_listener/topic_listener.cpp : $(PX4_BASE)/Tools/generate_listener.py + $(PX4_BASE)/Tools/generate_listener.py $(PX4_BASE) > $(PX4_BASE)/src/systemcmds/topic_listener/$@ + MODULE_COMMAND = listener SRCS = topic_listener.cpp From 5b91f172e397ec57420869680e35653f2f844edb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 09:52:28 -0700 Subject: [PATCH 161/258] topic_listener: moved generated file to Build dir Moved the generated topic_listener.cpp to the Build tree so it is cleaned when make clean is called. Signed-off-by: Mark Charlebois --- src/systemcmds/topic_listener/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/topic_listener/module.mk b/src/systemcmds/topic_listener/module.mk index 14c8deb9f8..4048c6b318 100644 --- a/src/systemcmds/topic_listener/module.mk +++ b/src/systemcmds/topic_listener/module.mk @@ -35,8 +35,8 @@ # Build the topic listener tool. # -$(PX4_BASE)/src/systemcmds/topic_listener/topic_listener.cpp : $(PX4_BASE)/Tools/generate_listener.py - $(PX4_BASE)/Tools/generate_listener.py $(PX4_BASE) > $(PX4_BASE)/src/systemcmds/topic_listener/$@ +./topic_listener.cpp : $(PX4_BASE)/Tools/generate_listener.py + $(PX4_BASE)/Tools/generate_listener.py $(PX4_BASE) > $@ MODULE_COMMAND = listener SRCS = topic_listener.cpp From 17233faaa0deef490d2d3de074dfafc195d35436 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 09:54:47 -0700 Subject: [PATCH 162/258] Removed topic_listener.cpp from gitignore The generated file is not created in the Build tree and is automatically ignored Signed-off-by: Mark Charlebois --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index d3fd211d13..cd3f8f26c2 100644 --- a/.gitignore +++ b/.gitignore @@ -50,4 +50,3 @@ unittests/build xcode src/platforms/posix/px4_messages/ src/platforms/qurt/px4_messages/ -src/systemcmds/topic_listener/topic_listener.cpp From 7390f50b670c314a31e288f745b6f0cc97fe9bff Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 12:21:00 -0700 Subject: [PATCH 163/258] Posix: cleanup of bad file rename Deleted obsolete file and fixed renaming of wqueue_start_posix.cpp Signed-off-by: Mark Charlebois --- src/drivers/rgbled/rgbled_posix.cpp | 739 ------------------ ...tart_poosix.cpp => wqueue_start_posix.cpp} | 0 2 files changed, 739 deletions(-) delete mode 100644 src/drivers/rgbled/rgbled_posix.cpp rename src/platforms/posix/tests/wqueue/{wqueue_start_poosix.cpp => wqueue_start_posix.cpp} (100%) diff --git a/src/drivers/rgbled/rgbled_posix.cpp b/src/drivers/rgbled/rgbled_posix.cpp deleted file mode 100644 index 4344b67929..0000000000 --- a/src/drivers/rgbled/rgbled_posix.cpp +++ /dev/null @@ -1,739 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file rgbled.cpp - * - * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. - * - * @author Julian Oes - * @author Anton Babushkin - */ - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - - -#include -#include -#include - -#include - -#include - -#define RGBLED_ONTIME 120 -#define RGBLED_OFFTIME 120 - -#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ -#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ -#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ -#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ -#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ -#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ - -#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ -#define SETTING_ENABLE 0x02 /**< on */ - - -class RGBLED : public device::I2C -{ -public: - RGBLED(int bus, int rgbled); - virtual ~RGBLED(); - - - virtual int init(); - virtual int probe(); - virtual int info(); - virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); - -private: - work_s _work; - - rgbled_mode_t _mode; - rgbled_pattern_t _pattern; - - uint8_t _r; - uint8_t _g; - uint8_t _b; - float _brightness; - - bool _running; - int _led_interval; - bool _should_run; - int _counter; - - void set_color(rgbled_color_t ledcolor); - void set_mode(rgbled_mode_t mode); - void set_pattern(rgbled_pattern_t *pattern); - - static void led_trampoline(void *arg); - void led(); - - int send_led_enable(bool enable); - int send_led_rgb(); - int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); -}; - -/* for now, we only support one RGBLED */ -namespace -{ -RGBLED *g_rgbled = nullptr; -} - -void rgbled_usage(); - -extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); - -RGBLED::RGBLED(int bus, int rgbled) : - I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled -#ifdef __PX4_NUTTX -,100000 /* maximum speed supported */ -#endif -), - _mode(RGBLED_MODE_OFF), - _r(0), - _g(0), - _b(0), - _brightness(1.0f), - _running(false), - _led_interval(0), - _should_run(false), - _counter(0) -{ - memset(&_work, 0, sizeof(_work)); - memset(&_pattern, 0, sizeof(_pattern)); -} - -RGBLED::~RGBLED() -{ -} - -int -RGBLED::init() -{ - int ret; - ret = I2C::init(); - - if (ret != OK) { - return ret; - } - - /* switch off LED on start */ - send_led_enable(false); - send_led_rgb(); - - return OK; -} - -int -RGBLED::probe() -{ - int ret; - bool on, powersave; - uint8_t r, g, b; - - /** - this may look strange, but is needed. There is a serial - EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to - a bunch of I2C addresses, including the 0x55 used by this - LED device. So we need to do enough operations to be sure - we are talking to the right device. These 3 operations seem - to be enough, as the 3rd one consistently fails if no - RGBLED is on the bus. - */ - - unsigned prevretries = _retries; - _retries = 4; - - if ((ret=get(on, powersave, r, g, b)) != OK || - (ret=send_led_enable(false) != OK) || - (ret=send_led_enable(false) != OK)) { - return ret; - } - - _retries = prevretries; - - return ret; -} - -int -RGBLED::info() -{ - int ret; - bool on, powersave; - uint8_t r, g, b; - - ret = get(on, powersave, r, g, b); - - if (ret == OK) { - /* we don't care about power-save mode */ - log("state: %s", on ? "ON" : "OFF"); - log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); - - } else { - warnx("failed to read led"); - } - - return ret; -} - -int -RGBLED::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) -{ - int ret = ENOTTY; - - switch (cmd) { - case RGBLED_SET_RGB: - /* set the specified color */ - _r = ((rgbled_rgbset_t *) arg)->red; - _g = ((rgbled_rgbset_t *) arg)->green; - _b = ((rgbled_rgbset_t *) arg)->blue; - send_led_rgb(); - return OK; - - case RGBLED_SET_COLOR: - /* set the specified color name */ - set_color((rgbled_color_t)arg); - send_led_rgb(); - return OK; - - case RGBLED_SET_MODE: - /* set the specified mode */ - set_mode((rgbled_mode_t)arg); - return OK; - - case RGBLED_SET_PATTERN: - /* set a special pattern */ - set_pattern((rgbled_pattern_t *)arg); - return OK; - - default: - /* see if the parent class can make any use of it */ - ret = VDev::ioctl(handlep, cmd, arg); - break; - } - - return ret; -} - - -void -RGBLED::led_trampoline(void *arg) -{ - RGBLED *rgbl = reinterpret_cast(arg); - - rgbl->led(); -} - -/** - * Main loop function - */ -void -RGBLED::led() -{ - if (!_should_run) { - _running = false; - return; - } - - switch (_mode) { - case RGBLED_MODE_BLINK_SLOW: - case RGBLED_MODE_BLINK_NORMAL: - case RGBLED_MODE_BLINK_FAST: - if (_counter >= 2) - _counter = 0; - - send_led_enable(_counter == 0); - - break; - - case RGBLED_MODE_BREATHE: - - if (_counter >= 62) - _counter = 0; - - int n; - - if (_counter < 32) { - n = _counter; - - } else { - n = 62 - _counter; - } - - _brightness = n * n / (31.0f * 31.0f); - send_led_rgb(); - break; - - case RGBLED_MODE_PATTERN: - - /* don't run out of the pattern array and stop if the next frame is 0 */ - if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) - _counter = 0; - - set_color(_pattern.color[_counter]); - send_led_rgb(); - _led_interval = _pattern.duration[_counter]; - break; - - default: - break; - } - - _counter++; - - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); -} - -/** - * Parse color constant and set _r _g _b values - */ -void -RGBLED::set_color(rgbled_color_t color) -{ - switch (color) { - case RGBLED_COLOR_OFF: - _r = 0; - _g = 0; - _b = 0; - break; - - case RGBLED_COLOR_RED: - _r = 255; - _g = 0; - _b = 0; - break; - - case RGBLED_COLOR_YELLOW: - _r = 255; - _g = 200; - _b = 0; - break; - - case RGBLED_COLOR_PURPLE: - _r = 255; - _g = 0; - _b = 255; - break; - - case RGBLED_COLOR_GREEN: - _r = 0; - _g = 255; - _b = 0; - break; - - case RGBLED_COLOR_BLUE: - _r = 0; - _g = 0; - _b = 255; - break; - - case RGBLED_COLOR_WHITE: - _r = 255; - _g = 255; - _b = 255; - break; - - case RGBLED_COLOR_AMBER: - _r = 255; - _g = 80; - _b = 0; - break; - - case RGBLED_COLOR_DIM_RED: - _r = 90; - _g = 0; - _b = 0; - break; - - case RGBLED_COLOR_DIM_YELLOW: - _r = 80; - _g = 30; - _b = 0; - break; - - case RGBLED_COLOR_DIM_PURPLE: - _r = 45; - _g = 0; - _b = 45; - break; - - case RGBLED_COLOR_DIM_GREEN: - _r = 0; - _g = 90; - _b = 0; - break; - - case RGBLED_COLOR_DIM_BLUE: - _r = 0; - _g = 0; - _b = 90; - break; - - case RGBLED_COLOR_DIM_WHITE: - _r = 30; - _g = 30; - _b = 30; - break; - - case RGBLED_COLOR_DIM_AMBER: - _r = 80; - _g = 20; - _b = 0; - break; - - default: - warnx("color unknown"); - break; - } -} - -/** - * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) - */ -void -RGBLED::set_mode(rgbled_mode_t mode) -{ - if (mode != _mode) { - _mode = mode; - - switch (mode) { - case RGBLED_MODE_OFF: - _should_run = false; - send_led_enable(false); - break; - - case RGBLED_MODE_ON: - _brightness = 1.0f; - send_led_rgb(); - send_led_enable(true); - break; - - case RGBLED_MODE_BLINK_SLOW: - _should_run = true; - _counter = 0; - _led_interval = 2000; - _brightness = 1.0f; - send_led_rgb(); - break; - - case RGBLED_MODE_BLINK_NORMAL: - _should_run = true; - _counter = 0; - _led_interval = 500; - _brightness = 1.0f; - send_led_rgb(); - break; - - case RGBLED_MODE_BLINK_FAST: - _should_run = true; - _counter = 0; - _led_interval = 100; - _brightness = 1.0f; - send_led_rgb(); - break; - - case RGBLED_MODE_BREATHE: - _should_run = true; - _counter = 0; - _led_interval = 25; - send_led_enable(true); - break; - - case RGBLED_MODE_PATTERN: - _should_run = true; - _counter = 0; - _brightness = 1.0f; - send_led_enable(true); - break; - - default: - warnx("mode unknown"); - break; - } - - /* if it should run now, start the workq */ - if (_should_run && !_running) { - _running = true; - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); - } - - } -} - -/** - * Set pattern for PATTERN mode, but don't change current mode - */ -void -RGBLED::set_pattern(rgbled_pattern_t *pattern) -{ - memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); -} - -/** - * Sent ENABLE flag to LED driver - */ -int -RGBLED::send_led_enable(bool enable) -{ - uint8_t settings_byte = 0; - - if (enable) - settings_byte |= SETTING_ENABLE; - - settings_byte |= SETTING_NOT_POWERSAVE; - - const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -/** - * Send RGB PWM settings to LED driver according to current color and brightness - */ -int -RGBLED::send_led_rgb() -{ - /* To scale from 0..255 -> 0..15 shift right by 4 bits */ - const uint8_t msg[6] = { - SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), - SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), - SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) - }; - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) -{ - uint8_t result[2]; - int ret; - - ret = transfer(nullptr, 0, &result[0], 2); - - if (ret == OK) { - on = result[0] & SETTING_ENABLE; - powersave = !(result[0] & SETTING_NOT_POWERSAVE); - /* XXX check, looks wrong */ - r = (result[0] & 0x0f) << 4; - g = (result[1] & 0xf0); - b = (result[1] & 0x0f) << 4; - } - - return ret; -} - -void -rgbled_usage() -{ - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); - warnx("options:"); - warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); - warnx(" -a addr (0x%x)", ADDR); -} - -int -rgbled_main(int argc, char *argv[]) -{ - int i2cdevice = -1; - int rgbledadr = ADDR; /* 7bit */ - - int ch; - - /* jump over start/off/etc and look at options first */ - int myoptind = 1; - const char *myoptarg = NULL; - while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'a': - rgbledadr = strtol(myoptarg, NULL, 0); - break; - - case 'b': - i2cdevice = strtol(myoptarg, NULL, 0); - break; - - default: - rgbled_usage(); - return 1; - } - } - - if (myoptind >= argc) { - rgbled_usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - int fd; - int ret; - - if (!strcmp(verb, "start")) { - if (g_rgbled != nullptr) { - warnx("already started"); - return 1; - } - - if (i2cdevice == -1) { - // try the external bus first - i2cdevice = PX4_I2C_BUS_EXPANSION; - g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); - - if (g_rgbled != nullptr && OK != g_rgbled->init()) { - delete g_rgbled; - g_rgbled = nullptr; - } - - if (g_rgbled == nullptr) { - // fall back to default bus - if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { - warnx("init failed"); - return 1; - } - i2cdevice = PX4_I2C_BUS_LED; - } - } - - if (g_rgbled == nullptr) { - g_rgbled = new RGBLED(i2cdevice, rgbledadr); - - if (g_rgbled == nullptr) { - warnx("new failed"); - return 1; - } - - if (OK != g_rgbled->init()) { - delete g_rgbled; - g_rgbled = nullptr; - warnx("init failed"); - return 1; - } - } - - return 0; - } - - /* need the driver past this point */ - if (g_rgbled == nullptr) { - warnx("not started"); - rgbled_usage(); - return 1; - } - - if (!strcmp(verb, "test")) { - fd = px4_open(RGBLED0_DEVICE_PATH, 0); - - if (fd == -1) { - warnx( "Unable to open " RGBLED0_DEVICE_PATH); - return 1; - } - - rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, - {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern - }; - - ret = px4_ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); - ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); - - px4_close(fd); - return ret; - } - - if (!strcmp(verb, "info")) { - g_rgbled->info(); - return 0; - } - - if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { - fd = px4_open(RGBLED0_DEVICE_PATH, 0); - - if (fd == -1) { - warnx("Unable to open " RGBLED0_DEVICE_PATH); - return 1; - } - - ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); - px4_close(fd); - /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ - if (!strcmp(verb, "stop")) { - delete g_rgbled; - g_rgbled = nullptr; - return 0; - } - return ret; - } - - if (!strcmp(verb, "rgb")) { - if (argc < 5) { - warnx("Usage: rgbled rgb "); - return 1; - } - - fd = px4_open(RGBLED0_DEVICE_PATH, 0); - - if (fd == -1) { - warnx("Unable to open " RGBLED0_DEVICE_PATH); - return 1; - } - - rgbled_rgbset_t v; - v.red = strtol(argv[2], NULL, 0); - v.green = strtol(argv[3], NULL, 0); - v.blue = strtol(argv[4], NULL, 0); - ret = px4_ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); - ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); - px4_close(fd); - return ret; - } - - rgbled_usage(); - return 1; -} diff --git a/src/platforms/posix/tests/wqueue/wqueue_start_poosix.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp similarity index 100% rename from src/platforms/posix/tests/wqueue/wqueue_start_poosix.cpp rename to src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp From 3c957e57e7b69cf991393bcd9cb141080a0055bc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 13:17:28 -0700 Subject: [PATCH 164/258] Posix: fixed time scaling for work queues In work queues, delay is in ticks. Needed to check elapsed time in ticks not in milliseconds. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/work_thread.c | 12 ++++++---- .../posix/tests/wqueue/wqueue_test.cpp | 23 +++++++++++++------ src/platforms/px4_config.h | 2 +- 3 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c index 9ff9ff8923..a266500325 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -85,13 +85,16 @@ struct wqueue_s g_work[NWORKERS]; * ****************************************************************************/ +#define USEC_TO_TICKS(x) (uint32_t)((x)/1000ull) +#define TICKS_TO_USEC(x) ((x)*1000ull) + static void work_process(FAR struct wqueue_s *wqueue) { volatile FAR struct work_s *work; worker_t worker; //irqstate_t flags; FAR void *arg; - uint32_t elapsed; + uint64_t elapsed; uint32_t remaining; uint32_t next; @@ -110,8 +113,8 @@ static void work_process(FAR struct wqueue_s *wqueue) * zero. Therefore a delay of zero will always execute immediately. */ - elapsed = clock_systimer() - work->qtime; - //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay); + elapsed = USEC_TO_TICKS(clock_systimer() - work->qtime); + //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ @@ -155,7 +158,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ /* Here: elapsed < work->delay */ - remaining = work->delay - elapsed; + remaining = TICKS_TO_USEC(work->delay - elapsed); if (remaining < next) { /* Yes.. Then schedule to wake up when the work is ready */ @@ -292,6 +295,7 @@ int work_usrthread(int argc, char *argv[]) uint32_t clock_systimer() { + //printf("clock_systimer: %0lx\n", hrt_absolute_time()); return (0x00000000ffffffff & hrt_absolute_time()); } #endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.cpp b/src/platforms/posix/tests/wqueue/wqueue_test.cpp index 3fc0e5a917..74eb88e7e9 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_test.cpp @@ -62,14 +62,25 @@ void WQueueTest::lp_worker_cb(void *p) void WQueueTest::do_lp_work() { + static int iter = 0; printf("done lp work\n"); - _lpwork_done = true; + if (iter > 5) + _lpwork_done = true; + ++iter; + + work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000); } void WQueueTest::do_hp_work() { + static int iter = 0; printf("done hp work\n"); - _hpwork_done = true; + if (iter > 5) + _hpwork_done = true; + ++iter; + + // requeue + work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1000); } int WQueueTest::main() @@ -77,19 +88,17 @@ int WQueueTest::main() appState.setRunning(true); //Put work on HP work queue - work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1); + work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1000); //Put work on LP work queue - work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1); + work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000); // Wait for work to finsh while (!appState.exitRequested() && !(_hpwork_done && _lpwork_done)) { - printf(" Sleeping...\n"); + printf(" Sleeping for 2 sec...\n"); sleep(2); - - printf(" Waiting on work...\n"); } return 0; diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index d6fcebd703..e724411080 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -52,7 +52,7 @@ #define CONFIG_ARCH_BOARD_POSIXTEST 1 /** time in ms between checks for work in work queues **/ -#define CONFIG_SCHED_WORKPERIOD 10 +#define CONFIG_SCHED_WORKPERIOD 50000 #define CONFIG_SCHED_INSTRUMENTATION 1 #define CONFIG_MAX_TASKS 32 From 7fa33d0d2b581410ef71e66b4d9b3bf9d75173fe Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 13:48:54 -0700 Subject: [PATCH 165/258] posix: workqueue uses TICK scaling from px4_defines.h px4_defines.h defines USEC2TICK(x) and TICKS_PER_USEC. These are now used and allow tick scaling. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/work_thread.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c index a266500325..d42b21fafd 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -113,7 +113,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * zero. Therefore a delay of zero will always execute immediately. */ - elapsed = USEC_TO_TICKS(clock_systimer() - work->qtime); + elapsed = USEC2TICK(clock_systimer() - work->qtime); //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); if (elapsed >= work->delay) { @@ -158,7 +158,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ /* Here: elapsed < work->delay */ - remaining = TICKS_TO_USEC(work->delay - elapsed); + remaining = USEC_PER_TICK*(work->delay - elapsed); if (remaining < next) { /* Yes.. Then schedule to wake up when the work is ready */ From 58a33dd26a0a5be1df02a427e3ed36ae228236b4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 14:00:06 -0700 Subject: [PATCH 166/258] Added simulated tone_alarm and enabled led for POSIX Added simulated tone_alarm class and enabled led class for posix build. The simulator implements the led_init, led_on, led_off, led_toggle calls. Signed-off-by: Mark Charlebois --- makefiles/posix/config_posix_default.mk | 6 +- posix-configs/posixtest/init/rc.S | 2 +- src/drivers/led/led.cpp | 23 +- src/modules/simulator/simulator.cpp | 45 + .../posix/drivers/tonealrmsim/module.mk | 42 + .../posix/drivers/tonealrmsim/tone_alarm.cpp | 811 ++++++++++++++++++ 6 files changed, 924 insertions(+), 5 deletions(-) create mode 100644 src/platforms/posix/drivers/tonealrmsim/module.mk create mode 100644 src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 7205371819..a9bb98d6c9 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -9,6 +9,7 @@ MODULES += drivers/device MODULES += drivers/blinkm MODULES += drivers/hil MODULES += drivers/rgbled +MODULES += drivers/led MODULES += modules/sensors #MODULES += drivers/ms5611 @@ -62,12 +63,13 @@ MODULES += platforms/posix/drivers/accelsim MODULES += platforms/posix/drivers/gyrosim MODULES += platforms/posix/drivers/adcsim MODULES += platforms/posix/drivers/barosim +MODULES += platforms/posix/drivers/tonealrmsim # # Unit tests # #MODULES += platforms/posix/tests/hello #MODULES += platforms/posix/tests/vcdev_test -#MODULES += platforms/posix/tests/hrt_test -#MODULES += platforms/posix/tests/wqueue +MODULES += platforms/posix/tests/hrt_test +MODULES += platforms/posix/tests/wqueue diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index 9fae9240d7..8ae514fec9 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -1,5 +1,5 @@ -simulator start uorb start +simulator start barosim start adcsim start accelsim start diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 4d1d24a92d..428a542f15 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -38,8 +38,10 @@ */ #include +#include #include #include +#include /* * Ideally we'd be able to get these from up_internal.h, @@ -55,18 +57,26 @@ extern void led_off(int led); extern void led_toggle(int led); __END_DECLS +#ifdef __PX4_NUTTX class LED : device::CDev +#else +class LED : device::VDev +#endif { public: LED(); virtual ~LED(); virtual int init(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); }; LED::LED() : +#ifdef __PX4_NUTTX CDev("led", LED0_DEVICE_PATH) +#else + VDev("led", LED0_DEVICE_PATH) +#endif { // force immediate init/device registration init(); @@ -79,14 +89,19 @@ LED::~LED() int LED::init() { + printf("Initializing LED\n"); +#ifdef __PX4_NUTTX CDev::init(); +#else + VDev::init(); +#endif led_init(); return 0; } int -LED::ioctl(struct file *filp, int cmd, unsigned long arg) +LED::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int result = OK; @@ -105,7 +120,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg) default: +#ifdef __PX4_NUTTX result = CDev::ioctl(filp, cmd, arg); +#else + result = VDev::ioctl(filp, cmd, arg); +#endif } return result; } diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 1b3b6fa2c4..bbfba4c4c5 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #ifndef __PX4_QURT #include #include @@ -79,6 +80,8 @@ int Simulator::start(int argc, char *argv[]) int ret = 0; _instance = new Simulator(); if (_instance) { + printf("Simulator started\n"); + drv_led_start(); #ifndef __PX4_QURT _instance->updateSamples(); #endif @@ -186,3 +189,45 @@ int simulator_main(int argc, char *argv[]) } +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +bool static _led_state = false; + +__EXPORT void led_init() +{ + printf("LED_ON\n"); +} + +__EXPORT void led_on(int led) +{ + if (led == 1) + { + printf("LED_ON\n"); + _led_state = true; + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1) + { + printf("LED_OFF\n"); + _led_state = false; + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1) + { + _led_state = !_led_state; + printf("LED_TOGGLE: %s\n", _led_state ? "ON" : "OFF"); + + } +} + diff --git a/src/platforms/posix/drivers/tonealrmsim/module.mk b/src/platforms/posix/drivers/tonealrmsim/module.mk new file mode 100644 index 0000000000..948b3912a5 --- /dev/null +++ b/src/platforms/posix/drivers/tonealrmsim/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 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. +# +############################################################################ + +# +# Tone alarm driver +# + +MODULE_COMMAND = tone_alarm + +SRCS = tone_alarm.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp new file mode 100644 index 0000000000..7c9e8cc912 --- /dev/null +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -0,0 +1,811 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * Driver for the PX4 audio alarm port, /dev/tone_alarm. + * + * The tone_alarm driver supports a set of predefined "alarm" + * tunes and one user-supplied tune. + * + * The TONE_SET_ALARM ioctl can be used to select a predefined + * alarm tune, from 1 - . Selecting tune zero silences + * the alarm. + * + * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY + * statement, with some exceptions and extensions. + * + * From Wikibooks: + * + * PLAY "[string expression]" + * + * Used to play notes and a score ... The tones are indicated by letters A through G. + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * immediately after the note letter. See this example: + * + * PLAY "C C# C C#" + * + * Whitespaces are ignored inside the string expression. There are also codes that + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators + * that change the properties are effective for the notes following that indicator. + * + * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration + * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. + * (L8, L16, L32, L64, ...). By default, n = 4. + * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. + * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" + * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. + * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. + * Remember that C- is equivalent to B. + * < > Changes the current octave respectively down or up one level. + * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) + * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. + * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. + * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. + * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. + * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. + * It can be used for a pause as well. + * + * Extensions/variations: + * + * MB MF The MF command causes the tune to play once and then stop. The MB command causes the + * tune to repeat when it ends. + * + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + + +class ToneAlarm : public device::VDev +{ +public: + ToneAlarm(); + ~ToneAlarm(); + + virtual int init(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len); + inline const char *name(int tune) { + return _tune_names[tune]; + } + +private: + static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes + const char * _default_tunes[TONE_NUMBER_OF_TUNES]; + const char * _tune_names[TONE_NUMBER_OF_TUNES]; + static const uint8_t _note_tab[]; + + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + + const char *_user_tune; + + const char *_tune; // current tune string + const char *_next; // next note in the string + + unsigned _tempo; + unsigned _note_length; + enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; + unsigned _octave; + unsigned _silence_length; // if nonzero, silence before next note + bool _repeat; // if true, tune restarts at end + + hrt_call _note_call; // HRT callout for note completion + + // Convert a note value in the range C1 to B7 into a divisor for + // the configured timer's clock. + // + unsigned note_to_divisor(unsigned note); + + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of + // dots following in the play string. + // + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + + // Calculate the duration in microseconds of a rest corresponding to + // a given note length. + // + unsigned rest_duration(unsigned rest_length, unsigned dots); + + // Start playing the note + // + void start_note(unsigned note); + + // Stop playing the current note and make the player 'safe' + // + void stop_note(); + + // Start playing the tune + // + void start_tune(const char *tune); + + // Parse the next note out of the string and play it + // + void next_note(); + + // Find the next character in the string, discard any whitespace and + // return the canonical (uppercase) version. + // + int next_char(); + + // Extract a number from the string, consuming all the digit characters. + // + unsigned next_number(); + + // Consume dot characters from the string, returning the number consumed. + // + unsigned next_dots(); + + // hrt_call trampoline for next_note + // + static void next_trampoline(void *arg); + +}; + +// semitone offsets from C for the characters 'A'-'G' +const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); + + +ToneAlarm::ToneAlarm() : + VDev("tone_alarm", TONEALARM0_DEVICE_PATH), + _default_tune_number(0), + _user_tune(nullptr), + _tune(nullptr), + _next(nullptr) +{ + // enable debug() calls + //_debug_enabled = true; + _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone + _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< 0) { + stop_note(); + hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + _silence_length = 0; + return; + } + + // make sure we still have a tune - may be removed by the write / ioctl handler + if ((_next == nullptr) || (_tune == nullptr)) { + stop_note(); + return; + } + + // parse characters out of the string until we have resolved a note + unsigned note = 0; + unsigned note_length = _note_length; + unsigned duration; + + while (note == 0) { + // we always need at least one character from the string + int c = next_char(); + if (c == 0) + goto tune_end; + _next++; + + switch (c) { + case 'L': // select note length + _note_length = next_number(); + if (_note_length < 1) + goto tune_error; + break; + + case 'O': // select octave + _octave = next_number(); + if (_octave > 6) + _octave = 6; + break; + + case '<': // decrease octave + if (_octave > 0) + _octave--; + break; + + case '>': // increase octave + if (_octave < 6) + _octave++; + break; + + case 'M': // select inter-note gap + c = next_char(); + if (c == 0) + goto tune_error; + _next++; + switch (c) { + case 'N': + _note_mode = MODE_NORMAL; + break; + case 'L': + _note_mode = MODE_LEGATO; + break; + case 'S': + _note_mode = MODE_STACCATO; + break; + case 'F': + _repeat = false; + break; + case 'B': + _repeat = true; + break; + default: + goto tune_error; + } + break; + + case 'P': // pause for a note length + stop_note(); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); + return; + + case 'T': { // change tempo + unsigned nt = next_number(); + + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + } else { + goto tune_error; + } + break; + } + + case 'N': // play an arbitrary note + note = next_number(); + if (note > 84) + goto tune_error; + if (note == 0) { + // this is a rest - pause for the current note length + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; + } + break; + + case 'A'...'G': // play a note in the current octave + note = _note_tab[c - 'A'] + (_octave * 12) + 1; + c = next_char(); + switch (c) { + case '#': // up a semitone + case '+': + if (note < 84) + note++; + _next++; + break; + case '-': // down a semitone + if (note > 1) + note--; + _next++; + break; + default: + // 0 / no next char here is OK + break; + } + // shorthand length notation + note_length = next_number(); + if (note_length == 0) + note_length = _note_length; + break; + + default: + goto tune_error; + } + } + + // compute the duration of the note and the following silence (if any) + duration = note_duration(_silence_length, note_length, next_dots()); + + // start playing the note + start_note(note); + + // and arrange a callback when the note should stop + hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); + return; + + // tune looks bad (unexpected EOF, bad character, etc.) +tune_error: + printf("tune error\n"); + _repeat = false; // don't loop on error + + // stop (and potentially restart) the tune +tune_end: + stop_note(); + if (_repeat) { + start_tune(_tune); + } else { + _tune = nullptr; + _default_tune_number = 0; + } + return; +} + +int +ToneAlarm::next_char() +{ + while (isspace(*_next)) { + _next++; + } + return toupper(*_next); +} + +unsigned +ToneAlarm::next_number() +{ + unsigned number = 0; + int c; + + for (;;) { + c = next_char(); + if (!isdigit(c)) + return number; + _next++; + number = (number * 10) + (c - '0'); + } +} + +unsigned +ToneAlarm::next_dots() +{ + unsigned dots = 0; + + while (next_char() == '.') { + _next++; + dots++; + } + return dots; +} + +void +ToneAlarm::next_trampoline(void *arg) +{ + ToneAlarm *ta = (ToneAlarm *)arg; + + ta->next_note(); +} + + +int +ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + int result = OK; + + debug("ioctl %i %u", cmd, arg); + +// irqstate_t flags = irqsave(); + + /* decide whether to increase the alarm level to cmd or leave it alone */ + switch (cmd) { + case TONE_SET_ALARM: + debug("TONE_SET_ALARM %u", arg); + + if (arg < TONE_NUMBER_OF_TUNES) { + if (arg == TONE_STOP_TUNE) { + // stop the tune + _tune = nullptr; + _next = nullptr; + _repeat = false; + _default_tune_number = 0; + } else { + /* always interrupt alarms, unless they are repeating and already playing */ + if (!(_repeat && _default_tune_number == arg)) { + /* play the selected tune */ + _default_tune_number = arg; + start_tune(_default_tunes[arg]); + } + } + } else { + result = -EINVAL; + } + + break; + + default: + result = -ENOTTY; + break; + } + +// irqrestore(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) + result = VDev::ioctl(filp, cmd, arg); + + return result; +} + +ssize_t +ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) +{ + // sanity-check the buffer for length and nul-termination + if (len > _tune_max) + return -EFBIG; + + // if we have an existing user tune, free it + if (_user_tune != nullptr) { + + // if we are playing the user tune, stop + if (_tune == _user_tune) { + _tune = nullptr; + _next = nullptr; + } + + // free the old user tune + free((void *)_user_tune); + _user_tune = nullptr; + } + + // if the new tune is empty, we're done + if (buffer[0] == '\0') + return OK; + + // allocate a copy of the new tune + _user_tune = strndup(buffer, len); + if (_user_tune == nullptr) + return -ENOMEM; + + // and play it + start_tune(_user_tune); + + return len; +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +ToneAlarm *g_dev; + +int +play_tune(unsigned tune) +{ + int fd, ret; + + fd = px4_open(TONEALARM0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + return 1; + } + + ret = px4_ioctl(fd, TONE_SET_ALARM, tune); + px4_close(fd); + + if (ret != 0) { + warn("TONE_SET_ALARM"); + return 1; + } + + return ret; +} + +int +play_string(const char *str, bool free_buffer) +{ + int fd, ret; + + fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); + + if (fd < 0) { + warn("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + return 1; + } + + ret = px4_write(fd, str, strlen(str) + 1); + px4_close(fd); + + if (free_buffer) + free((void *)str); + + if (ret < 0) { + warn("play tune"); + return 1; + } + return ret; +} + +} // namespace + +int +tone_alarm_main(int argc, char *argv[]) +{ + unsigned tune; + int ret = 1; + + /* start the driver lazily */ + if (g_dev == nullptr) { + g_dev = new ToneAlarm; + + if (g_dev == nullptr) { + warnx("couldn't allocate the ToneAlarm driver"); + return 1; + } + + if (g_dev->init() != OK) { + delete g_dev; + warnx("ToneAlarm init failed"); + return 1; + } + } + + if (argc > 1) { + const char *argv1 = argv[1]; + + if (!strcmp(argv1, "start")) { + ret = play_tune(TONE_STOP_TUNE); + } + + else if (!strcmp(argv1, "stop")) + ret = play_tune(TONE_STOP_TUNE); + + else if ((tune = strtol(argv1, nullptr, 10)) != 0) + ret = play_tune(tune); + + /* If it is a file name then load and play it as a string */ + else if (*argv1 == '/') { + FILE *fd = fopen(argv1, "r"); + int sz; + char *buffer; + if (fd == nullptr) { + warnx("couldn't open '%s'", argv1); + return 1; + } + fseek(fd, 0, SEEK_END); + sz = ftell(fd); + fseek(fd, 0, SEEK_SET); + buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) { + warnx("not enough memory memory"); + return 1; + } + fread(buffer, sz, 1, fd); + /* terminate the string */ + buffer[sz] = 0; + ret = play_string(buffer, true); + } + + /* if it looks like a PLAY string... */ + else if (strlen(argv1) > 2) { + if (*argv1 == 'M') { + ret = play_string(argv1, false); + } + } + else { + /* It might be a tune name */ + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + if (!strcmp(g_dev->name(tune), argv1)) { + ret = play_tune(tune); + return ret; + } + warnx("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); + ret = 1; + } + } + + return ret; +} + From a284a7b6d932c2230309453a00f01a01e3fa6909 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 14:03:23 -0700 Subject: [PATCH 167/258] POSIX: added separators between commands run from shell Output a separator and the command called to make the ouput easier to read. Signed-off-by: Mark Charlebois --- src/platforms/posix/main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index b340ebb6d1..b76a12ef9c 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -53,6 +53,7 @@ typedef int (*px4_main_t)(int argc, char *argv[]); static void run_cmd(const vector &appargs) { // command is appargs[0] string command = appargs[0]; + cout << "----------------------------------\n"; if (apps.find(command) != apps.end()) { const char *arg[appargs.size()+2]; @@ -62,11 +63,12 @@ static void run_cmd(const vector &appargs) { ++i; } arg[i] = (char *)0; + cout << "Running: " << command << "\n"; apps[command](i,(char **)arg); } else { - cout << "Invalid command" << endl; + cout << "Invalid command: " << command << endl; list_builtins(); } } From 58595e2e78175f21e5b75bd872ce775acc8a1463 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 16:21:30 -0700 Subject: [PATCH 168/258] QuRT: fixed hard coded path in toolchain_hexagon.mk Signed-off-by: Mark Charlebois --- makefiles/toolchain_hexagon.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 75bcec41ba..57c5d1a3c8 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -85,7 +85,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -D__EXPORT= \ -Dnoreturn_function= \ -Drestrict= \ - -I/opt/6.4.05/gnu/hexagon/include \ + -I$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/include \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ -I$(PX4_BASE)/../dspal/include \ From 6ab25ae890cb1dda691b85fea876bf04cf932743 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 16:51:33 -0700 Subject: [PATCH 169/258] QuRT: workaround for __sync_bool_compare_and_swap The Hexagon compiler version does not support __sync_bool_compare_and_swap. Signed-off-by: Mark Charlebois --- src/drivers/device/ringbuffer.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index e77402b0d1..0da2467fd6 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -235,7 +235,20 @@ RingBuffer::force(double val) } // FIXME - clang crashes on this get() call -#ifndef __PX4_QURT +#ifdef __PX4_QURT +#define __PX4_SBCAP my_sync_bool_compare_and_swap +static bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c) +{ + if (*a == b) { + *a = c; + return true; + } + return false; +} + +#else +#define __PX4_SBCAP __sync_bool_compare_and_swap +#endif bool RingBuffer::get(void *val, size_t val_size) { @@ -258,14 +271,13 @@ RingBuffer::get(void *val, size_t val_size) memcpy(val, &_buf[candidate * _item_size], val_size); /* if the tail pointer didn't change, we got our item */ - } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); + } while (!__PX4_SBCAP(&_tail, candidate, next)); return true; } else { return false; } } -#endif bool RingBuffer::get(int8_t &val) From 09718fa3240fed2652bec891de222ecaf72a01a2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 21:58:54 -0700 Subject: [PATCH 170/258] Revamped debug macros Created px4_debug,h to define: PX4_DBG PX4_INFO PX4_WARN PX4_ERR These enable OS specific mappings to be made, filtering, etc. Signed-off-by: Mark Charlebois --- src/modules/simulator/simulator.cpp | 77 ++++----- .../posix/drivers/accelsim/accelsim.cpp | 34 ++-- src/platforms/posix/drivers/adcsim/adcsim.cpp | 11 +- .../posix/drivers/airspeedsim/airspeedsim.cpp | 4 +- src/platforms/posix/drivers/barosim/baro.cpp | 121 ++++++------- .../posix/drivers/gyrosim/gyrosim.cpp | 160 ++++++++++-------- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 26 +-- .../posix/px4_layer/px4_posix_tasks.cpp | 31 ++-- src/platforms/px4_debug.h | 71 ++++++++ src/platforms/px4_defines.h | 11 +- 10 files changed, 325 insertions(+), 221 deletions(-) create mode 100644 src/platforms/px4_debug.h diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index bbfba4c4c5..96af6aa134 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -36,6 +36,7 @@ * A device simulator */ +#include #include #include #include @@ -80,14 +81,14 @@ int Simulator::start(int argc, char *argv[]) int ret = 0; _instance = new Simulator(); if (_instance) { - printf("Simulator started\n"); + PX4_INFO("Simulator started\n"); drv_led_start(); #ifndef __PX4_QURT _instance->updateSamples(); #endif } else { - printf("Simulator creation failed\n"); + PX4_WARN("Simulator creation failed\n"); ret = 1; } return ret; @@ -99,54 +100,54 @@ void Simulator::updateSamples() { // get samples from external provider struct sockaddr_in myaddr; - struct sockaddr_in srcaddr; - socklen_t addrlen = sizeof(srcaddr); - int len, fd; + struct sockaddr_in srcaddr; + socklen_t addrlen = sizeof(srcaddr); + int len, fd; const int buflen = 200; const int port = 9876; - unsigned char buf[buflen]; + unsigned char buf[buflen]; - if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - printf("create socket failed\n"); + if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_WARN("create socket failed\n"); return; - } + } - memset((char *)&myaddr, 0, sizeof(myaddr)); - myaddr.sin_family = AF_INET; - myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - myaddr.sin_port = htons(port); + memset((char *)&myaddr, 0, sizeof(myaddr)); + myaddr.sin_family = AF_INET; + myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + myaddr.sin_port = htons(port); - if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { - printf("bind failed\n"); + if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + PX4_WARN("bind failed\n"); return; - } + } - for (;;) { - len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); - if (len > 0) { + for (;;) { + len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); + if (len > 0) { if (len == sizeof(RawMPUData)) { - printf("received: MPU data\n"); + PX4_DBG("received: MPU data\n"); _mpu.writeData(buf); } else if (len == sizeof(RawAccelData)) { - printf("received: accel data\n"); + PX4_DBG("received: accel data\n"); _accel.writeData(buf); } else if (len == sizeof(RawBaroData)) { - printf("received: accel data\n"); + PX4_DBG("received: accel data\n"); _baro.writeData(buf); } else { - printf("bad packet: len = %d\n", len); + PX4_DBG("bad packet: len = %d\n", len); } - } - } + } + } } #endif static void usage() { - warnx("Usage: simulator {start|stop}"); + PX4_WARN("Usage: simulator {start|stop}"); } extern "C" { @@ -172,7 +173,7 @@ int simulator_main(int argc, char *argv[]) } else if (strcmp(argv[1], "stop") == 0) { if (g_sim_task < 0) { - warnx("Simulator not running"); + PX4_WARN("Simulator not running"); } else { px4_task_delete(g_sim_task); @@ -196,37 +197,37 @@ extern void led_off(int led); extern void led_toggle(int led); __END_DECLS -bool static _led_state = false; +bool static _led_state[2] = { false , false }; __EXPORT void led_init() { - printf("LED_ON\n"); + PX4_DBG("LED_INIT\n"); } __EXPORT void led_on(int led) { - if (led == 1) + if (led == 1 || led == 0) { - printf("LED_ON\n"); - _led_state = true; + PX4_DBG("LED%d_ON", led); + _led_state[led] = true; } } __EXPORT void led_off(int led) { - if (led == 1) + if (led == 1 || led == 0) { - printf("LED_OFF\n"); - _led_state = false; + PX4_DBG("LED%d_OFF", led); + _led_state[led] = false; } } __EXPORT void led_toggle(int led) { - if (led == 1) + if (led == 1 || led == 0) { - _led_state = !_led_state; - printf("LED_TOGGLE: %s\n", _led_state ? "ON" : "OFF"); + _led_state[led] = !_led_state[led]; + PX4_DBG("LED%d_TOGGLE: %s\n", led, _led_state[led] ? "ON" : "OFF"); } } diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index b108f4969b..42e9eb0b1c 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -446,7 +446,7 @@ ACCELSIM::init() /* do SIM init first */ if (VDev::init() != OK) { - warnx("SIM init failed"); + PX4_WARN("SIM init failed"); goto out; } @@ -466,7 +466,7 @@ ACCELSIM::init() /* do VDev init for the mag device node */ ret = _mag->init(); if (ret != OK) { - warnx("MAG init failed"); + PX4_WARN("MAG init failed"); goto out; } @@ -482,7 +482,7 @@ ACCELSIM::init() &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { - warnx("ADVERT ERR"); + PX4_WARN("ADVERT ERR"); } @@ -497,7 +497,7 @@ ACCELSIM::init() &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == (orb_advert_t)(-1)) { - warnx("ADVERT ERR"); + PX4_WARN("ADVERT ERR"); } out: @@ -1261,7 +1261,7 @@ start(enum Rotation rotation) { int fd, fd_mag; if (g_dev != nullptr) { - warnx( "already started"); + PX4_WARN( "already started"); return 0; } @@ -1269,12 +1269,12 @@ start(enum Rotation rotation) g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); if (g_dev == nullptr) { - warnx("failed instantiating ACCELSIM obj"); + PX4_ERR("failed instantiating ACCELSIM obj"); goto fail; } if (OK != g_dev->init()) { - warnx("failed init of ACCELSIM obj"); + PX4_ERR("failed init of ACCELSIM obj"); goto fail; } @@ -1282,12 +1282,12 @@ start(enum Rotation rotation) fd = px4_open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) { - warnx("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); goto fail; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); px4_close(fd); goto fail; } @@ -1297,12 +1297,12 @@ start(enum Rotation rotation) /* don't fail if mag dev cannot be opened */ if (0 <= fd_mag) { if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } } else { - warnx("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } px4_close(fd); @@ -1316,7 +1316,7 @@ fail: g_dev = nullptr; } - warnx("driver start failed"); + PX4_ERR("driver start failed"); return 1; } @@ -1327,11 +1327,11 @@ int info() { if (g_dev == nullptr) { - warnx("driver not running\n"); + PX4_ERR("driver not running"); return 1; } - printf("state @ %p\n", g_dev); + PX4_DBG("state @ %p", g_dev); //g_dev->print_info(); return 0; @@ -1340,9 +1340,9 @@ info() void usage() { - warnx("Usage: accelsim 'start', 'info'"); - warnx("options:"); - warnx(" -R rotation"); + PX4_WARN("Usage: accelsim 'start', 'info'"); + PX4_WARN("options:"); + PX4_WARN(" -R rotation"); } } // namespace diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 893c30948c..fa6690ac21 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -241,7 +241,7 @@ test(void) int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - warnx("can't open ADCSIM device"); + PX4_ERR("can't open ADCSIM device"); return 1; } @@ -250,17 +250,16 @@ test(void) ssize_t count = px4_read(fd, data, sizeof(data)); if (count < 0) { - warnx("read error"); + PX4_ERR("read error"); return 1; } unsigned channels = count / sizeof(data[0]); for (unsigned j = 0; j < channels; j++) { - printf ("%d: %lu ", data[j].am_channel, (unsigned long)data[j].am_data); + PX4_INFO("%d: %lu ", data[j].am_channel, (unsigned long)data[j].am_data); } - printf("\n"); usleep(500000); } @@ -279,13 +278,13 @@ adcsim_main(int argc, char *argv[]) g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); if (g_adc == nullptr) { - warnx("couldn't allocate the ADCSIM driver"); + PX4_ERR("couldn't allocate the ADCSIM driver"); return 1; } if (g_adc->init() != OK) { delete g_adc; - warnx("ADCSIM init failed"); + PX4_ERR("ADCSIM init failed"); return 1; } } diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index 2a500af794..a0933971a4 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -147,7 +147,7 @@ AirspeedSim::init() _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub < 0) - warnx("uORB started?"); + PX4_WARN("uORB started?"); } ret = OK; @@ -395,7 +395,7 @@ AirspeedSim::print_info() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - warnx("poll interval: %u ticks", _measure_ticks); + PX4_INFO("poll interval: %u ticks", _measure_ticks); _reports->print_info("report queue"); } diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index eb6c6c29f7..751ffd43db 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -250,7 +250,7 @@ int BAROSIM::init() { int ret; - warnx("BAROSIM::init"); + PX4_WARN("BAROSIM::init"); ret = VDev::init(); if (ret != OK) { @@ -280,7 +280,7 @@ BAROSIM::init() /* do temperature first */ if (OK != measure()) { ret = -EIO; - warnx("temp measure failed"); + PX4_WARN("temp measure failed"); break; } @@ -288,14 +288,14 @@ BAROSIM::init() if (OK != collect()) { ret = -EIO; - warnx("temp collect failed"); + PX4_WARN("temp collect failed"); break; } /* now do a pressure measurement */ if (OK != measure()) { ret = -EIO; - warnx("pressure collect failed"); + PX4_WARN("pressure collect failed"); break; } @@ -303,7 +303,7 @@ BAROSIM::init() if (OK != collect()) { ret = -EIO; - warnx("pressure collect failed"); + PX4_WARN("pressure collect failed"); break; } @@ -316,9 +316,9 @@ BAROSIM::init() &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == (orb_advert_t)(-1)) { - warnx("failed to create sensor_baro publication"); + PX4_WARN("failed to create sensor_baro publication"); } - //warnx("sensor_baro publication %ld", _baro_topic); + //PX4_WARN("sensor_baro publication %ld", _baro_topic); } while (0); @@ -728,7 +728,7 @@ BAROSIM::collect() orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } else { - printf("BAROSIM::collect _baro_topic not initialized\n"); + PX4_ERR("BAROSIM::collect _baro_topic not initialized"); } } @@ -754,23 +754,23 @@ BAROSIM::print_info() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + PX4_INFO("poll interval: %u ticks", _measure_ticks); _reports->print_info("report queue"); - printf("TEMP: %ld\n", (long)_TEMP); - printf("SENS: %lld\n", (long long)_SENS); - printf("OFF: %lld\n", (long long)_OFF); - printf("P: %.3f\n", (double)_P); - printf("T: %.3f\n", (double)_T); - printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + PX4_INFO("TEMP: %ld", (long)_TEMP); + PX4_INFO("SENS: %lld", (long long)_SENS); + PX4_INFO("OFF: %lld", (long long)_OFF); + PX4_INFO("P: %.3f", (double)_P); + PX4_INFO("T: %.3f", (double)_T); + PX4_INFO("MSL pressure: %10.4f", (double)(_msl_pressure / 100.f)); - printf("factory_setup %u\n", _prom.factory_setup); - printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.serial_and_crc); + PX4_INFO("factory_setup %u", _prom.factory_setup); + PX4_INFO("c1_pressure_sens %u", _prom.c1_pressure_sens); + PX4_INFO("c2_pressure_offset %u", _prom.c2_pressure_offset); + PX4_INFO("c3_temp_coeff_pres_sens %u", _prom.c3_temp_coeff_pres_sens); + PX4_INFO("c4_temp_coeff_pres_offset %u", _prom.c4_temp_coeff_pres_offset); + PX4_INFO("c5_reference_temp %u", _prom.c5_reference_temp); + PX4_INFO("c6_temp_coeff_temp %u", _prom.c6_temp_coeff_temp); + PX4_INFO("serial_and_crc %u", _prom.serial_and_crc); } /** @@ -855,7 +855,7 @@ bool start_bus(struct barosim_bus_option &bus) { if (bus.dev != nullptr) { - warnx("bus option already started"); + PX4_ERR("bus option already started"); return false; } @@ -863,7 +863,7 @@ start_bus(struct barosim_bus_option &bus) device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); if (interface->init() != OK) { delete interface; - warnx("no device on bus %u", (unsigned)bus.busid); + PX4_ERR("no device on bus %u", (unsigned)bus.busid); return false; } @@ -871,7 +871,7 @@ start_bus(struct barosim_bus_option &bus) if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; - warnx("bus init failed %p", bus.dev); + PX4_ERR("bus init failed %p", bus.dev); return false; } @@ -879,12 +879,12 @@ start_bus(struct barosim_bus_option &bus) /* set the poll rate to default, starts automatic data collection */ if (fd == -1) { - warnx("can't open baro device"); + PX4_ERR("can't open baro device"); return false; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); - warnx("failed setting default poll rate"); + PX4_ERR("failed setting default poll rate"); return false; } @@ -907,7 +907,7 @@ start() started |= start_bus(bus_options[0]); if (!started) { - warnx("driver start failed"); + PX4_ERR("driver start failed"); return 1; } @@ -933,7 +933,7 @@ test() fd = px4_open(bus.devpath, O_RDONLY); if (fd < 0) { - warn("open failed (try 'barosim start' if the driver is not running)"); + PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; } @@ -941,25 +941,27 @@ test() sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("immediate read failed"); + PX4_ERR("immediate read failed"); return 1; } - warnx("single read"); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", (long long)report.timestamp); + PX4_INFO("single read"); + PX4_INFO("pressure: %10.4f", (double)report.pressure); + PX4_INFO("altitude: %11.4f", (double)report.altitude); + PX4_INFO("temperature: %8.4f", (double)report.temperature); + PX4_INFO("time: %lld", (long long)report.timestamp); /* set the queue depth to 10 */ if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { - warnx("failed to set queue depth"); + PX4_ERR("failed to set queue depth"); + px4_close(fd); return 1; } /* start the sensor polling at 2Hz */ if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - warnx("failed to set 2Hz poll rate"); + PX4_ERR("failed to set 2Hz poll rate"); + px4_close(fd); return 1; } @@ -973,26 +975,27 @@ test() ret = px4_poll(&fds, 1, 2000); if (ret != 1) { - warnx("timed out waiting for sensor data"); + PX4_WARN("timed out waiting for sensor data"); } /* now go get it */ sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("periodic read failed"); + PX4_ERR("periodic read failed"); + px4_close(fd); return 1; } - warnx("periodic read %u", i); - warnx("pressure: %10.4f", (double)report.pressure); - warnx("altitude: %11.4f", (double)report.altitude); - warnx("temperature: %8.4f", (double)report.temperature); - warnx("time: %lld", (long long)report.timestamp); + PX4_INFO("periodic read %u", i); + PX4_INFO("pressure: %10.4f", (double)report.pressure); + PX4_INFO("altitude: %11.4f", (double)report.altitude); + PX4_INFO("temperature: %8.4f", (double)report.temperature); + PX4_INFO("time: %lld", (long long)report.timestamp); } px4_close(fd); - warnx("PASS"); + PX4_INFO("PASS"); return 0; } @@ -1007,17 +1010,17 @@ reset() fd = px4_open(bus.devpath, O_RDONLY); if (fd < 0) { - warn("failed "); + PX4_ERR("failed "); return 1; } if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - warn("driver reset failed"); + PX4_ERR("driver reset failed"); return 1; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warn("driver poll restart failed"); + PX4_ERR("driver poll restart failed"); return 1; } return 0; @@ -1032,7 +1035,7 @@ info() for (uint8_t i=0; iprint_info(); } } @@ -1055,13 +1058,13 @@ calibrate(unsigned altitude) fd = px4_open(bus.devpath, O_RDONLY); if (fd < 0) { - warn("open failed (try 'barosim start' if the driver is not running)"); + PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; } /* start the sensor polling at max */ if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { - warnx("failed to set poll rate"); + PX4_ERR("failed to set poll rate"); return 1; } @@ -1079,7 +1082,7 @@ calibrate(unsigned altitude) ret = px4_poll(&fds, 1, 1000); if (ret != 1) { - warnx("timed out waiting for sensor data"); + PX4_ERR("timed out waiting for sensor data"); return 1; } @@ -1087,7 +1090,7 @@ calibrate(unsigned altitude) sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("sensor read failed"); + PX4_ERR("sensor read failed"); return 1; } @@ -1103,17 +1106,17 @@ calibrate(unsigned altitude) const float g = 9.80665f; /* gravity constant in m/s/s */ const float R = 287.05f; /* ideal gas constant in J/kg/K */ - warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); + PX4_INFO("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - warnx("calculated MSL pressure %10.4fkPa", (double)p1); + PX4_INFO("calculated MSL pressure %10.4fkPa", (double)p1); /* save as integer Pa */ p1 *= 1000.0f; if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { - warn("BAROIOCSMSLPRESSURE"); + PX4_WARN("BAROIOCSMSLPRESSURE"); return 1; } @@ -1124,7 +1127,7 @@ calibrate(unsigned altitude) void usage() { - warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate '"); + PX4_WARN("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate '"); } } // namespace @@ -1170,7 +1173,7 @@ barosim_main(int argc, char *argv[]) */ else if (!strcmp(verb, "calibrate")) { if (argc < 3) { - warnx("missing altitude"); + PX4_WARN("missing altitude"); barosim::usage(); return 1; } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 44e85150c0..e0d50ce25d 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -566,25 +566,25 @@ GYROSIM::init() /* if probe/setup failed, bail now */ if (ret != OK) { - warnx("VDev setup failed"); + PX4_WARN("VDev setup failed"); return ret; } /* allocate basic report buffers */ _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) { - warnx("_accel_reports creation failed"); + PX4_WARN("_accel_reports creation failed"); goto out; } _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) { - warnx("_gyro_reports creation failed"); + PX4_WARN("_gyro_reports creation failed"); goto out; } if (reset() != OK) { - warnx("reset failed"); + PX4_WARN("reset failed"); goto out; } @@ -625,7 +625,7 @@ GYROSIM::init() &_accel_orb_class_instance, ORB_PRIO_HIGH); if (_accel_topic < 0) { - warnx("ADVERT FAIL"); + PX4_WARN("ADVERT FAIL"); } @@ -637,7 +637,7 @@ GYROSIM::init() &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); if (_gyro->_gyro_topic < 0) { - warnx("ADVERT FAIL"); + PX4_WARN("ADVERT FAIL"); } out: @@ -669,11 +669,11 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } else if (cmd & DIR_READ) { - printf("Reading %u bytes from register %u\n", len-1, reg); + PX4_DBG("Reading %u bytes from register %u", len-1, reg); memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); } else { - printf("Writing %u bytes to register %u\n", len-1, reg); + PX4_DBG("Writing %u bytes to register %u", len-1, reg); if (recv) memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); } @@ -1509,31 +1509,38 @@ GYROSIM::print_info() perf_print_counter(_reset_retries); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - ::printf("checked_next: %u\n", _checked_next); + PX4_WARN("checked_next: %u", _checked_next); for (uint8_t i=0; iprint_info(); return 0; @@ -1805,11 +1833,11 @@ regdump() { GYROSIM **g_dev_ptr = &g_dev_sim; if (*g_dev_ptr == nullptr) { - warnx("driver not running"); + PX4_ERR("driver not running"); return 1; } - printf("regdump @ %p\n", *g_dev_ptr); + PX4_INFO("regdump @ %p", *g_dev_ptr); (*g_dev_ptr)->print_registers(); return 0; @@ -1818,9 +1846,9 @@ regdump() void usage() { - warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump'"); - warnx("options:"); - warnx(" -R rotation"); + PX4_WARN("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'"); + PX4_WARN("options:"); + PX4_WARN(" -R rotation"); } } // namespace diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index 7c9e8cc912..def293ea4c 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -331,6 +331,10 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) return rest_period; } +static void do_something(unsigned x) +{ +} + void ToneAlarm::start_note(unsigned note) { @@ -344,7 +348,9 @@ ToneAlarm::start_note(unsigned note) // calculate the timer period for the selected prescaler value unsigned period = (divisor / (prescale + 1)) - 1; - printf("ToneAlarm::start_note %u\n", period); + // Silence warning of unused var + do_something(period); + PX4_DBG("ToneAlarm::start_note %u", period); } void @@ -688,7 +694,7 @@ play_tune(unsigned tune) fd = px4_open(TONEALARM0_DEVICE_PATH, 0); if (fd < 0) { - warnx("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); return 1; } @@ -696,7 +702,7 @@ play_tune(unsigned tune) px4_close(fd); if (ret != 0) { - warn("TONE_SET_ALARM"); + PX4_WARN("TONE_SET_ALARM"); return 1; } @@ -711,7 +717,7 @@ play_string(const char *str, bool free_buffer) fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); if (fd < 0) { - warn("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); return 1; } @@ -722,7 +728,7 @@ play_string(const char *str, bool free_buffer) free((void *)str); if (ret < 0) { - warn("play tune"); + PX4_WARN("play tune"); return 1; } return ret; @@ -741,13 +747,13 @@ tone_alarm_main(int argc, char *argv[]) g_dev = new ToneAlarm; if (g_dev == nullptr) { - warnx("couldn't allocate the ToneAlarm driver"); + PX4_WARN("couldn't allocate the ToneAlarm driver"); return 1; } if (g_dev->init() != OK) { delete g_dev; - warnx("ToneAlarm init failed"); + PX4_WARN("ToneAlarm init failed"); return 1; } } @@ -771,7 +777,7 @@ tone_alarm_main(int argc, char *argv[]) int sz; char *buffer; if (fd == nullptr) { - warnx("couldn't open '%s'", argv1); + PX4_WARN("couldn't open '%s'", argv1); return 1; } fseek(fd, 0, SEEK_END); @@ -779,7 +785,7 @@ tone_alarm_main(int argc, char *argv[]) fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); if (buffer == nullptr) { - warnx("not enough memory memory"); + PX4_WARN("not enough memory memory"); return 1; } fread(buffer, sz, 1, fd); @@ -801,7 +807,7 @@ tone_alarm_main(int argc, char *argv[]) ret = play_tune(tune); return ret; } - warnx("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); + PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); ret = 1; } } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index fd58637273..5b8f4ea666 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -37,6 +37,7 @@ * Implementation of existing task API for Linux */ +#include #include #include #include @@ -82,9 +83,9 @@ static void *entry_adapter ( void *ptr ) data->entry(data->argc, data->argv); free(ptr); - printf("Before px4_task_exit\n"); + PX4_DBG("Before px4_task_exit"); px4_task_exit(0); - printf("After px4_task_exit\n"); + PX4_DBG("After px4_task_exit"); return NULL; } @@ -92,7 +93,7 @@ static void *entry_adapter ( void *ptr ) void px4_systemreset(bool to_bootloader) { - printf("Called px4_system_reset\n"); + PX4_WARN("Called px4_system_reset"); } px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) @@ -138,17 +139,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_attr_init(&attr); if (rv != 0) { - printf("px4_task_spawn_cmd: failed to init thread attrs\n"); + PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set inherit sched\n"); + PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } rv = pthread_attr_setschedpolicy(&attr, scheduler); if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set sched policy\n"); + PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; } @@ -156,7 +157,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_attr_setschedparam(&attr, ¶m); if (rv != 0) { - printf("px4_task_spawn_cmd: failed to set sched param\n"); + PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } @@ -167,7 +168,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); if (rv != 0) { - printf("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); return (rv < 0) ? rv : -rv; } } @@ -194,7 +195,7 @@ int px4_task_delete(px4_task_t id) { int rv = 0; pthread_t pid; - printf("Called px4_task_delete\n"); + PX4_WARN("Called px4_task_delete"); if (id < PX4_MAX_TASKS && taskmap[id].isused) pid = taskmap[id].pid; @@ -227,9 +228,9 @@ void px4_task_exit(int ret) } } if (i>=PX4_MAX_TASKS) - printf("px4_task_exit: self task not found!\n"); + PX4_ERR("px4_task_exit: self task not found!"); else - printf("px4_task_exit: %s\n", taskmap[i].name.c_str()); + PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); pthread_exit((void *)(unsigned long)ret); } @@ -249,7 +250,7 @@ int px4_task_kill(px4_task_t id, int sig) { int rv = 0; pthread_t pid; - //printf("Called px4_task_delete\n"); + PX4_DBG("Called px4_task_kill %d", sig); if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) pid = taskmap[id].pid; @@ -267,15 +268,15 @@ void px4_show_tasks() int idx; int count = 0; - printf("Active Tasks:\n"); + PX4_INFO("Active Tasks:"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %lu\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } if (count == 0) - printf(" No running tasks\n"); + PX4_INFO(" No running tasks"); } diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_debug.h new file mode 100644 index 0000000000..0f5b5ec34c --- /dev/null +++ b/src/platforms/px4_debug.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file px4_debug.h + * Platform dependant debug + */ + +#pragma once + +#if defined(__PX4_LINUX) || defined(__PX4_QURT) +#include + +#define PX4_DBG(...) +#define PX4_INFO(...) warnx(__VA_ARGS__) +#define PX4_WARN(...) warnx(__VA_ARGS__) +#define PX4_ERR(...) { warnx("ERROR %s %s:", __FILE__, __LINE__); warnx(__VA_ARGS__); } + +#elif defined(__PX4_ROS) + +#define PX4_DBG(...) +#define PX4_INFO(...) ROS_WARN(__VA_ARGS__) +#define PX4_WARN(...) ROS_WARN(__VA_ARGS__) +#define PX4_ERR(...) ROS_WARN(__VA_ARGS__) + +#elif defined(__PX4_NUTTX) +#include + +#define PX4_DBG(...) +#define PX4_INFO(...) warnx(__VA_ARGS__) +#define PX4_WARN(...) warnx(__VA_ARGS__) +#define PX4_ERR(...) warnx(__VA_ARGS__) + +#else + +#define PX4_DBG(...) +#define PX4_WARN(...) +#define PX4_INFO(...) +#define PX4_ERR(...) + +#endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 8fcc9695ad..1b2cf1dea5 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -38,6 +38,9 @@ */ #pragma once + +#include + /* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT @@ -60,10 +63,6 @@ /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) -/* Print/output wrappers */ -#define PX4_WARN ROS_WARN -#define PX4_INFO ROS_INFO - /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) @@ -75,10 +74,6 @@ /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) -/* Print/output wrappers */ -#define PX4_WARN warnx -#define PX4_INFO warnx - /* Parameter handle datatype */ #include typedef param_t px4_param_t; From 0bf690d36a801dd369c9c19fce39f55a172acca8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 22:15:18 -0700 Subject: [PATCH 171/258] Used new debug macros for sim.cpp sim.cpp was causing the posix shell to have continuous debug output. Used debug macros to suppress output Signed-off-by: Mark Charlebois --- src/drivers/device/sim.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp index f1040ea8e7..75127d0e2d 100644 --- a/src/drivers/device/sim.cpp +++ b/src/drivers/device/sim.cpp @@ -40,6 +40,7 @@ * that is supplied. Should we just depend on the bus knowing? */ +#include #include #include #include @@ -62,7 +63,7 @@ SIM::SIM(const char *name, _devname(devname) { - printf("SIM::SIM name = %s devname = %s\n", name, devname); + PX4_DBG("SIM::SIM name = %s devname = %s", name, devname); // fill in _device_id fields for a SIM device _device_id.devid_s.bus_type = DeviceBusType_SIM; _device_id.devid_s.bus = bus; @@ -87,7 +88,7 @@ SIM::init() ret = Device::init(); if (ret != PX4_OK) { - debug("VDev::init failed"); + PX4_ERR("VDev::init failed"); return ret; } @@ -98,16 +99,16 @@ int SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { if (send_len > 0) { - printf("SIM: sending %d bytes\n", send_len); + PX4_DBG("SIM: sending %d bytes", send_len); } if (recv_len > 0) { - printf("SIM: receiving %d bytes\n", recv_len); + PX4_DBG("SIM: receiving %d bytes", recv_len); // TODO - write data to recv; } - printf("I2C SIM: transfer_4 on %s\n", _devname); + PX4_DBG("I2C SIM: transfer_4 on %s", _devname); return PX4_OK; } From c832f4c55c07a8ebfc918724038671ecc4c0eefb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 27 Apr 2015 22:28:39 -0700 Subject: [PATCH 172/258] Small fixes for debug macros Fixed print format for __LINE__ to %d Fixed if/else that breaks with the debug macro expansion. The if/else needs to use braces to allow macro expansion. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 6 ++++-- src/platforms/posix/px4_layer/work_thread.c | 3 --- src/platforms/px4_debug.h | 2 +- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 5b8f4ea666..54fc555e0b 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -227,10 +227,12 @@ void px4_task_exit(int ret) break; } } - if (i>=PX4_MAX_TASKS) + if (i>=PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - else + } + else { PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); + } pthread_exit((void *)(unsigned long)ret); } diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c index d42b21fafd..57e6e327f1 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -85,9 +85,6 @@ struct wqueue_s g_work[NWORKERS]; * ****************************************************************************/ -#define USEC_TO_TICKS(x) (uint32_t)((x)/1000ull) -#define TICKS_TO_USEC(x) ((x)*1000ull) - static void work_process(FAR struct wqueue_s *wqueue) { volatile FAR struct work_s *work; diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_debug.h index 0f5b5ec34c..2776ad5337 100644 --- a/src/platforms/px4_debug.h +++ b/src/platforms/px4_debug.h @@ -44,7 +44,7 @@ #define PX4_DBG(...) #define PX4_INFO(...) warnx(__VA_ARGS__) #define PX4_WARN(...) warnx(__VA_ARGS__) -#define PX4_ERR(...) { warnx("ERROR %s %s:", __FILE__, __LINE__); warnx(__VA_ARGS__); } +#define PX4_ERR(...) { warnx("ERROR file %s line %d:", __FILE__, __LINE__); warnx(__VA_ARGS__); } #elif defined(__PX4_ROS) From 2446dfec165d40de76fcd22366cc5dfb67d974b4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 28 Apr 2015 12:28:10 -0700 Subject: [PATCH 173/258] Fixups after merge from master MuORB was missing the orb_exists() function added to uORB.cpp gyro_calibration.cpp still had some merge conflicts that had not been resolved. Signed-off-by: Mark Charlebois --- src/modules/commander/gyro_calibration.cpp | 63 ++++------------------ src/modules/uORB/MuORB.cpp | 20 +++++++ 2 files changed, 30 insertions(+), 53 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 1f45c4be5c..d444553b31 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -85,7 +85,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient struct gyro_report gyro_report; unsigned poll_errcount = 0; - struct pollfd fds[max_gyros]; + px4_pollfd_struct_t fds[max_gyros]; for (unsigned s = 0; s < max_gyros; s++) { fds[s].fd = worker_data->gyro_sensor_sub[s]; fds[s].events = POLLIN; @@ -182,25 +182,16 @@ int do_gyro_calibration(int mavlink_fd) // Reset all offsets to 0 and scales to 1 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale)); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd >= 0) { worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - close(fd); + px4_close(fd); -<<<<<<< HEAD - for (unsigned s = 0; s < max_gyros; s++) { - px4_close(sub_sensor_gyro[s]); - - gyro_scale[s].x_offset /= calibration_counter[s]; - gyro_scale[s].y_offset /= calibration_counter[s]; - gyro_scale[s].z_offset /= calibration_counter[s]; -======= if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); return ERROR; } ->>>>>>> upstream/master } } @@ -223,7 +214,7 @@ int do_gyro_calibration(int mavlink_fd) calibrate_cancel_unsubscribe(cancel_sub); for (unsigned s = 0; s < max_gyros; s++) { - close(worker_data.gyro_sensor_sub[s]); + px4_close(worker_data.gyro_sensor_sub[s]); } if (cal_return == calibrate_return_cancelled) { @@ -242,15 +233,9 @@ int do_gyro_calibration(int mavlink_fd) /* maximum allowable calibration error in radians */ const float maxoff = 0.0055f; -<<<<<<< HEAD - if (!PX4_ISFINITE(gyro_scale[0].x_offset) || - !PX4_ISFINITE(gyro_scale[0].y_offset) || - !PX4_ISFINITE(gyro_scale[0].z_offset) || -======= - if (!isfinite(worker_data.gyro_scale[0].x_offset) || - !isfinite(worker_data.gyro_scale[0].y_offset) || - !isfinite(worker_data.gyro_scale[0].z_offset) || ->>>>>>> upstream/master + if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || + !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) || + !PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) || fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { @@ -278,43 +263,15 @@ int do_gyro_calibration(int mavlink_fd) /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { failed = true; continue; } -<<<<<<< HEAD - /* if any reasonable amount of data is missing, skip */ - if (calibration_counter[s] < calibration_count / 2) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].x_offset))); - (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].y_offset))); - (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - failed |= (OK != param_set_no_notification(param_find(str), &(gyro_scale[s].z_offset))); - (void)sprintf(str, "CAL_GYRO%u_ID", s); - failed |= (OK != param_set_no_notification(param_find(str), &(device_id[s]))); - - /* apply new scaling and offsets */ - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); - - if (fd < 0) { - failed = true; - continue; - } - - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]); - px4_close(fd); -======= - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]); - close(fd); ->>>>>>> upstream/master + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]); + px4_close(fd); if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index 528212dc12..a835ffba11 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -1235,6 +1236,25 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve } // namespace +int +orb_exists(const struct orb_metadata *meta, int instance) +{ + /* + * Generate the path to the node and try to open it. + */ + char path[orb_maxpath]; + int inst = instance; + int ret = node_mkpath(path, PUBSUB, meta, &inst); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + struct stat buffer; + return stat(path, &buffer); +} + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { From 523a4aa7851d364b9bbd3f008221bed5898f53c4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 28 Apr 2015 12:29:50 -0700 Subject: [PATCH 174/258] Clang warning fix Clang build fails due to -Werr and warning on use of if (!condition != other_condition && some_condition) Clang wants to be clear that the initial '!' wasn't intended for the whole expression. Signed-off-by: Mark Charlebois --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73411bb1c3..e4644c03a3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -546,7 +547,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s else { // Refuse to arm if preflight checks have failed - if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { + if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed."); cmd_result = VEHICLE_CMD_RESULT_DENIED; break; From c6226366018f04fb5db6eaaf29ca4bbce5a01f34 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 28 Apr 2015 12:49:04 -0700 Subject: [PATCH 175/258] Nuttx: fixed include of systemlib/err.h The new px4_debug.h included "err.h" instead of "systemlib/err.h" for NuttX. Signed-off-by: Mark Charlebois --- src/platforms/px4_debug.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_debug.h index 2776ad5337..0331875096 100644 --- a/src/platforms/px4_debug.h +++ b/src/platforms/px4_debug.h @@ -39,6 +39,7 @@ #pragma once #if defined(__PX4_LINUX) || defined(__PX4_QURT) + #include #define PX4_DBG(...) @@ -54,7 +55,8 @@ #define PX4_ERR(...) ROS_WARN(__VA_ARGS__) #elif defined(__PX4_NUTTX) -#include + +#include #define PX4_DBG(...) #define PX4_INFO(...) warnx(__VA_ARGS__) From b408983d4c385216f6863ccd30ff62b6aa70332b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 28 Apr 2015 17:27:14 -0700 Subject: [PATCH 176/258] mavlink: added back MODULE_COMMAND The MODULE_COMMAND was inadvertently removed during merge of master Signed-off-by: Mark Charlebois --- src/modules/mavlink/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 0b035f0258..7b2905f6b7 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -35,6 +35,8 @@ # MAVLink protocol to uORB interface process # +MODULE_COMMAND = mavlink + SRCS += mavlink.c \ mavlink_main.cpp \ mavlink_mission.cpp \ From a209fdc8efe6fcd4bcd0d0fb81bd53d22eb955ec Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 29 Apr 2015 17:04:30 -0700 Subject: [PATCH 177/258] Added missing lock() unlock() to MuORB The commented out lock and unlock were determined to be needed and added back. The unit test for VDev was updated. It showed the race between the poll and a write that only does a poll_notify(). Signed-off-by: Mark Charlebois --- src/modules/uORB/MuORB.cpp | 6 +- .../tests/vcdev_test/vcdevtest_example.cpp | 67 +++++++++++-------- 2 files changed, 42 insertions(+), 31 deletions(-) diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index a835ffba11..5bf8e06a26 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -310,8 +310,7 @@ ORBDevNode::read(device::file_t *filp, char *buffer, size_t buflen) /* * Perform an atomic copy & state update */ - // FIXME - This used to disable interrupts - //lock(); + lock(); /* if the caller doesn't want the data, don't give it to them */ if (nullptr != buffer) @@ -329,7 +328,7 @@ ORBDevNode::read(device::file_t *filp, char *buffer, size_t buflen) */ sd->update_reported = false; - //unlock(); + unlock(); return _meta->o_size; } @@ -366,7 +365,6 @@ ORBDevNode::write(device::file_t *filp, const char *buffer, size_t buflen) return -EIO; /* Perform an atomic copy. */ - // FIXME - make sure lock is what we want here lock(); memcpy(_data, buffer, _meta->o_size); unlock(); diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index c773da8ba1..c7652c3399 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -41,7 +41,8 @@ #include #include "vcdevtest_example.h" -#include "drivers/device/device.h" +#include +#include #include #include @@ -61,28 +62,35 @@ static int writer_main(int argc, char *argv[]) return -px4_errno; } - // Wait for 3 seconds - printf("--- Sleeping for 3 sec\n"); - int ret = sleep(3); - if (ret < 0) { - printf("--- usleep failed %d %d\n", ret, errno); - return ret; - } + int ret; + int i=0; + while (i<3) { + // Wait for 3 seconds + printf("--- Sleeping for 4 sec\n"); + ret = sleep(4); + if (ret < 0) { + printf("--- sleep failed %d %d\n", ret, errno); + return ret; + } - printf("--- writing to fd\n"); - return px4_write(fd, buf, 1); + printf("--- writing to fd\n"); + ret = px4_write(fd, buf, 1); + ++i; + } + px4_close(fd); + return ret; } -class VCDevNode : public CDev { +class VCDevNode : public VDev { public: - VCDevNode() : CDev("vcdevtest", TESTDEV) {}; + VCDevNode() : VDev("vcdevtest", TESTDEV) {}; ~VCDevNode() {} - virtual ssize_t write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen); + virtual ssize_t write(device::file_t *handlep, const char *buffer, size_t buflen); }; -ssize_t VCDevNode::write(px4_dev_handle_t *handlep, const char *buffer, size_t buflen) +ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen) { // ignore what was written, but let pollers know something was written poll_notify(POLLIN); @@ -99,13 +107,13 @@ VCDevExample::~VCDevExample() { static int test_pub_block(int fd, unsigned long blocked) { - int ret = px4_ioctl(fd, PX4_DEVIOCSPUBBLOCK, blocked); + int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked); if (ret < 0) { printf("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } - ret = px4_ioctl(fd, PX4_DEVIOCGPUBBLOCK, 0); + ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0); if (ret < 0) { printf("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; @@ -139,9 +147,9 @@ int VCDevExample::main() } void *p = 0; - int ret = px4_ioctl(fd, PX4_DIOC_GETPRIV, (unsigned long)&p); + int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p); if (ret < 0) { - printf("ioctl PX4_DIOC_GETPRIV failed %d %d", ret, px4_errno); + printf("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } printf("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); @@ -164,28 +172,33 @@ int VCDevExample::main() writer_main, (char* const*)NULL); - while (!appState.exitRequested() && i<3) { + while (!appState.exitRequested() && i<13) { + printf("=====================\n"); + printf("==== sleeping 2 sec ====\n"); sleep(2); - printf(" polling...\n"); fds[0].fd = fd; fds[0].events = POLLIN; fds[0].revents = 0; - printf(" Calling Poll\n"); + printf("==== Calling Poll\n"); ret = px4_poll(fds, 1, 1000); - printf(" Done poll\n"); + printf("==== Done poll\n"); if (ret < 0) { - printf("poll failed %d %d\n", ret, px4_errno); + printf("==== poll failed %d %d\n", ret, px4_errno); px4_close(fd); } - else if (i > 0 && ret == 0) - printf(" Nothing to read - PASS\n"); + else if (i > 0) { + if (ret == 0) + printf("==== Nothing to read - PASS\n"); + else + printf("==== poll returned %d\n", ret); + } else if (i == 0) { if (ret == 1) - printf(" %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); + printf("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); else - printf(" %d to read - FAIL\n", ret); + printf("==== %d to read - FAIL\n", ret); } ++i; From 93a3eeb56988157bcfd17f238992ceef5ade1736 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 29 Apr 2015 23:45:54 -0700 Subject: [PATCH 178/258] Simulator: Added Roman's sensors combined topic Simulator can work as before with -s flag or with Roman's additions to publish the sensors combined topic using -p flag. Signed-off-by: Mark Charlebois --- posix-configs/posixtest/init/rc.S | 2 +- src/modules/simulator/simulator.cpp | 121 ++++++++++++++++++++++++---- src/modules/simulator/simulator.h | 13 +++ 3 files changed, 118 insertions(+), 18 deletions(-) diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index 8ae514fec9..896062639e 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -1,5 +1,5 @@ uorb start -simulator start +simulator start -s barosim start adcsim start accelsim start diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 96af6aa134..3f6f763193 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -49,6 +49,7 @@ #include #endif #include "simulator.h" +#include using namespace simulator; @@ -83,9 +84,13 @@ int Simulator::start(int argc, char *argv[]) if (_instance) { PX4_INFO("Simulator started\n"); drv_led_start(); + if (argv[2][1] == 's') { #ifndef __PX4_QURT - _instance->updateSamples(); + _instance->updateSamples(); #endif + } else { + _instance->publishSensorsCombined(); + } } else { PX4_WARN("Simulator creation failed\n"); @@ -94,6 +99,71 @@ int Simulator::start(int argc, char *argv[]) return ret; } +void Simulator::publishSensorsCombined() { + + struct baro_report baro; + memset(&baro,0,sizeof(baro)); + baro.pressure = 120000.0f; + + // acceleration report + struct accel_report accel; + memset(&accel,0,sizeof(accel)); + accel.z = 9.81f; + accel.range_m_s2 = 80.0f; + + // gyro report + struct gyro_report gyro; + memset(&gyro, 0 ,sizeof(gyro)); + + // mag report + struct mag_report mag; + memset(&mag, 0 ,sizeof(mag)); + // init publishers + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + // fill sensors with some data + sensors.accelerometer_m_s2[2] = 9.81f; + sensors.magnetometer_ga[0] = 0.2f; + sensors.timestamp = hrt_absolute_time(); + sensors.accelerometer_timestamp = hrt_absolute_time(); + sensors.magnetometer_timestamp = hrt_absolute_time(); + sensors.baro_timestamp = hrt_absolute_time(); + // advertise + _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors); + + + hrt_abstime time_last = hrt_absolute_time(); + uint64_t delta; + for(;;) { + delta = hrt_absolute_time() - time_last; + if(delta > (uint64_t)1000000) { + time_last = hrt_absolute_time(); + sensors.timestamp = time_last; + sensors.accelerometer_timestamp = time_last; + sensors.magnetometer_timestamp = time_last; + sensors.baro_timestamp = time_last; + baro.timestamp = time_last; + accel.timestamp = time_last; + gyro.timestamp = time_last; + mag.timestamp = time_last; + // publish the sensor values + //printf("Publishing SensorsCombined\n"); + orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &sensors); + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + else { + usleep(1000000-delta); + } + } +} #ifndef __PX4_QURT void Simulator::updateSamples() @@ -147,7 +217,9 @@ void Simulator::updateSamples() static void usage() { - PX4_WARN("Usage: simulator {start|stop}"); + PX4_WARN("Usage: simulator {start -[sc] |stop}"); + PX4_WARN("Simulate raw sensors: simulator start -s"); + PX4_WARN("Publish sensors combined: simulator start -p"); } extern "C" { @@ -155,23 +227,38 @@ extern "C" { int simulator_main(int argc, char *argv[]) { int ret = 0; - if (argc != 2) { - usage(); - return 1; - } - if (strcmp(argv[1], "start") == 0) { - if (g_sim_task >= 0) { - warnx("Simulator already started"); - return 0; + if (argc == 3 && strcmp(argv[1], "start") == 0) { + if (strcmp(argv[2], "-s") == 0) { + if (g_sim_task >= 0) { + warnx("Simulator already started"); + return 0; + } + g_sim_task = px4_task_spawn_cmd("Simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + Simulator::start, + argv); + } + else if (strcmp(argv[2], "-p") == 0) { + if (g_sim_task >= 0) { + warnx("Simulator already started"); + return 0; + } + g_sim_task = px4_task_spawn_cmd("Simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + Simulator::start, + argv); + } + else + { + usage(); + ret = -EINVAL; } - g_sim_task = px4_task_spawn_cmd("Simulator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - Simulator::start, - nullptr); } - else if (strcmp(argv[1], "stop") == 0) { + else if (argc == 2 && strcmp(argv[1], "stop") == 0) { if (g_sim_task < 0) { PX4_WARN("Simulator not running"); } diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index f27b2fe017..d2ca2fa8ab 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -39,6 +39,12 @@ #pragma once #include +#include +#include +#include +#include +#include +#include namespace simulator { @@ -151,11 +157,18 @@ private: #ifndef __PX4_QURT void updateSamples(); #endif + void publishSensorsCombined(); static Simulator *_instance; simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; + + orb_advert_t _accel_pub; + orb_advert_t _baro_pub; + orb_advert_t _gyro_pub; + orb_advert_t _mag_pub; + orb_advert_t _sensor_combined_pub; }; From b7120f1b9f7f1bf74e0d3fd4425fe44c7b483fd2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 30 Apr 2015 09:02:04 -0700 Subject: [PATCH 179/258] Fixed call to poll to be px4_poll Signed-off-by: Mark Charlebois --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 6ac1192744..4ed9dd4d39 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -767,7 +767,7 @@ MulticopterAttitudeControl::task_main() while (!_task_should_exit) { /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) From 2f8cad6c00f5c4d90d11cdc04e1323c0ad719a7b Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 30 Apr 2015 09:12:39 -0700 Subject: [PATCH 180/258] Fixed bad update of poll to px4_poll change I updated poll to px4_poll but forgot to change struct pollfd to px4_pollfd_struct_t. Signed-off-by: Mark Charlebois --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 4ed9dd4d39..b432ecad6c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -759,7 +760,7 @@ MulticopterAttitudeControl::task_main() parameters_update(); /* wakeup source: vehicle attitude */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = _v_att_sub; fds[0].events = POLLIN; From 5557ecf3d71fa22eb790fd261a1702d2153e7e72 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 30 Apr 2015 13:16:03 -0700 Subject: [PATCH 181/258] POSIX: Added airspeed simulator This seems to be a dependency for the system to start up. Signed-off-by: Mark Charlebois --- makefiles/posix/config_posix_default.mk | 5 ++- src/drivers/airspeed/airspeed.h | 9 +++- .../posix/drivers/airspeedsim/airspeedsim.cpp | 41 +++++++++---------- 3 files changed, 29 insertions(+), 26 deletions(-) diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index a9bb98d6c9..8c2d84897a 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -64,12 +64,13 @@ MODULES += platforms/posix/drivers/gyrosim MODULES += platforms/posix/drivers/adcsim MODULES += platforms/posix/drivers/barosim MODULES += platforms/posix/drivers/tonealrmsim +MODULES += platforms/posix/drivers/airspeedsim # # Unit tests # #MODULES += platforms/posix/tests/hello #MODULES += platforms/posix/tests/vcdev_test -MODULES += platforms/posix/tests/hrt_test -MODULES += platforms/posix/tests/wqueue +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index b440338cf9..3fbf3f9ddf 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -55,9 +55,14 @@ #include #include +#ifdef __PX4_NUTTX #include #include #include +# +#else +#include +#endif #include @@ -95,8 +100,8 @@ public: virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index a0933971a4..ea9d92025e 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -34,8 +34,9 @@ /** * @file ets_airspeed.cpp * @author Simon Wilks + * @author Mark Charlebois (simulator) * - * Driver for the Eagle Tree Airspeed V3 connected via I2C. + * Driver for the Simulated Airspeed Sensor based on Eagle Tree Airspeed V3. */ #include @@ -55,10 +56,6 @@ #include #include -#include -#include -#include - #include #include @@ -76,7 +73,7 @@ #include -AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) : +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : I2C("Airspeed", path, bus, address), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -100,7 +97,7 @@ AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, con memset(&_work, 0, sizeof(_work)); } -AirspeedSim::~AirspeedSim() +Airspeed::~Airspeed() { /* make sure we are truly inactive */ stop(); @@ -119,7 +116,7 @@ AirspeedSim::~AirspeedSim() } int -AirspeedSim::init() +Airspeed::init() { int ret = ERROR; @@ -157,7 +154,7 @@ out: } int -AirspeedSim::probe() +Airspeed::probe() { /* on initial power up the device may need more than one retry for detection. Once it is running the number of retries can @@ -172,7 +169,7 @@ AirspeedSim::probe() } int -AirspeedSim::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) +Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -243,12 +240,12 @@ AirspeedSim::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) if ((arg < 1) || (arg > 100)) return -EINVAL; - irqstate_t flags = irqsave(); + //irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { - irqrestore(flags); + //irqrestore(flags); return -ENOMEM; } - irqrestore(flags); + //irqrestore(flags); return OK; } @@ -275,12 +272,12 @@ AirspeedSim::ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg) default: /* give it to the superclass */ - return I2C::ioctl(handlep, cmd, arg); + return I2C::ioctl(filp, cmd, arg); } } ssize_t -AirspeedSim::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) +Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(differential_pressure_s); differential_pressure_s *abuf = reinterpret_cast(buffer); @@ -339,24 +336,24 @@ AirspeedSim::read(px4_dev_handle_t *handlep, char *buffer, size_t buflen) } void -AirspeedSim::start() +Airspeed::start() { /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); } void -AirspeedSim::stop() +Airspeed::stop() { work_cancel(HPWORK, &_work); } void -AirspeedSim::update_status() +Airspeed::update_status() { if (_sensor_ok != _last_published_sensor_ok) { /* notify about state change */ @@ -378,7 +375,7 @@ AirspeedSim::update_status() } void -AirspeedSim::cycle_trampoline(void *arg) +Airspeed::cycle_trampoline(void *arg) { Airspeed *dev = (Airspeed *)arg; @@ -390,7 +387,7 @@ AirspeedSim::cycle_trampoline(void *arg) } void -AirspeedSim::print_info() +Airspeed::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -400,7 +397,7 @@ AirspeedSim::print_info() } void -AirspeedSim::new_report(const differential_pressure_s &report) +Airspeed::new_report(const differential_pressure_s &report) { if (!_reports->force(&report)) perf_count(_buffer_overflows); From 6acdc2ae3f9358d32e5056c2927777b1e4ae3073 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 30 Apr 2015 13:17:16 -0700 Subject: [PATCH 182/258] POSIX: workaround for poll notification Sensor combined topic notification wasnot working because the calls to hrt_called() and hrt_call_after() in ORBDevNode::appears_updated() are not working correctly. This commit ifdefs out those calls, and the poling seems to work correctly. This is a workaround until the issue is resolved. Signed-off-by: Mark Charlebois --- src/modules/uORB/MuORB.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp index 5bf8e06a26..932af53c8f 100644 --- a/src/modules/uORB/MuORB.cpp +++ b/src/modules/uORB/MuORB.cpp @@ -514,6 +514,9 @@ ORBDevNode::appears_updated(SubscriberData *sd) break; } +// FIXME - the calls to hrt_called and hrt_call_after seem not to work in the +// POSIX build +#ifndef __PX4_POSIX /* * If the interval timer is still running, the topic should not * appear updated, even though at this point we know that it has. @@ -533,6 +536,7 @@ ORBDevNode::appears_updated(SubscriberData *sd) sd->update_interval, &ORBDevNode::update_deferred_trampoline, (void *)this); +#endif /* * Remember that we have told the subscriber that there is data. From ebdf178ba3496142a113d4a5e6b6c56ec346deb5 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 30 Apr 2015 16:24:43 -0700 Subject: [PATCH 183/258] Removed annoying "I2C SIM transfer_4" message The debug message made it difficult to use the shell for the posix build. Commented out the debug line. Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_posix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index c04cfaafb3..f02b021ffc 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -165,7 +165,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re packets.nmsgs = msgs; if (simulate) { - warnx("I2C SIM: transfer_4 on %s", get_devname()); + //warnx("I2C SIM: transfer_4 on %s", get_devname()); ret = PX4_OK; } else { From 8a873df9d03afaaad0bd41c3d5dcc250e7ffbe62 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 3 May 2015 17:00:35 +0200 Subject: [PATCH 184/258] ported mixer app --- src/systemcmds/mixer/mixer.cpp | 50 +++++++++++++++++++++------------- 1 file changed, 31 insertions(+), 19 deletions(-) diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 2f5ed3265d..0e8eda9cf6 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -37,6 +37,8 @@ * Mixer utility. */ +#include +#include #include #include #include @@ -44,7 +46,6 @@ #include #include #include -#include #include #include @@ -58,22 +59,27 @@ extern "C" __EXPORT int mixer_main(int argc, char *argv[]); static void usage(const char *reason) noreturn_function; -static void load(const char *devname, const char *fname) noreturn_function; +static int load(const char *devname, const char *fname) noreturn_function; int mixer_main(int argc, char *argv[]) { - if (argc < 2) + if (argc < 2) { usage("missing command"); + return 1; + } if (!strcmp(argv[1], "load")) { if (argc < 4) usage("missing device or filename"); - load(argv[2], argv[3]); + int ret = load(argv[2], argv[3]); + if(ret !=0) { + warnx("failed to load mixer"); + return 1; + } } - - usage("unrecognised command"); + return 0; } static void @@ -84,31 +90,37 @@ usage(const char *reason) fprintf(stderr, "usage:\n"); fprintf(stderr, " mixer load \n"); - /* XXX other useful commands? */ - exit(1); } -static void +static int load(const char *devname, const char *fname) { int dev; char buf[2048]; /* open the device */ - if ((dev = open(devname, 0)) < 0) - err(1, "can't open %s\n", devname); + if ((dev = px4_open(devname, 0)) < 0) { + warnx("can't open %s\n", devname); + return 1; + } /* reset mixers on the device */ - if (ioctl(dev, MIXERIOCRESET, 0)) - err(1, "can't reset mixers on %s", devname); + if (px4_ioctl(dev, MIXERIOCRESET, 0)) { + warnx("can't reset mixers on %s", devname); + return 1; + } - if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) - err(1, "can't load mixer: %s", fname); + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { + warnx("can't load mixer: %s", fname); + return 1; + } /* XXX pass the buffer to the device */ - int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); + int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); - if (ret < 0) - err(1, "error loading mixers from %s", fname); - exit(0); + if (ret < 0) { + warnx("error loading mixers from %s", fname); + return 1; + } + return 0; } From 1a8bd15d983c3953d8611d9d7e9791f66e18487e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 3 May 2015 11:20:07 -0700 Subject: [PATCH 185/258] Minor cleanup of mixer changes Signed-off-by: Mark Charlebois --- makefiles/posix/config_posix_default.mk | 1 + src/systemcmds/mixer/mixer.cpp | 9 ++++++--- src/systemcmds/mixer/module.mk | 2 ++ 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 8c2d84897a..d5c8d5be55 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -17,6 +17,7 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/param +MODULES += systemcmds/mixer MODULES += systemcmds/topic_listener # diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 0e8eda9cf6..686a50f436 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -58,8 +58,8 @@ */ extern "C" __EXPORT int mixer_main(int argc, char *argv[]); -static void usage(const char *reason) noreturn_function; -static int load(const char *devname, const char *fname) noreturn_function; +static void usage(const char *reason); +static int load(const char *devname, const char *fname); int mixer_main(int argc, char *argv[]) @@ -70,8 +70,10 @@ mixer_main(int argc, char *argv[]) } if (!strcmp(argv[1], "load")) { - if (argc < 4) + if (argc < 4) { usage("missing device or filename"); + return 1; + } int ret = load(argv[2], argv[3]); if(ret !=0) { @@ -79,6 +81,7 @@ mixer_main(int argc, char *argv[]) return 1; } } + usage("Unknown command"); return 0; } diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index 0fb899c671..5d5cf84857 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -42,5 +42,7 @@ MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os +ifdef ($(PX4_TARGET_OS),nuttx) EXTRACXXFLAGS = -Wframe-larger-than=2048 +endif From 17267a7f66c8392ab1a96e8c0921499b4a9ef473 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 3 May 2015 16:38:55 +0200 Subject: [PATCH 186/258] enable receiving mavlink highres imu message (via udp) from external simulator --- src/modules/simulator/module.mk | 8 +- src/modules/simulator/simulator.cpp | 142 +++++++++++++++++++++++++++- src/modules/simulator/simulator.h | 20 +++- 3 files changed, 167 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/module.mk b/src/modules/simulator/module.mk index 483204d5e4..0555c5dbd0 100644 --- a/src/modules/simulator/module.mk +++ b/src/modules/simulator/module.mk @@ -37,4 +37,10 @@ MODULE_COMMAND = simulator -SRCS = simulator.cpp +SRCS = simulator.cpp + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 3f6f763193..2520f8a28a 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -99,6 +99,90 @@ int Simulator::start(int argc, char *argv[]) return ret; } +void Simulator::fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu) { + hrt_abstime timestamp = hrt_absolute_time(); + sensor->timestamp = timestamp; + sensor->gyro_raw[0] = imu->xgyro * 1000.0f; + sensor->gyro_raw[1] = imu->ygyro * 1000.0f; + sensor->gyro_raw[2] = imu->zgyro * 1000.0f; + sensor->gyro_rad_s[0] = imu->xgyro; + sensor->gyro_rad_s[1] = imu->ygyro; + sensor->gyro_rad_s[2] = imu->zgyro; + + sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2; + sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2; + sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2; + sensor->accelerometer_m_s2[0] = imu->xacc; + sensor->accelerometer_m_s2[1] = imu->yacc; + sensor->accelerometer_m_s2[2] = imu->zacc; + sensor->accelerometer_mode = 0; // TODO what is this? + sensor->accelerometer_range_m_s2 = 32.7f; // int16 + sensor->accelerometer_timestamp = timestamp; + sensor->timestamp = timestamp; + + sensor->adc_voltage_v[0] = 0.0f; + sensor->adc_voltage_v[1] = 0.0f; + sensor->adc_voltage_v[2] = 0.0f; + + sensor->magnetometer_raw[0] = imu->xmag * 1000.0f; + sensor->magnetometer_raw[1] = imu->ymag * 1000.0f; + sensor->magnetometer_raw[2] = imu->zmag * 1000.0f; + sensor->magnetometer_ga[0] = imu->xmag; + sensor->magnetometer_ga[1] = imu->ymag; + sensor->magnetometer_ga[2] = imu->zmag; + sensor->magnetometer_range_ga = 32.7f; // int16 + sensor->magnetometer_mode = 0; // TODO what is this + sensor->magnetometer_cuttoff_freq_hz = 50.0f; + sensor->magnetometer_timestamp = timestamp; + + sensor->baro_pres_mbar = imu->abs_pressure; + sensor->baro_alt_meter = imu->pressure_alt; + sensor->baro_temp_celcius = imu->temperature; + sensor->baro_timestamp = timestamp; + + sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa + sensor->differential_pressure_timestamp = timestamp; +} + +void Simulator::fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) { + manual->timestamp = hrt_absolute_time(); + manual->x = man_msg->x / 1000.0f; + manual->y = man_msg->y / 1000.0f; + manual->r = man_msg->r / 1000.0f; + manual->z = man_msg->z / 1000.0f; +} + +void Simulator::handle_message(mavlink_message_t *msg) { + switch(msg->msgid) { + case MAVLINK_MSG_ID_HIGHRES_IMU: + mavlink_highres_imu_t imu; + mavlink_msg_highres_imu_decode(msg, &imu); + fill_sensors_from_imu_msg(&_sensor, &imu); + + // publish message + if(_sensor_combined_pub < 0) { + _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); + } else { + orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); + } + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + + mavlink_manual_control_t man_ctrl_sp; + mavlink_msg_manual_control_decode(msg, &man_ctrl_sp); + fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp); + + // publish message + if(_manual_control_sp_pub < 0) { + _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); + } + break; + } +} + void Simulator::publishSensorsCombined() { struct baro_report baro; @@ -168,13 +252,36 @@ void Simulator::publishSensorsCombined() { #ifndef __PX4_QURT void Simulator::updateSamples() { + struct baro_report baro; + memset(&baro,0,sizeof(baro)); + baro.pressure = 120000.0f; + + // acceleration report + struct accel_report accel; + memset(&accel,0,sizeof(accel)); + accel.z = 9.81f; + accel.range_m_s2 = 80.0f; + + // gyro report + struct gyro_report gyro; + memset(&gyro, 0 ,sizeof(gyro)); + + // mag report + struct mag_report mag; + memset(&mag, 0 ,sizeof(mag)); + // init publishers + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + // get samples from external provider struct sockaddr_in myaddr; struct sockaddr_in srcaddr; socklen_t addrlen = sizeof(srcaddr); int len, fd; const int buflen = 200; - const int port = 9876; + const int port = 14550; unsigned char buf[buflen]; if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { @@ -192,6 +299,38 @@ void Simulator::updateSamples() return; } + // wait for new mavlink messages to arrive + for (;;) { + len = 0; + len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); + if (len > 0) { + mavlink_message_t msg; + mavlink_status_t status; + for (int i = 0; i < len; ++i) + { + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + { + // have a message, handle it + handle_message(&msg); + } + } + } + + // publish these messages so that attitude estimator does not complain + hrt_abstime time_last = hrt_absolute_time(); + baro.timestamp = time_last; + accel.timestamp = time_last; + gyro.timestamp = time_last; + mag.timestamp = time_last; + // publish the sensor values + //printf("Publishing SensorsCombined\n"); + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + + /* for (;;) { len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); if (len > 0) { @@ -212,6 +351,7 @@ void Simulator::updateSamples() } } } + */ } #endif diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index d2ca2fa8ab..97b2420b2a 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -40,11 +40,14 @@ #include #include +#include #include #include #include #include #include +#include +#include namespace simulator { @@ -151,13 +154,24 @@ public: bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); private: - Simulator() : _accel(1), _mpu(1), _baro(1) {} + Simulator() : + _accel(1), + _mpu(1), + _baro(1), + _sensor_combined_pub(-1), + _manual_control_sp_pub(-1), + _sensor{}, + _manual_control_sp{} + {} ~Simulator() { _instance=NULL; } #ifndef __PX4_QURT void updateSamples(); #endif void publishSensorsCombined(); + void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu); + void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg); + void handle_message(mavlink_message_t *msg); static Simulator *_instance; @@ -170,5 +184,9 @@ private: orb_advert_t _gyro_pub; orb_advert_t _mag_pub; orb_advert_t _sensor_combined_pub; + orb_advert_t _manual_control_sp_pub; + + struct sensor_combined_s _sensor; + struct manual_control_setpoint_s _manual_control_sp; }; From 4b34d0c297b2953eff7af65e0e71e351fdadffda Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 3 May 2015 23:30:18 +0200 Subject: [PATCH 187/258] improved topic listener --- Tools/generate_listener.py | 45 +++++++++++++++++++++++++++++++++----- 1 file changed, 39 insertions(+), 6 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 805b390901..50fc17267c 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -9,19 +9,33 @@ import sys raw_messages = glob.glob(sys.argv[1]+"/msg/*.msg") messages = [] -message_floats = [] +message_elements = [] for index,m in enumerate(raw_messages): - temp_list = [] + temp_list_floats = [] + temp_list_uint64 = [] + temp_list_bool = [] if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m): + temp_list = [] f = open(m,'r') for line in f.readlines(): if(line.split(' ')[0] == "float32"): - temp_list.append(line.split(' ')[1].split('\t')[0].split('\n')[0]) + temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "uint64"): + temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif (line.split(' ')[0] == "bool"): + temp_list.append(("bool",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif (line.split(' ')[0] == "uint8") and len(line.split('=')) == 1: + temp_list.append(("uint8",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif ('float32[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + f.close() messages.append(m.split('/')[-1].split('.')[0]) - message_floats.append(temp_list) + message_elements.append(temp_list) + num_messages = len(messages); print @@ -76,6 +90,7 @@ print """ #include #include #include +#include """ for m in messages: print "#include " % m @@ -104,10 +119,28 @@ for index,m in enumerate(messages[1:]): print "\t\tID = ORB_ID(%s);" % m print "\t\tstruct %s_s container;" % m print "\t\tmemset(&container, 0, sizeof(container));" + print "\t\tbool updated;" print "\t\tfor(uint32_t i = 0;i Date: Mon, 4 May 2015 15:33:08 -0700 Subject: [PATCH 188/258] POSIX: Added missing include path and -Wno-packed flag The changes to the simulator added an include of mavlink/include/v1.0/... to simulator.h which was not in the included paths. The included header file also causes clang to issue a -Wpacked warning that had to be silenced. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 763b25ce86..dd80e9a6ea 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -124,6 +124,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Dnoreturn_function= \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/posix/include \ + -I$(PX4_BASE)/mavlink/include/mavlink \ -Wno-error=shadow # optimisation flags @@ -157,6 +158,7 @@ ARCHWARNINGS = -Wall \ -Wpacked \ -Wno-unused-parameter \ -Wno-gnu-array-member-paren-init \ + -Wno-packed \ -Werror=format-security \ -Werror=array-bounds \ -Wfatal-errors \ From 7ac9fc38e48406a33c59836c1d2336b5372db549 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 4 May 2015 15:48:06 -0700 Subject: [PATCH 189/258] Commented out 1st definition of MAVLINK_SRC The variable MAVLINK_SRC was defined and then redefined. Commented out the first definition and moved beside that overriding definition for visibility. Signed-off-by: Mark Charlebois --- makefiles/setup.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 0cdaa1e835..f14dfe531b 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -54,8 +54,8 @@ export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ -export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ +#export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ From ed621a6a8d0a8d188823763a0514719018219158 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 4 May 2015 16:09:00 -0700 Subject: [PATCH 190/258] POSIX: Enabled hrt queued work to be run In STM32, the ISR handler calls hrt_call_invoke(). There is no interrupt timer inthe POSIX port so a work item is put on the high priority work queue to expire at the next event (in ticks) or at the next max delay interval. Counter wrapping is likely still not handled properly in this code. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/drv_hrt.c | 109 +++++++++++++++++- .../posix/px4_layer/px4_posix_impl.cpp | 3 + .../posix/tests/hrt_test/hrt_test.cpp | 18 +++ 3 files changed, 127 insertions(+), 3 deletions(-) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 066abe70d5..7094ca7b8d 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -37,9 +37,12 @@ * High-resolution timer with callouts and timekeeping. */ +#include #include +#include #include #include +#include static struct sq_queue_s callout_queue; @@ -51,9 +54,26 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; static void hrt_call_reschedule(void); +// Intervals in ms #define HRT_INTERVAL_MIN 50 #define HRT_INTERVAL_MAX 50000 +static sem_t _hrt_lock; +static struct work_s _hrt_work; + +static void +hrt_call_invoke(void); + +static void hrt_lock(void) +{ + sem_wait(&_hrt_lock); +} + +static void hrt_unlock(void) +{ + sem_post(&_hrt_lock); +} + /* * Get absolute time. */ @@ -119,7 +139,7 @@ bool hrt_called(struct hrt_call *entry) */ void hrt_cancel(struct hrt_call *entry) { - // FIXME - need a lock + hrt_lock(); sq_rem(&entry->link, &callout_queue); entry->deadline = 0; @@ -127,6 +147,7 @@ void hrt_cancel(struct hrt_call *entry) * being re-entered when the callout returns. */ entry->period = 0; + hrt_unlock(); // endif } @@ -155,7 +176,10 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) */ void hrt_init(void) { + //printf("hrt_init\n"); sq_init(&callout_queue); + sem_init(&_hrt_lock, 0, 1); + memset(&_hrt_work, 0, sizeof(_hrt_work)); } static void @@ -163,6 +187,7 @@ hrt_call_enter(struct hrt_call *entry) { struct hrt_call *call, *next; + //printf("hrt_call_enter\n"); call = (struct hrt_call *)sq_peek(&callout_queue); if ((call == NULL) || (entry->deadline < call->deadline)) { @@ -186,6 +211,25 @@ hrt_call_enter(struct hrt_call *entry) //lldbg("scheduled\n"); } +/** + * Timer interrupt handler + * + * This routine simulates a timer interrupt handler + */ +static void +hrt_tim_isr(void *p) +{ + hrt_lock(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + + hrt_unlock(); +} + /** * Reschedule the next timer interrupt. * @@ -197,7 +241,10 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + //printf("hrt_call_reschedule\n"); + /* * Determine what the next deadline will be. * @@ -215,18 +262,25 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - deadline = now + HRT_INTERVAL_MIN; + ticks = USEC2TICK(HRT_INTERVAL_MIN); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - deadline = next->deadline; + ticks = USEC2TICK((next->deadline - now)*1000); } } + + // There is no timer ISR, so simulate one by putting an event on the + // high priority work queue + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) { + //printf("hrt_call_internal\n"); + hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised entry->link here, but it is safe as sq_rem() doesn't @@ -244,6 +298,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte entry->arg = arg; hrt_call_enter(entry); + hrt_unlock(); } /* @@ -254,6 +309,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte */ void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { + //printf("hrt_call_after\n"); hrt_call_internal(entry, hrt_absolute_time() + delay, 0, @@ -291,3 +347,50 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); #endif +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) + break; + + if (call->deadline > now) + break; + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } +} + diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 3b1415fd72..d164337687 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -51,6 +51,8 @@ __BEGIN_DECLS long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); +extern void hrt_init(void); + __END_DECLS extern struct wqueue_s gwork[NWORKERS]; @@ -78,6 +80,7 @@ void init(int argc, char *argv[], const char *app_name) work_lpthread, (char* const*)NULL); + hrt_init(); } } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index baa8670332..c54e35430a 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -46,6 +46,11 @@ px4::AppState HRTTest::appState; +static void timer_expired(void *arg) +{ + printf("Test\n"); +} + int HRTTest::main() { appState.setRunning(true); @@ -62,5 +67,18 @@ int HRTTest::main() printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); printf("Start time %llu\n", (unsigned long long)t); + struct hrt_call t1; + int update_interval = 1; + + memset(&t1, 0, sizeof(t1)); + + printf("HRT_CALL %d\n", hrt_called(&t1)); + + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); + sleep(2); + printf("HRT_CALL %d\n", hrt_called(&t1)); + hrt_cancel(&t1); + printf("HRT_CALL %d\n", hrt_called(&t1)); + return 0; } From e24405d3747b2732cbfa562b5cc1683bf5ae752a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 00:27:14 -0700 Subject: [PATCH 191/258] POSIX: fixed hrt call and workqueue implementation The HRT call processing normally happens via HW timer interrupt handler. Since the POSIX port has no ISR handling, the HP work queue is used. Instead of irq_save() and irq_restore() calls to disable/enable interrupts, a mutex is used to protect each queue. Signed-off-by: Mark Charlebois --- src/platforms/posix/main.cpp | 6 +++ src/platforms/posix/px4_layer/drv_hrt.c | 16 +++++- .../posix/px4_layer/px4_posix_impl.cpp | 28 +++------- src/platforms/posix/px4_layer/work_lock.h | 51 +++++++++++++++++++ src/platforms/posix/px4_layer/work_queue.c | 7 ++- src/platforms/posix/px4_layer/work_thread.c | 47 +++++++++++++---- .../posix/tests/hrt_test/hrt_test.cpp | 15 ++++-- .../posix/tests/hrt_test/hrt_test_main.cpp | 6 +-- src/platforms/px4_workqueue.h | 9 ++++ 9 files changed, 144 insertions(+), 41 deletions(-) create mode 100644 src/platforms/posix/px4_layer/work_lock.h diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index b76a12ef9c..b497133b36 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -43,6 +43,10 @@ #include #include +namespace px4 { +void init_once(void); +} + using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); @@ -94,6 +98,8 @@ int main(int argc, char **argv) string mystr; + px4::init_once(); + px4::init(argc, argv, "mainapp"); while(1) { diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 7094ca7b8d..f5c351b187 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -66,11 +66,13 @@ hrt_call_invoke(void); static void hrt_lock(void) { + //printf("hrt_lock\n"); sem_wait(&_hrt_lock); } static void hrt_unlock(void) { + //printf("hrt_unlock\n"); sem_post(&_hrt_lock); } @@ -219,11 +221,13 @@ hrt_call_enter(struct hrt_call *entry) static void hrt_tim_isr(void *p) { - hrt_lock(); + //printf("hrt_tim_isr\n"); /* run any callouts that have met their deadline */ hrt_call_invoke(); + hrt_lock(); + /* and schedule the next interrupt */ hrt_call_reschedule(); @@ -262,7 +266,7 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - ticks = USEC2TICK(HRT_INTERVAL_MIN); + ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); } else if (next->deadline < deadline) { //lldbg("due soon\n"); @@ -272,6 +276,7 @@ hrt_call_reschedule() // There is no timer ISR, so simulate one by putting an event on the // high priority work queue + //printf("ticks = %u\n", ticks); work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } @@ -353,6 +358,7 @@ hrt_call_invoke(void) struct hrt_call *call; hrt_abstime deadline; + hrt_lock(); while (true) { /* get the current time */ hrt_abstime now = hrt_absolute_time(); @@ -376,8 +382,13 @@ hrt_call_invoke(void) /* invoke the callout (if there is one) */ if (call->callout) { + // Unlock so we don't deadlock in callback + hrt_unlock(); + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); call->callout(call->arg); + + hrt_lock(); } /* if the callout has a non-zero period, it has to be re-entered */ @@ -392,5 +403,6 @@ hrt_call_invoke(void) hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index d164337687..3f1916a51a 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -55,32 +55,20 @@ extern void hrt_init(void); __END_DECLS -extern struct wqueue_s gwork[NWORKERS]; - namespace px4 { +void init_once(void); + +void init_once(void) +{ + work_queues_init(); + hrt_init(); +} + void init(int argc, char *argv[], const char *app_name) { printf("App name: %s\n", app_name); - - // Create high priority worker thread - g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 2000, - work_hpthread, - (char* const*)NULL); - - // Create low priority worker thread - g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", - SCHED_DEFAULT, - SCHED_PRIORITY_MIN, - 2000, - work_lpthread, - (char* const*)NULL); - - hrt_init(); } } diff --git a/src/platforms/posix/px4_layer/work_lock.h b/src/platforms/posix/px4_layer/work_lock.h new file mode 100644 index 0000000000..89e8b65eca --- /dev/null +++ b/src/platforms/posix/px4_layer/work_lock.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include + +#pragma once +extern sem_t _work_lock[]; + +inline void work_lock(int id) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +inline void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} + diff --git a/src/platforms/posix/px4_layer/work_queue.c b/src/platforms/posix/px4_layer/work_queue.c index cd96aacd2f..46ca359842 100644 --- a/src/platforms/posix/px4_layer/work_queue.c +++ b/src/platforms/posix/px4_layer/work_queue.c @@ -43,7 +43,10 @@ #include #include #include +#include +#include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -117,13 +120,13 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_ * from with task logic or interrupt handlers. */ - //flags = irqsave(); + work_lock(qid); work->qtime = clock_systimer(); /* Time work queued */ dq_addlast((dq_entry_t *)work, &wqueue->q); px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ - //irqrestore(flags); + work_unlock(qid); return PX4_OK; } diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c index 57e6e327f1..1128a80944 100644 --- a/src/platforms/posix/px4_layer/work_thread.c +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -45,6 +45,7 @@ #include #include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,6 +67,7 @@ struct wqueue_s g_work[NWORKERS]; /**************************************************************************** * Private Variables ****************************************************************************/ +sem_t _work_lock[NWORKERS]; /**************************************************************************** * Private Functions @@ -85,11 +87,10 @@ struct wqueue_s g_work[NWORKERS]; * ****************************************************************************/ -static void work_process(FAR struct wqueue_s *wqueue) +static void work_process(FAR struct wqueue_s *wqueue, int lock_id) { volatile FAR struct work_s *work; worker_t worker; - //irqstate_t flags; FAR void *arg; uint64_t elapsed; uint32_t remaining; @@ -100,7 +101,9 @@ static void work_process(FAR struct wqueue_s *wqueue) */ next = CONFIG_SCHED_WORKPERIOD; - //flags = irqsave(); + + work_lock(lock_id); + work = (FAR struct work_s *)wqueue->q.head; while (work) { @@ -133,7 +136,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * performed... we don't have any idea how long that will take! */ - //irqrestore(flags); + work_unlock(lock_id); if (!worker) { printf("MESSED UP: worker = 0\n"); } @@ -145,7 +148,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * back at the head of the list. */ - //flags = irqsave(); + work_lock(lock_id); work = (FAR struct work_s *)wqueue->q.head; } else @@ -172,14 +175,40 @@ static void work_process(FAR struct wqueue_s *wqueue) /* Wait awhile to check the work list. We will wait here until either * the time elapses or until we are awakened by a signal. */ + work_unlock(lock_id); usleep(next); - //irqrestore(flags); } /**************************************************************************** * Public Functions ****************************************************************************/ +void work_queues_init(void) +{ + sem_init(&_work_lock[HPWORK], 0, 1); + sem_init(&_work_lock[LPWORK], 0, 1); +#ifdef CONFIG_SCHED_USRWORK + sem_init(&_work_lock[USRWORK], 0, 1); +#endif + + // Create high priority worker thread + g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX-1, + 2000, + work_hpthread, + (char* const*)NULL); + + // Create low priority worker thread + g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2000, + work_lpthread, + (char* const*)NULL); + +} + /**************************************************************************** * Name: work_hpthread, work_lpthread, and work_usrthread * @@ -234,7 +263,7 @@ int work_hpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[HPWORK]); + work_process(&g_work[HPWORK], HPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -261,7 +290,7 @@ int work_lpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[LPWORK]); + work_process(&g_work[LPWORK], LPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -282,7 +311,7 @@ int work_usrthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[USRWORK]); + work_process(&g_work[USRWORK], USRWORK); } return PX4_OK; /* To keep some compilers happy */ diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index c54e35430a..877462a44b 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -46,9 +46,17 @@ px4::AppState HRTTest::appState; +static struct hrt_call t1; +static int update_interval = 1; + static void timer_expired(void *arg) { + static int i = 0; printf("Test\n"); + if (i < 5) { + i++; + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); + } } int HRTTest::main() @@ -67,18 +75,15 @@ int HRTTest::main() printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); printf("Start time %llu\n", (unsigned long long)t); - struct hrt_call t1; - int update_interval = 1; - memset(&t1, 0, sizeof(t1)); printf("HRT_CALL %d\n", hrt_called(&t1)); hrt_call_after(&t1, update_interval, timer_expired, (void *)0); sleep(2); - printf("HRT_CALL %d\n", hrt_called(&t1)); + printf("HRT_CALL - %d\n", hrt_called(&t1)); hrt_cancel(&t1); - printf("HRT_CALL %d\n", hrt_called(&t1)); + printf("HRT_CALL + %d\n", hrt_called(&t1)); return 0; } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp index b7c332cd3b..9a01690e38 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file hello_main.cpp + * @file hrt_test_main.cpp * Example for Linux * * @author Mark Charlebois @@ -44,9 +44,9 @@ int PX4_MAIN(int argc, char **argv) { - px4::init(argc, argv, "hello"); + px4::init(argc, argv, "hrt_test"); - printf("hello\n"); + printf("starting\n"); HRTTest test; test.main(); diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 67c2fcdfa7..b9ae6b4b6c 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -73,6 +73,15 @@ struct work_s uint32_t delay; /* Delay until work performed */ }; +/**************************************************************************** + * Name: work_queues_init() + * + * Description: + * Initialize the work queues. + * + ****************************************************************************/ +void work_queues_init(void); + /**************************************************************************** * Name: work_queue * From e33a164ddb9558445c71559ead95832fe3cf7ab2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 10:52:15 -0700 Subject: [PATCH 192/258] uORB: Major refactoring uORB was refactored in order to support the MuORB changes required for QURT. Those changes wil be added in a subsequent commit. The files are split out by posix and nuttx so the changes are visible. When this has been tested, the files can be re-merged and re-tested. Signed-off-by: Mark Charlebois --- src/modules/uORB/MuORB.cpp | 1366 ---------------------- src/modules/uORB/module.mk | 13 +- src/modules/uORB/uORB.cpp | 1448 ++++-------------------- src/modules/uORB/uORBCommon.hpp | 65 ++ src/modules/uORB/uORBDevices.hpp | 39 + src/modules/uORB/uORBDevices_nuttx.cpp | 531 +++++++++ src/modules/uORB/uORBDevices_nuttx.hpp | 190 ++++ src/modules/uORB/uORBDevices_posix.cpp | 551 +++++++++ src/modules/uORB/uORBDevices_posix.hpp | 122 ++ src/modules/uORB/uORBMain.cpp | 124 ++ src/modules/uORB/uORBManager.hpp | 325 ++++++ src/modules/uORB/uORBManager_nuttx.cpp | 282 +++++ src/modules/uORB/uORBManager_posix.cpp | 296 +++++ src/modules/uORB/uORBTest_UnitTest.cpp | 287 +++++ src/modules/uORB/uORBTest_UnitTest.hpp | 128 +++ src/modules/uORB/uORBUtils.cpp | 63 ++ src/modules/uORB/uORBUtils.hpp | 55 + 17 files changed, 3284 insertions(+), 2601 deletions(-) delete mode 100644 src/modules/uORB/MuORB.cpp create mode 100644 src/modules/uORB/uORBCommon.hpp create mode 100644 src/modules/uORB/uORBDevices.hpp create mode 100644 src/modules/uORB/uORBDevices_nuttx.cpp create mode 100644 src/modules/uORB/uORBDevices_nuttx.hpp create mode 100644 src/modules/uORB/uORBDevices_posix.cpp create mode 100644 src/modules/uORB/uORBDevices_posix.hpp create mode 100644 src/modules/uORB/uORBMain.cpp create mode 100644 src/modules/uORB/uORBManager.hpp create mode 100644 src/modules/uORB/uORBManager_nuttx.cpp create mode 100644 src/modules/uORB/uORBManager_posix.cpp create mode 100644 src/modules/uORB/uORBTest_UnitTest.cpp create mode 100644 src/modules/uORB/uORBTest_UnitTest.hpp create mode 100644 src/modules/uORB/uORBUtils.cpp create mode 100644 src/modules/uORB/uORBUtils.hpp diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp deleted file mode 100644 index 932af53c8f..0000000000 --- a/src/modules/uORB/MuORB.cpp +++ /dev/null @@ -1,1366 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file uORB.cpp - * A lightweight object broker. - */ - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include - -#include "uORB.h" - -/** - * Utility functions. - */ -namespace -{ - -/* internal use only */ -static const unsigned orb_maxpath = 64; - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -enum Flavor { - PUBSUB, - PARAM -}; - -struct orb_advertdata { - const struct orb_metadata *meta; - int *instance; - int priority; -}; - -int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) -{ - unsigned len; - - unsigned index = 0; - - if (instance != nullptr) { - index = *instance; - } - - len = snprintf(buf, orb_maxpath, "/%s/%s%d", - (f == PUBSUB) ? "obj" : "param", - meta->o_name, index); - - if (len >= orb_maxpath) { - return -ENAMETOOLONG; - } - - return PX4_OK; -} - -} - -/** - * Per-object device instance. - */ -class ORBDevNode : public device::VDev -{ -public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); - ~ORBDevNode(); - - virtual int open(device::file_t *filp); - virtual int close(device::file_t *filp); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); - -protected: - virtual pollevent_t poll_state(device::file_t *filp); - virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); - -private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ - struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ - }; - - const struct orb_metadata *_meta; /**< object metadata information */ - uint8_t *_data; /**< allocated object buffer */ - hrt_abstime _last_update; /**< time the object was last updated */ - volatile unsigned _generation; /**< object generation count */ - pid_t _publisher; /**< if nonzero, current publisher */ - const int _priority; /**< priority of topic */ - - SubscriberData *filp_to_sd(device::file_t *filp); - - /** - * Perform a deferred update for a rate-limited subscriber. - */ - void update_deferred(); - - /** - * Bridge from hrt_call to update_deferred - * - * void *arg ORBDevNode pointer for which the deferred update is performed. - */ - static void update_deferred_trampoline(void *arg); - - /** - * Check whether a topic appears updated to a subscriber. - * - * @param sd The subscriber for whom to check. - * @return True if the topic should appear updated to the subscriber - */ - bool appears_updated(SubscriberData *sd); -}; - -ORBDevNode::SubscriberData *ORBDevNode::filp_to_sd(device::file_t *filp) -{ - ORBDevNode::SubscriberData *sd; - if (filp) { - sd = (ORBDevNode::SubscriberData *)(filp->priv); - } - else { - sd = 0; - } - return sd; -} - -ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : - VDev(name, path), - _meta(meta), - _data(nullptr), - _last_update(0), - _generation(0), - _publisher(0), - _priority(priority) -{ - // enable debug() calls - //_debug_enabled = true; -} - -ORBDevNode::~ORBDevNode() -{ - if (_data != nullptr) - delete[] _data; - -} - -int -ORBDevNode::open(device::file_t *filp) -{ - int ret; - - /* is this a publisher? */ - if (filp->flags == PX4_F_WRONLY) { - - /* become the publisher if we can */ - lock(); - - if (_publisher == 0) { - _publisher = getpid(); - ret = PX4_OK; - - } else { - ret = -EBUSY; - } - - unlock(); - - /* now complete the open */ - if (ret == PX4_OK) { - ret = VDev::open(filp); - - /* open failed - not the publisher anymore */ - if (ret != PX4_OK) - _publisher = 0; - } - - return ret; - } - - /* is this a new subscriber? */ - if (filp->flags == PX4_F_RDONLY) { - - /* allocate subscriber data */ - SubscriberData *sd = new SubscriberData; - - if (nullptr == sd) - return -ENOMEM; - - memset(sd, 0, sizeof(*sd)); - - /* default to no pending update */ - sd->generation = _generation; - - /* set priority */ - sd->priority = _priority; - - filp->priv = (void *)sd; - - ret = VDev::open(filp); - - if (ret != PX4_OK) { - warnx("ERROR: VDev::open failed\n"); - delete sd; - } - - //warnx("ORBDevNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); - return ret; - } - - /* can only be pub or sub, not both */ - return -EINVAL; -} - -int -ORBDevNode::close(device::file_t *filp) -{ - //warnx("ORBDevNode::close fd = %d", filp->fd); - /* is this the publisher closing? */ - if (getpid() == _publisher) { - _publisher = 0; - - } else { - SubscriberData *sd = filp_to_sd(filp); - - if (sd != nullptr) { - hrt_cancel(&sd->update_call); - delete sd; - } - } - - return VDev::close(filp); -} - -ssize_t -ORBDevNode::read(device::file_t *filp, char *buffer, size_t buflen) -{ - //warnx("ORBDevNode::read fd = %d\n", filp->fd); - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); - - /* if the object has not been written yet, return zero */ - if (_data == nullptr) - return 0; - - /* if the caller's buffer is the wrong size, that's an error */ - if (buflen != _meta->o_size) - return -EIO; - - /* - * Perform an atomic copy & state update - */ - lock(); - - /* if the caller doesn't want the data, don't give it to them */ - if (nullptr != buffer) - memcpy(buffer, _data, _meta->o_size); - - /* track the last generation that the file has seen */ - sd->generation = _generation; - - /* set priority */ - sd->priority = _priority; - - /* - * Clear the flag that indicates that an update has been reported, as - * we have just collected it. - */ - sd->update_reported = false; - - unlock(); - - return _meta->o_size; -} - -ssize_t -ORBDevNode::write(device::file_t *filp, const char *buffer, size_t buflen) -{ - //warnx("ORBDevNode::write filp = %p (null is normal)", filp); - /* - * Writes are legal from interrupt context as long as the - * object has already been initialised from thread context. - * - * Writes outside interrupt context will allocate the object - * if it has not yet been allocated. - * - * Note that filp will usually be NULL. - */ - if (nullptr == _data) { - lock(); - - /* re-check size */ - if (nullptr == _data) - _data = new uint8_t[_meta->o_size]; - - unlock(); - - /* failed or could not allocate */ - if (nullptr == _data) - return -ENOMEM; - } - - /* If write size does not match, that is an error */ - if (_meta->o_size != buflen) - return -EIO; - - /* Perform an atomic copy. */ - lock(); - memcpy(_data, buffer, _meta->o_size); - unlock(); - - /* update the timestamp and generation count */ - _last_update = hrt_absolute_time(); - _generation++; - - /* notify any poll waiters */ - poll_notify(POLLIN); - - return _meta->o_size; -} - -int -ORBDevNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - //warnx("ORBDevNode::ioctl fd = %d cmd = %d", filp->fd, cmd); - SubscriberData *sd = filp_to_sd(filp); - - switch (cmd) { - case ORBIOCLASTUPDATE: - *(hrt_abstime *)arg = _last_update; - return PX4_OK; - - case ORBIOCUPDATED: - *(bool *)arg = appears_updated(sd); - return PX4_OK; - - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return PX4_OK; - - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return PX4_OK; - - case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; - return PX4_OK; - - default: - /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); - } -} - -ssize_t -ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) -{ - //warnx("ORBDevNode::publish meta = %p", meta); - - if (handle < 0) { - warnx("ORBDevNode::publish called with invalid handle"); - errno = EINVAL; - return ERROR; - } - - ORBDevNode *devnode = (ORBDevNode *)handle; - int ret; - - /* this is a bit risky, since we are trusting the handle in order to deref it */ - if (devnode->_meta != meta) { - errno = EINVAL; - return ERROR; - } - - /* call the devnode write method with no file pointer */ - ret = devnode->write(nullptr, (const char *)data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return PX4_OK; -} - -pollevent_t -ORBDevNode::poll_state(device::file_t *filp) -{ - //warnx("ORBDevNode::poll_state fd = %d", filp->fd); - SubscriberData *sd = filp_to_sd(filp); - - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) - return POLLIN; - - return 0; -} - -void -ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) -{ - //warnx("ORBDevNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); - SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); - - /* - * If the topic looks updated to the subscriber, go ahead and notify them. - */ - if (appears_updated(sd)) - VDev::poll_notify_one(fds, events); -} - -bool -ORBDevNode::appears_updated(SubscriberData *sd) -{ - //warnx("ORBDevNode::appears_updated sd = %p", sd); - /* assume it doesn't look updated */ - bool ret = false; - - /* check if this topic has been published yet, if not bail out */ - if (_data == nullptr) { - ret = false; - goto out; - } - - /* - * If the subscriber's generation count matches the update generation - * count, there has been no update from their perspective; if they - * don't match then we might have a visible update. - */ - while (sd->generation != _generation) { - - /* - * Handle non-rate-limited subscribers. - */ - if (sd->update_interval == 0) { - ret = true; - break; - } - - /* - * If we have previously told the subscriber that there is data, - * and they have not yet collected it, continue to tell them - * that there has been an update. This mimics the non-rate-limited - * behaviour where checking / polling continues to report an update - * until the topic is read. - */ - if (sd->update_reported) { - ret = true; - break; - } - -// FIXME - the calls to hrt_called and hrt_call_after seem not to work in the -// POSIX build -#ifndef __PX4_POSIX - /* - * If the interval timer is still running, the topic should not - * appear updated, even though at this point we know that it has. - * We have previously been through here, so the subscriber - * must have collected the update we reported, otherwise - * update_reported would still be true. - */ - if (!hrt_called(&sd->update_call)) - break; - - /* - * Make sure that we don't consider the topic to be updated again - * until the interval has passed once more by restarting the interval - * timer and thereby re-scheduling a poll notification at that time. - */ - hrt_call_after(&sd->update_call, - sd->update_interval, - &ORBDevNode::update_deferred_trampoline, - (void *)this); -#endif - - /* - * Remember that we have told the subscriber that there is data. - */ - sd->update_reported = true; - ret = true; - - break; - } - -out: - /* consider it updated */ - return ret; -} - -void -ORBDevNode::update_deferred() -{ - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); -} - -void -ORBDevNode::update_deferred_trampoline(void *arg) -{ - ORBDevNode *node = (ORBDevNode *)arg; - - node->update_deferred(); -} - -/** - * Master control device for ObjDev. - * - * Used primarily to create new objects via the ORBIOCCREATE - * ioctl. - */ -class ORBDevMaster : public device::VDev -{ -public: - ORBDevMaster(Flavor f); - ~ORBDevMaster(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); -private: - Flavor _flavor; -}; - -ORBDevMaster::ORBDevMaster(Flavor f) : - VDev((f == PUBSUB) ? "obj_master" : "param_master", - (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), - _flavor(f) -{ - // enable debug() calls - _debug_enabled = true; - -} - -ORBDevMaster::~ORBDevMaster() -{ -} - -int -ORBDevMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - int ret; - - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; - const struct orb_metadata *meta = adv->meta; - const char *objname; - const char *devpath; - char nodepath[orb_maxpath]; - ORBDevNode *node; - - /* set instance to zero - we could allow selective multi-pubs later based on value */ - if (adv->instance != nullptr) { - *(adv->instance) = 0; - } - - /* construct a path to the node - this also checks the node name */ - ret = node_mkpath(nodepath, _flavor, meta, adv->instance); - - if (ret != PX4_OK) { - return ret; - } - - /* ensure that only one advertiser runs through this critical section */ - lock(); - - ret = ERROR; - - /* try for topic groups */ - const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - do { - /* if path is modifyable change try index */ - if (adv->instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *(adv->instance) = group_tries; - } - - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } - - /* driver wants a permanent copy of the path, so make one here */ - devpath = strdup(nodepath); - - if (devpath == nullptr) { - // FIXME - looks like we leaked memory here for objname - return -ENOMEM; - } - - /* construct the new node */ - node = new ORBDevNode(meta, objname, devpath, adv->priority); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - unlock(); - - // FIXME - looks like we leaked memory here for devpath and objname - return -ENOMEM; - } - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); - - /* if init failed, discard the node and its name */ - if (ret != PX4_OK) { - delete node; - free((void *)objname); - free((void *)devpath); - } - - group_tries++; - - } while (ret != PX4_OK && (group_tries < max_group_tries)); - - if (group_tries > max_group_tries) { - ret = -ENOMEM; - } - - /* the file handle for the driver has been created, unlock */ - unlock(); - - return ret; - } - - default: - /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); - } -} - - -/** - * Local functions in support of the shell command. - */ - -namespace -{ - -ORBDevMaster *g_dev; -bool pubsubtest_passed = false; -bool pubsubtest_print = true; -int pubsubtest_res = PX4_OK; - -struct orb_test { - int val; - hrt_abstime time; -}; - -ORB_DEFINE(orb_test, struct orb_test); -ORB_DEFINE(orb_multitest, struct orb_test); - -struct orb_test_medium { - int val; - hrt_abstime time; - char junk[64]; -}; - -ORB_DEFINE(orb_test_medium, struct orb_test_medium); - -struct orb_test_large { - int val; - hrt_abstime time; - char junk[512]; -}; - -ORB_DEFINE(orb_test_large, struct orb_test_large); - -int -test_fail(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "FAIL: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return ERROR; -} - -int -test_note(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "note: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return PX4_OK; -} - -int pubsublatency_main(void) -{ - /* poll on test topic and output latency */ - float latency_integral = 0.0f; - - /* wakeup source(s) */ - px4_pollfd_struct_t fds[3]; - - int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); - int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); - int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); - - struct orb_test_large t; - - /* clear all ready flags */ - orb_copy(ORB_ID(orb_test), test_multi_sub, &t); - orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); - orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); - - fds[0].fd = test_multi_sub; - fds[0].events = POLLIN; - fds[1].fd = test_multi_sub_medium; - fds[1].events = POLLIN; - fds[2].fd = test_multi_sub_large; - fds[2].events = POLLIN; - - const unsigned maxruns = 1000; - unsigned timingsgroup = 0; - - unsigned *timings = new unsigned[maxruns]; - - for (unsigned i = 0; i < maxruns; i++) { - /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(orb_test), test_multi_sub, &t); - timingsgroup = 0; - } else if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); - timingsgroup = 1; - } else if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); - timingsgroup = 2; - } - - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - hrt_abstime elt = hrt_elapsed_time(&t.time); - latency_integral += elt; - timings[i] = elt; - } - - orb_unsubscribe(test_multi_sub); - orb_unsubscribe(test_multi_sub_medium); - orb_unsubscribe(test_multi_sub_large); - - if (pubsubtest_print) { - char fname[32]; - //sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); - sprintf(fname, "/tmp/timings%u.txt", timingsgroup); - FILE *f = fopen(fname, "w"); - if (f == NULL) { - warnx("Error opening file!\n"); - return ERROR; - } - - for (unsigned i = 0; i < maxruns; i++) { - fprintf(f, "%u\n", timings[i]); - } - - fclose(f); - } - - delete[] timings; - - warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); - - pubsubtest_passed = true; - - if (static_cast(latency_integral / maxruns) > 30.0f) { - pubsubtest_res = ERROR; - } else { - pubsubtest_res = PX4_OK; - } - - return pubsubtest_res; -} - -template int latency_test(orb_id_t T, bool print); - -int -test() -{ - struct orb_test t, u; - int pfd, sfd; - bool updated; - - t.val = 0; - pfd = orb_advertise(ORB_ID(orb_test), &t); - - if (pfd < 0) - return test_fail("advertise failed: %d", errno); - - test_note("publish handle 0x%08x", pfd); - sfd = orb_subscribe(ORB_ID(orb_test)); - - if (sfd < 0) - return test_fail("subscribe failed: %d", errno); - - test_note("subscribe fd %d", sfd); - u.val = 1; - - if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(1) failed: %d", errno); - - if (u.val != t.val) - return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); - - if (PX4_OK != orb_check(sfd, &updated)) - return test_fail("check(1) failed"); - - if (updated) - return test_fail("spurious updated flag"); - - t.val = 2; - test_note("try publish"); - - if (PX4_OK != orb_publish(ORB_ID(orb_test), pfd, &t)) - return test_fail("publish failed"); - - if (PX4_OK != orb_check(sfd, &updated)) - return test_fail("check(2) failed"); - - if (!updated) - return test_fail("missing updated flag"); - - if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(2) failed: %d", errno); - - if (u.val != t.val) - return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); - - orb_unsubscribe(sfd); - close(pfd); - - /* this routine tests the multi-topic support */ - test_note("try multi-topic support"); - - int instance0; - int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - - test_note("advertised"); - - int instance1; - int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); - - if (instance0 != 0) - return test_fail("mult. id0: %d", instance0); - - if (instance1 != 1) - return test_fail("mult. id1: %d", instance1); - - t.val = 103; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) - return test_fail("mult. pub0 fail"); - - test_note("published"); - - t.val = 203; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) - return test_fail("mult. pub1 fail"); - - /* subscribe to both topics and ensure valid data is received */ - int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - - if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) - return test_fail("sub #0 copy failed: %d", errno); - - if (u.val != 103) - return test_fail("sub #0 val. mismatch: %d", u.val); - - int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); - - if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) - return test_fail("sub #1 copy failed: %d", errno); - - if (u.val != 203) - return test_fail("sub #1 val. mismatch: %d", u.val); - - /* test priorities */ - int prio; - if (PX4_OK != orb_priority(sfd0, &prio)) - return test_fail("prio #0"); - - if (prio != ORB_PRIO_MAX) - return test_fail("prio: %d", prio); - - if (PX4_OK != orb_priority(sfd1, &prio)) - return test_fail("prio #1"); - - if (prio != ORB_PRIO_MIN) - return test_fail("prio: %d", prio); - - if (PX4_OK != latency_test(ORB_ID(orb_test), false)) - return test_fail("latency test failed"); - - return test_note("PASS"); -} - -template int -latency_test(orb_id_t T, bool print) -{ - test_note("---------------- LATENCY TEST ------------------"); - S t; - t.val = 308; - t.time = hrt_absolute_time(); - - int pfd0 = orb_advertise(T, &t); - - pubsubtest_print = print; - - pubsubtest_passed = false; - - /* test pub / sub latency */ - - int pubsub_task = px4_task_spawn_cmd("uorb_latency", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&pubsublatency_main, - nullptr); - - /* give the test task some data */ - while (!pubsubtest_passed) { - t.val = 308; - t.time = hrt_absolute_time(); - if (PX4_OK != orb_publish(T, pfd0, &t)) - return test_fail("mult. pub0 timing fail"); - - /* simulate >800 Hz system operation */ - usleep(1000); - } - - close(pfd0); - - if (pubsub_task < 0) { - return test_fail("failed launching task"); - } - - return pubsubtest_res; -} - -int -info() -{ - return PX4_OK; -} - - -} // namespace - -static void usage() -{ - warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); -} - -/* - * uORB server 'main'. - */ -extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } - -int -uorb_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return -EINVAL; - } - - /* - * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) { - warnx("already loaded"); - /* user wanted to start uorb, its already running, no error */ - return 0; - } - - /* create the driver */ - g_dev = new ORBDevMaster(PUBSUB); - - if (g_dev == nullptr) { - warnx("driver alloc failed"); - return -ENOMEM; - } - - if (PX4_OK != g_dev->init()) { - warnx("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - return PX4_OK; - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - return test(); - - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { - - if (argc > 2 && !strcmp(argv[2], "medium")) { - return latency_test(ORB_ID(orb_test_medium), true); - } else if (argc > 2 && !strcmp(argv[2], "large")) { - return latency_test(ORB_ID(orb_test_large), true); - } else { - return latency_test(ORB_ID(orb_test), true); - } - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) - return info(); - - usage(); - return -EINVAL; -} - -/* - * Library functions. - */ -namespace -{ - -/** - * Advertise a node; don't consider it an error if the node has - * already been advertised. - * - * @todo verify that the existing node is the same as the one - * we tried to advertise. - */ -int -node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) -{ - int fd = -1; - int ret = ERROR; - - /* fill advertiser data */ - const struct orb_advertdata adv = { meta, instance, priority }; - - /* open the control device */ - fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); - - if (fd < 0) - goto out; - - /* advertise the object */ - ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); - - /* it's PX4_OK if it already exists */ - if ((PX4_OK != ret) && (EEXIST == errno)) { - ret = PX4_OK; - } - -out: - - if (fd >= 0) - px4_close(fd); - - return ret; -} - -/** - * Common implementation for orb_advertise and orb_subscribe. - * - * Handles creation of the object and the initial publication for - * advertisers. - */ -int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) -{ - char path[orb_maxpath]; - int fd, ret; - - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return ERROR; - } - - /* - * Advertiser must publish an initial value. - */ - if (advertiser && (data == nullptr)) { - errno = EINVAL; - return ERROR; - } - - /* - * Generate the path to the node and try to open it. - */ - - // FIXME - if *instance is uninitialized, why is this being called? Seems risky and - // its definiately a waste. This is the case in muli-topic test. - ret = node_mkpath(path, f, meta, instance); - - if (ret != PX4_OK) { - errno = -ret; - return ERROR; - } - - /* open the path as either the advertiser or the subscriber */ - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); - - /* if we want to advertise and the node existed, we have to re-try again */ - if ((fd >= 0) && (instance != nullptr) && (advertiser)) { - /* close the fd, we want a new one */ - px4_close(fd); - - /* the node_advertise call will automatically go for the next free entry */ - fd = -1; - } - - /* we may need to advertise the node... */ - if (fd < 0) { - - /* try to create the node */ - ret = node_advertise(meta, instance, priority); - - if (ret == PX4_OK) { - /* update the path, as it might have been updated during the node_advertise call */ - ret = node_mkpath(path, f, meta, instance); - - if (ret != PX4_OK) { - errno = -ret; - return ERROR; - } - } - - /* on success, try the open again */ - if (ret == PX4_OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); - } - } - - if (fd < 0) { - errno = EIO; - return ERROR; - } - - /* everything has been PX4_OK, we can return the handle now */ - return fd; -} - -} // namespace - -int -orb_exists(const struct orb_metadata *meta, int instance) -{ - /* - * Generate the path to the node and try to open it. - */ - char path[orb_maxpath]; - int inst = instance; - int ret = node_mkpath(path, PUBSUB, meta, &inst); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - struct stat buffer; - return stat(path, &buffer); -} - -orb_advert_t -orb_advertise(const struct orb_metadata *meta, const void *data) -{ - //warnx("orb_advertise meta = %p", meta); - return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); -} - -orb_advert_t -orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) -{ - int result, fd; - orb_advert_t advertiser; - - //warnx("orb_advertise_multi meta = %p\n", meta); - - /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true, instance, priority); - if (fd == ERROR) { - warnx("node_open as advertiser failed."); - return ERROR; - } - - /* get the advertiser handle and close the node */ - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - px4_close(fd); - if (result == ERROR) { - warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); - return ERROR; - } - - /* the advertiser must perform an initial publish to initialise the object */ - result = orb_publish(meta, advertiser, data); - if (result == ERROR) { - warnx("orb_publish failed"); - return ERROR; - } - - return advertiser; -} - -int -orb_subscribe(const struct orb_metadata *meta) -{ - return node_open(PUBSUB, meta, nullptr, false); -} - -int -orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) -{ - int inst = instance; - return node_open(PUBSUB, meta, nullptr, false, &inst); -} - -int -orb_unsubscribe(int fd) -{ - return px4_close(fd); -} - -int -orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) -{ - return ORBDevNode::publish(meta, handle, data); -} - -int -orb_copy(const struct orb_metadata *meta, int handle, void *buffer) -{ - int ret; - - ret = px4_read(handle, buffer, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return PX4_OK; -} - -int -orb_check(int handle, bool *updated) -{ - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); -} - -int -orb_stat(int handle, uint64_t *time) -{ - return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); -} - -int -orb_priority(int handle, int *priority) -{ - return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); -} - -int -orb_set_interval(int handle, unsigned interval) -{ - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); -} - diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 82fb9a2c65..88f1d6807a 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -40,12 +40,19 @@ MODULE_COMMAND = uorb MODULE_STACKSIZE = 2048 ifeq ($(PX4_TARGET_OS),nuttx) -SRCS = uORB.cpp +SRCS = uORBDevices_nuttx.cpp \ + uORBManager_nuttx.cpp + else -SRCS = MuORB.cpp +SRCS = uORBDevices_posix.cpp \ + uORBTest_UnitTest.cpp \ + uORBManager_posix.cpp endif SRCS += objects_common.cpp \ Publication.cpp \ - Subscription.cpp + Subscription.cpp \ + uORBUtils.cpp \ + uORB.cpp \ + uORBMain.cpp MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index a878853dff..c1266552fe 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -36,1281 +36,265 @@ * A lightweight object broker. */ -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - #include "uORB.h" +#include "uORBManager.hpp" +#include "uORBCommon.hpp" /** - * Utility functions. - */ -namespace -{ - -/* internal use only */ -static const unsigned orb_maxpath = 64; - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -enum Flavor { - PUBSUB, - PARAM -}; - -struct orb_advertdata { - const struct orb_metadata *meta; - int *instance; - int priority; -}; - -int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) -{ - unsigned len; - - unsigned index = 0; - - if (instance != nullptr) { - index = *instance; - } - - len = snprintf(buf, orb_maxpath, "/%s/%s%d", - (f == PUBSUB) ? "obj" : "param", - meta->o_name, index); - - if (len >= orb_maxpath) { - return -ENAMETOOLONG; - } - - return OK; -} - -} - -/** - * Per-object device instance. - */ -class ORBDevNode : public device::CDev -{ -public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); - ~ORBDevNode(); - - virtual int open(struct file *filp); - virtual int close(struct file *filp); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); - -protected: - virtual pollevent_t poll_state(struct file *filp); - virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); - -private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ - struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ - }; - - const struct orb_metadata *_meta; /**< object metadata information */ - uint8_t *_data; /**< allocated object buffer */ - hrt_abstime _last_update; /**< time the object was last updated */ - volatile unsigned _generation; /**< object generation count */ - pid_t _publisher; /**< if nonzero, current publisher */ - const int _priority; /**< priority of topic */ - - SubscriberData *filp_to_sd(struct file *filp) { - SubscriberData *sd = (SubscriberData *)(filp->f_priv); - return sd; - } - - /** - * Perform a deferred update for a rate-limited subscriber. - */ - void update_deferred(); - - /** - * Bridge from hrt_call to update_deferred - * - * void *arg ORBDevNode pointer for which the deferred update is performed. - */ - static void update_deferred_trampoline(void *arg); - - /** - * Check whether a topic appears updated to a subscriber. - * - * @param sd The subscriber for whom to check. - * @return True if the topic should appear updated to the subscriber - */ - bool appears_updated(SubscriberData *sd); -}; - -ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : - CDev(name, path), - _meta(meta), - _data(nullptr), - _last_update(0), - _generation(0), - _publisher(0), - _priority(priority) -{ - // enable debug() calls - _debug_enabled = true; -} - -ORBDevNode::~ORBDevNode() -{ - if (_data != nullptr) - delete[] _data; - -} - -int -ORBDevNode::open(struct file *filp) -{ - int ret; - - /* is this a publisher? */ - if (filp->f_oflags == O_WRONLY) { - - /* become the publisher if we can */ - lock(); - - if (_publisher == 0) { - _publisher = getpid(); - ret = OK; - - } else { - ret = -EBUSY; - } - - unlock(); - - /* now complete the open */ - if (ret == OK) { - ret = CDev::open(filp); - - /* open failed - not the publisher anymore */ - if (ret != OK) - _publisher = 0; - } - - return ret; - } - - /* is this a new subscriber? */ - if (filp->f_oflags == O_RDONLY) { - - /* allocate subscriber data */ - SubscriberData *sd = new SubscriberData; - - if (nullptr == sd) - return -ENOMEM; - - memset(sd, 0, sizeof(*sd)); - - /* default to no pending update */ - sd->generation = _generation; - - /* set priority */ - sd->priority = _priority; - - filp->f_priv = (void *)sd; - - ret = CDev::open(filp); - - if (ret != OK) - delete sd; - - return ret; - } - - /* can only be pub or sub, not both */ - return -EINVAL; -} - -int -ORBDevNode::close(struct file *filp) -{ - /* is this the publisher closing? */ - if (getpid() == _publisher) { - _publisher = 0; - - } else { - SubscriberData *sd = filp_to_sd(filp); - - if (sd != nullptr) { - hrt_cancel(&sd->update_call); - delete sd; - } - } - - return CDev::close(filp); -} - -ssize_t -ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) -{ - SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); - - /* if the object has not been written yet, return zero */ - if (_data == nullptr) - return 0; - - /* if the caller's buffer is the wrong size, that's an error */ - if (buflen != _meta->o_size) - return -EIO; - - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); - - /* if the caller doesn't want the data, don't give it to them */ - if (nullptr != buffer) - memcpy(buffer, _data, _meta->o_size); - - /* track the last generation that the file has seen */ - sd->generation = _generation; - - /* set priority */ - sd->priority = _priority; - - /* - * Clear the flag that indicates that an update has been reported, as - * we have just collected it. - */ - sd->update_reported = false; - - irqrestore(flags); - - return _meta->o_size; -} - -ssize_t -ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) -{ - /* - * Writes are legal from interrupt context as long as the - * object has already been initialised from thread context. - * - * Writes outside interrupt context will allocate the object - * if it has not yet been allocated. - * - * Note that filp will usually be NULL. - */ - if (nullptr == _data) { - if (!up_interrupt_context()) { - - lock(); - - /* re-check size */ - if (nullptr == _data) - _data = new uint8_t[_meta->o_size]; - - unlock(); - } - - /* failed or could not allocate */ - if (nullptr == _data) - return -ENOMEM; - } - - /* If write size does not match, that is an error */ - if (_meta->o_size != buflen) - return -EIO; - - /* Perform an atomic copy. */ - irqstate_t flags = irqsave(); - memcpy(_data, buffer, _meta->o_size); - irqrestore(flags); - - /* update the timestamp and generation count */ - _last_update = hrt_absolute_time(); - _generation++; - - /* notify any poll waiters */ - poll_notify(POLLIN); - - return _meta->o_size; -} - -int -ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - SubscriberData *sd = filp_to_sd(filp); - - switch (cmd) { - case ORBIOCLASTUPDATE: - *(hrt_abstime *)arg = _last_update; - return OK; - - case ORBIOCUPDATED: - *(bool *)arg = appears_updated(sd); - return OK; - - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return OK; - - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return OK; - - case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; - return OK; - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -ssize_t -ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) -{ - ORBDevNode *devnode = (ORBDevNode *)handle; - int ret; - - /* this is a bit risky, since we are trusting the handle in order to deref it */ - if (devnode->_meta != meta) { - errno = EINVAL; - return ERROR; - } - - /* call the devnode write method with no file pointer */ - ret = devnode->write(nullptr, (const char *)data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; -} - -pollevent_t -ORBDevNode::poll_state(struct file *filp) -{ - SubscriberData *sd = filp_to_sd(filp); - - /* - * If the topic appears updated to the subscriber, say so. - */ - if (appears_updated(sd)) - return POLLIN; - - return 0; -} - -void -ORBDevNode::poll_notify_one(struct pollfd *fds, pollevent_t events) -{ - SubscriberData *sd = filp_to_sd((struct file *)fds->priv); - - /* - * If the topic looks updated to the subscriber, go ahead and notify them. - */ - if (appears_updated(sd)) - CDev::poll_notify_one(fds, events); -} - -bool -ORBDevNode::appears_updated(SubscriberData *sd) -{ - /* assume it doesn't look updated */ - bool ret = false; - - /* avoid racing between interrupt and non-interrupt context calls */ - irqstate_t state = irqsave(); - - /* check if this topic has been published yet, if not bail out */ - if (_data == nullptr) { - ret = false; - goto out; - } - - /* - * If the subscriber's generation count matches the update generation - * count, there has been no update from their perspective; if they - * don't match then we might have a visible update. - */ - while (sd->generation != _generation) { - - /* - * Handle non-rate-limited subscribers. - */ - if (sd->update_interval == 0) { - ret = true; - break; - } - - /* - * If we have previously told the subscriber that there is data, - * and they have not yet collected it, continue to tell them - * that there has been an update. This mimics the non-rate-limited - * behaviour where checking / polling continues to report an update - * until the topic is read. - */ - if (sd->update_reported) { - ret = true; - break; - } - - /* - * If the interval timer is still running, the topic should not - * appear updated, even though at this point we know that it has. - * We have previously been through here, so the subscriber - * must have collected the update we reported, otherwise - * update_reported would still be true. - */ - if (!hrt_called(&sd->update_call)) - break; - - /* - * Make sure that we don't consider the topic to be updated again - * until the interval has passed once more by restarting the interval - * timer and thereby re-scheduling a poll notification at that time. - */ - hrt_call_after(&sd->update_call, - sd->update_interval, - &ORBDevNode::update_deferred_trampoline, - (void *)this); - - /* - * Remember that we have told the subscriber that there is data. - */ - sd->update_reported = true; - ret = true; - - break; - } - -out: - irqrestore(state); - - /* consider it updated */ - return ret; -} - -void -ORBDevNode::update_deferred() -{ - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); -} - -void -ORBDevNode::update_deferred_trampoline(void *arg) -{ - ORBDevNode *node = (ORBDevNode *)arg; - - node->update_deferred(); -} - -/** - * Master control device for ObjDev. + * Advertise as the publisher of a topic. * - * Used primarily to create new objects via the ORBIOCCREATE - * ioctl. - */ -class ORBDevMaster : public device::CDev -{ -public: - ORBDevMaster(Flavor f); - ~ORBDevMaster(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); -private: - Flavor _flavor; -}; - -ORBDevMaster::ORBDevMaster(Flavor f) : - CDev((f == PUBSUB) ? "obj_master" : "param_master", - (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), - _flavor(f) -{ - // enable debug() calls - _debug_enabled = true; - -} - -ORBDevMaster::~ORBDevMaster() -{ -} - -int -ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret; - - switch (cmd) { - case ORBIOCADVERTISE: { - const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; - const struct orb_metadata *meta = adv->meta; - const char *objname; - const char *devpath; - char nodepath[orb_maxpath]; - ORBDevNode *node; - - /* set instance to zero - we could allow selective multi-pubs later based on value */ - if (adv->instance != nullptr) { - *(adv->instance) = 0; - } - - /* construct a path to the node - this also checks the node name */ - ret = node_mkpath(nodepath, _flavor, meta, adv->instance); - - if (ret != OK) { - return ret; - } - - /* ensure that only one advertiser runs through this critical section */ - lock(); - - ret = ERROR; - - /* try for topic groups */ - const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - do { - /* if path is modifyable change try index */ - if (adv->instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *(adv->instance) = group_tries; - } - - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } - - /* driver wants a permanent copy of the path, so make one here */ - devpath = strdup(nodepath); - - if (devpath == nullptr) { - return -ENOMEM; - } - - /* construct the new node */ - node = new ORBDevNode(meta, objname, devpath, adv->priority); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - unlock(); - return -ENOMEM; - } - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); - - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); - free((void *)devpath); - } - - group_tries++; - - } while (ret != OK && (group_tries < max_group_tries)); - - if (group_tries > max_group_tries) { - ret = -ENOMEM; - } - - /* the file handle for the driver has been created, unlock */ - unlock(); - - return ret; - } - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - - -/** - * Local functions in support of the shell command. - */ - -namespace -{ - -ORBDevMaster *g_dev; -bool pubsubtest_passed = false; -bool pubsubtest_print = false; -int pubsubtest_res = OK; - -struct orb_test { - int val; - hrt_abstime time; -}; - -ORB_DEFINE(orb_test, struct orb_test); -ORB_DEFINE(orb_multitest, struct orb_test); - -struct orb_test_medium { - int val; - hrt_abstime time; - char junk[64]; -}; - -ORB_DEFINE(orb_test_medium, struct orb_test_medium); - -struct orb_test_large { - int val; - hrt_abstime time; - char junk[512]; -}; - -ORB_DEFINE(orb_test_large, struct orb_test_large); - -int -test_fail(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "FAIL: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return ERROR; -} - -int -test_note(const char *fmt, ...) -{ - va_list ap; - - fprintf(stderr, "note: "); - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - return OK; -} - -int pubsublatency_main(void) -{ - /* poll on test topic and output latency */ - float latency_integral = 0.0f; - - /* wakeup source(s) */ - struct pollfd fds[3]; - - int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); - int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); - int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); - - struct orb_test_large t; - - /* clear all ready flags */ - orb_copy(ORB_ID(orb_test), test_multi_sub, &t); - orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); - orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); - - fds[0].fd = test_multi_sub; - fds[0].events = POLLIN; - fds[1].fd = test_multi_sub_medium; - fds[1].events = POLLIN; - fds[2].fd = test_multi_sub_large; - fds[2].events = POLLIN; - - const unsigned maxruns = 1000; - unsigned timingsgroup = 0; - - unsigned *timings = new unsigned[maxruns]; - - for (unsigned i = 0; i < maxruns; i++) { - /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(orb_test), test_multi_sub, &t); - timingsgroup = 0; - } else if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); - timingsgroup = 1; - } else if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); - timingsgroup = 2; - } - - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - hrt_abstime elt = hrt_elapsed_time(&t.time); - latency_integral += elt; - timings[i] = elt; - } - - orb_unsubscribe(test_multi_sub); - orb_unsubscribe(test_multi_sub_medium); - orb_unsubscribe(test_multi_sub_large); - - if (pubsubtest_print) { - char fname[32]; - sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); - FILE *f = fopen(fname, "w"); - if (f == NULL) { - warnx("Error opening file!\n"); - return ERROR; - } - - for (unsigned i = 0; i < maxruns; i++) { - fprintf(f, "%u\n", timings[i]); - } - - fclose(f); - } - - delete[] timings; - - warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); - - pubsubtest_passed = true; - - if (static_cast(latency_integral / maxruns) > 30.0f) { - pubsubtest_res = ERROR; - } else { - pubsubtest_res = OK; - } - - return pubsubtest_res; -} - -template int latency_test(orb_id_t T, bool print); - -int -test() -{ - struct orb_test t, u; - int pfd, sfd; - bool updated; - - t.val = 0; - pfd = orb_advertise(ORB_ID(orb_test), &t); - - if (pfd < 0) - return test_fail("advertise failed: %d", errno); - - test_note("publish handle 0x%08x", pfd); - sfd = orb_subscribe(ORB_ID(orb_test)); - - if (sfd < 0) - return test_fail("subscribe failed: %d", errno); - - test_note("subscribe fd %d", sfd); - u.val = 1; - - if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(1) failed: %d", errno); - - if (u.val != t.val) - return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); - - if (OK != orb_check(sfd, &updated)) - return test_fail("check(1) failed"); - - if (updated) - return test_fail("spurious updated flag"); - - t.val = 2; - test_note("try publish"); - - if (OK != orb_publish(ORB_ID(orb_test), pfd, &t)) - return test_fail("publish failed"); - - if (OK != orb_check(sfd, &updated)) - return test_fail("check(2) failed"); - - if (!updated) - return test_fail("missing updated flag"); - - if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) - return test_fail("copy(2) failed: %d", errno); - - if (u.val != t.val) - return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); - - orb_unsubscribe(sfd); - close(pfd); - - /* this routine tests the multi-topic support */ - test_note("try multi-topic support"); - - int instance0; - int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - - test_note("advertised"); - - int instance1; - int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); - - if (instance0 != 0) - return test_fail("mult. id0: %d", instance0); - - if (instance1 != 1) - return test_fail("mult. id1: %d", instance1); - - t.val = 103; - if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) - return test_fail("mult. pub0 fail"); - - test_note("published"); - - t.val = 203; - if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) - return test_fail("mult. pub1 fail"); - - /* subscribe to both topics and ensure valid data is received */ - int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - - if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) - return test_fail("sub #0 copy failed: %d", errno); - - if (u.val != 103) - return test_fail("sub #0 val. mismatch: %d", u.val); - - int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); - - if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) - return test_fail("sub #1 copy failed: %d", errno); - - if (u.val != 203) - return test_fail("sub #1 val. mismatch: %d", u.val); - - /* test priorities */ - int prio; - if (OK != orb_priority(sfd0, &prio)) - return test_fail("prio #0"); - - if (prio != ORB_PRIO_MAX) - return test_fail("prio: %d", prio); - - if (OK != orb_priority(sfd1, &prio)) - return test_fail("prio #1"); - - if (prio != ORB_PRIO_MIN) - return test_fail("prio: %d", prio); - - if (OK != latency_test(ORB_ID(orb_test), false)) - return test_fail("latency test failed"); - - return test_note("PASS"); -} - -template int -latency_test(orb_id_t T, bool print) -{ - S t; - t.val = 308; - t.time = hrt_absolute_time(); - - int pfd0 = orb_advertise(T, &t); - - pubsubtest_print = print; - - pubsubtest_passed = false; - - /* test pub / sub latency */ - - int pubsub_task = task_spawn_cmd("uorb_latency", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (main_t)&pubsublatency_main, - nullptr); - - /* give the test task some data */ - while (!pubsubtest_passed) { - t.val = 308; - t.time = hrt_absolute_time(); - if (OK != orb_publish(T, pfd0, &t)) - return test_fail("mult. pub0 timing fail"); - - /* simulate >800 Hz system operation */ - usleep(1000); - } - - close(pfd0); - - if (pubsub_task < 0) { - return test_fail("failed launching task"); - } - - return pubsubtest_res; -} - -int -info() -{ - return OK; -} - - -} // namespace - -/* - * uORB server 'main'. - */ -extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } - -int -uorb_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) { - warnx("already loaded"); - /* user wanted to start uorb, its already running, no error */ - return 0; - } - - /* create the driver */ - g_dev = new ORBDevMaster(PUBSUB); - - if (g_dev == nullptr) { - warnx("driver alloc failed"); - return -ENOMEM; - } - - if (OK != g_dev->init()) { - warnx("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - return OK; - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - return test(); - - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { - - if (argc > 2 && !strcmp(argv[2], "medium")) { - return latency_test(ORB_ID(orb_test_medium), true); - } else if (argc > 2 && !strcmp(argv[2], "large")) { - return latency_test(ORB_ID(orb_test_large), true); - } else { - return latency_test(ORB_ID(orb_test), true); - } - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) - return info(); - - errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); -} - -/* - * Library functions. - */ -namespace -{ - -/** - * Advertise a node; don't consider it an error if the node has - * already been advertised. + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. * - * @todo verify that the existing node is the same as the one - * we tried to advertise. + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. */ -int -node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - int fd = -1; - int ret = ERROR; - - /* fill advertiser data */ - const struct orb_advertdata adv = { meta, instance, priority }; - - /* open the control device */ - fd = open(TOPIC_MASTER_DEVICE_PATH, 0); - - if (fd < 0) - goto out; - - /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); - - /* it's OK if it already exists */ - if ((OK != ret) && (EEXIST == errno)) { - ret = OK; - } - -out: - - if (fd >= 0) - close(fd); - - return ret; + return uORB::Manager::get_instance()->orb_advertise( meta, data ); } /** - * Common implementation for orb_advertise and orb_subscribe. + * Advertise as the publisher of a topic. * - * Handles creation of the object and the initial publication for - * advertisers. + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. */ -int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) +orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + int priority) { - char path[orb_maxpath]; - int fd, ret; - - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return ERROR; - } - - /* - * Advertiser must publish an initial value. - */ - if (advertiser && (data == nullptr)) { - errno = EINVAL; - return ERROR; - } - - /* - * Generate the path to the node and try to open it. - */ - ret = node_mkpath(path, f, meta, instance); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - /* open the path as either the advertiser or the subscriber */ - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); - - /* if we want to advertise and the node existed, we have to re-try again */ - if ((fd >= 0) && (instance != nullptr) && (advertiser)) { - /* close the fd, we want a new one */ - close(fd); - /* the node_advertise call will automatically go for the next free entry */ - fd = -1; - } - - /* we may need to advertise the node... */ - if (fd < 0) { - - /* try to create the node */ - ret = node_advertise(meta, instance, priority); - - if (ret == OK) { - /* update the path, as it might have been updated during the node_advertise call */ - ret = node_mkpath(path, f, meta, instance); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - } - - /* on success, try the open again */ - if (ret == OK) { - fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); - } - } - - if (fd < 0) { - errno = EIO; - return ERROR; - } - - /* everything has been OK, we can return the handle now */ - return fd; + return uORB::Manager::get_instance()->orb_advertise_multi( meta, data, instance, priority ); } -} // namespace -int -orb_exists(const struct orb_metadata *meta, int instance) +/** + * Publish new data to a topic. + * + * The data is atomically published to the topic and any waiting subscribers + * will be notified. Subscribers that are not waiting can check the topic + * for updates using orb_check and/or orb_stat. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @handle The handle returned from orb_advertise. + * @param data A pointer to the data to be published. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - /* - * Generate the path to the node and try to open it. - */ - char path[orb_maxpath]; - int inst = instance; - int ret = node_mkpath(path, PUBSUB, meta, &inst); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - struct stat buffer; - return stat(path, &buffer); + return uORB::Manager::get_instance()->orb_publish( meta, handle, data ); } -orb_advert_t -orb_advertise(const struct orb_metadata *meta, const void *data) +/** + * Subscribe to a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +int orb_subscribe(const struct orb_metadata *meta) { - return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); + return uORB::Manager::get_instance()->orb_subscribe( meta ); } -orb_advert_t -orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +/** + * Subscribe to a multi-instance of a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param instance The instance of the topic. Instance 0 matches the + * topic of the orb_subscribe() call, higher indices + * are for topics created with orb_publish_multi(). + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { - int result, fd; - orb_advert_t advertiser; - - /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true, instance, priority); - if (fd == ERROR) - return ERROR; - - /* get the advertiser handle and close the node */ - result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - close(fd); - if (result == ERROR) - return ERROR; - - /* the advertiser must perform an initial publish to initialise the object */ - result = orb_publish(meta, advertiser, data); - if (result == ERROR) - return ERROR; - - return advertiser; + return uORB::Manager::get_instance()->orb_subscribe_multi( meta, instance ); } -int -orb_subscribe(const struct orb_metadata *meta) +/** + * Unsubscribe from a topic. + * + * @param handle A handle returned from orb_subscribe. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +int orb_unsubscribe(int handle) { - return node_open(PUBSUB, meta, nullptr, false); + return uORB::Manager::get_instance()->orb_unsubscribe( handle ); } -int -orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +/** + * Fetch data from a topic. + * + * This is the only operation that will reset the internal marker that + * indicates that a topic has been updated for a subscriber. Once poll + * or check return indicating that an updaet is available, this call + * must be used to update the subscription. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param handle A handle returned from orb_subscribe. + * @param buffer Pointer to the buffer receiving the data, or NULL + * if the caller wants to clear the updated flag without + * using the data. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { - int inst = instance; - return node_open(PUBSUB, meta, nullptr, false, &inst); + return uORB::Manager::get_instance()->orb_copy( meta, handle, buffer ); } -int -orb_unsubscribe(int handle) +/** + * Check whether a topic has been published to since the last orb_copy. + * + * This check can be used to determine whether to copy the topic when + * not using poll(), or to avoid the overhead of calling poll() when the + * topic is likely to have updated. + * + * Updates are tracked on a per-handle basis; this call will continue to + * return true until orb_copy is called using the same handle. This interface + * should be preferred over calling orb_stat due to the race window between + * stat and copy that can lead to missed updates. + * + * @param handle A handle returned from orb_subscribe. + * @param updated Set to true if the topic has been updated since the + * last time it was copied using this handle. + * @return OK if the check was successful, ERROR otherwise with + * errno set accordingly. + */ +int orb_check(int handle, bool *updated) { - return close(handle); + return uORB::Manager::get_instance()->orb_check( handle, updated ); } -int -orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +/** + * Return the last time that the topic was updated. + * + * @param handle A handle returned from orb_subscribe. + * @param time Returns the absolute time that the topic was updated, or zero if it has + * never been updated. Time is measured in microseconds. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +int orb_stat(int handle, uint64_t *time) { - return ORBDevNode::publish(meta, handle, data); + return uORB::Manager::get_instance()->orb_stat( handle, time ); } -int -orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +/** + * Check if a topic has already been created. + * + * @param meta ORB topic metadata. + * @param instance ORB instance + * @return OK if the topic exists, ERROR otherwise with errno set accordingly. + */ +int orb_exists(const struct orb_metadata *meta, int instance) { - int ret; - - ret = read(handle, buffer, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; + return uORB::Manager::get_instance()->orb_exists( meta, instance ); } -int -orb_check(int handle, bool *updated) +/** + * Return the priority of the topic + * + * @param handle A handle returned from orb_subscribe. + * @param priority Returns the priority of this topic. This is only relevant for + * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) + * and allows a subscriber to automatically pick the topic with the highest + * priority, independent of the startup order of the associated publishers. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +int orb_priority(int handle, int *priority) { - return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + return uORB::Manager::get_instance()->orb_priority( handle, priority ); } -int -orb_stat(int handle, uint64_t *time) +/** + * Set the minimum interval between which updates are seen for a subscription. + * + * If this interval is set, the subscriber will not see more than one update + * within the period. + * + * Specifically, the first time an update is reported to the subscriber a timer + * is started. The update will continue to be reported via poll and orb_check, but + * once fetched via orb_copy another update will not be reported until the timer + * expires. + * + * This feature can be used to pace a subscriber that is watching a topic that + * would otherwise update too quickly. + * + * @param handle A handle returned from orb_subscribe. + * @param interval An interval period in milliseconds. + * @return OK on success, ERROR otherwise with ERRNO set accordingly. + */ +int orb_set_interval(int handle, unsigned interval) { - return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); -} - -int -orb_priority(int handle, int *priority) -{ - return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); -} - -int -orb_set_interval(int handle, unsigned interval) -{ - return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + return uORB::Manager::get_instance()->orb_set_interval( handle, interval ); } diff --git a/src/modules/uORB/uORBCommon.hpp b/src/modules/uORB/uORBCommon.hpp new file mode 100644 index 0000000000..06f731e823 --- /dev/null +++ b/src/modules/uORB/uORBCommon.hpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#ifndef _uORBCommon_hpp_ +#define _uORBCommon_hpp_ + +#include +#include +#include +#include "uORB.h" +#include + + +namespace uORB +{ + static const unsigned orb_maxpath = 64; + + #ifdef ERROR + # undef ERROR + #endif + /* ERROR is not defined for c++ */ + const int ERROR = -1; + + enum Flavor { + PUBSUB, + PARAM + }; + + struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; + }; +} +#endif // _uORBCommon_hpp_ diff --git a/src/modules/uORB/uORBDevices.hpp b/src/modules/uORB/uORBDevices.hpp new file mode 100644 index 0000000000..52f4ac6e8f --- /dev/null +++ b/src/modules/uORB/uORBDevices.hpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ +#pragma once + +#ifdef __PX4_NUTTX +#include "uORBDevices_nuttx.hpp" +#elif defined (__PX4_POSIX) +#include "uORBDevices_posix.hpp" +#endif diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp new file mode 100644 index 0000000000..d3c30f0508 --- /dev/null +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -0,0 +1,531 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include "uORBDevices_nuttx.hpp" +#include "uORBUtils.hpp" +#include + +uORB::DeviceNode::DeviceNode +( + const struct orb_metadata *meta, + const char *name, + const char *path, + int priority + ) : + CDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0), + _priority(priority) +{ + // enable debug() calls + _debug_enabled = true; +} + +uORB::DeviceNode::~DeviceNode() +{ + if (_data != nullptr) + delete[] _data; + +} + +int +uORB::DeviceNode::open(struct file *filp) +{ + int ret; + + /* is this a publisher? */ + if (filp->f_oflags == O_WRONLY) { + + /* become the publisher if we can */ + lock(); + + if (_publisher == 0) { + _publisher = getpid(); + ret = OK; + + } else { + ret = -EBUSY; + } + + unlock(); + + /* now complete the open */ + if (ret == OK) { + ret = CDev::open(filp); + + /* open failed - not the publisher anymore */ + if (ret != OK) + _publisher = 0; + } + + return ret; + } + + /* is this a new subscriber? */ + if (filp->f_oflags == O_RDONLY) { + + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; + + if (nullptr == sd) + return -ENOMEM; + + memset(sd, 0, sizeof(*sd)); + + /* default to no pending update */ + sd->generation = _generation; + + /* set priority */ + sd->priority = _priority; + + filp->f_priv = (void *)sd; + + ret = CDev::open(filp); + + if (ret != OK) + delete sd; + + return ret; + } + + /* can only be pub or sub, not both */ + return -EINVAL; +} + +int +uORB::DeviceNode::close(struct file *filp) +{ + /* is this the publisher closing? */ + if (getpid() == _publisher) { + _publisher = 0; + + } else { + SubscriberData *sd = filp_to_sd(filp); + + if (sd != nullptr) { + hrt_cancel(&sd->update_call); + delete sd; + } + } + + return CDev::close(filp); +} + +ssize_t +uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen) +{ + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + + /* if the object has not been written yet, return zero */ + if (_data == nullptr) + return 0; + + /* if the caller's buffer is the wrong size, that's an error */ + if (buflen != _meta->o_size) + return -EIO; + + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) + memcpy(buffer, _data, _meta->o_size); + + /* track the last generation that the file has seen */ + sd->generation = _generation; + + /* set priority */ + sd->priority = _priority; + + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; + + irqrestore(flags); + + return _meta->o_size; +} + +ssize_t +uORB::DeviceNode::write(struct file *filp, const char *buffer, size_t buflen) +{ + /* + * Writes are legal from interrupt context as long as the + * object has already been initialised from thread context. + * + * Writes outside interrupt context will allocate the object + * if it has not yet been allocated. + * + * Note that filp will usually be NULL. + */ + if (nullptr == _data) { + if (!up_interrupt_context()) { + + lock(); + + /* re-check size */ + if (nullptr == _data) + _data = new uint8_t[_meta->o_size]; + + unlock(); + } + + /* failed or could not allocate */ + if (nullptr == _data) + return -ENOMEM; + } + + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) + return -EIO; + + /* Perform an atomic copy. */ + irqstate_t flags = irqsave(); + memcpy(_data, buffer, _meta->o_size); + irqrestore(flags); + + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; + + /* notify any poll waiters */ + poll_notify(POLLIN); + + return _meta->o_size; +} + +int +uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + SubscriberData *sd = filp_to_sd(filp); + + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return OK; + + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return OK; + + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return OK; + + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return OK; + + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +uORB::DeviceNode::publish +( + const orb_metadata *meta, + orb_advert_t handle, + const void *data +) +{ + uORB::DeviceNode *DeviceNode = (uORB::DeviceNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (DeviceNode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the DeviceNode write method with no file pointer */ + ret = DeviceNode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + +pollevent_t +uORB::DeviceNode::poll_state(struct file *filp) +{ + SubscriberData *sd = filp_to_sd(filp); + + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) + return POLLIN; + + return 0; +} + +void +uORB::DeviceNode::poll_notify_one(struct pollfd *fds, pollevent_t events) +{ + SubscriberData *sd = filp_to_sd((struct file *)fds->priv); + + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) + CDev::poll_notify_one(fds, events); +} + +bool +uORB::DeviceNode::appears_updated(SubscriberData *sd) +{ + /* assume it doesn't look updated */ + bool ret = false; + + /* avoid racing between interrupt and non-interrupt context calls */ + irqstate_t state = irqsave(); + + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + + /* + * If the subscriber's generation count matches the update generation + * count, there has been no update from their perspective; if they + * don't match then we might have a visible update. + */ + while (sd->generation != _generation) { + + /* + * Handle non-rate-limited subscribers. + */ + if (sd->update_interval == 0) { + ret = true; + break; + } + + /* + * If we have previously told the subscriber that there is data, + * and they have not yet collected it, continue to tell them + * that there has been an update. This mimics the non-rate-limited + * behaviour where checking / polling continues to report an update + * until the topic is read. + */ + if (sd->update_reported) { + ret = true; + break; + } + + /* + * If the interval timer is still running, the topic should not + * appear updated, even though at this point we know that it has. + * We have previously been through here, so the subscriber + * must have collected the update we reported, otherwise + * update_reported would still be true. + */ + if (!hrt_called(&sd->update_call)) + break; + + /* + * Make sure that we don't consider the topic to be updated again + * until the interval has passed once more by restarting the interval + * timer and thereby re-scheduling a poll notification at that time. + */ + hrt_call_after(&sd->update_call, + sd->update_interval, + &uORB::DeviceNode::update_deferred_trampoline, + (void *)this); + + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; + + break; + } + +out: + irqrestore(state); + + /* consider it updated */ + return ret; +} + +void +uORB::DeviceNode::update_deferred() +{ + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); +} + +void +uORB::DeviceNode::update_deferred_trampoline(void *arg) +{ + uORB::DeviceNode *node = (uORB::DeviceNode *)arg; + + node->update_deferred(); +} + +uORB::DeviceMaster::DeviceMaster(Flavor f) : + CDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) +{ + // enable debug() calls + _debug_enabled = true; + +} + +uORB::DeviceMaster::~DeviceMaster() +{ +} + +int +uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret; + + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; + const char *objname; + const char *devpath; + char nodepath[orb_maxpath]; + uORB::DeviceNode *node; + + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + + /* construct a path to the node - this also checks the node name */ + ret = uORB::Utils::node_mkpath( nodepath, _flavor, meta, adv->instance); + + if (ret != OK) { + return ret; + } + + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + if (devpath == nullptr) { + return -ENOMEM; + } + + /* construct the new node */ + node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + free((void *)devpath); + } + + group_tries++; + + } while (ret != OK && (group_tries < max_group_tries)); + + if (group_tries > max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + + return ret; + } + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp new file mode 100644 index 0000000000..3d8c0f90ad --- /dev/null +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#ifndef _uORBDevices_nuttx_hpp_ +#define _uORBDevices_nuttx_hpp_ + +#include +#include "uORBCommon.hpp" + + +namespace uORB +{ + class DeviceNode; + class DeviceMaster; +} + +/** + * Per-object device instance. + */ +class uORB::DeviceNode : public device::CDev +{ +public: + /** + * Constructor + */ + DeviceNode + ( + const struct orb_metadata *meta, + const char *name, + const char *path, + int priority + ); + + /** + * Destructor + */ + ~DeviceNode(); + + /** + * Method to create a subscriber instance and return the struct + * pointing to the subscriber as a file pointer. + */ + virtual int open(struct file *filp); + + /** + * Method to close a subscriber for this topic. + */ + virtual int close(struct file *filp); + + /** + * reads data from a subscriber node to the buffer provided. + * @param filp + * The subscriber from which the data needs to be read from. + * @param buffer + * The buffer into which the data is read into. + * @param buflen + * the length of the buffer + * @return + * ssize_t the number of bytes read. + */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + /** + * writes the published data to the internal buffer to be read by + * subscribers later. + * @param filp + * the subscriber; this is not used. + * @param buffer + * The buffer for the input data + * @param buflen + * the length of the buffer. + * @return ssize_t + * The number of bytes that are written + */ + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + + /** + * IOCTL control for the subscriber. + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Method to publish a data to this node. + */ + static ssize_t publish + ( + const orb_metadata *meta, + orb_advert_t handle, + const void *data + ); + +protected: + virtual pollevent_t poll_state(struct file *filp); + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + +private: + struct SubscriberData { + unsigned generation; /**< last generation the subscriber has seen */ + unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ + void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ + bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ + }; + + const struct orb_metadata *_meta; /**< object metadata information */ + uint8_t *_data; /**< allocated object buffer */ + hrt_abstime _last_update; /**< time the object was last updated */ + volatile unsigned _generation; /**< object generation count */ + pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ + +private: // private class methods. + + SubscriberData *filp_to_sd(struct file *filp) { + SubscriberData *sd = (SubscriberData *)(filp->f_priv); + return sd; + } + + /** + * Perform a deferred update for a rate-limited subscriber. + */ + void update_deferred(); + + /** + * Bridge from hrt_call to update_deferred + * + * void *arg ORBDevNode pointer for which the deferred update is performed. + */ + static void update_deferred_trampoline(void *arg); + + /** + * Check whether a topic appears updated to a subscriber. + * + * @param sd The subscriber for whom to check. + * @return True if the topic should appear updated to the subscriber + */ + bool appears_updated(SubscriberData *sd); +}; + +/** + * Master control device for ObjDev. + * + * Used primarily to create new objects via the ORBIOCCREATE + * ioctl. + */ +class uORB::DeviceMaster : public device::CDev +{ +public: + DeviceMaster(Flavor f); + virtual ~DeviceMaster(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); +private: + Flavor _flavor; +}; + + + +#endif diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp new file mode 100644 index 0000000000..e3530926e1 --- /dev/null +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -0,0 +1,551 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include "uORBDevices_posix.hpp" +#include "uORBUtils.hpp" +#include + + +uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp) +{ + uORB::DeviceNode::SubscriberData *sd; + if (filp) { + sd = (uORB::DeviceNode::SubscriberData *)(filp->priv); + } + else { + sd = 0; + } + return sd; +} + +uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : + VDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0), + _priority(priority) +{ + // enable debug() calls + //_debug_enabled = true; +} + +uORB::DeviceNode::~DeviceNode() +{ + if (_data != nullptr) + delete[] _data; + +} + +int +uORB::DeviceNode::open(device::file_t *filp) +{ + int ret; + + /* is this a publisher? */ + if (filp->flags == PX4_F_WRONLY) { + + /* become the publisher if we can */ + lock(); + + if (_publisher == 0) { + _publisher = getpid(); + ret = PX4_OK; + + } else { + ret = -EBUSY; + } + + unlock(); + + /* now complete the open */ + if (ret == PX4_OK) { + ret = VDev::open(filp); + + /* open failed - not the publisher anymore */ + if (ret != PX4_OK) + _publisher = 0; + } + + return ret; + } + + /* is this a new subscriber? */ + if (filp->flags == PX4_F_RDONLY) { + + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; + + if (nullptr == sd) + return -ENOMEM; + + memset(sd, 0, sizeof(*sd)); + + /* default to no pending update */ + sd->generation = _generation; + + /* set priority */ + sd->priority = _priority; + + filp->priv = (void *)sd; + + ret = VDev::open(filp); + + if (ret != PX4_OK) { + warnx("ERROR: VDev::open failed\n"); + delete sd; + } + + //warnx("uORB::DeviceNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev); + return ret; + } + + /* can only be pub or sub, not both */ + return -EINVAL; +} + +int +uORB::DeviceNode::close(device::file_t *filp) +{ + //warnx("uORB::DeviceNode::close fd = %d", filp->fd); + /* is this the publisher closing? */ + if (getpid() == _publisher) { + _publisher = 0; + + } else { + SubscriberData *sd = filp_to_sd(filp); + + if (sd != nullptr) { + hrt_cancel(&sd->update_call); + delete sd; + } + } + + return VDev::close(filp); +} + +ssize_t +uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen) +{ + //warnx("uORB::DeviceNode::read fd = %d\n", filp->fd); + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + + /* if the object has not been written yet, return zero */ + if (_data == nullptr) + return 0; + + /* if the caller's buffer is the wrong size, that's an error */ + if (buflen != _meta->o_size) + return -EIO; + + /* + * Perform an atomic copy & state update + */ + lock(); + + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) + memcpy(buffer, _data, _meta->o_size); + + /* track the last generation that the file has seen */ + sd->generation = _generation; + + /* set priority */ + sd->priority = _priority; + + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; + + unlock(); + + return _meta->o_size; +} + +ssize_t +uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen) +{ + //warnx("uORB::DeviceNode::write filp = %p (null is normal)", filp); + /* + * Writes are legal from interrupt context as long as the + * object has already been initialised from thread context. + * + * Writes outside interrupt context will allocate the object + * if it has not yet been allocated. + * + * Note that filp will usually be NULL. + */ + if (nullptr == _data) { + lock(); + + /* re-check size */ + if (nullptr == _data) + _data = new uint8_t[_meta->o_size]; + + unlock(); + + /* failed or could not allocate */ + if (nullptr == _data) + return -ENOMEM; + } + + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) + return -EIO; + + /* Perform an atomic copy. */ + lock(); + memcpy(_data, buffer, _meta->o_size); + unlock(); + + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; + + /* notify any poll waiters */ + poll_notify(POLLIN); + + return _meta->o_size; +} + +int +uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + //warnx("uORB::DeviceNode::ioctl fd = %d cmd = %d", filp->fd, cmd); + SubscriberData *sd = filp_to_sd(filp); + + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return PX4_OK; + + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return PX4_OK; + + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return PX4_OK; + + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return PX4_OK; + + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return PX4_OK; + + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +{ + //warnx("uORB::DeviceNode::publish meta = %p", meta); + + if (handle < 0) { + warnx("uORB::DeviceNode::publish called with invalid handle"); + errno = EINVAL; + return ERROR; + } + + uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (devnode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = devnode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return PX4_OK; +} + +pollevent_t +uORB::DeviceNode::poll_state(device::file_t *filp) +{ + //warnx("uORB::DeviceNode::poll_state fd = %d", filp->fd); + SubscriberData *sd = filp_to_sd(filp); + + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) + return POLLIN; + + return 0; +} + +void +uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +{ + //warnx("uORB::DeviceNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv); + SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv); + + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) + VDev::poll_notify_one(fds, events); +} + +bool +uORB::DeviceNode::appears_updated(SubscriberData *sd) +{ + //warnx("uORB::DeviceNode::appears_updated sd = %p", sd); + /* assume it doesn't look updated */ + bool ret = false; + + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + + /* + * If the subscriber's generation count matches the update generation + * count, there has been no update from their perspective; if they + * don't match then we might have a visible update. + */ + while (sd->generation != _generation) { + + /* + * Handle non-rate-limited subscribers. + */ + if (sd->update_interval == 0) { + ret = true; + break; + } + + /* + * If we have previously told the subscriber that there is data, + * and they have not yet collected it, continue to tell them + * that there has been an update. This mimics the non-rate-limited + * behaviour where checking / polling continues to report an update + * until the topic is read. + */ + if (sd->update_reported) { + ret = true; + break; + } + +// FIXME - the calls to hrt_called and hrt_call_after seem not to work in the +// POSIX build +#ifndef __PX4_POSIX + /* + * If the interval timer is still running, the topic should not + * appear updated, even though at this point we know that it has. + * We have previously been through here, so the subscriber + * must have collected the update we reported, otherwise + * update_reported would still be true. + */ + if (!hrt_called(&sd->update_call)) + break; + + /* + * Make sure that we don't consider the topic to be updated again + * until the interval has passed once more by restarting the interval + * timer and thereby re-scheduling a poll notification at that time. + */ + hrt_call_after(&sd->update_call, + sd->update_interval, + &uORB::DeviceNode::update_deferred_trampoline, + (void *)this); +#endif + + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; + + break; + } + +out: + /* consider it updated */ + return ret; +} + +void +uORB::DeviceNode::update_deferred() +{ + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); +} + +void +uORB::DeviceNode::update_deferred_trampoline(void *arg) +{ + uORB::DeviceNode *node = (uORB::DeviceNode *)arg; + + node->update_deferred(); +} + +uORB::DeviceMaster::DeviceMaster(Flavor f) : + VDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) +{ + // enable debug() calls + _debug_enabled = true; + +} + +uORB::DeviceMaster::~DeviceMaster() +{ +} + +int +uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + int ret; + + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; + const char *objname; + const char *devpath; + char nodepath[orb_maxpath]; + uORB::DeviceNode *node; + + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + + /* construct a path to the node - this also checks the node name */ + ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance); + + if (ret != PX4_OK) { + return ret; + } + + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + if (devpath == nullptr) { + // FIXME - looks like we leaked memory here for objname + return -ENOMEM; + } + + /* construct the new node */ + node = new uORB::DeviceNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + + // FIXME - looks like we leaked memory here for devpath and objname + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != PX4_OK) { + delete node; + free((void *)objname); + free((void *)devpath); + } + + group_tries++; + + } while (ret != PX4_OK && (group_tries < max_group_tries)); + + if (group_tries > max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + + return ret; + } + + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } +} + diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp new file mode 100644 index 0000000000..ce22b84321 --- /dev/null +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#ifndef _uORBDevices_posix_hpp_ +#define _uORBDevices_posix_hpp_ + +#include +#include "uORBCommon.hpp" + + +namespace uORB +{ + class DeviceNode; + class DeviceMaster; +} + +class uORB::DeviceNode : public device::VDev +{ +public: + DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); + ~DeviceNode(); + + virtual int open(device::file_t *filp); + virtual int close(device::file_t *filp); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + +protected: + virtual pollevent_t poll_state(device::file_t *filp); + virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + +private: + struct SubscriberData { + unsigned generation; /**< last generation the subscriber has seen */ + unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ + void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ + bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ + }; + + const struct orb_metadata *_meta; /**< object metadata information */ + uint8_t *_data; /**< allocated object buffer */ + hrt_abstime _last_update; /**< time the object was last updated */ + volatile unsigned _generation; /**< object generation count */ + pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ + + SubscriberData *filp_to_sd(device::file_t *filp); + + /** + * Perform a deferred update for a rate-limited subscriber. + */ + void update_deferred(); + + /** + * Bridge from hrt_call to update_deferred + * + * void *arg ORBDevNode pointer for which the deferred update is performed. + */ + static void update_deferred_trampoline(void *arg); + + /** + * Check whether a topic appears updated to a subscriber. + * + * @param sd The subscriber for whom to check. + * @return True if the topic should appear updated to the subscriber + */ + bool appears_updated(SubscriberData *sd); +}; + +/** + * Master control device for ObjDev. + * + * Used primarily to create new objects via the ORBIOCCREATE + * ioctl. + */ +class uORB::DeviceMaster : public device::VDev +{ +public: + DeviceMaster(Flavor f); + ~DeviceMaster(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); +private: + Flavor _flavor; +}; + +#endif /* _uORBDeviceNode_posix.hpp */ diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp new file mode 100644 index 0000000000..f144ac8e27 --- /dev/null +++ b/src/modules/uORB/uORBMain.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include "uORBDevices.hpp" +#include "uORB.h" +#include "uORBTest_UnitTest.hpp" +#include "uORBCommon.hpp" + +extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } + +static uORB::DeviceMaster *g_dev = nullptr; +static void usage() +{ + warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); +} + + +int +uorb_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -EINVAL; + } + + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + warnx("already loaded"); + /* user wanted to start uorb, its already running, no error */ + return 0; + } + + /* create the driver */ + g_dev = new uORB::DeviceMaster(uORB::PUBSUB); + + if (g_dev == nullptr) { + warnx("driver alloc failed"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + warnx("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + return OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + { + uORBTest::UnitTest t; + return t.test(); + } + + /* + * Test the latency. + */ + if (!strcmp(argv[1], "latency_test")) { + + uORBTest::UnitTest t; + if (argc > 2 && !strcmp(argv[2], "medium")) { + return t.latency_test(ORB_ID(orb_test_medium), true); + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return t.latency_test(ORB_ID(orb_test_large), true); + } else { + return t.latency_test(ORB_ID(orb_test), true); + } + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) + { + return OK; + } + + usage(); + errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); +} + + diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp new file mode 100644 index 0000000000..e1a94a5e0f --- /dev/null +++ b/src/modules/uORB/uORBManager.hpp @@ -0,0 +1,325 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#ifndef _uORBManager_hpp_ +#define _uORBManager_hpp_ + +#include "uORBCommon.hpp" +#include + +namespace uORB +{ + class Manager; +} + +/** + * This is implemented as a singleton. This class manages creating the + * uORB nodes for each uORB topics and also implements the behavor of the + * uORB Api's. + */ +class uORB::Manager +{ + public: + // public interfaces for this class. + + /** + * Method to get the singleton instance for the uORB::Manager. + * @return uORB::Manager* + */ + static uORB::Manager* get_instance(); + + // ==== uORB interface methods ==== + /** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data); + + /** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ + orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + int priority) ; + + + /** + * Publish new data to a topic. + * + * The data is atomically published to the topic and any waiting subscribers + * will be notified. Subscribers that are not waiting can check the topic + * for updates using orb_check and/or orb_stat. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @handle The handle returned from orb_advertise. + * @param data A pointer to the data to be published. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) ; + + /** + * Subscribe to a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ + int orb_subscribe(const struct orb_metadata *meta) ; + + /** + * Subscribe to a multi-instance of a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param instance The instance of the topic. Instance 0 matches the + * topic of the orb_subscribe() call, higher indices + * are for topics created with orb_publish_multi(). + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ + int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) ; + + /** + * Unsubscribe from a topic. + * + * @param handle A handle returned from orb_subscribe. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_unsubscribe(int handle) ; + + /** + * Fetch data from a topic. + * + * This is the only operation that will reset the internal marker that + * indicates that a topic has been updated for a subscriber. Once poll + * or check return indicating that an updaet is available, this call + * must be used to update the subscription. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param handle A handle returned from orb_subscribe. + * @param buffer Pointer to the buffer receiving the data, or NULL + * if the caller wants to clear the updated flag without + * using the data. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) ; + + /** + * Check whether a topic has been published to since the last orb_copy. + * + * This check can be used to determine whether to copy the topic when + * not using poll(), or to avoid the overhead of calling poll() when the + * topic is likely to have updated. + * + * Updates are tracked on a per-handle basis; this call will continue to + * return true until orb_copy is called using the same handle. This interface + * should be preferred over calling orb_stat due to the race window between + * stat and copy that can lead to missed updates. + * + * @param handle A handle returned from orb_subscribe. + * @param updated Set to true if the topic has been updated since the + * last time it was copied using this handle. + * @return OK if the check was successful, ERROR otherwise with + * errno set accordingly. + */ + int orb_check(int handle, bool *updated) ; + + /** + * Return the last time that the topic was updated. + * + * @param handle A handle returned from orb_subscribe. + * @param time Returns the absolute time that the topic was updated, or zero if it has + * never been updated. Time is measured in microseconds. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_stat(int handle, uint64_t *time) ; + + /** + * Check if a topic has already been created. + * + * @param meta ORB topic metadata. + * @param instance ORB instance + * @return OK if the topic exists, ERROR otherwise with errno set accordingly. + */ + int orb_exists(const struct orb_metadata *meta, int instance) ; + + /** + * Return the priority of the topic + * + * @param handle A handle returned from orb_subscribe. + * @param priority Returns the priority of this topic. This is only relevant for + * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) + * and allows a subscriber to automatically pick the topic with the highest + * priority, independent of the startup order of the associated publishers. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ + int orb_priority(int handle, int *priority) ; + + /** + * Set the minimum interval between which updates are seen for a subscription. + * + * If this interval is set, the subscriber will not see more than one update + * within the period. + * + * Specifically, the first time an update is reported to the subscriber a timer + * is started. The update will continue to be reported via poll and orb_check, but + * once fetched via orb_copy another update will not be reported until the timer + * expires. + * + * This feature can be used to pace a subscriber that is watching a topic that + * would otherwise update too quickly. + * + * @param handle A handle returned from orb_subscribe. + * @param interval An interval period in milliseconds. + * @return OK on success, ERROR otherwise with ERRNO set accordingly. + */ + int orb_set_interval(int handle, unsigned interval) ; + + private: // class methods + /** + * Advertise a node; don't consider it an error if the node has + * already been advertised. + * + * @todo verify that the existing node is the same as the one + * we tried to advertise. + */ + int + node_advertise + ( + const struct orb_metadata *meta, + int *instance = nullptr, + int priority = ORB_PRIO_DEFAULT + ); + + /** + * Common implementation for orb_advertise and orb_subscribe. + * + * Handles creation of the object and the initial publication for + * advertisers. + */ + int + node_open + ( + Flavor f, + const struct orb_metadata *meta, + const void *data, + bool advertiser, + int *instance = nullptr, + int priority = ORB_PRIO_DEFAULT + ); + + private: // data members + static Manager _Instance; + + private: //class methods + Manager(); + +}; + +#endif /* _uORBManager_hpp_ */ diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp new file mode 100644 index 0000000000..85746c0c27 --- /dev/null +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBDevices.hpp" + + +//========================= Static initializations ================= +uORB::Manager uORB::Manager::_Instance; + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +uORB::Manager* uORB::Manager::get_instance() +{ + return &_Instance; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +uORB::Manager::Manager() +{ +} + +int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) +{ + /* + * Generate the path to the node and try to open it. + */ + char path[orb_maxpath]; + int inst = instance; + int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst); + + if (ret != OK) { + errno = -ret; + return uORB::ERROR; + } + + struct stat buffer; + return stat(path, &buffer); +} + +orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data) +{ + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); +} + +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +{ + int result, fd; + orb_advert_t advertiser; + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true, instance, priority); + if (fd == ERROR) + return ERROR; + + /* get the advertiser handle and close the node */ + result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + close(fd); + if (result == ERROR) + return ERROR; + + /* the advertiser must perform an initial publish to initialise the object */ + result = orb_publish(meta, advertiser, data); + if (result == ERROR) + return ERROR; + + return advertiser; +} + +int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(PUBSUB, meta, nullptr, false); +} + +int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); +} + +int uORB::Manager::orb_unsubscribe(int handle) +{ + return close(handle); +} + +int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ + return uORB::DeviceNode::publish(meta, handle, data); +} + +int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = read(handle, buffer, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + +int uORB::Manager::orb_check(int handle, bool *updated) +{ + return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int uORB::Manager::orb_stat(int handle, uint64_t *time) +{ + return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); +} + +int uORB::Manager::orb_priority(int handle, int *priority) +{ + return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); +} + +int uORB::Manager::orb_set_interval(int handle, unsigned interval) +{ + return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + + +int uORB::Manager::node_advertise +( + const struct orb_metadata *meta, + int *instance, + int priority +) +{ + int fd = -1; + int ret = ERROR; + + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; + + /* open the control device */ + fd = open(TOPIC_MASTER_DEVICE_PATH, 0); + + if (fd < 0) + goto out; + + /* advertise the object */ + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); + + /* it's OK if it already exists */ + if ((OK != ret) && (EEXIST == errno)) { + ret = OK; + } + +out: + + if (fd >= 0) + close(fd); + + return ret; +} + +int uORB::Manager::node_open +( + Flavor f, + const struct orb_metadata *meta, + const void *data, + bool advertiser, + int *instance, + int priority +) +{ + char path[orb_maxpath]; + int fd, ret; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return ERROR; + } + + /* + * Advertiser must publish an initial value. + */ + if (advertiser && (data == nullptr)) { + errno = EINVAL; + return ERROR; + } + + /* + * Generate the path to the node and try to open it. + */ + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + close(fd); + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + + /* try to create the node */ + ret = node_advertise(meta, instance, priority); + + if (ret == OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + } + + /* on success, try the open again */ + if (ret == OK) { + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } + } + + if (fd < 0) { + errno = EIO; + return ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp new file mode 100644 index 0000000000..a9dad24dcd --- /dev/null +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBDevices.hpp" +#include "px4_config.h" + + +//========================= Static initializations ================= +uORB::Manager uORB::Manager::_Instance; + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +uORB::Manager* uORB::Manager::get_instance() +{ + return &_Instance; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +uORB::Manager::Manager() +{ +} + +int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) +{ + /* + * Generate the path to the node and try to open it. + */ + char path[orb_maxpath]; + int inst = instance; + int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + struct stat buffer; + return stat(path, &buffer); +} + +orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data) +{ + //warnx("orb_advertise meta = %p", meta); + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); +} + +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +{ + int result, fd; + orb_advert_t advertiser; + + //warnx("orb_advertise_multi meta = %p\n", meta); + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true, instance, priority); + if (fd == ERROR) { + warnx("node_open as advertiser failed."); + return ERROR; + } + + /* get the advertiser handle and close the node */ + result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + px4_close(fd); + if (result == ERROR) { + warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); + return ERROR; + } + + /* the advertiser must perform an initial publish to initialise the object */ + result = orb_publish(meta, advertiser, data); + if (result == ERROR) { + warnx("orb_publish failed"); + return ERROR; + } + + return advertiser; +} + +int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(PUBSUB, meta, nullptr, false); +} + +int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); +} + +int uORB::Manager::orb_unsubscribe(int fd) +{ + return px4_close(fd); +} + +int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ + return uORB::DeviceNode::publish(meta, handle, data); +} + +int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = px4_read(handle, buffer, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return PX4_OK; +} + +int uORB::Manager::orb_check(int handle, bool *updated) +{ + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int uORB::Manager::orb_stat(int handle, uint64_t *time) +{ + return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); +} + +int uORB::Manager::orb_priority(int handle, int *priority) +{ + return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); +} + +int uORB::Manager::orb_set_interval(int handle, unsigned interval) +{ + return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + + +int uORB::Manager::node_advertise +( + const struct orb_metadata *meta, + int *instance, + int priority +) +{ + int fd = -1; + int ret = ERROR; + + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; + + /* open the control device */ + fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0); + + if (fd < 0) + goto out; + + /* advertise the object */ + ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); + + /* it's PX4_OK if it already exists */ + if ((PX4_OK != ret) && (EEXIST == errno)) { + ret = PX4_OK; + } + +out: + + if (fd >= 0) + px4_close(fd); + + return ret; +} + +int uORB::Manager::node_open +( + Flavor f, + const struct orb_metadata *meta, + const void *data, + bool advertiser, + int *instance, + int priority +) +{ + char path[orb_maxpath]; + int fd, ret; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return ERROR; + } + + /* + * Advertiser must publish an initial value. + */ + if (advertiser && (data == nullptr)) { + errno = EINVAL; + return ERROR; + } + + /* + * Generate the path to the node and try to open it. + */ + + // FIXME - if *instance is uninitialized, why is this being called? Seems risky and + // its definiately a waste. This is the case in muli-topic test. + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != PX4_OK) { + errno = -ret; + return ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + px4_close(fd); + + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + + /* try to create the node */ + ret = node_advertise(meta, instance, priority); + + if (ret == PX4_OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = uORB::Utils::node_mkpath(path, f, meta, instance); + + if (ret != PX4_OK) { + errno = -ret; + return ERROR; + } + } + + /* on success, try the open again */ + if (ret == PX4_OK) { + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + } + } + + if (fd < 0) { + errno = EIO; + return ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp new file mode 100644 index 0000000000..6fd960352f --- /dev/null +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -0,0 +1,287 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include "uORBTest_UnitTest.hpp" +#include "uORBCommon.hpp" +#include +#include + +int uORBTest::UnitTest::pubsublatency_main(void) +{ + /* poll on test topic and output latency */ + float latency_integral = 0.0f; + + /* wakeup source(s) */ + px4_pollfd_struct_t fds[3]; + + int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0); + int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); + int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); + + struct orb_test_large t; + + /* clear all ready flags */ + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + + fds[0].fd = test_multi_sub; + fds[0].events = POLLIN; + fds[1].fd = test_multi_sub_medium; + fds[1].events = POLLIN; + fds[2].fd = test_multi_sub_large; + fds[2].events = POLLIN; + + const unsigned maxruns = 1000; + unsigned timingsgroup = 0; + + unsigned *timings = new unsigned[maxruns]; + + for (unsigned i = 0; i < maxruns; i++) { + /* wait for up to 500ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(orb_test), test_multi_sub, &t); + timingsgroup = 0; + } else if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t); + timingsgroup = 1; + } else if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t); + timingsgroup = 2; + } + + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + hrt_abstime elt = hrt_elapsed_time(&t.time); + latency_integral += elt; + timings[i] = elt; + } + + orb_unsubscribe(test_multi_sub); + orb_unsubscribe(test_multi_sub_medium); + orb_unsubscribe(test_multi_sub_large); + + if (pubsubtest_print) { + char fname[32]; + //sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); + sprintf(fname, "/tmp/timings%u.txt", timingsgroup); + FILE *f = fopen(fname, "w"); + if (f == NULL) { + warnx("Error opening file!\n"); + return uORB::ERROR; + } + + for (unsigned i = 0; i < maxruns; i++) { + fprintf(f, "%u\n", timings[i]); + } + + fclose(f); + } + + delete[] timings; + + warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); + + pubsubtest_passed = true; + + if (static_cast(latency_integral / maxruns) > 30.0f) { + pubsubtest_res = uORB::ERROR; + } else { + pubsubtest_res = PX4_OK; + } + + return pubsubtest_res; +} + +int uORBTest::UnitTest::test() +{ + struct orb_test t, u; + int pfd, sfd; + bool updated; + + t.val = 0; + pfd = orb_advertise(ORB_ID(orb_test), &t); + + if (pfd < 0) + return test_fail("advertise failed: %d", errno); + + test_note("publish handle 0x%08x", pfd); + sfd = orb_subscribe(ORB_ID(orb_test)); + + if (sfd < 0) + return test_fail("subscribe failed: %d", errno); + + test_note("subscribe fd %d", sfd); + u.val = 1; + + if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(1) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + + if (PX4_OK != orb_check(sfd, &updated)) + return test_fail("check(1) failed"); + + if (updated) + return test_fail("spurious updated flag"); + + t.val = 2; + test_note("try publish"); + + if (PX4_OK != orb_publish(ORB_ID(orb_test), pfd, &t)) + return test_fail("publish failed"); + + if (PX4_OK != orb_check(sfd, &updated)) + return test_fail("check(2) failed"); + + if (!updated) + return test_fail("missing updated flag"); + + if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(2) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); + + orb_unsubscribe(sfd); + close(pfd); + + /* this routine tests the multi-topic support */ + test_note("try multi-topic support"); + + int instance0; + int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); + + test_note("advertised"); + + int instance1; + int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); + + if (instance0 != 0) + return test_fail("mult. id0: %d", instance0); + + if (instance1 != 1) + return test_fail("mult. id1: %d", instance1); + + t.val = 103; + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 fail"); + + test_note("published"); + + t.val = 203; + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) + return test_fail("mult. pub1 fail"); + + /* subscribe to both topics and ensure valid data is received */ + int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) + return test_fail("sub #0 copy failed: %d", errno); + + if (u.val != 103) + return test_fail("sub #0 val. mismatch: %d", u.val); + + int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + + if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) + return test_fail("sub #1 copy failed: %d", errno); + + if (u.val != 203) + return test_fail("sub #1 val. mismatch: %d", u.val); + + /* test priorities */ + int prio; + if (PX4_OK != orb_priority(sfd0, &prio)) + return test_fail("prio #0"); + + if (prio != ORB_PRIO_MAX) + return test_fail("prio: %d", prio); + + if (PX4_OK != orb_priority(sfd1, &prio)) + return test_fail("prio #1"); + + if (prio != ORB_PRIO_MIN) + return test_fail("prio: %d", prio); + + if (PX4_OK != latency_test(ORB_ID(orb_test), false)) + return test_fail("latency test failed"); + + return test_note("PASS"); +} + +int uORBTest::UnitTest::info() +{ + return OK; +} + +int uORBTest::UnitTest::test_fail(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "FAIL: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return uORB::ERROR; +} + +int uORBTest::UnitTest::test_note(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "note: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return OK; +} + +int uORBTest::UnitTest::pubsubtest_threadEntry( char* const* data ) +{ + uORBTest::UnitTest* t = (uORBTest::UnitTest*) data; + if( data != nullptr ) + { + return t->pubsublatency_main(); + } + return uORB::ERROR; +} diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORBTest_UnitTest.hpp new file mode 100644 index 0000000000..bbfa895fa6 --- /dev/null +++ b/src/modules/uORB/uORBTest_UnitTest.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#ifndef _uORBTest_UnitTest_hpp_ +#define _uORBTest_UnitTest_hpp_ +#include "uORBCommon.hpp" +#include "uORB.h" + +struct orb_test { + int val; + hrt_abstime time; +}; +ORB_DEFINE(orb_test, struct orb_test); +ORB_DEFINE(orb_multitest, struct orb_test); + +struct orb_test_medium { + int val; + hrt_abstime time; + char junk[64]; +}; +ORB_DEFINE(orb_test_medium, struct orb_test_medium); + +struct orb_test_large { + int val; + hrt_abstime time; + char junk[512]; +}; +ORB_DEFINE(orb_test_large, struct orb_test_large); + + +namespace uORBTest +{ + class UnitTest; +} + +class uORBTest::UnitTest +{ +public: + + int test(); + template int latency_test(orb_id_t T, bool print); + int info(); + +private: + static int pubsubtest_threadEntry( char* const* argv ); + int pubsublatency_main(void); + bool pubsubtest_passed = false; + bool pubsubtest_print = false; + int pubsubtest_res = OK; + + int test_fail(const char *fmt, ...); + int test_note(const char *fmt, ...); +}; + +template +int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) +{ + test_note("---------------- LATENCY TEST ------------------"); + S t; + t.val = 308; + t.time = hrt_absolute_time(); + + int pfd0 = orb_advertise(T, &t); + + pubsubtest_print = print; + + pubsubtest_passed = false; + + /* test pub / sub latency */ + + int pubsub_task = px4_task_spawn_cmd("uorb_latency", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&uORBTest::UnitTest::pubsubtest_threadEntry, + (char* const*) this); + + /* give the test task some data */ + while (!pubsubtest_passed) { + t.val = 308; + t.time = hrt_absolute_time(); + if (PX4_OK != orb_publish(T, pfd0, &t)) + return test_fail("mult. pub0 timing fail"); + + /* simulate >800 Hz system operation */ + usleep(1000); + } + + close(pfd0); + + if (pubsub_task < 0) { + return test_fail("failed launching task"); + } + + return pubsubtest_res; +} + +#endif // _uORBTest_UnitTest_hpp_ diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp new file mode 100644 index 0000000000..c9d49222bc --- /dev/null +++ b/src/modules/uORB/uORBUtils.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include "uORBUtils.hpp" +#include +#include + +int uORB::Utils::node_mkpath +( + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance +) +{ + unsigned len; + + unsigned index = 0; + + if (instance != nullptr) { + index = *instance; + } + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); + + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } + + return OK; +} diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp new file mode 100644 index 0000000000..dc2fb9b78e --- /dev/null +++ b/src/modules/uORB/uORBUtils.hpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ +#ifndef _uORBUtils_hpp_ +#define _uORBUtils_hpp_ + +#include "uORBCommon.hpp" + +namespace uORB +{ + class Utils; +} + +class uORB::Utils +{ +public: + static int node_mkpath + ( + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance = nullptr + ); +}; + +#endif // _uORBUtils_hpp_ From f2af8b08edb1b389f0f6fcc0b3a329ec72a88db2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 12:09:19 -0700 Subject: [PATCH 193/258] uORB: fix segfault in unit test The unit test was not passing a null pointer terminated argv. The posix port depends on argv being null terminated to determine how may args were passed since PX4 API doesn't pass argc when spawning a new task. Signed-off-by: Mark Charlebois --- src/modules/uORB/uORBTest_UnitTest.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORBTest_UnitTest.hpp index bbfa895fa6..0e68aa6c8c 100644 --- a/src/modules/uORB/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORBTest_UnitTest.hpp @@ -35,6 +35,7 @@ #define _uORBTest_UnitTest_hpp_ #include "uORBCommon.hpp" #include "uORB.h" +#include struct orb_test { int val; @@ -92,6 +93,8 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) int pfd0 = orb_advertise(T, &t); + char * const args[2] = { (char* const) this, 0 }; + pubsubtest_print = print; pubsubtest_passed = false; @@ -103,7 +106,7 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) SCHED_PRIORITY_MAX - 5, 1500, (px4_main_t)&uORBTest::UnitTest::pubsubtest_threadEntry, - (char* const*) this); + args); /* give the test task some data */ while (!pubsubtest_passed) { From 0cea93a55cc2b2762475c5834b385c12c67ad902 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 12:13:39 -0700 Subject: [PATCH 194/258] POSIX and QuRT: fixed calls needing px4_ prefix There were some missed calls to open and ioctl that need to be px4_open and px4_ioctl. QuRT also does not provide usleep() so px4_time.h has to be included in files calling usleep. Signed-off-by: Mark Charlebois --- makefiles/toolchain_hexagon.mk | 1 + .../commander/accelerometer_calibration.cpp | 1 + src/modules/commander/calibration_routines.cpp | 4 ++-- src/modules/commander/esc_calibration.cpp | 15 ++++++++------- src/modules/commander/gyro_calibration.cpp | 4 ++-- src/modules/simulator/simulator.cpp | 1 + src/platforms/px4_debug.h | 4 ++++ 7 files changed, 19 insertions(+), 11 deletions(-) diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 57c5d1a3c8..e250f12b37 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -90,6 +90,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -I$(PX4_BASE)/src/platforms/qurt/include \ -I$(PX4_BASE)/../dspal/include \ -I$(PX4_BASE)/../dspal/sys \ + -I$(PX4_BASE)/mavlink/include/mavlink \ -Wno-error=shadow # optimisation flags diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f7aace7753..44b5a43e87 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -128,6 +128,7 @@ #include "commander_helper.h" #include +#include #include #include #include diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index cecf22a0c3..68e094f7c4 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -528,11 +528,11 @@ static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &c bool calibrate_cancel_check(int mavlink_fd, int cancel_sub) { - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = cancel_sub; fds[0].events = POLLIN; - if (poll(&fds[0], 1, 0) > 0) { + if (px4_poll(&fds[0], 1, 0) > 0) { struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 541aca0531..2e8f1e6f34 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -48,7 +48,8 @@ #include #include #include -#include +#include +#include #include "drivers/drv_pwm_output.h" #include #include @@ -78,7 +79,7 @@ int check_if_batt_disconnected(int mavlink_fd) { int do_esc_calibration(int mavlink_fd) { - int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0); + int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); int ret; if(fd < 0) { @@ -86,15 +87,15 @@ int do_esc_calibration(int mavlink_fd) { } /* tell IO/FMU that its ok to disable its safety with the switch */ - ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) err(1, "PWM_SERVO_SET_ARM_OK"); /* tell IO/FMU that the system is armed (it will output values if safety is off) */ - ret = ioctl(fd, PWM_SERVO_ARM, 0); + ret = px4_ioctl(fd, PWM_SERVO_ARM, 0); if (ret != OK) err(1, "PWM_SERVO_ARM"); /* tell IO to switch off safety without using the safety switch */ - ret = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); + ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); if(ret!=0) { err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF"); } @@ -143,10 +144,10 @@ int do_esc_calibration(int mavlink_fd) { } /* disarm */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); + ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0); if (ret != OK) err(1, "PWM_SERVO_DISARM"); mavlink_log_info(mavlink_fd,"ESC calibration finished"); return OK; - } \ No newline at end of file + } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d444553b31..71e44c3d1e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -184,8 +184,8 @@ int do_gyro_calibration(int mavlink_fd) sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); if (fd >= 0) { - worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); + worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); px4_close(fd); if (res != OK) { diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 2520f8a28a..e6cecbb08b 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_debug.h index 0331875096..6cac039b28 100644 --- a/src/platforms/px4_debug.h +++ b/src/platforms/px4_debug.h @@ -40,7 +40,11 @@ #if defined(__PX4_LINUX) || defined(__PX4_QURT) +#if defined(__PX4_LINUX) #include +#else +#include +#endif #define PX4_DBG(...) #define PX4_INFO(...) warnx(__VA_ARGS__) From b7918ee45ab49802e0456ce0587e3c6afb6be699 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 12:16:23 -0700 Subject: [PATCH 195/258] Nuttx: remove -Wframe-larger-than=1024 The build fails when modules override this flag with a larger value, and this lower value is still checked. The new flag seems to be in addition to the old flag, not a replacement. Signed-off-by: Mark Charlebois --- makefiles/toolchain_gnu-arm-eabi.mk | 1 - 1 file changed, 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7cf72bdad8..6c4e64f54c 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -138,7 +138,6 @@ ARCHWARNINGS = -Wall \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ - -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ -Wmissing-declarations \ From 6d2efd0e8f525b4886d28ff8011e0eabafd7ecfc Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 13:07:54 -0700 Subject: [PATCH 196/258] uORB: Unit test called close vs px4_close The unit test should have called px4_close(), not close(). Signed-off-by: Mark Charlebois --- src/modules/uORB/uORBTest_UnitTest.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORBTest_UnitTest.hpp index 0e68aa6c8c..366765fa18 100644 --- a/src/modules/uORB/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORBTest_UnitTest.hpp @@ -119,7 +119,7 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) usleep(1000); } - close(pfd0); + px4_close(pfd0); if (pubsub_task < 0) { return test_fail("failed launching task"); From e28049a387f5972159490a758448964d07598bc1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 5 May 2015 16:43:54 -0700 Subject: [PATCH 197/258] POSIX: changed SIGCONT to SIGALRM QuRT doesn't seemto support SIGCONT Signed-off-by: Mark Charlebois --- src/platforms/qurt/px4_layer/work_queue.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c index cd96aacd2f..644b25e54b 100644 --- a/src/platforms/qurt/px4_layer/work_queue.c +++ b/src/platforms/qurt/px4_layer/work_queue.c @@ -121,7 +121,7 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_ work->qtime = clock_systimer(); /* Time work queued */ dq_addlast((dq_entry_t *)work, &wqueue->q); - px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */ + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ //irqrestore(flags); return PX4_OK; From 5299f767069be3bd8014a86a11a1748371e952a3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 09:51:31 -0700 Subject: [PATCH 198/258] POSIX: initialize before running script The initialization functions were called after the script commands were run causing a deadlock waiting for an uninitialized semaphore. Signed-off-by: Mark Charlebois --- src/platforms/posix/main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index b497133b36..f756749d52 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -87,6 +87,10 @@ static void process_line(string &line) int main(int argc, char **argv) { + px4::init_once(); + + px4::init(argc, argv, "mainapp"); + // Execute a command list of provided if (argc == 2) { ifstream infile(argv[1]); @@ -98,10 +102,6 @@ int main(int argc, char **argv) string mystr; - px4::init_once(); - - px4::init(argc, argv, "mainapp"); - while(1) { cout << "Enter a command and its args:" << endl; getline (cin,mystr); From 872e1ebda05cb1699d2ee52a384ff7bc45516e76 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 11:45:23 -0700 Subject: [PATCH 199/258] GCC fixes for warnings GCC was more picky about prototypes for inlines being required. The generate_listener.py script used incorrect printf formats and was casting %f params to float, but printf casts all %f params to double per the spec. Signed-off-by: Mark Charlebois --- Tools/generate_listener.py | 8 ++++---- makefiles/toolchain_native.mk | 3 ++- src/modules/simulator/simulator.cpp | 15 ++++++++------- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 3 ++- src/platforms/posix/px4_layer/work_lock.h | 2 ++ 5 files changed, 18 insertions(+), 13 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 50fc17267c..e52d3cd26f 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -127,17 +127,17 @@ for index,m in enumerate(messages[1:]): print "\t\t\torb_copy(ID,sub,&container);" for item in message_elements[index+1]: if item[0] == "float": - print "\t\t\tprintf(\"%s: %%f\\n \",container.%s);" % (item[1], item[1]) + print "\t\t\tprintf(\"%s: %%f\\n \",(double)container.%s);" % (item[1], item[1]) elif item[0] == "float_array": print "\t\t\tprintf(\"%s:\");" % item[1] print "\t\t\tfor (int j=0;j<%d;j++) {" % item[2] - print "\t\t\t\tprintf(\"%%f \",container.%s[j]);" % item[1] + print "\t\t\t\tprintf(\"%%f \",(double)container.%s[j]);" % item[1] print "\t\t\t}" print "\t\t\tprintf(\"\\n\");" elif item[0] == "uint64": - print "\t\t\tprintf(\"%s: %%f\\n \",(float)container.%s);" % (item[1], item[1]) + print "\t\t\tprintf(\"%s: %%lu\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "uint8": - print "\t\t\tprintf(\"%s: %%f\\n \",(float)container.%s);" % (item[1], item[1]) + print "\t\t\tprintf(\"%s: %%u\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "bool": print "\t\t\tprintf(\"%s: %%s\\n \",container.%s ? \"True\" : \"False\");" % (item[1], item[1]) print "\t\t\t}" diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index dd80e9a6ea..2455cda4c4 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -157,7 +157,6 @@ ARCHWARNINGS = -Wall \ -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter \ - -Wno-gnu-array-member-paren-init \ -Wno-packed \ -Werror=format-security \ -Werror=array-bounds \ @@ -176,6 +175,8 @@ ARCHWARNINGS += -Wdouble-promotion \ -Werror=unused-but-set-variable \ -Werror=double-promotion ARCHOPTIMIZATION += -fno-strength-reduce +else +ARCHWARNINGS += -Wno-gnu-array-member-paren-init endif # -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+ diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index e6cecbb08b..9f7b0c9597 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -363,6 +363,14 @@ static void usage() PX4_WARN("Publish sensors combined: simulator start -p"); } +__BEGIN_DECLS +extern int simulator_main(int argc, char *argv[]); +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + extern "C" { int simulator_main(int argc, char *argv[]) @@ -418,13 +426,6 @@ int simulator_main(int argc, char *argv[]) } -__BEGIN_DECLS -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -extern void led_toggle(int led); -__END_DECLS - bool static _led_state[2] = { false , false }; __EXPORT void led_init() diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index def293ea4c..232da9cb2d 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -788,7 +788,8 @@ tone_alarm_main(int argc, char *argv[]) PX4_WARN("not enough memory memory"); return 1; } - fread(buffer, sz, 1, fd); + // FIXME - Make GCC happy + if (fread(buffer, sz, 1, fd)) { } /* terminate the string */ buffer[sz] = 0; ret = play_string(buffer, true); diff --git a/src/platforms/posix/px4_layer/work_lock.h b/src/platforms/posix/px4_layer/work_lock.h index 89e8b65eca..f77f228b36 100644 --- a/src/platforms/posix/px4_layer/work_lock.h +++ b/src/platforms/posix/px4_layer/work_lock.h @@ -37,12 +37,14 @@ #pragma once extern sem_t _work_lock[]; +inline void work_lock(int id); inline void work_lock(int id) { //printf("work_lock %d\n", id); sem_wait(&_work_lock[id]); } +inline void work_unlock(int id); inline void work_unlock(int id) { //printf("work_unlock %d\n", id); From 7ebee7ca6fcb85576b5464cb37334a54aad5f013 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 13:19:52 -0700 Subject: [PATCH 200/258] uORB: Unit test fix The latency_test used to pass an object pointer as argv which won't work in the posix port because it expects argv to be a null terminated array of character pointers (which it makes a copy of). The test was refactored to use a singleton pattern and avoid having to pass the object pointer to the thread. Signed-off-by: Mark Charlebois --- src/modules/uORB/uORBMain.cpp | 8 +++----- src/modules/uORB/uORBTest_UnitTest.cpp | 16 +++++++++------- src/modules/uORB/uORBTest_UnitTest.hpp | 19 ++++++++++++++----- 3 files changed, 26 insertions(+), 17 deletions(-) diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index f144ac8e27..147ce5fbb3 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -90,7 +90,7 @@ uorb_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "test")) { - uORBTest::UnitTest t; + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); return t.test(); } @@ -99,7 +99,7 @@ uorb_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "latency_test")) { - uORBTest::UnitTest t; + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); if (argc > 2 && !strcmp(argv[2], "medium")) { return t.latency_test(ORB_ID(orb_test_medium), true); } else if (argc > 2 && !strcmp(argv[2], "large")) { @@ -118,7 +118,5 @@ uorb_main(int argc, char *argv[]) } usage(); - errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); + return -EINVAL; } - - diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp index 6fd960352f..c1ea73959f 100644 --- a/src/modules/uORB/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -36,6 +36,12 @@ #include #include +uORBTest::UnitTest &uORBTest::UnitTest::instance() +{ + static uORBTest::UnitTest t; + return t; +} + int uORBTest::UnitTest::pubsublatency_main(void) { /* poll on test topic and output latency */ @@ -276,12 +282,8 @@ int uORBTest::UnitTest::test_note(const char *fmt, ...) return OK; } -int uORBTest::UnitTest::pubsubtest_threadEntry( char* const* data ) +int uORBTest::UnitTest::pubsubtest_threadEntry(char* const argv[]) { - uORBTest::UnitTest* t = (uORBTest::UnitTest*) data; - if( data != nullptr ) - { - return t->pubsublatency_main(); - } - return uORB::ERROR; + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + return t.pubsublatency_main(); } diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORBTest_UnitTest.hpp index 366765fa18..f19c95451d 100644 --- a/src/modules/uORB/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORBTest_UnitTest.hpp @@ -68,15 +68,22 @@ class uORBTest::UnitTest { public: + // Singleton pattern + static uORBTest::UnitTest &instance(); + ~UnitTest() {} int test(); template int latency_test(orb_id_t T, bool print); int info(); private: - static int pubsubtest_threadEntry( char* const* argv ); + UnitTest() : pubsubtest_passed(false), pubsubtest_print(false) {} + + // Disallow copy + UnitTest(const uORBTest::UnitTest &) {}; + static int pubsubtest_threadEntry(char* const argv[]); int pubsublatency_main(void); - bool pubsubtest_passed = false; - bool pubsubtest_print = false; + bool pubsubtest_passed; + bool pubsubtest_print; int pubsubtest_res = OK; int test_fail(const char *fmt, ...); @@ -93,14 +100,16 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) int pfd0 = orb_advertise(T, &t); - char * const args[2] = { (char* const) this, 0 }; + char * const args[1] = { NULL }; pubsubtest_print = print; - pubsubtest_passed = false; /* test pub / sub latency */ + // Can't pass a pointer in args, must be a null terminated + // array of strings because the strings are copied to + // prevent access if the caller data goes out of scope int pubsub_task = px4_task_spawn_cmd("uorb_latency", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, From eed2479dfcde620a93fa87f471d4eaf046593840 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 14:20:08 -0700 Subject: [PATCH 201/258] Added px4_ prefix to task_spawn_cmd Signed-off-by: Mark Charlebois --- src/modules/dataman/dataman.c | 2 +- src/modules/sdlog2/sdlog2.c | 2 +- src/systemcmds/tests/test_dataman.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 08f3240b86..e454568825 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { + if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0cf5e885cb..719db93a7a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -320,7 +320,7 @@ int sdlog2_main(int argc, char *argv[]) } main_thread_should_exit = false; - deamon_task = task_spawn_cmd("sdlog2", + deamon_task = px4_task_spawn_cmd("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, 3000, diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 02fe5d963f..da73f68964 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -168,7 +168,7 @@ int test_dataman(int argc, char *argv[]) sem_init(sems + i, 1, 0); /* start the task */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { + if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { warn("task start failed"); } } From 3654aec3a502906829158d1fe01270f2e915a49a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 14:34:23 -0700 Subject: [PATCH 202/258] POSIX: ported px4_daemon_app Signed-off-by: Mark Charlebois --- src/examples/px4_daemon_app/px4_daemon_app.c | 15 +++--- src/modules/systemlib/systemlib.c | 57 -------------------- 2 files changed, 8 insertions(+), 64 deletions(-) delete mode 100644 src/modules/systemlib/systemlib.c diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index c1d3f8f0bf..7440a07eb4 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -75,7 +75,7 @@ usage(const char *reason) warnx("%s\n", reason); } - errx(1, "usage: daemon {start|stop|status} [-p ]\n\n"); + warnx("usage: daemon {start|stop|status} [-p ]\n\n"); } /** @@ -90,6 +90,7 @@ int px4_daemon_app_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -97,22 +98,22 @@ int px4_daemon_app_main(int argc, char *argv[]) if (thread_running) { warnx("daemon already running\n"); /* this is not an error */ - exit(0); + return 0; } thread_should_exit = false; - daemon_task = task_spawn_cmd("daemon", + daemon_task = px4_task_spawn_cmd("daemon", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, px4_daemon_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { @@ -123,11 +124,11 @@ int px4_daemon_app_main(int argc, char *argv[]) warnx("\tnot started\n"); } - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } int px4_daemon_thread_main(int argc, char *argv[]) diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c deleted file mode 100644 index b86e107737..0000000000 --- a/src/modules/systemlib/systemlib.c +++ /dev/null @@ -1,57 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file systemlib.c - * Implementation of commonly used low-level system-call like functions. - */ - -#include -#include "systemlib.h" - -void -systemreset(bool to_bootloader) -{ - px4_systemreset(to_bootloader); -} - -void killall() -{ - px4_killall(); -} - -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) -{ - return px4_task_spawn_cmd(name, scheduler, priority, stack_size, entry, argv); -} From c5237f7f6f3600e762a867669e9645cca09d42a7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 14:43:11 -0700 Subject: [PATCH 203/258] Removed extra abstracton layer in systemlib The calls to task_spawn_cmd, kill_all, and systemreset were wrappers around the px4_{task_spawn_cmd|kill_all|systemreset} implementations. Removed the wrappers and changed all calls to the px4_ equivalents. NuttX specific code was moved into px4_tasks.h Signed-off-by: Mark Charlebois --- .../ardrone_interface/ardrone_interface.c | 2 +- src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +- src/drivers/gps/gps.cpp | 2 +- .../hott/hott_sensors/hott_sensors.cpp | 2 +- .../hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/md25/md25_main.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 2 +- src/examples/fixedwing_control/main.c | 4 ++-- .../flow_position_estimator_main.c | 4 ++-- .../matlab_csv_serial/matlab_csv_serial.c | 4 ++-- .../publisher/publisher_start_nuttx.cpp | 2 +- src/examples/rover_steering_control/main.cpp | 4 ++-- .../subscriber/subscriber_start_nuttx.cpp | 2 +- .../attitude_estimator_ekf_main.cpp | 4 ++-- .../attitude_estimator_so3_main.cpp | 4 ++-- src/modules/bottle_drop/bottle_drop.cpp | 2 +- src/modules/commander/commander.cpp | 6 ++--- .../ekf_att_pos_estimator_main.cpp | 2 +- .../fixedwing_backside_main.cpp | 2 +- .../fw_att_control/fw_att_control_main.cpp | 2 +- .../fw_pos_control_l1_main.cpp | 2 +- .../land_detector/land_detector_main.cpp | 2 +- .../mc_att_control/mc_att_control_main.cpp | 2 +- .../mc_att_control_start_nuttx.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 2 +- .../mc_pos_control_start_nuttx.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- .../position_estimator_inav_main.c | 2 +- src/modules/segway/segway_main.cpp | 2 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/module.mk | 1 - src/modules/systemlib/systemlib.h | 23 ++----------------- src/modules/uavcan/uavcan_main.cpp | 2 +- .../vtol_att_control_main.cpp | 2 +- src/platforms/px4_tasks.h | 10 ++++++++ 38 files changed, 55 insertions(+), 65 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index cfc80ace22..1777e7f27c 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[]) } thread_should_exit = false; - ardrone_interface_task = task_spawn_cmd("ardrone_interface", + ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 15, 1100, diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 5035600ef2..75d1124a61 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -216,7 +216,7 @@ int frsky_telemetry_main(int argc, char *argv[]) errx(0, "frsky_telemetry already running"); thread_should_exit = false; - frsky_task = task_spawn_cmd("frsky_telemetry", + frsky_task = px4_task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index da92c851bc..47a6e4c965 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,7 +229,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_spawn_cmd("gps", SCHED_DEFAULT, + _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 7bc3ca8353..29680a279f 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -209,7 +209,7 @@ hott_sensors_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd(daemon_name, + deamon_task = px4_task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index df6fe950b9..a1a5b080c5 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -235,7 +235,7 @@ hott_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd(daemon_name, + deamon_task = px4_task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index afbd10c5cf..a20bf7c7c6 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -110,7 +110,7 @@ int md25_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("md25", + deamon_task = px4_task_spawn_cmd("md25", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5fc1dd6b21..d1484a2f01 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -302,7 +302,7 @@ MK::init(unsigned motors) } /* start the IO interface task */ - _task = task_spawn_cmd("mkblctrl", + _task = px4_task_spawn_cmd("mkblctrl", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 1500, diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f0412584fa..e876ac75ce 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -341,7 +341,7 @@ PX4FMU::init() gpio_reset(); /* start the IO interface task */ - _task = task_spawn_cmd("fmuservo", + _task = px4_task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1600, diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 92e6dde7ff..e0e866555b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -854,7 +854,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_spawn_cmd("px4io", + _task = px4_task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1800, diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index e8462ef5f8..aa5e6dcc0b 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -105,7 +105,7 @@ int roboclaw_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("roboclaw", + deamon_task = px4_task_spawn_cmd("roboclaw", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 168c2bfa92..97f9b412a7 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -414,7 +414,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int ex_fixedwing_control_main(int argc, char *argv[]) { @@ -431,7 +431,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("ex_fixedwing_control", + deamon_task = px4_px4_task_spawn_cmd("ex_fixedwing_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index db21a9cb31..a1a81d6361 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -95,7 +95,7 @@ static void usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int flow_position_estimator_main(int argc, char *argv[]) { @@ -111,7 +111,7 @@ int flow_position_estimator_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("flow_position_estimator", + daemon_task = px4_px4_task_spawn_cmd("flow_position_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 98d2849580..b840283b6d 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -87,7 +87,7 @@ static void usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int matlab_csv_serial_main(int argc, char *argv[]) { @@ -103,7 +103,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("matlab_csv_serial", + daemon_task = px4_px4_task_spawn_cmd("matlab_csv_serial", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index aaf28fdb7f..2379d7aa68 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -68,7 +68,7 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("publisher", + daemon_task = px4_task_spawn_cmd("publisher", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index a64c169863..e607acc2c2 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -408,7 +408,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int rover_steering_control_main(int argc, char *argv[]) { @@ -425,7 +425,7 @@ int rover_steering_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn_cmd("rover_steering_control", + deamon_task = px4_px4_task_spawn_cmd("rover_steering_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 91543d5d23..424e49e9c1 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -68,7 +68,7 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("subscriber", + daemon_task = px4_task_spawn_cmd("subscriber", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6c90f74a8f..4a086be5dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -113,7 +113,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int attitude_estimator_ekf_main(int argc, char *argv[]) { @@ -131,7 +131,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", + attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 7700, diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 11c302a8c8..76dc209c12 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -132,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to px4_px4_task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { @@ -149,7 +149,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", + attitude_estimator_so3_task = px4_px4_task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 00e618609c..987b930d28 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -225,7 +225,7 @@ BottleDrop::start() ASSERT(_main_task == -1); /* start the task */ - _main_task = task_spawn_cmd("bottle_drop", + _main_task = px4_task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, 1500, diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a349cfca95..dcf7cdddf0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -269,7 +269,7 @@ int commander_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", + daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 3400, @@ -2652,13 +2652,13 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(100000); /* reboot */ - systemreset(false); + px4_systemreset(false); } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(100000); /* reboot to bootloader */ - systemreset(true); + px4_systemreset(true); } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 15a663bc1e..1b6e17ac37 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1038,7 +1038,7 @@ int AttitudePositionEstimatorEKF::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", + _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 7500, diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 447e229b1b..06559de979 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -111,7 +111,7 @@ int fixedwing_backside_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_backside", + deamon_task = px4_task_spawn_cmd("fixedwing_backside", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 70ac4b74ea..fb6f74f357 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1117,7 +1117,7 @@ FixedwingAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("fw_att_control", + _control_task = px4_task_spawn_cmd("fw_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 20c2bd46d2..3525c5674b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1621,7 +1621,7 @@ FixedwingPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("fw_pos_control_l1", + _control_task = px4_task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1600, diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index b4b7c33a20..09b5fd799b 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -136,7 +136,7 @@ static int land_detector_start(const char *mode) } //Start new thread task - _landDetectorTaskID = task_spawn_cmd("land_detector", + _landDetectorTaskID = px4_task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1000, diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b432ecad6c..c40ebdb4ed 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -906,7 +906,7 @@ MulticopterAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("mc_att_control", + _control_task = px4_task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index dec1e1745d..93efd90e7f 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -68,7 +68,7 @@ int mc_att_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("mc_att_control_m", + daemon_task = px4_task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1900, diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6ffb37d970..fe6db135f8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1423,7 +1423,7 @@ MulticopterPositionControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("mc_pos_control", + _control_task = px4_task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp index 0db67d83ff..92416249d8 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -68,7 +68,7 @@ int mc_pos_control_m_main(int argc, char *argv[]) task_should_exit = false; - daemon_task = task_spawn_cmd("mc_pos_control_m", + daemon_task = px4_task_spawn_cmd("mc_pos_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bc6d3ea9f1..e9733b40ce 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -515,7 +515,7 @@ Navigator::start() ASSERT(_navigator_task == -1); /* start the task */ - _navigator_task = task_spawn_cmd("navigator", + _navigator_task = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, 1700, diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 84ae34df2b..5660bcc194 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -153,7 +153,7 @@ int position_estimator_inav_main(int argc, char *argv[]) } thread_should_exit = false; - position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", + position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, (argv) ? (char * const *) &argv[2] : (char * const *) NULL); diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index d633f2a420..67acd94bf7 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -106,7 +106,7 @@ int segway_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = task_spawn_cmd("segway", + deamon_task = px4_task_spawn_cmd("segway", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b2867eeffa..c1e5720aec 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2252,7 +2252,7 @@ Sensors::start() ASSERT(_sensors_task == -1); /* start the task */ - _sensors_task = task_spawn_cmd("sensors_task", + _sensors_task = px4_task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 4d805c57c0..e648c577d9 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -42,7 +42,6 @@ SRCS = err.c \ cpuload.c \ getopt_long.c \ pid/pid.c \ - systemlib.c \ airspeed.c \ system_params.c \ mavlink_log.c \ diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 2196133f8d..29b6eb5cef 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,31 +42,12 @@ #include #include #include + +// OS specific settings defined in px4_tasks.h #include __BEGIN_DECLS -/** Reboots the board */ -__EXPORT void systemreset(bool to_bootloader) noreturn_function; - -/** Sends SIGUSR1 to all processes */ -__EXPORT void killall(void); - -/** Default scheduler type */ -#if CONFIG_RR_INTERVAL > 0 -# define SCHED_DEFAULT SCHED_RR -#else -# define SCHED_DEFAULT SCHED_FIFO -#endif - -/** Starts a task and performs any specific accounting, scheduler setup, etc. */ -__EXPORT int task_spawn_cmd(const char *name, - int priority, - int scheduler, - int stack_size, - px4_main_t entry, - char * const argv[]); - enum MULT_PORTS { MULT_0_US2_RXTX = 0, MULT_1_US2_FLOW, diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index bc89b956b3..75e8eeff95 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -194,7 +194,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, + _instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 5fd7985d04..51304a046c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -861,7 +861,7 @@ VtolAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("vtol_att_control", + _control_task = px4_task_spawn_cmd("vtol_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 817fbbbb61..d03f17b85d 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -43,16 +43,26 @@ #include #ifdef __PX4_ROS + #error "PX4 tasks not supported in ROS" + #elif defined(__PX4_NUTTX) typedef int px4_task_t; +/** Default scheduler type */ +#if CONFIG_RR_INTERVAL > 0 +# define SCHED_DEFAULT SCHED_RR +#else +# define SCHED_DEFAULT SCHED_FIFO +#endif + #define px4_task_exit(x) _exit(x) #elif defined(__PX4_POSIX) || defined(__PX4_QURT) #include #include +/** Default scheduler type */ #define SCHED_DEFAULT SCHED_FIFO #ifdef __PX4_LINUX #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO) From 0c1c58c4184d5f95b0b385f1d449c7647cf58a3d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 15:32:04 -0700 Subject: [PATCH 204/258] Fixed overzealous px4_ prefixing Some files had px4_px4_ prefixed functions. Signed-off-by: Mark Charlebois --- src/examples/fixedwing_control/main.c | 4 ++-- .../flow_position_estimator/flow_position_estimator_main.c | 4 ++-- src/examples/matlab_csv_serial/matlab_csv_serial.c | 4 ++-- src/examples/rover_steering_control/main.cpp | 4 ++-- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 4 ++-- src/systemcmds/reboot/reboot.c | 2 +- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 97f9b412a7..d6e78fd821 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -414,7 +414,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_px4_task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int ex_fixedwing_control_main(int argc, char *argv[]) { @@ -431,7 +431,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = px4_px4_task_spawn_cmd("ex_fixedwing_control", + deamon_task = px4_task_spawn_cmd("ex_fixedwing_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index a1a81d6361..027511df20 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -95,7 +95,7 @@ static void usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_px4_task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int flow_position_estimator_main(int argc, char *argv[]) { @@ -111,7 +111,7 @@ int flow_position_estimator_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = px4_px4_task_spawn_cmd("flow_position_estimator", + daemon_task = px4_task_spawn_cmd("flow_position_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index b840283b6d..139a182b22 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -87,7 +87,7 @@ static void usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_px4_task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int matlab_csv_serial_main(int argc, char *argv[]) { @@ -103,7 +103,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = px4_px4_task_spawn_cmd("matlab_csv_serial", + daemon_task = px4_task_spawn_cmd("matlab_csv_serial", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index e607acc2c2..887ea86309 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -408,7 +408,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_px4_task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int rover_steering_control_main(int argc, char *argv[]) { @@ -425,7 +425,7 @@ int rover_steering_control_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = px4_px4_task_spawn_cmd("rover_steering_control", + deamon_task = px4_task_spawn_cmd("rover_steering_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, 2048, diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 76dc209c12..68a61488f9 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -132,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to px4_px4_task_spawn_cmd(). + * to px4_task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { @@ -149,7 +149,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) } thread_should_exit = false; - attitude_estimator_so3_task = px4_px4_task_spawn_cmd("attitude_estimator_so3", + attitude_estimator_so3_task = px4_task_spawn_cmd("attitude_estimator_so3", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 14000, diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index ee9f2e0d8d..de4d77d5fd 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -65,7 +65,7 @@ int reboot_main(int argc, char *argv[]) } } - systemreset(to_bootloader); + px4_systemreset(to_bootloader); } From 04b564920ff747d423f96fd006effda323ff18eb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 15:44:37 -0700 Subject: [PATCH 205/258] POSIX: Make binutils BDF linker the default ld.gold does not support the -Ur flags and it seems some people have ld as a link to ld.gold. Made LD = ld.bfd to avoid confusion. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 2455cda4c4..7c42ee4ba1 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -92,7 +92,7 @@ DEV_VER_SUPPORTED = 4.2.1 endif #LD = ld.gold -LD = ld +LD = ld.bfd AR = ar rcs NM = nm OBJCOPY = objcopy From 3a651873030243574f51235b83e05cb72774e9c6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 16:00:00 -0700 Subject: [PATCH 206/258] Fixed bad merge Forgot to remove old file line when merging the changes from master. Signed-off-by: Mark Charlebois --- src/modules/commander/accelerometer_calibration.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1b80d5c09d..5d7852816b 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -435,7 +435,6 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc board_rotation = board_rotation_offset * board_rotation; px4_pollfd_struct_t fds[max_accel_sens]; - struct pollfd fds[max_accel_sens]; for (unsigned i = 0; i < max_accel_sens; i++) { fds[i].fd = subs[i]; From 948b47bd3312d9400f58a1390a18502aa07d2974 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 16:04:20 -0700 Subject: [PATCH 207/258] Removed px4_killall killall and px4_killall are not used in the codebase so it was removed. Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/px4_posix_tasks.cpp | 11 ----------- src/platforms/px4_tasks.h | 3 --- src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 11 ----------- 3 files changed, 25 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 54fc555e0b..c831ae04dd 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -237,17 +237,6 @@ void px4_task_exit(int ret) pthread_exit((void *)(unsigned long)ret); } -void px4_killall(void) -{ - //printf("Called px4_killall\n"); - for (int i=0; i Date: Wed, 6 May 2015 16:24:18 -0700 Subject: [PATCH 208/258] blinkm: merged NuttX and POSIX impelmentations Signed-off-by: Mark Charlebois --- .../blinkm/{blinkm_posix.cpp => blinkm.cpp} | 6 +- src/drivers/blinkm/blinkm_nuttx.cpp | 1014 ----------------- src/drivers/blinkm/module.mk | 6 +- 3 files changed, 6 insertions(+), 1020 deletions(-) rename src/drivers/blinkm/{blinkm_posix.cpp => blinkm.cpp} (99%) delete mode 100644 src/drivers/blinkm/blinkm_nuttx.cpp diff --git a/src/drivers/blinkm/blinkm_posix.cpp b/src/drivers/blinkm/blinkm.cpp similarity index 99% rename from src/drivers/blinkm/blinkm_posix.cpp rename to src/drivers/blinkm/blinkm.cpp index 04d0e5bcb5..185565d8b6 100644 --- a/src/drivers/blinkm/blinkm_posix.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -275,7 +275,11 @@ const char *const BlinkM::script_names[] = { extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); BlinkM::BlinkM(int bus, int blinkm) : - I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm), + I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm +#ifdef __PX4_NUTTX + , 100000 +#endif + ), led_color_1(LED_OFF), led_color_2(LED_OFF), led_color_3(LED_OFF), diff --git a/src/drivers/blinkm/blinkm_nuttx.cpp b/src/drivers/blinkm/blinkm_nuttx.cpp deleted file mode 100644 index 91cf50af91..0000000000 --- a/src/drivers/blinkm/blinkm_nuttx.cpp +++ /dev/null @@ -1,1014 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 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. - * - ****************************************************************************/ - -/** - * @file blinkm.cpp - * - * Driver for the BlinkM LED controller connected via I2C. - * - * Connect the BlinkM to I2C3 and put the following line to the rc startup-script: - * blinkm start - * - * To start the system monitor put in the next line after the blinkm start: - * blinkm systemmonitor - * - * - * Description: - * After startup, the Application checked how many lipo cells are connected to the System. - * The recognized number off cells, will be blinked 5 times in purple color. - * 2 Cells = 2 blinks - * ... - * 6 Cells = 6 blinks - * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. - * - * System disarmed and safe: - * The BlinkM should light solid cyan. - * - * System safety off but not armed: - * The BlinkM should light flashing orange - * - * System armed: - * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. - * - * X-X-X-X-_-_-_-_-_-_- - * ------------------------- - * G G G M - * P P P O - * S S S D - * E - * - * (X = on, _=off) - * - * The first 3 blinks indicates the status of the GPS-Signal (red): - * 0-4 satellites = X-X-X-X-X-_-_-_-_-_- - * 5 satellites = X-X-_-X-X-_-_-_-_-_- - * 6 satellites = X-_-_-X-X-_-_-_-_-_- - * >=7 satellites = _-_-_-X-X-_-_-_-_-_- - * If no GPS is found the first 3 blinks are white - * - * The fourth Blink indicates the Flightmode: - * MANUAL : amber - * STABILIZED : yellow - * HOLD : blue - * AUTO : green - * - * Battery Warning (low Battery Level): - * Continuously blinking in yellow X-X-X-X-X-X-X-X-X-X - * - * Battery Alert (critical Battery Level) - * Continuously blinking in red X-X-X-X-X-X-X-X-X-X - * - * General Error (no uOrb Data) - * Continuously blinking in white X-X-X-X-X-X-X-X-X-X - * - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -static const float MAX_CELL_VOLTAGE = 4.3f; -static const int LED_ONTIME = 120; -static const int LED_OFFTIME = 120; -static const int LED_BLINK = 1; -static const int LED_NOBLINK = 0; - -class BlinkM : public device::I2C -{ -public: - BlinkM(int bus, int blinkm); - virtual ~BlinkM(); - - - virtual int init(); - virtual int probe(); - virtual int setMode(int mode); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - static const char *const script_names[]; - -private: - enum ScriptID { - USER = 0, - RGB, - WHITE_FLASH, - RED_FLASH, - GREEN_FLASH, - BLUE_FLASH, - CYAN_FLASH, - MAGENTA_FLASH, - YELLOW_FLASH, - BLACK, - HUE_CYCLE, - MOOD_LIGHT, - VIRTUAL_CANDLE, - WATER_REFLECTIONS, - OLD_NEON, - THE_SEASONS, - THUNDERSTORM, - STOP_LIGHT, - MORSE_CODE - }; - - enum ledColors { - LED_OFF, - LED_RED, - LED_ORANGE, - LED_YELLOW, - LED_PURPLE, - LED_GREEN, - LED_BLUE, - LED_CYAN, - LED_WHITE, - LED_AMBER - }; - - work_s _work; - - int led_color_1; - int led_color_2; - int led_color_3; - int led_color_4; - int led_color_5; - int led_color_6; - int led_color_7; - int led_color_8; - int led_blink; - - bool systemstate_run; - - int vehicle_status_sub_fd; - int vehicle_control_mode_sub_fd; - int vehicle_gps_position_sub_fd; - int actuator_armed_sub_fd; - int safety_sub_fd; - - int num_of_cells; - int detected_cells_runcount; - - int t_led_color[8]; - int t_led_blink; - int led_thread_runcount; - int led_interval; - - bool topic_initialized; - bool detected_cells_blinked; - bool led_thread_ready; - - int num_of_used_sats; - - void setLEDColor(int ledcolor); - static void led_trampoline(void *arg); - void led(); - - int set_rgb(uint8_t r, uint8_t g, uint8_t b); - - int fade_rgb(uint8_t r, uint8_t g, uint8_t b); - int fade_hsb(uint8_t h, uint8_t s, uint8_t b); - - int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b); - int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b); - - int set_fade_speed(uint8_t s); - - int play_script(uint8_t script_id); - int play_script(const char *script_name); - int stop_script(); - - int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3); - int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]); - int set_script(uint8_t length, uint8_t repeats); - - int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b); - - int get_firmware_version(uint8_t version[2]); -}; - -/* for now, we only support one BlinkM */ -namespace -{ - BlinkM *g_blinkm; -} - -/* list of script names, must match script ID numbers */ -const char *const BlinkM::script_names[] = { - "USER", - "RGB", - "WHITE_FLASH", - "RED_FLASH", - "GREEN_FLASH", - "BLUE_FLASH", - "CYAN_FLASH", - "MAGENTA_FLASH", - "YELLOW_FLASH", - "BLACK", - "HUE_CYCLE", - "MOOD_LIGHT", - "VIRTUAL_CANDLE", - "WATER_REFLECTIONS", - "OLD_NEON", - "THE_SEASONS", - "THUNDERSTORM", - "STOP_LIGHT", - "MORSE_CODE", - nullptr -}; - - -extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); - -BlinkM::BlinkM(int bus, int blinkm) : - I2C("blinkm", BLINKM0_DEVICE_PATH, bus, blinkm, 100000), - led_color_1(LED_OFF), - led_color_2(LED_OFF), - led_color_3(LED_OFF), - led_color_4(LED_OFF), - led_color_5(LED_OFF), - led_color_6(LED_OFF), - led_color_7(LED_OFF), - led_color_8(LED_OFF), - led_blink(LED_NOBLINK), - systemstate_run(false), - vehicle_status_sub_fd(-1), - vehicle_control_mode_sub_fd(-1), - vehicle_gps_position_sub_fd(-1), - actuator_armed_sub_fd(-1), - safety_sub_fd(-1), - num_of_cells(0), - detected_cells_runcount(0), - t_led_color{0}, - t_led_blink(0), - led_thread_runcount(0), - led_interval(1000), - topic_initialized(false), - detected_cells_blinked(false), - led_thread_ready(true), - num_of_used_sats(0) -{ - memset(&_work, 0, sizeof(_work)); -} - -BlinkM::~BlinkM() -{ -} - -int -BlinkM::init() -{ - int ret; - ret = I2C::init(); - - if (ret != OK) { - return ret; - } - - stop_script(); - set_rgb(0,0,0); - - return OK; -} - -int -BlinkM::setMode(int mode) -{ - if(mode == 1) { - if(systemstate_run == false) { - stop_script(); - set_rgb(0,0,0); - systemstate_run = true; - work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1); - } - } else { - systemstate_run = false; - } - - return OK; -} - -int -BlinkM::probe() -{ - uint8_t version[2]; - int ret; - - ret = get_firmware_version(version); - - if (ret == OK) - log("found BlinkM firmware version %c%c", version[1], version[0]); - - return ret; -} - -int -BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = ENOTTY; - - switch (cmd) { - case BLINKM_PLAY_SCRIPT_NAMED: - if (arg == 0) { - ret = EINVAL; - break; - } - ret = play_script((const char *)arg); - break; - - case BLINKM_PLAY_SCRIPT: - ret = play_script(arg); - break; - - case BLINKM_SET_USER_SCRIPT: { - if (arg == 0) { - ret = EINVAL; - break; - } - - unsigned lines = 0; - const uint8_t *script = (const uint8_t *)arg; - - while ((lines < 50) && (script[1] != 0)) { - ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); - if (ret != OK) - break; - script += 5; - } - if (ret == OK) - ret = set_script(lines, 0); - break; - } - - default: - break; - } - - return ret; -} - - -void -BlinkM::led_trampoline(void *arg) -{ - BlinkM *bm = (BlinkM *)arg; - - bm->led(); -} - - - -void -BlinkM::led() -{ - - if(!topic_initialized) { - vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(vehicle_status_sub_fd, 250); - - vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(vehicle_control_mode_sub_fd, 250); - - actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(actuator_armed_sub_fd, 250); - - vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(vehicle_gps_position_sub_fd, 250); - - /* Subscribe to safety topic */ - safety_sub_fd = orb_subscribe(ORB_ID(safety)); - orb_set_interval(safety_sub_fd, 250); - - topic_initialized = true; - } - - if(led_thread_ready == true) { - if(!detected_cells_blinked) { - if(num_of_cells > 0) { - t_led_color[0] = LED_PURPLE; - } - if(num_of_cells > 1) { - t_led_color[1] = LED_PURPLE; - } - if(num_of_cells > 2) { - t_led_color[2] = LED_PURPLE; - } - if(num_of_cells > 3) { - t_led_color[3] = LED_PURPLE; - } - if(num_of_cells > 4) { - t_led_color[4] = LED_PURPLE; - } - if(num_of_cells > 5) { - t_led_color[5] = LED_PURPLE; - } - t_led_color[6] = LED_OFF; - t_led_color[7] = LED_OFF; - t_led_blink = LED_BLINK; - } else { - t_led_color[0] = led_color_1; - t_led_color[1] = led_color_2; - t_led_color[2] = led_color_3; - t_led_color[3] = led_color_4; - t_led_color[4] = led_color_5; - t_led_color[5] = led_color_6; - t_led_color[6] = led_color_7; - t_led_color[7] = led_color_8; - t_led_blink = led_blink; - } - led_thread_ready = false; - } - - if (led_thread_runcount & 1) { - if (t_led_blink) - setLEDColor(LED_OFF); - led_interval = LED_OFFTIME; - } else { - setLEDColor(t_led_color[(led_thread_runcount / 2) % 8]); - //led_interval = (led_thread_runcount & 1) : LED_ONTIME; - led_interval = LED_ONTIME; - } - - if (led_thread_runcount == 15) { - /* obtained data for the first file descriptor */ - struct vehicle_status_s vehicle_status_raw; - struct vehicle_control_mode_s vehicle_control_mode; - struct actuator_armed_s actuator_armed; - struct vehicle_gps_position_s vehicle_gps_position_raw; - struct safety_s safety; - - memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); - memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); - memset(&safety, 0, sizeof(safety)); - - bool new_data_vehicle_status; - bool new_data_vehicle_control_mode; - bool new_data_actuator_armed; - bool new_data_vehicle_gps_position; - bool new_data_safety; - - orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); - - int no_data_vehicle_status = 0; - int no_data_vehicle_control_mode = 0; - int no_data_actuator_armed = 0; - int no_data_vehicle_gps_position = 0; - - if (new_data_vehicle_status) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw); - no_data_vehicle_status = 0; - } else { - no_data_vehicle_status++; - if(no_data_vehicle_status >= 3) - no_data_vehicle_status = 3; - } - - orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); - - if (new_data_vehicle_control_mode) { - orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); - no_data_vehicle_control_mode = 0; - } else { - no_data_vehicle_control_mode++; - if(no_data_vehicle_control_mode >= 3) - no_data_vehicle_control_mode = 3; - } - - orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); - - if (new_data_actuator_armed) { - orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); - no_data_actuator_armed = 0; - } else { - no_data_actuator_armed++; - if(no_data_actuator_armed >= 3) - no_data_actuator_armed = 3; - } - - orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); - - if (new_data_vehicle_gps_position) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw); - no_data_vehicle_gps_position = 0; - } else { - no_data_vehicle_gps_position++; - if(no_data_vehicle_gps_position >= 3) - no_data_vehicle_gps_position = 3; - } - - /* update safety topic */ - orb_check(safety_sub_fd, &new_data_safety); - - if (new_data_safety) { - orb_copy(ORB_ID(safety), safety_sub_fd, &safety); - } - - /* get number of used satellites in navigation */ - num_of_used_sats = vehicle_gps_position_raw.satellites_used; - - if (new_data_vehicle_status || no_data_vehicle_status < 3) { - if (num_of_cells == 0) { - /* looking for lipo cells that are connected */ - printf(" checking cells\n"); - for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; - } - printf(" cells found:%d\n", num_of_cells); - - } else { - if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { - /* LED Pattern for battery critical alerting */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_RED; - led_color_5 = LED_RED; - led_color_6 = LED_RED; - led_color_7 = LED_RED; - led_color_8 = LED_RED; - led_blink = LED_BLINK; - - } else if(vehicle_status_raw.rc_signal_lost) { - /* LED Pattern for FAILSAFE */ - led_color_1 = LED_BLUE; - led_color_2 = LED_BLUE; - led_color_3 = LED_BLUE; - led_color_4 = LED_BLUE; - led_color_5 = LED_BLUE; - led_color_6 = LED_BLUE; - led_color_7 = LED_BLUE; - led_color_8 = LED_BLUE; - led_blink = LED_BLINK; - - } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { - /* LED Pattern for battery low warning */ - led_color_1 = LED_YELLOW; - led_color_2 = LED_YELLOW; - led_color_3 = LED_YELLOW; - led_color_4 = LED_YELLOW; - led_color_5 = LED_YELLOW; - led_color_6 = LED_YELLOW; - led_color_7 = LED_YELLOW; - led_color_8 = LED_YELLOW; - led_blink = LED_BLINK; - - } else { - /* no battery warnings here */ - - if(actuator_armed.armed == false) { - /* system not armed */ - if(safety.safety_off){ - led_color_1 = LED_ORANGE; - led_color_2 = LED_ORANGE; - led_color_3 = LED_ORANGE; - led_color_4 = LED_ORANGE; - led_color_5 = LED_ORANGE; - led_color_6 = LED_ORANGE; - led_color_7 = LED_ORANGE; - led_color_8 = LED_ORANGE; - led_blink = LED_BLINK; - }else{ - led_color_1 = LED_CYAN; - led_color_2 = LED_CYAN; - led_color_3 = LED_CYAN; - led_color_4 = LED_CYAN; - led_color_5 = LED_CYAN; - led_color_6 = LED_CYAN; - led_color_7 = LED_CYAN; - led_color_8 = LED_CYAN; - led_blink = LED_NOBLINK; - } - } else { - /* armed system - initial led pattern */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_OFF; - led_color_5 = LED_OFF; - led_color_6 = LED_OFF; - led_color_7 = LED_OFF; - led_color_8 = LED_OFF; - led_blink = LED_BLINK; - - if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { - /* indicate main control state */ - if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) - led_color_4 = LED_GREEN; - /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) - led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) - led_color_4 = LED_YELLOW; - else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) - led_color_4 = LED_WHITE; - else - led_color_4 = LED_OFF; - led_color_5 = led_color_4; - } - - if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used satus */ - if(num_of_used_sats >= 7) { - led_color_1 = LED_OFF; - led_color_2 = LED_OFF; - led_color_3 = LED_OFF; - } else if(num_of_used_sats == 6) { - led_color_2 = LED_OFF; - led_color_3 = LED_OFF; - } else if(num_of_used_sats == 5) { - led_color_3 = LED_OFF; - } - - } else { - /* no vehicle_gps_position data */ - led_color_1 = LED_WHITE; - led_color_2 = LED_WHITE; - led_color_3 = LED_WHITE; - - } - - } - } - } - } else { - /* LED Pattern for general Error - no vehicle_status can retrieved */ - led_color_1 = LED_WHITE; - led_color_2 = LED_WHITE; - led_color_3 = LED_WHITE; - led_color_4 = LED_WHITE; - led_color_5 = LED_WHITE; - led_color_6 = LED_WHITE; - led_color_7 = LED_WHITE; - led_color_8 = LED_WHITE; - led_blink = LED_BLINK; - - } - - /* - printf( " Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n", - vehicle_status_raw.voltage_battery, - vehicle_status_raw.flag_system_armed, - vehicle_status_raw.flight_mode, - num_of_cells, - no_data_vehicle_status, - no_data_vehicle_gps_position, - num_of_used_sats, - vehicle_gps_position_raw.fix_type, - vehicle_gps_position_raw.satellites_visible); - */ - - led_thread_runcount=0; - led_thread_ready = true; - led_interval = LED_OFFTIME; - - if(detected_cells_runcount < 4){ - detected_cells_runcount++; - } else { - detected_cells_blinked = true; - } - - } else { - led_thread_runcount++; - } - - if(systemstate_run == true) { - /* re-queue ourselves to run again later */ - work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval); - } else { - stop_script(); - set_rgb(0,0,0); - } -} - -void BlinkM::setLEDColor(int ledcolor) { - switch (ledcolor) { - case LED_OFF: // off - set_rgb(0,0,0); - break; - case LED_RED: // red - set_rgb(255,0,0); - break; - case LED_ORANGE: // orange - set_rgb(255,150,0); - break; - case LED_YELLOW: // yellow - set_rgb(200,200,0); - break; - case LED_PURPLE: // purple - set_rgb(255,0,255); - break; - case LED_GREEN: // green - set_rgb(0,255,0); - break; - case LED_BLUE: // blue - set_rgb(0,0,255); - break; - case LED_CYAN: // cyan - set_rgb(0,128,128); - break; - case LED_WHITE: // white - set_rgb(255,255,255); - break; - case LED_AMBER: // amber - set_rgb(255,65,0); - break; - } -} - -int -BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b) -{ - const uint8_t msg[4] = { 'n', r, g, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b) -{ - const uint8_t msg[4] = { 'c', r, g, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b) -{ - const uint8_t msg[4] = { 'h', h, s, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b) -{ - const uint8_t msg[4] = { 'C', r, g, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b) -{ - const uint8_t msg[4] = { 'H', h, s, b }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::set_fade_speed(uint8_t s) -{ - const uint8_t msg[2] = { 'f', s }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::play_script(uint8_t script_id) -{ - const uint8_t msg[4] = { 'p', script_id, 0, 0 }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::play_script(const char *script_name) -{ - /* handle HTML colour encoding */ - if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) { - char code[3]; - uint8_t r, g, b; - - code[2] = '\0'; - - code[0] = script_name[1]; - code[1] = script_name[2]; - r = strtol(code, 0, 16); - code[0] = script_name[3]; - code[1] = script_name[4]; - g = strtol(code, 0, 16); - code[0] = script_name[5]; - code[1] = script_name[6]; - b = strtol(code, 0, 16); - - stop_script(); - return set_rgb(r, g, b); - } - - for (unsigned i = 0; script_names[i] != nullptr; i++) - if (!strcasecmp(script_name, script_names[i])) - return play_script(i); - - return -1; -} - -int -BlinkM::stop_script() -{ - const uint8_t msg[1] = { 'o' }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3) -{ - const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]) -{ - const uint8_t msg[3] = { 'R', 0, line }; - uint8_t result[5]; - - int ret = transfer(msg, sizeof(msg), result, sizeof(result)); - - if (ret == OK) { - ticks = result[0]; - cmd[0] = result[1]; - cmd[1] = result[2]; - cmd[2] = result[3]; - cmd[3] = result[4]; - } - - return ret; -} - -int -BlinkM::set_script(uint8_t len, uint8_t repeats) -{ - const uint8_t msg[4] = { 'L', 0, len, repeats }; - - return transfer(msg, sizeof(msg), nullptr, 0); -} - -int -BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) -{ - const uint8_t msg = 'g'; - uint8_t result[3]; - - int ret = transfer(&msg, sizeof(msg), result, sizeof(result)); - - if (ret == OK) { - r = result[0]; - g = result[1]; - b = result[2]; - } - - return ret; -} - -int -BlinkM::get_firmware_version(uint8_t version[2]) -{ - const uint8_t msg = 'Z'; - - return transfer(&msg, sizeof(msg), version, 2); -} - -void blinkm_usage(); - -void blinkm_usage() { - warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); - warnx("options:"); - warnx("\t-b --bus i2cbus (3)"); - warnx("\t-a --blinkmaddr blinkmaddr (9)"); -} - -int -blinkm_main(int argc, char *argv[]) -{ - - int i2cdevice = PX4_I2C_BUS_EXPANSION; - int blinkmadr = 9; - - int x; - - for (x = 1; x < argc; x++) { - if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) { - if (argc > x + 1) { - i2cdevice = atoi(argv[x + 1]); - } - } - - if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) { - if (argc > x + 1) { - blinkmadr = atoi(argv[x + 1]); - } - } - - } - - if (!strcmp(argv[1], "start")) { - if (g_blinkm != nullptr) - errx(1, "already started"); - - g_blinkm = new BlinkM(i2cdevice, blinkmadr); - - if (g_blinkm == nullptr) - errx(1, "new failed"); - - if (OK != g_blinkm->init()) { - delete g_blinkm; - g_blinkm = nullptr; - errx(1, "no BlinkM found"); - } - - exit(0); - } - - - if (g_blinkm == nullptr) { - fprintf(stderr, "not started\n"); - blinkm_usage(); - exit(0); - } - - if (!strcmp(argv[1], "systemstate")) { - g_blinkm->setMode(1); - exit(0); - } - - if (!strcmp(argv[1], "ledoff")) { - g_blinkm->setMode(0); - exit(0); - } - - - if (!strcmp(argv[1], "list")) { - for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) - fprintf(stderr, " %s\n", BlinkM::script_names[i]); - fprintf(stderr, " \n"); - exit(0); - } - - /* things that require access to the device */ - int fd = open(BLINKM0_DEVICE_PATH, 0); - if (fd < 0) - err(1, "can't open BlinkM device"); - - g_blinkm->setMode(0); - if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) - exit(0); - - blinkm_usage(); - exit(0); -} diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk index 1547a17e92..c906733175 100644 --- a/src/drivers/blinkm/module.mk +++ b/src/drivers/blinkm/module.mk @@ -37,10 +37,6 @@ MODULE_COMMAND = blinkm -ifeq ($(PX4_TARGET_OS),nuttx) -SRCS = blinkm_nuttx.cpp -else -SRCS = blinkm_posix.cpp -endif +SRCS = blinkm.cpp MAXOPTIMIZATION = -Os From 35e6822d95bdacfbe7d02f25004174b9cdb2da5c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 16:25:45 -0700 Subject: [PATCH 209/258] Added missing px4_ prefixes NuttX build required missing px4_ prefix for systemreset and task_spawn_cmd Signed-off-by: Mark Charlebois --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +- src/systemcmds/tests/test_mount.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a1c56aa1f8..f9099cd167 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -202,7 +202,7 @@ int AttitudeEstimatorQ::start() { ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("attitude_estimator_q", + _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 68052258ef..107d616b6c 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -288,7 +288,7 @@ test_mount(int argc, char *argv[]) fsync(fileno(stdout)); fsync(fileno(stderr)); usleep(50000); - systemreset(false); + px4_systemreset(false); /* never going to get here */ return 0; From 6db77dc8bbae32ee15a17e7a5caa90f7e6191b2c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 6 May 2015 22:12:45 -0700 Subject: [PATCH 210/258] Experimental virtual file support QuRT does not have a filesystem, so creating a virtual filesystem that could be implemented as an in-memory file or a remote file over fastRPC. Signed-off-by: Mark Charlebois --- Tools/posix_apps.py | 7 +++ src/drivers/device/module.mk | 1 + src/drivers/device/vdev.cpp | 18 +++++++- src/drivers/device/vdev.h | 2 + src/drivers/device/vdev_posix.cpp | 32 +++++++++++-- src/drivers/device/vfile.cpp | 65 +++++++++++++++++++++++++++ src/drivers/device/vfile.h | 58 ++++++++++++++++++++++++ src/modules/systemlib/bson/tinybson.c | 7 +-- src/modules/systemlib/param/param.c | 10 +++-- src/platforms/px4_posix.h | 16 ++++--- src/systemcmds/param/param.c | 13 +++--- 11 files changed, 206 insertions(+), 23 deletions(-) create mode 100644 src/drivers/device/vfile.cpp create mode 100644 src/drivers/device/vfile.h diff --git a/Tools/posix_apps.py b/Tools/posix_apps.py index fe457e778e..375e71fc2d 100755 --- a/Tools/posix_apps.py +++ b/Tools/posix_apps.py @@ -61,6 +61,7 @@ for app in apps: print """ static int shutdown_main(int argc, char *argv[]); static int list_tasks_main(int argc, char *argv[]); +static int list_files_main(int argc, char *argv[]); static int list_devices_main(int argc, char *argv[]); static int list_topics_main(int argc, char *argv[]); } @@ -77,6 +78,7 @@ for app in apps: print '\tapps["shutdown"] = shutdown_main;' print '\tapps["list_tasks"] = list_tasks_main;' +print '\tapps["list_files"] = list_files_main;' print '\tapps["list_devices"] = list_devices_main;' print '\tapps["list_topics"] = list_topics_main;' print """ @@ -115,5 +117,10 @@ static int list_topics_main(int argc, char *argv[]) px4_show_topics(); return 0; } +static int list_files_main(int argc, char *argv[]) +{ + px4_show_files(); + return 0; +} """ diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 22e7b15622..c206a5037c 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -47,6 +47,7 @@ else SRCS = \ device_posix.cpp \ vdev.cpp \ + vfile.cpp \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 699bc817b1..25a5253cc0 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -64,7 +64,7 @@ private: px4_dev_t() {} }; -#define PX4_MAX_DEV 100 +#define PX4_MAX_DEV 30 static px4_dev_t *devmap[PX4_MAX_DEV]; /* @@ -448,8 +448,12 @@ VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) VDev *VDev::getDev(const char *path) { + printf("VDev::getDev\n"); int i=0; for (; iname, path); + } if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) { return (VDev *)(devmap[i]->cdev); } @@ -479,6 +483,18 @@ void VDev::showTopics() } } +void VDev::showFiles() +{ + int i=0; + printf("Files:\n"); + for (; iname, "/obj/", 5) != 0 && + strncmp(devmap[i]->name, "/dev/", 5) != 0) { + printf(" %s\n", devmap[i]->name); + } + } +} + const char *VDev::topicList(unsigned int *next) { for (;*next #include #include "device.h" +#include "vfile.h" #include #include @@ -85,16 +86,26 @@ inline bool valid_fd(int fd) return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); } -int px4_open(const char *path, int flags) +int px4_open(const char *path, int flags, ...) { + printf("px4_open\n"); VDev *dev = VDev::getDev(path); int ret = 0; int i; + mode_t mode; - if (!dev) { - ret = -EINVAL; + if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0) + { + va_list p; + va_start(p, flags); + mode = va_arg(p, mode_t); + va_end(p); + + // Create the file + warnx("Creating virtual file %s\n", path); + dev = VFile::createFile(path, mode); } - else { + if (dev) { for (i=0; i + */ + +#include "vfile.h" +#include + +using namespace device; + +VFile::VFile(const char *fname, mode_t mode) : + VDev("vfile", fname) +{ +} + +VFile * VFile::createFile(const char *fname, mode_t mode) +{ + VFile *me = new VFile(fname, mode); + me->register_driver(fname, me); + return me; +} + +ssize_t VFile::write(device::file_t *handlep, const char *buffer, size_t buflen) +{ + // ignore what was written, but let pollers know something was written + poll_notify(POLLIN); + + return buflen; +} + diff --git a/src/drivers/device/vfile.h b/src/drivers/device/vfile.h new file mode 100644 index 0000000000..d7c5e15d7f --- /dev/null +++ b/src/drivers/device/vfile.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +/** + * @file vfile.cpp + * Virtual file + * + * @author Mark Charlebois + */ + +#include +#include +#include "device.h" +#include +#include + +class VFile : public device::VDev { +public: + + static VFile *createFile(const char *fname, mode_t mode); + ~VFile() {} + + virtual ssize_t write(device::file_t *handlep, const char *buffer, size_t buflen); + +private: + VFile(const char *fname, mode_t mode); + VFile(const VFile &); +}; diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 49403c98bd..5908ace6c7 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -37,6 +37,7 @@ * A simple subset SAX-style BSON parser and generator. */ +#include #include #include #include @@ -59,7 +60,7 @@ read_x(bson_decoder_t decoder, void *p, size_t s) CODER_CHECK(decoder); if (decoder->fd > -1) - return (read(decoder->fd, p, s) == (int)s) ? 0 : -1; + return (px4_read(decoder->fd, p, s) == (int)s) ? 0 : -1; if (decoder->buf != NULL) { /* staged operations to avoid integer overflow for corrupt data */ @@ -301,7 +302,7 @@ write_x(bson_encoder_t encoder, const void *p, size_t s) CODER_CHECK(encoder); if (encoder->fd > -1) - return (write(encoder->fd, p, s) == (int)s) ? 0 : -1; + return (px4_write(encoder->fd, p, s) == (int)s) ? 0 : -1; /* do we need to extend the buffer? */ while ((encoder->bufpos + s) > encoder->bufsize) { @@ -408,7 +409,7 @@ bson_encoder_fini(bson_encoder_t encoder) } /* sync file */ - fsync(encoder->fd); + px4_fsync(encoder->fd); return 0; } diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2b9a09b712..19e91b4b2f 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -43,6 +43,7 @@ //#include #include +#include #include #include #include @@ -684,7 +685,7 @@ param_save_default(void) const char *filename = param_get_default_file(); /* write parameters to temp file */ - fd = open(filename, O_WRONLY | O_CREAT, 0x777); + fd = px4_open(filename, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("failed to open param file: %s", filename); @@ -697,7 +698,7 @@ param_save_default(void) warnx("failed to write parameters to file: %s", filename); } - close(fd); + px4_close(fd); return res; } @@ -708,7 +709,8 @@ param_save_default(void) int param_load_default(void) { - int fd_load = open(param_get_default_file(), O_RDONLY); + warnx("param_load_default\n"); + int fd_load = px4_open(param_get_default_file(), O_RDONLY); if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ @@ -720,7 +722,7 @@ param_load_default(void) } int result = param_load(fd_load); - close(fd_load); + px4_close(fd_load); if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index b891ab9982..67c9a47963 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -43,14 +43,16 @@ #include #include #include +#include #include #include -#define PX4_F_RDONLY 1 -#define PX4_F_WRONLY 2 #ifdef __PX4_NUTTX +#define PX4_F_RDONLY 1 +#define PX4_F_WRONLY 2 + typedef struct pollfd px4_pollfd_struct_t; #if defined(__cplusplus) @@ -64,11 +66,13 @@ typedef struct pollfd px4_pollfd_struct_t; #define px4_write _GLOBAL write #define px4_read _GLOBAL read #define px4_poll _GLOBAL poll +#define px4_fsync _GLOBAL fsync #elif defined(__PX4_POSIX) -#define PX4_F_RDONLY 1 -#define PX4_F_WRONLY 2 +#define PX4_F_RDONLY O_RDONLY +#define PX4_F_WRONLY O_WRONLY +#define PX4_F_CREAT O_CREAT typedef short pollevent_t; @@ -85,12 +89,13 @@ typedef struct { __BEGIN_DECLS -__EXPORT int px4_open(const char *path, int flags); +__EXPORT int px4_open(const char *path, int flags, ...); __EXPORT int px4_close(int fd); __EXPORT ssize_t px4_read(int fd, void *buffer, size_t buflen); __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); __EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); +__EXPORT int px4_fsync(int fd); __END_DECLS #else @@ -101,6 +106,7 @@ __BEGIN_DECLS extern int px4_errno; __EXPORT void px4_show_devices(void); +__EXPORT void px4_show_files(void); __EXPORT const char * px4_get_device_names(unsigned int *handle); __EXPORT void px4_show_topics(void); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 716a5b2d0b..20de5fd7a7 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -40,6 +40,7 @@ */ #include +#include #include #include @@ -182,7 +183,7 @@ static int do_save(const char *param_file_name) { /* create the file */ - int fd = open(param_file_name, O_WRONLY | O_CREAT, 0x777); + int fd = px4_open(param_file_name, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("opening '%s' failed", param_file_name); @@ -190,7 +191,7 @@ do_save(const char *param_file_name) } int result = param_export(fd, false); - close(fd); + px4_close(fd); if (result < 0) { (void)unlink(param_file_name); @@ -204,7 +205,7 @@ do_save(const char *param_file_name) static int do_load(const char *param_file_name) { - int fd = open(param_file_name, O_RDONLY); + int fd = px4_open(param_file_name, O_RDONLY); if (fd < 0) { warn("open '%s'", param_file_name); @@ -212,7 +213,7 @@ do_load(const char *param_file_name) } int result = param_load(fd); - close(fd); + px4_close(fd); if (result < 0) { warnx("error importing from '%s'", param_file_name); @@ -225,7 +226,7 @@ do_load(const char *param_file_name) static int do_import(const char *param_file_name) { - int fd = open(param_file_name, O_RDONLY); + int fd = px4_open(param_file_name, O_RDONLY); if (fd < 0) { warn("open '%s'", param_file_name); @@ -233,7 +234,7 @@ do_import(const char *param_file_name) } int result = param_import(fd); - close(fd); + px4_close(fd); if (result < 0) { warnx("error importing from '%s'", param_file_name); From 5e2af2b227767fb65bd5ef02b07798ae5584ed7d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 09:38:03 -0700 Subject: [PATCH 211/258] POSIX: fixed return values to be posix compliant px4_read, px4_write, and px4_opctl were not returning the correct value on error. They were returning -errno vs -1. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev_posix.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 168fc703de..9578f67a2c 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -144,6 +144,7 @@ int px4_close(int fd) } if (ret < 0) { px4_errno = -ret; + ret = PX4_ERROR; } return ret; } @@ -161,13 +162,14 @@ ssize_t px4_read(int fd, void *buffer, size_t buflen) } if (ret < 0) { px4_errno = -ret; + ret = PX4_ERROR; } return ret; } ssize_t px4_write(int fd, const void *buffer, size_t buflen) { - int ret = PX4_ERROR; + int ret; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_write fd = %d\n", fd); @@ -178,13 +180,14 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen) } if (ret < 0) { px4_errno = -ret; + ret = PX4_ERROR; } return ret; } int px4_ioctl(int fd, int cmd, unsigned long arg) { - int ret = PX4_ERROR; + int ret = 0; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); PX4_DEBUG("px4_ioctl fd = %d\n", fd); @@ -196,10 +199,8 @@ int px4_ioctl(int fd, int cmd, unsigned long arg) if (ret < 0) { px4_errno = -ret; } - else { - px4_errno = -EINVAL; - } - return ret; + + return (ret == 0) ? PX4_OK : PX4_ERROR; } int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) From 2002d4e774ac62ea327a926e377125e46602acb1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 10:07:18 -0700 Subject: [PATCH 212/258] POSIX: disable UART in mavlink Most of the current POSIX builds will not use the UART and it is just a unnecessary dependency to satisfy when running in a test environment. Signed-off-by: Mark Charlebois --- src/modules/mavlink/mavlink_main.cpp | 20 ++++++++++---------- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f44ca43b70..57abf4f09a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -51,7 +51,7 @@ #include #include #include -#ifndef __PX4_QURT +#ifndef __PX4_POSIX #include #endif #include @@ -149,7 +149,7 @@ Mavlink::Mavlink() : _forwarding_on(false), _passing_on(false), _ftp_on(false), -#ifndef __PX4_QURT +#ifndef __PX4_POSIX _uart_fd(-1), #endif _baudrate(57600), @@ -412,7 +412,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) } } -#ifndef __PX4_QURT +#ifndef __PX4_POSIX int Mavlink::get_uart_fd(unsigned index) { @@ -430,7 +430,7 @@ Mavlink::get_uart_fd() { return _uart_fd; } -#endif // __PX4_QURT +#endif // __PX4_POSIX int Mavlink::get_instance_id() @@ -572,7 +572,7 @@ int Mavlink::get_component_id() return mavlink_system.compid; } -#ifndef __PX4_QURT +#ifndef __PX4_POSIX int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -758,7 +758,7 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; -#ifndef __PX4_QURT +#ifndef __PX4_POSIX // No FIONWRITE on Linux #if !defined(__PX4_LINUX) @@ -832,7 +832,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); -#ifndef __PX4_QURT +#ifndef __PX4_POSIX /* send message to UART */ ssize_t ret = ::write(_uart_fd, buf, packet_len); @@ -884,7 +884,7 @@ Mavlink::resend_message(mavlink_message_t *msg) buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); -#ifndef __PX4_QURT +#ifndef __PX4_POSIX /* send message to UART */ ssize_t ret = ::write(_uart_fd, buf, packet_len); @@ -1344,7 +1344,7 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); -#ifndef __PX4_QURT +#ifndef __PX4_POSIX struct termios uart_config_original; /* default values for arguments */ @@ -1616,7 +1616,7 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(_receive_thread, NULL); -#ifndef __PX4_QURT +#ifndef __PX4_POSIX /* reset the UART flags to original state */ tcsetattr(_uart_fd, TCSANOW, &uart_config_original); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 180a923774..77b2bfb0be 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -60,7 +60,7 @@ #include #include #include -#ifndef __PX4_QURT +#ifndef __PX4_POSIX #include #include #endif @@ -1480,7 +1480,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { -#ifndef __PX4_QURT +#ifndef __PX4_POSIX int uart_fd = _mavlink->get_uart_fd(); const int timeout = 500; @@ -1570,7 +1570,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); -#ifndef __PX4_QURT +#ifndef __PX4_POSIX // set to non-blocking read int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); From 4216a0d64dadaa370b7214e884d19d28faf5cad9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 10:46:10 -0700 Subject: [PATCH 213/258] POSIX: remove check for /tmp/ttyS{0|1} The posix build now disables the UART code in mavlink. Signed-off-by: Mark Charlebois --- Tools/posix_run.sh | 7 ------- 1 file changed, 7 deletions(-) diff --git a/Tools/posix_run.sh b/Tools/posix_run.sh index 500261fce8..fdeefb4a29 100755 --- a/Tools/posix_run.sh +++ b/Tools/posix_run.sh @@ -1,12 +1,5 @@ #!/bin/bash -if [ ! -c /tmp/ttyS0 ] || [ ! -c /tmp/ttyS1 ] - then - echo "Need to create /tmp/ttyS[01]" - echo "socat PTY,link=/tmp/ttyS0 PTY,link=/tmp/ttyS1" - exit 1 -fi - if [ ! -d /fs/msdcard ] && [ ! -w /fs/msdcard ] then echo "Need to create/mount writable /fs/microsd" From 99822038f3c5fee218ecea38e195fb44d2c107e8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 10:50:23 -0700 Subject: [PATCH 214/258] POSIX: Fixed px4_open code to not create a file when opening a device The code to created a virtual file was preventing the creation of device nodes. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 6 +++--- src/drivers/device/vdev_posix.cpp | 6 ++++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 25a5253cc0..9efdb63e8a 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -451,9 +451,9 @@ VDev *VDev::getDev(const char *path) printf("VDev::getDev\n"); int i=0; for (; iname, path); - } + //if (devmap[i]) { + // printf("%s %s\n", devmap[i]->name, path); + //} if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) { return (VDev *)(devmap[i]->cdev); } diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 9578f67a2c..ca75c91923 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -94,7 +94,9 @@ int px4_open(const char *path, int flags, ...) int i; mode_t mode; - if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0) + if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0 && + strncmp(path, "/obj/", 5) != 0 && + strncmp(path, "/dev/", 5) != 0) { va_list p; va_start(p, flags); @@ -187,10 +189,10 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen) int px4_ioctl(int fd, int cmd, unsigned long arg) { + PX4_DEBUG("px4_ioctl fd = %d\n", fd); int ret = 0; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_ioctl fd = %d\n", fd); ret = dev->ioctl(filemap[fd], cmd, arg); } else { From a0d548db9afff119710cd85798bd8feb6ae4a26a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 11:35:01 -0700 Subject: [PATCH 215/258] Changed circuit_breaker to not use px4.h The inclusion of px4.h requires C++ features not supported in the Hexagon toolchain. The features are not required so the required headers are used instead. Signed-off-by: Mark Charlebois --- src/modules/systemlib/circuit_breaker.cpp | 3 ++- src/modules/systemlib/module.mk | 10 +++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index f5ff0afd4b..ba9e9f6219 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -42,7 +42,8 @@ * parameter needs to set to the key (magic). */ -#include +#include +#include #include bool circuit_breaker_enabled(const char *breaker, int32_t magic) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index e648c577d9..0e023cf1b1 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -49,17 +49,17 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - mcu_version.c + mcu_version.c \ + bson/tinybson.c \ + circuit_breaker.cpp \ + circuit_breaker_params.c ifeq ($(PX4_TARGET_OS),nuttx) SRCS += up_cxxinitialize.c endif ifneq ($(PX4_TARGET_OS),qurt) -SRCS += hx_stream.c \ - circuit_breaker.cpp \ - circuit_breaker_params.c \ - bson/tinybson.c +SRCS += hx_stream.c endif MAXOPTIMIZATION = -Os From 632f77df49e136a7b2c065da65dd4c7af19a0a65 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 11:55:24 -0700 Subject: [PATCH 216/258] QuRT: add drivers/led The drivers/led module was missing from the QuRT config Signed-off-by: Mark Charlebois --- makefiles/qurt/config_qurt_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index ba7ca3e143..48fcefffe5 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -13,6 +13,7 @@ MODULES += drivers/device MODULES += drivers/blinkm MODULES += drivers/hil +MODULES += drivers/led MODULES += drivers/rgbled MODULES += modules/sensors #MODULES += drivers/ms5611 From fd3715912eb9b2b8db30b06fcef441aa4565b386 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 12:54:39 -0700 Subject: [PATCH 217/258] QuRT: stub out missing functions Stub out the missing functions to enable running in the simulator Signed-off-by: Mark Charlebois --- src/platforms/qurt/main.cpp | 21 ++++++++++- .../qurt/px4_layer/px4_qurt_impl.cpp | 37 +++++++++++++++++++ 2 files changed, 56 insertions(+), 2 deletions(-) diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 55f9ad9a8d..217fa68d0c 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -51,8 +51,25 @@ #include "apps.h" #include "px4_middleware.h" -static const char *commands = "hello start\n" - "list_tasks"; +static const char *commands = +"uorb start\n" +"simulator start -s\n" +"barosim start\n" +"adcsim start\n" +"accelsim start\n" +"gyrosim start\n" +"param set CAL_GYRO0_ID 2293760\n" +"param set CAL_ACC0_ID 1310720\n" +"param set CAL_ACC1_ID 1376256\n" +"param set CAL_MAG0_ID 196608\n" +"rgbled start\n" +"mavlink start -d /tmp/ttyS0\n" +"sensors start\n" +"hil mode_pwm\n" +"commander start\n" +"list_devices\n" +"list_topics\n"; + static void run_cmd(const vector &appargs) { // command is appargs[0] diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 36c795a213..5b7df7d6df 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -52,6 +53,9 @@ __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported long PX4_TICKS_PER_SEC = 1000; +void usleep(useconds_t usec) {} +unsigned int sleep(unsigned int sec) { return 0; } + __END_DECLS extern struct wqueue_s gwork[NWORKERS]; @@ -83,3 +87,36 @@ void init(int argc, char *argv[], const char *app_name) } +/** Retrieve from the data manager store */ +ssize_t +dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned char index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +) +{ + return 0; +} + +/** write to the data manager store */ +ssize_t +dm_write( + dm_item_t item, /* The item type to store */ + unsigned char index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ +) +{ + return 0; +} + +size_t strnlen(const char *s, size_t maxlen) +{ + size_t i=0; + while (s[i] != '\0' && i < maxlen) + ++i; + + return i; +} From 8caefc183dab2d1dc942dda99c36ce36deac477e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 13:23:45 -0700 Subject: [PATCH 218/258] QuRT: updated toolchain file to link against libdspal.a Signed-off-by: Mark Charlebois --- makefiles/toolchain_hexagon.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index e250f12b37..62863a908f 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -179,6 +179,8 @@ endif HEXAGON_LIB_PATH = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0 LIB_HEXAGON = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a +EXTRA_LIBS += $(PX4_BASE)../dspal_libs/libdspal.a + # Flags we pass to the assembler # AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ @@ -286,7 +288,7 @@ define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) echo $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) - $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 + $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(EXTRA_LIBS) # $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) From 8dfb09418b4c393041e5bd3067f486d80bfddba5 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 14:40:31 -0700 Subject: [PATCH 219/258] QuRT: added #include to posix unit tests QuRT does not define usleep and sleep, so they are stubbed out for now. Signed-off-by: Mark Charlebois --- src/platforms/posix/tests/hrt_test/hrt_test.cpp | 1 + src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp | 1 + src/platforms/posix/tests/wqueue/wqueue_test.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index 877462a44b..f5291b7dd7 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -39,6 +39,7 @@ * @author Mark Charlebois */ +#include #include #include "hrt_test.h" #include diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index c7652c3399..7f1c452d33 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -40,6 +40,7 @@ */ #include +#include #include "vcdevtest_example.h" #include #include diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.cpp b/src/platforms/posix/tests/wqueue/wqueue_test.cpp index 74eb88e7e9..6c9ececc14 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_test.cpp @@ -39,6 +39,7 @@ * @author Mark Charlebois */ +#include #include #include "wqueue_test.h" #include From ad1865ef9f1ae87fbd95358ba1a37ca72c2a879e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 14:55:28 -0700 Subject: [PATCH 220/258] QuRT: Move to POSIX API for threads Use the pthread APIs to implement task support Signed-off-by: Mark Charlebois --- .../qurt/px4_layer/px4_qurt_tasks.cpp | 171 +++++++++++++----- 1 file changed, 127 insertions(+), 44 deletions(-) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index b4554bf4fe..456a4a442b 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -33,10 +33,11 @@ ****************************************************************************/ /** - * @file px4_linux_tasks.c + * @file px4_qurt_tasks.c * Implementation of existing task API for Linux */ +#include #include #include #include @@ -60,11 +61,10 @@ #define PX4_MAX_TASKS 100 struct task_entry { - int pid; + pthread_t pid; std::string name; bool isused; task_entry() : isused(false) {} - void *sp; }; static task_entry taskmap[PX4_MAX_TASKS]; @@ -77,28 +77,28 @@ typedef struct // strings are allocated after the } pthdata_t; -static void entry_adapter ( void *ptr ) +static void *entry_adapter ( void *ptr ) { - pthdata_t *data = (pthdata_t *) ptr; + PX4_DBG("entry_adapter\n"); + pthdata_t *data; + data = (pthdata_t *) ptr; - printf("TEST3\n"); -#if 0 - //data->entry(data->argc, data->argv); - printf("TEST4\n"); - printf("Before px4_task_exit\n"); + data->entry(data->argc, data->argv); + free(ptr); + PX4_DBG("Before px4_task_exit"); px4_task_exit(0); - //free(ptr); - printf("After px4_task_exit\n"); -#endif + PX4_DBG("After px4_task_exit"); + + return NULL; } void px4_systemreset(bool to_bootloader) { - printf("Called px4_system_reset\n"); + PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) { int rv; int argc = 0; @@ -106,20 +106,19 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p; + char * p = (char *)argv; + + pthread_t task; + pthread_attr_t attr; + struct sched_param param; - //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); - printf("arg %d %p\n", argc, argv); // Calculate argc - if (argv) { - for(;;) { - p = argv[argc]; - if (p == (char *)0) - break; - printf("arg %d %s\n", argc, argv[argc]); - ++argc; - len += strlen(p)+1; - } + while (p != (char *)0) { + p = argv[argc]; + if (p == (char *)0) + break; + ++argc; + len += strlen(p)+1; } structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; @@ -132,7 +131,6 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskdata->argc = argc; for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); @@ -141,38 +139,124 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; + rv = pthread_attr_init(&attr); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); + return (rv < 0) ? rv : -rv; + } +#if 0 + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); + return (rv < 0) ? rv : -rv; + } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); + return (rv < 0) ? rv : -rv; + } + + param.sched_priority = priority; + + rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); + return (rv < 0) ? rv : -rv; + } +#endif + + rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { + + if (rv == EPERM) { + //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); + rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { + PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + return (rv < 0) ? rv : -rv; + } + } + else { + return (rv < 0) ? rv : -rv; + } + } + for (i=0; i=PX4_MAX_TASKS) { + return -ENOSPC; + } + return i; } int px4_task_delete(px4_task_t id) { - printf("Called px4_task_delete\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_WARN("Called px4_task_delete"); + + if (id < PX4_MAX_TASKS && taskmap[id].isused) + pid = taskmap[id].pid; + else + return -EINVAL; + + // If current thread then exit, otherwise cancel + if (pthread_self() == pid) { + taskmap[id].isused = false; + pthread_exit(0); + } else { + rv = pthread_cancel(pid); + } + + taskmap[id].isused = false; + + return rv; } void px4_task_exit(int ret) { - thread_stop(); + int i; + pthread_t pid = pthread_self(); - // Free stack + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) { + PX4_ERR("px4_task_exit: self task not found!"); + } + else { + PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); + } + + pthread_exit((void *)(unsigned long)ret); } int px4_task_kill(px4_task_t id, int sig) { - printf("Called px4_task_kill\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_DBG("Called px4_task_kill %d", sig); + + if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + pid = taskmap[id].pid; + else + return -EINVAL; + + // If current thread then exit, otherwise cancel + rv = pthread_kill(pid, sig); + + return rv; } void px4_show_tasks() @@ -180,17 +264,16 @@ void px4_show_tasks() int idx; int count = 0; - printf("Active Tasks:\n"); + PX4_INFO("Active Tasks:"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s", taskmap[idx].name.c_str()); count++; } } if (count == 0) - printf(" No running tasks\n"); - + PX4_INFO(" No running tasks"); } // STUBS From 3225edfabb43e3b5914af0fe41bf7b67cf71f131 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 21:51:02 -0700 Subject: [PATCH 221/258] QuRT: Really reverted to non-posix APIs Using non-posix APIs for now. Signed-off-by: Mark Charlebois --- .../qurt/px4_layer/px4_qurt_tasks.cpp | 171 +++++------------- 1 file changed, 44 insertions(+), 127 deletions(-) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 456a4a442b..b4554bf4fe 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -33,11 +33,10 @@ ****************************************************************************/ /** - * @file px4_qurt_tasks.c + * @file px4_linux_tasks.c * Implementation of existing task API for Linux */ -#include #include #include #include @@ -61,10 +60,11 @@ #define PX4_MAX_TASKS 100 struct task_entry { - pthread_t pid; + int pid; std::string name; bool isused; task_entry() : isused(false) {} + void *sp; }; static task_entry taskmap[PX4_MAX_TASKS]; @@ -77,28 +77,28 @@ typedef struct // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void entry_adapter ( void *ptr ) { - PX4_DBG("entry_adapter\n"); - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data = (pthdata_t *) ptr; - data->entry(data->argc, data->argv); - free(ptr); - PX4_DBG("Before px4_task_exit"); + printf("TEST3\n"); +#if 0 + //data->entry(data->argc, data->argv); + printf("TEST4\n"); + printf("Before px4_task_exit\n"); px4_task_exit(0); - PX4_DBG("After px4_task_exit"); - - return NULL; + //free(ptr); + printf("After px4_task_exit\n"); +#endif } void px4_systemreset(bool to_bootloader) { - PX4_WARN("Called px4_system_reset"); + printf("Called px4_system_reset\n"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) { int rv; int argc = 0; @@ -106,19 +106,20 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; - - pthread_t task; - pthread_attr_t attr; - struct sched_param param; + char * p; + //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); + printf("arg %d %p\n", argc, argv); // Calculate argc - while (p != (char *)0) { - p = argv[argc]; - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; + if (argv) { + for(;;) { + p = argv[argc]; + if (p == (char *)0) + break; + printf("arg %d %s\n", argc, argv[argc]); + ++argc; + len += strlen(p)+1; + } } structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; @@ -131,6 +132,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskdata->argc = argc; for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); @@ -139,124 +141,38 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; - rv = pthread_attr_init(&attr); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); - return (rv < 0) ? rv : -rv; - } -#if 0 - rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); - return (rv < 0) ? rv : -rv; - } - rv = pthread_attr_setschedpolicy(&attr, scheduler); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); - return (rv < 0) ? rv : -rv; - } - - param.sched_priority = priority; - - rv = pthread_attr_setschedparam(&attr, ¶m); - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); - return (rv < 0) ? rv : -rv; - } -#endif - - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); - if (rv != 0) { - - if (rv == EPERM) { - //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); - if (rv != 0) { - PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); - return (rv < 0) ? rv : -rv; - } - } - else { - return (rv < 0) ? rv : -rv; - } - } - for (i=0; i=PX4_MAX_TASKS) { - return -ENOSPC; - } - return i; + printf("TEST2\n"); + thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); + + return i+1; } int px4_task_delete(px4_task_t id) { - int rv = 0; - pthread_t pid; - PX4_WARN("Called px4_task_delete"); - - if (id < PX4_MAX_TASKS && taskmap[id].isused) - pid = taskmap[id].pid; - else - return -EINVAL; - - // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { - taskmap[id].isused = false; - pthread_exit(0); - } else { - rv = pthread_cancel(pid); - } - - taskmap[id].isused = false; - - return rv; + printf("Called px4_task_delete\n"); + return -EINVAL; } void px4_task_exit(int ret) { - int i; - pthread_t pid = pthread_self(); + thread_stop(); - // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { - PX4_ERR("px4_task_exit: self task not found!"); - } - else { - PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); - } - - pthread_exit((void *)(unsigned long)ret); + // Free stack } int px4_task_kill(px4_task_t id, int sig) { - int rv = 0; - pthread_t pid; - PX4_DBG("Called px4_task_kill %d", sig); - - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) - pid = taskmap[id].pid; - else - return -EINVAL; - - // If current thread then exit, otherwise cancel - rv = pthread_kill(pid, sig); - - return rv; + printf("Called px4_task_kill\n"); + return -EINVAL; } void px4_show_tasks() @@ -264,16 +180,17 @@ void px4_show_tasks() int idx; int count = 0; - PX4_INFO("Active Tasks:"); + printf("Active Tasks:\n"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_INFO(" %-10s", taskmap[idx].name.c_str()); + printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } if (count == 0) - PX4_INFO(" No running tasks"); + printf(" No running tasks\n"); + } // STUBS From 84ca66dcf7a40b98d2eab64d5deb778f3f73a8a0 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 22:58:51 -0700 Subject: [PATCH 222/258] QuRT: updated task support based on posix fixes The posix layer implementations should work on QuRT. QuRT needs to provide a way for getting the current time. Signed-off-by: Mark Charlebois --- makefiles/qurt/config_qurt_default.mk | 4 +- src/platforms/px4_time.h | 4 + src/platforms/qurt/main.cpp | 12 +- src/platforms/qurt/px4_layer/drv_hrt.c | 123 +++++++++++++++++- .../qurt/px4_layer/px4_qurt_impl.cpp | 10 ++ .../qurt/px4_layer/px4_qurt_tasks.cpp | 30 ++--- src/platforms/qurt/px4_layer/work_lock.h | 53 ++++++++ src/platforms/qurt/px4_layer/work_queue.c | 7 +- src/platforms/qurt/px4_layer/work_thread.c | 60 ++++++--- 9 files changed, 261 insertions(+), 42 deletions(-) create mode 100644 src/platforms/qurt/px4_layer/work_lock.h diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 48fcefffe5..2e2323f4f0 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -72,7 +72,7 @@ MODULES += platforms/posix/drivers/barosim # Unit tests # MODULES += platforms/qurt/tests/hello -#MODULES += platforms/posix/tests/vcdev_test +MODULES += platforms/posix/tests/vcdev_test #MODULES += platforms/posix/tests/hrt_test -#MODULES += platforms/posix/tests/wqueue +MODULES += platforms/posix/tests/wqueue diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index d72cdbb8b1..0c9b7f24c9 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -10,10 +10,13 @@ #elif defined(__PX4_QURT) +#include + #define CLOCK_REALTIME 1 __BEGIN_DECLS +#if 0 #if !defined(__cplusplus) struct timespec { @@ -21,6 +24,7 @@ struct timespec long tv_nsec; }; #endif +#endif int px4_clock_gettime(clockid_t clk_id, struct timespec *tp); int px4_clock_settime(clockid_t clk_id, struct timespec *tp); diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 217fa68d0c..58d37da24b 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -52,6 +52,9 @@ #include "px4_middleware.h" static const char *commands = +"x\n" +"hello start" +#if 0 "uorb start\n" "simulator start -s\n" "barosim start\n" @@ -68,7 +71,9 @@ static const char *commands = "hil mode_pwm\n" "commander start\n" "list_devices\n" -"list_topics\n"; +"list_topics\n" +#endif +; static void run_cmd(const vector &appargs) { @@ -148,8 +153,13 @@ static void process_commands(const char *cmds) } } +namespace px4 { +extern void init_once(void); +}; + int main(int argc, char **argv) { + px4::init_once(); px4::init(argc, argv, "mainapp"); process_commands(commands); for (;;) {} diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index 39e0f73b37..3edf34f64c 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -37,9 +37,12 @@ * High-resolution timer with callouts and timekeeping. */ +#include #include +#include #include #include +#include static struct sq_queue_s callout_queue; @@ -51,9 +54,28 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; static void hrt_call_reschedule(void); +// Intervals in ms #define HRT_INTERVAL_MIN 50 #define HRT_INTERVAL_MAX 50000 +static sem_t _hrt_lock; +static struct work_s _hrt_work; + +static void +hrt_call_invoke(void); + +static void hrt_lock(void) +{ + //printf("hrt_lock\n"); + sem_wait(&_hrt_lock); +} + +static void hrt_unlock(void) +{ + //printf("hrt_unlock\n"); + sem_post(&_hrt_lock); +} + /* * Get absolute time. */ @@ -61,7 +83,7 @@ hrt_abstime hrt_absolute_time(void) { struct timespec ts; - // FIXME - not supported in QURT + // FIXME - clock_gettime unsupported in QuRT //clock_gettime(CLOCK_MONOTONIC, &ts); return ts_to_abstime(&ts); } @@ -120,7 +142,7 @@ bool hrt_called(struct hrt_call *entry) */ void hrt_cancel(struct hrt_call *entry) { - // FIXME - need a lock + hrt_lock(); sq_rem(&entry->link, &callout_queue); entry->deadline = 0; @@ -128,6 +150,7 @@ void hrt_cancel(struct hrt_call *entry) * being re-entered when the callout returns. */ entry->period = 0; + hrt_unlock(); // endif } @@ -156,7 +179,10 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) */ void hrt_init(void) { + //printf("hrt_init\n"); sq_init(&callout_queue); + sem_init(&_hrt_lock, 0, 1); + memset(&_hrt_work, 0, sizeof(_hrt_work)); } static void @@ -164,6 +190,7 @@ hrt_call_enter(struct hrt_call *entry) { struct hrt_call *call, *next; + //printf("hrt_call_enter\n"); call = (struct hrt_call *)sq_peek(&callout_queue); if ((call == NULL) || (entry->deadline < call->deadline)) { @@ -187,6 +214,27 @@ hrt_call_enter(struct hrt_call *entry) //lldbg("scheduled\n"); } +/** + * Timer interrupt handler + * + * This routine simulates a timer interrupt handler + */ +static void +hrt_tim_isr(void *p) +{ + + //printf("hrt_tim_isr\n"); + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + hrt_lock(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + + hrt_unlock(); +} + /** * Reschedule the next timer interrupt. * @@ -198,7 +246,10 @@ hrt_call_reschedule() hrt_abstime now = hrt_absolute_time(); struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); hrt_abstime deadline = now + HRT_INTERVAL_MAX; + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + //printf("hrt_call_reschedule\n"); + /* * Determine what the next deadline will be. * @@ -216,18 +267,26 @@ hrt_call_reschedule() if (next->deadline <= (now + HRT_INTERVAL_MIN)) { //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ - deadline = now + HRT_INTERVAL_MIN; + ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); } else if (next->deadline < deadline) { //lldbg("due soon\n"); - deadline = next->deadline; + ticks = USEC2TICK((next->deadline - now)*1000); } } + + // There is no timer ISR, so simulate one by putting an event on the + // high priority work queue + //printf("ticks = %u\n", ticks); + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) { + //printf("hrt_call_internal\n"); + hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised entry->link here, but it is safe as sq_rem() doesn't @@ -245,6 +304,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte entry->arg = arg; hrt_call_enter(entry); + hrt_unlock(); } /* @@ -255,6 +315,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte */ void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { + //printf("hrt_call_after\n"); hrt_call_internal(entry, hrt_absolute_time() + delay, 0, @@ -292,3 +353,57 @@ void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); #endif +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + hrt_lock(); + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) + break; + + if (call->deadline > now) + break; + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + // Unlock so we don't deadlock in callback + hrt_unlock(); + + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + + hrt_lock(); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } + hrt_unlock(); +} + diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 5b7df7d6df..f878a8d140 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -56,6 +56,8 @@ long PX4_TICKS_PER_SEC = 1000; void usleep(useconds_t usec) {} unsigned int sleep(unsigned int sec) { return 0; } +extern void hrt_init(void); + __END_DECLS extern struct wqueue_s gwork[NWORKERS]; @@ -63,6 +65,14 @@ extern struct wqueue_s gwork[NWORKERS]; namespace px4 { +void init_once(void); + +void init_once(void) +{ + work_queues_init(); + hrt_init(); +} + void init(int argc, char *argv[], const char *app_name) { printf("App name: %s\n", app_name); diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index b4554bf4fe..6431536e75 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -79,17 +79,15 @@ typedef struct static void entry_adapter ( void *ptr ) { + printf("entry_adapter\n"); pthdata_t *data = (pthdata_t *) ptr; - printf("TEST3\n"); -#if 0 - //data->entry(data->argc, data->argv); - printf("TEST4\n"); + data->entry(data->argc, data->argv); + free(ptr); + printf("after entry\n"); printf("Before px4_task_exit\n"); px4_task_exit(0); - //free(ptr); printf("After px4_task_exit\n"); -#endif } void @@ -106,21 +104,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p; + char * p = (char *)argv; - //printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); - printf("arg %d %p\n", argc, argv); // Calculate argc - if (argv) { - for(;;) { - p = argv[argc]; - if (p == (char *)0) - break; - printf("arg %d %s\n", argc, argv[argc]); - ++argc; - len += strlen(p)+1; - } + while (p != (char *)0) { + p = argv[argc]; + if (p == (char *)0) + break; + ++argc; + len += strlen(p)+1; } + printf("arg %d %p\n", argc, argv); structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h new file mode 100644 index 0000000000..f77f228b36 --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_lock.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include + +#pragma once +extern sem_t _work_lock[]; + +inline void work_lock(int id); +inline void work_lock(int id) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +inline void work_unlock(int id); +inline void work_unlock(int id) +{ + //printf("work_unlock %d\n", id); + sem_post(&_work_lock[id]); +} + diff --git a/src/platforms/qurt/px4_layer/work_queue.c b/src/platforms/qurt/px4_layer/work_queue.c index 644b25e54b..1746359b63 100644 --- a/src/platforms/qurt/px4_layer/work_queue.c +++ b/src/platforms/qurt/px4_layer/work_queue.c @@ -43,7 +43,10 @@ #include #include #include +#include +#include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -117,13 +120,13 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_ * from with task logic or interrupt handlers. */ - //flags = irqsave(); + work_lock(qid); work->qtime = clock_systimer(); /* Time work queued */ dq_addlast((dq_entry_t *)work, &wqueue->q); px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ - //irqrestore(flags); + work_unlock(qid); return PX4_OK; } diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c index cb8cbe9766..a57cf4b10a 100644 --- a/src/platforms/qurt/px4_layer/work_thread.c +++ b/src/platforms/qurt/px4_layer/work_thread.c @@ -39,12 +39,14 @@ #include #include +#include #include #include #include #include #include #include +#include "work_lock.h" #ifdef CONFIG_SCHED_WORKQUEUE @@ -66,6 +68,7 @@ struct wqueue_s g_work[NWORKERS]; /**************************************************************************** * Private Variables ****************************************************************************/ +sem_t _work_lock[NWORKERS]; /**************************************************************************** * Private Functions @@ -85,13 +88,12 @@ struct wqueue_s g_work[NWORKERS]; * ****************************************************************************/ -static void work_process(FAR struct wqueue_s *wqueue) +static void work_process(FAR struct wqueue_s *wqueue, int lock_id) { volatile FAR struct work_s *work; worker_t worker; - //irqstate_t flags; FAR void *arg; - uint32_t elapsed; + uint64_t elapsed; uint32_t remaining; uint32_t next; @@ -100,7 +102,9 @@ static void work_process(FAR struct wqueue_s *wqueue) */ next = CONFIG_SCHED_WORKPERIOD; - //flags = irqsave(); + + work_lock(lock_id); + work = (FAR struct work_s *)wqueue->q.head; while (work) { @@ -110,8 +114,8 @@ static void work_process(FAR struct wqueue_s *wqueue) * zero. Therefore a delay of zero will always execute immediately. */ - elapsed = clock_systimer() - work->qtime; - //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay); + elapsed = USEC2TICK(clock_systimer() - work->qtime); + //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay); if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ @@ -133,7 +137,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * performed... we don't have any idea how long that will take! */ - //irqrestore(flags); + work_unlock(lock_id); if (!worker) { printf("MESSED UP: worker = 0\n"); } @@ -145,7 +149,7 @@ static void work_process(FAR struct wqueue_s *wqueue) * back at the head of the list. */ - //flags = irqsave(); + work_lock(lock_id); work = (FAR struct work_s *)wqueue->q.head; } else @@ -155,7 +159,7 @@ static void work_process(FAR struct wqueue_s *wqueue) */ /* Here: elapsed < work->delay */ - remaining = work->delay - elapsed; + remaining = USEC_PER_TICK*(work->delay - elapsed); if (remaining < next) { /* Yes.. Then schedule to wake up when the work is ready */ @@ -172,15 +176,40 @@ static void work_process(FAR struct wqueue_s *wqueue) /* Wait awhile to check the work list. We will wait here until either * the time elapses or until we are awakened by a signal. */ + work_unlock(lock_id); - //FIXME - DSPAL doesn't support usleep - //usleep(next); - //irqrestore(flags); + usleep(next); } /**************************************************************************** * Public Functions ****************************************************************************/ +void work_queues_init(void) +{ + sem_init(&_work_lock[HPWORK], 0, 1); + sem_init(&_work_lock[LPWORK], 0, 1); +#ifdef CONFIG_SCHED_USRWORK + sem_init(&_work_lock[USRWORK], 0, 1); +#endif + + // Create high priority worker thread + g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX-1, + 2000, + work_hpthread, + (char* const*)NULL); + + // Create low priority worker thread + g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2000, + work_lpthread, + (char* const*)NULL); + +} + /**************************************************************************** * Name: work_hpthread, work_lpthread, and work_usrthread * @@ -235,7 +264,7 @@ int work_hpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[HPWORK]); + work_process(&g_work[HPWORK], HPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -262,7 +291,7 @@ int work_lpthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[LPWORK]); + work_process(&g_work[LPWORK], LPWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -283,7 +312,7 @@ int work_usrthread(int argc, char *argv[]) * we process items in the work list. */ - work_process(&g_work[USRWORK]); + work_process(&g_work[USRWORK], USRWORK); } return PX4_OK; /* To keep some compilers happy */ @@ -293,6 +322,7 @@ int work_usrthread(int argc, char *argv[]) uint32_t clock_systimer() { + //printf("clock_systimer: %0lx\n", hrt_absolute_time()); return (0x00000000ffffffff & hrt_absolute_time()); } #endif /* CONFIG_SCHED_WORKQUEUE */ From 0ba5305e94b519cdcb1cdbcd0c5c32c391a50e62 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 May 2015 23:21:24 -0700 Subject: [PATCH 223/258] QuRT: satisfy missing deps There is no ioctl or write. Added stubs. Signed-off-by: Mark Charlebois --- src/drivers/device/i2c_posix.cpp | 4 ++++ src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index f02b021ffc..473307b1cc 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -108,6 +108,10 @@ I2C::init() return PX4_ERROR; } +#ifdef __PX4_QURT + simulate = true; +#endif + if (simulate) { _fd = 10000; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 6431536e75..c077e21c8a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -82,6 +82,7 @@ static void entry_adapter ( void *ptr ) printf("entry_adapter\n"); pthdata_t *data = (pthdata_t *) ptr; + printf("data->entry = %p\n", data->entry); data->entry(data->argc, data->argv); free(ptr); printf("after entry\n"); @@ -106,6 +107,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; + printf("px4_task_spawn_cmd entry = %p\n", entry); // Calculate argc while (p != (char *)0) { p = argv[argc]; @@ -193,4 +195,7 @@ extern "C" { void hrt_sleep(unsigned long) { } + } +int ioctl(int d, int request, unsigned long foo) { return 0; } +int write(int a, char const*b, int c) { return c; } From 39711ca9089c727c493513f109fb2ba2e04eedaf Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 8 May 2015 21:51:21 +0200 Subject: [PATCH 224/258] implemented bidirectional udp communication with simulator --- src/modules/simulator/simulator.cpp | 146 ++++++++++++++++++++-------- src/modules/simulator/simulator.h | 52 ++++++++-- 2 files changed, 148 insertions(+), 50 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 9f7b0c9597..d69ea01c65 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -50,7 +50,6 @@ #include #endif #include "simulator.h" -#include using namespace simulator; @@ -58,6 +57,9 @@ static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = NULL; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + Simulator *Simulator::getInstance() { return _instance; @@ -100,6 +102,83 @@ int Simulator::start(int argc, char *argv[]) return ret; } +void Simulator::poll_topics() { + // copy new data if available + bool updated; + orb_check(_actuator_outputs_sub, &updated); + if(updated) { + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); + } + + orb_check(_vehicle_attitude_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); + } +} + +void Simulator::send_data() { + // check if it's time to send new data + hrt_abstime time_now = hrt_absolute_time(); + if (time_now - _time_last >= (hrt_abstime)(_interval * 1000)) { + _time_last = time_now; + mavlink_message_t msg; + pack_actuator_message(&msg); + send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200); + // can add more messages here, can also setup different timings + } +} + +void Simulator::pack_actuator_message(mavlink_message_t *msg) { + // pack message and send + mavlink_servo_output_raw_t actuator_msg; + + actuator_msg.time_usec = hrt_absolute_time(); + actuator_msg.port = 8; // hardcoded for now + actuator_msg.servo1_raw = _actuators.output[0]; + actuator_msg.servo2_raw = _actuators.output[1]; + actuator_msg.servo3_raw = _actuators.output[2]; + actuator_msg.servo4_raw = _actuators.output[3]; + actuator_msg.servo5_raw = _actuators.output[4]; + actuator_msg.servo6_raw = _actuators.output[5]; + actuator_msg.servo7_raw = _actuators.output[6]; + actuator_msg.servo8_raw = _actuators.output[7]; + + // encode the message + mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg); +} + +void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { + uint8_t payload_len = mavlink_message_lengths[msgid]; + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* no idea which numbers should be here*/ + buf[2] = 100; + buf[3] = 1; + buf[4] = component_ID; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen); + if (len <= 0) { + PX4_WARN("Failed sending mavlink message"); + } +} + void Simulator::fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu) { hrt_abstime timestamp = hrt_absolute_time(); sensor->timestamp = timestamp; @@ -221,7 +300,6 @@ void Simulator::publishSensorsCombined() { // advertise _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors); - hrt_abstime time_last = hrt_absolute_time(); uint64_t delta; for(;;) { @@ -270,46 +348,47 @@ void Simulator::updateSamples() // mag report struct mag_report mag; memset(&mag, 0 ,sizeof(mag)); + // init publishers _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - // get samples from external provider - struct sockaddr_in myaddr; - struct sockaddr_in srcaddr; - socklen_t addrlen = sizeof(srcaddr); - int len, fd; - const int buflen = 200; - const int port = 14550; - unsigned char buf[buflen]; + // subscribe to topics + _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); + _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + // try to setup udp socket for communcation with simulator + memset((char *)&_myaddr, 0, sizeof(_myaddr)); + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(_port); + + if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { PX4_WARN("create socket failed\n"); return; } - memset((char *)&myaddr, 0, sizeof(myaddr)); - myaddr.sin_family = AF_INET; - myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - myaddr.sin_port = htons(port); - - if (bind(fd, (struct sockaddr *)&myaddr, sizeof(myaddr)) < 0) { + if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { PX4_WARN("bind failed\n"); return; } + // this is used to time message sending + _time_last = hrt_absolute_time(); + + int len = 0; // wait for new mavlink messages to arrive for (;;) { - len = 0; - len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); + // see if we can receive data from simulator + len = recvfrom(_fd, _buf, _buflen, 0, (struct sockaddr *)&_srcaddr, &_addrlen); if (len > 0) { mavlink_message_t msg; mavlink_status_t status; for (int i = 0; i < len; ++i) { - if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { // have a message, handle it handle_message(&msg); @@ -324,35 +403,16 @@ void Simulator::updateSamples() gyro.timestamp = time_last; mag.timestamp = time_last; // publish the sensor values - //printf("Publishing SensorsCombined\n"); orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - /* - for (;;) { - len = recvfrom(fd, buf, buflen, 0, (struct sockaddr *)&srcaddr, &addrlen); - if (len > 0) { - if (len == sizeof(RawMPUData)) { - PX4_DBG("received: MPU data\n"); - _mpu.writeData(buf); - } - else if (len == sizeof(RawAccelData)) { - PX4_DBG("received: accel data\n"); - _accel.writeData(buf); - } - else if (len == sizeof(RawBaroData)) { - PX4_DBG("received: accel data\n"); - _baro.writeData(buf); - } - else { - PX4_DBG("bad packet: len = %d\n", len); - } - } + // see if there is new data to send to simulator + poll_topics(); + send_data(); + usleep(10000); } - */ } #endif diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 97b2420b2a..72b8cb33fb 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -41,10 +41,13 @@ #include #include #include +#include +#include #include #include #include #include +#include #include #include #include @@ -160,25 +163,30 @@ private: _baro(1), _sensor_combined_pub(-1), _manual_control_sp_pub(-1), + _actuator_outputs_sub(-1), + _vehicle_attitude_sub(-1), _sensor{}, - _manual_control_sp{} - {} + _manual_control_sp{}, + _actuators{}, + _attitude{}, + _interval(1000) + { + _buf = new unsigned char [_buflen]; + } ~Simulator() { _instance=NULL; } #ifndef __PX4_QURT void updateSamples(); #endif - void publishSensorsCombined(); - void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu); - void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg); - void handle_message(mavlink_message_t *msg); static Simulator *_instance; + // simulated sensor instances simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; + // uORB publisher handlers orb_advert_t _accel_pub; orb_advert_t _baro_pub; orb_advert_t _gyro_pub; @@ -186,7 +194,37 @@ private: orb_advert_t _sensor_combined_pub; orb_advert_t _manual_control_sp_pub; + // uORB subscription handlers + int _actuator_outputs_sub; + int _vehicle_attitude_sub; + + // uORB data containers struct sensor_combined_s _sensor; struct manual_control_setpoint_s _manual_control_sp; -}; + struct actuator_outputs_s _actuators; + struct vehicle_attitude_s _attitude; + // udp socket data + struct sockaddr_in _myaddr; + struct sockaddr_in _srcaddr; + socklen_t _addrlen = sizeof(_srcaddr); + int _fd; + const int _buflen = 200; + const int _port = 14550; + unsigned char *_buf; + + hrt_abstime _time_last; + int _interval; + + // class methods + int setup_udp_socket(); + void do_subscriptions(); + void poll_topics(); + void publishSensorsCombined(); + void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu); + void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg); + void handle_message(mavlink_message_t *msg); + void send_data(); + void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); + void pack_actuator_message(mavlink_message_t *msg); +}; From 9119687177e5432649c381caa79c7ad12125bbd6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 9 May 2015 10:10:23 +0200 Subject: [PATCH 225/258] make socket non-blocking, moved socket includes to header file to avoid forward declarations --- src/modules/simulator/simulator.cpp | 10 ++++++---- src/modules/simulator/simulator.h | 4 ++++ 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index d69ea01c65..f595f2e507 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -45,10 +45,7 @@ #include #include #include -#ifndef __PX4_QURT -#include -#include -#endif + #include "simulator.h" using namespace simulator; @@ -378,6 +375,11 @@ void Simulator::updateSamples() // this is used to time message sending _time_last = hrt_absolute_time(); + // make socket non-blocking + int flags = fcntl(_fd,F_GETFL); + flags |= O_NONBLOCK; + fcntl(_fd, F_SETFL, flags); + int len = 0; // wait for new mavlink messages to arrive for (;;) { diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 72b8cb33fb..654ddf0f5d 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -51,6 +51,10 @@ #include #include #include +#include +#ifndef __PX4_QURT +#include +#endif namespace simulator { From c074d6e91396c959fa375a16dfb59160f6df6c4b Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 11 May 2015 23:54:48 +0200 Subject: [PATCH 226/258] implemented polling of socket file descriptor and uorb topic file descriptor --- src/modules/simulator/simulator.cpp | 107 ++++++++++++++++++++++------ src/modules/simulator/simulator.h | 2 + 2 files changed, 87 insertions(+), 22 deletions(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index f595f2e507..57155d1350 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include #include @@ -326,6 +328,43 @@ void Simulator::publishSensorsCombined() { } #ifndef __PX4_QURT + +void *Simulator::sending_trampoline(void *) { + _instance->send(); + return 0; // why do I have to put this??? +} + +void Simulator::send() { + px4_pollfd_struct_t fds[1]; + fds[0].fd = _actuator_outputs_sub; + fds[0].events = POLLIN; + + _time_last = hrt_absolute_time(); + + while(true) { + // wait for up to 100ms for data + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + //timed out + if (pret == 0) + continue; + + // this is undesirable but not much we can do + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (fds[0].revents & POLLIN) { + // got new data to read, update all topics + poll_topics(); + send_data(); + } + } +} + void Simulator::updateSamples() { struct baro_report baro; @@ -372,28 +411,57 @@ void Simulator::updateSamples() return; } - // this is used to time message sending - _time_last = hrt_absolute_time(); + // create a thread for sending data to the simulator + pthread_t sender_thread; - // make socket non-blocking - int flags = fcntl(_fd,F_GETFL); - flags |= O_NONBLOCK; - fcntl(_fd, F_SETFL, flags); + // initialize threads + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, 1000); + + struct sched_param param; + (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 30; + (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); + pthread_attr_destroy(&sender_thread_attr); + + struct pollfd socket_fds; + socket_fds.fd = _fd; + socket_fds.events = POLLIN; int len = 0; // wait for new mavlink messages to arrive - for (;;) { - // see if we can receive data from simulator - len = recvfrom(_fd, _buf, _buflen, 0, (struct sockaddr *)&_srcaddr, &_addrlen); - if (len > 0) { - mavlink_message_t msg; - mavlink_status_t status; - for (int i = 0; i < len; ++i) - { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) + while (true) { + + int socket_pret = ::poll(&socket_fds, (size_t)1, 100); + + //timed out + if (socket_pret == 0) + continue; + + // this is undesirable but not much we can do + if (socket_pret < 0) { + PX4_WARN("poll error %d, %d", socket_pret, errno); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (socket_fds.revents & POLLIN) { + len = recvfrom(_fd, _buf, _buflen, 0, (struct sockaddr *)&_srcaddr, &_addrlen); + if (len > 0) { + mavlink_message_t msg; + mavlink_status_t status; + for (int i = 0; i < len; ++i) { - // have a message, handle it - handle_message(&msg); + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) + { + // have a message, handle it + handle_message(&msg); + } } } } @@ -409,11 +477,6 @@ void Simulator::updateSamples() orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - - // see if there is new data to send to simulator - poll_topics(); - send_data(); - usleep(10000); } } #endif diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 654ddf0f5d..fe2e466c69 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -231,4 +231,6 @@ private: void send_data(); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void pack_actuator_message(mavlink_message_t *msg); + static void *sending_trampoline(void *); + void send(); }; From a99f916bdff4a805712dda4d862d4a8698c6c43e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 11 May 2015 16:04:39 -0700 Subject: [PATCH 227/258] POSIX: Changed px4_debug.h to px4_log.h Also changed use of printf to PX4_WARN or PX4_INFO in posix and qurt tests. Signed-off-by: Mark Charlebois --- src/drivers/device/vdev_posix.cpp | 2 - src/modules/simulator/simulator.cpp | 2 +- .../posix/px4_layer/px4_posix_tasks.cpp | 2 +- .../posix/tests/hello/hello_start_posix.cpp | 10 ++-- .../tests/vcdev_test/vcdevtest_example.cpp | 42 ++++++++--------- .../posix/tests/wqueue/wqueue_main.cpp | 5 +- .../posix/tests/wqueue/wqueue_start_posix.cpp | 11 +++-- src/platforms/px4_defines.h | 2 +- src/platforms/{px4_debug.h => px4_log.h} | 18 ++++++-- .../qurt/px4_layer/px4_qurt_tasks.cpp | 46 +++++++++++-------- src/platforms/qurt/px4_layer/work_thread.c | 2 +- .../qurt/tests/hello/hello_example.cpp | 4 +- src/platforms/qurt/tests/hello/hello_main.cpp | 5 +- .../qurt/tests/hello/hello_start_qurt.cpp | 14 +++--- 14 files changed, 93 insertions(+), 72 deletions(-) rename src/platforms/{px4_debug.h => px4_log.h} (84%) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index ca75c91923..d3b54c3db5 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -48,8 +48,6 @@ #include #include -#define PX4_DEBUG(...) - using namespace device; extern "C" { diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 9f7b0c9597..139a25ac65 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -36,7 +36,7 @@ * A device simulator */ -#include +#include #include #include #include diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index c831ae04dd..c09845b79a 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -37,7 +37,7 @@ * Implementation of existing task API for Linux */ -#include +#include #include #include #include diff --git a/src/platforms/posix/tests/hello/hello_start_posix.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp index 962645fabe..8dde729a6e 100644 --- a/src/platforms/posix/tests/hello/hello_start_posix.cpp +++ b/src/platforms/posix/tests/hello/hello_start_posix.cpp @@ -57,14 +57,14 @@ int hello_main(int argc, char *argv[]) { if (argc < 2) { - printf("usage: hello {start|stop|status}\n"); + PX4_WARN("usage: hello {start|stop|status}\n"); return 1; } if (!strcmp(argv[1], "start")) { if (HelloExample::appState.isRunning()) { - printf("already running\n"); + PX4_INFO("already running\n"); /* this is not an error */ return 0; } @@ -86,15 +86,15 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (HelloExample::appState.isRunning()) { - printf("is running\n"); + PX4_INFO("is running\n"); } else { - printf("not started\n"); + PX4_INFO("not started\n"); } return 0; } - printf("usage: hello_main {start|stop|status}\n"); + PX4_WARN("usage: hello_main {start|stop|status}\n"); return 1; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index 7f1c452d33..0721ae035a 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -59,7 +59,7 @@ static int writer_main(int argc, char *argv[]) int fd = px4_open(TESTDEV, PX4_F_RDONLY); if (fd < 0) { - printf("--- Open failed %d %d", fd, px4_errno); + PX4_INFO("--- Open failed %d %d", fd, px4_errno); return -px4_errno; } @@ -67,14 +67,14 @@ static int writer_main(int argc, char *argv[]) int i=0; while (i<3) { // Wait for 3 seconds - printf("--- Sleeping for 4 sec\n"); + PX4_INFO("--- Sleeping for 4 sec\n"); ret = sleep(4); if (ret < 0) { - printf("--- sleep failed %d %d\n", ret, errno); + PX4_INFO("--- sleep failed %d %d\n", ret, errno); return ret; } - printf("--- writing to fd\n"); + PX4_INFO("--- writing to fd\n"); ret = px4_write(fd, buf, 1); ++i; } @@ -110,16 +110,16 @@ static int test_pub_block(int fd, unsigned long blocked) { int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked); if (ret < 0) { - printf("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); + PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0); if (ret < 0) { - printf("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); + PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } - printf("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); return 0; } @@ -131,29 +131,29 @@ int VCDevExample::main() _node = new VCDevNode(); if (_node == 0) { - printf("Failed to allocate VCDevNode\n"); + PX4_INFO("Failed to allocate VCDevNode\n"); return -ENOMEM; } if (_node->init() != PX4_OK) { - printf("Failed to init VCDevNode\n"); + PX4_INFO("Failed to init VCDevNode\n"); return 1; } int fd = px4_open(TESTDEV, PX4_F_RDONLY); if (fd < 0) { - printf("Open failed %d %d", fd, px4_errno); + PX4_INFO("Open failed %d %d", fd, px4_errno); return -px4_errno; } void *p = 0; int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p); if (ret < 0) { - printf("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); + PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } - printf("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); + PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL"); ret = test_pub_block(fd, 1); if (ret < 0) @@ -174,32 +174,32 @@ int VCDevExample::main() (char* const*)NULL); while (!appState.exitRequested() && i<13) { - printf("=====================\n"); - printf("==== sleeping 2 sec ====\n"); + PX4_INFO("=====================\n"); + PX4_INFO("==== sleeping 2 sec ====\n"); sleep(2); fds[0].fd = fd; fds[0].events = POLLIN; fds[0].revents = 0; - printf("==== Calling Poll\n"); + PX4_INFO("==== Calling Poll\n"); ret = px4_poll(fds, 1, 1000); - printf("==== Done poll\n"); + PX4_INFO("==== Done poll\n"); if (ret < 0) { - printf("==== poll failed %d %d\n", ret, px4_errno); + PX4_INFO("==== poll failed %d %d\n", ret, px4_errno); px4_close(fd); } else if (i > 0) { if (ret == 0) - printf("==== Nothing to read - PASS\n"); + PX4_INFO("==== Nothing to read - PASS\n"); else - printf("==== poll returned %d\n", ret); + PX4_INFO("==== poll returned %d\n", ret); } else if (i == 0) { if (ret == 1) - printf("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); + PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); else - printf("==== %d to read - FAIL\n", ret); + PX4_INFO("==== %d to read - FAIL\n", ret); } ++i; diff --git a/src/platforms/posix/tests/wqueue/wqueue_main.cpp b/src/platforms/posix/tests/wqueue/wqueue_main.cpp index a7117cca09..8dd5cc9707 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_main.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_main.cpp @@ -37,6 +37,7 @@ * * @author Mark Charlebois */ +#include #include #include #include "wqueue_test.h" @@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv) { px4::init(argc, argv, "wqueue_test"); - printf("wqueue hello\n"); + PX4_INFO("wqueue hello\n"); WQueueTest wq; wq.main(); - printf("goodbye\n"); + PX4_INFO("goodbye\n"); return 0; } diff --git a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp index 20c9975578..2479020097 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp @@ -39,6 +39,7 @@ */ #include "wqueue_test.h" #include +#include #include #include #include @@ -53,14 +54,14 @@ int wqueue_test_main(int argc, char *argv[]) { if (argc < 2) { - printf("usage: wqueue_test {start|stop|status}\n"); + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); return 1; } if (!strcmp(argv[1], "start")) { if (WQueueTest::appState.isRunning()) { - printf("already running\n"); + PX4_INFO("already running\n"); /* this is not an error */ return 0; } @@ -82,15 +83,15 @@ int wqueue_test_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (WQueueTest::appState.isRunning()) { - printf("is running\n"); + PX4_INFO("is running\n"); } else { - printf("not started\n"); + PX4_INFO("not started\n"); } return 0; } - printf("usage: wqueue_test {start|stop|status}\n"); + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); return 1; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 1b2cf1dea5..c17b39e4c5 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,7 +39,7 @@ #pragma once -#include +#include /* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT diff --git a/src/platforms/px4_debug.h b/src/platforms/px4_log.h similarity index 84% rename from src/platforms/px4_debug.h rename to src/platforms/px4_log.h index 6cac039b28..e0d3cff8d4 100644 --- a/src/platforms/px4_debug.h +++ b/src/platforms/px4_log.h @@ -32,13 +32,25 @@ ****************************************************************************/ /** - * @file px4_debug.h - * Platform dependant debug + * @file px4_log.h + * Platform dependant logging/debug */ #pragma once -#if defined(__PX4_LINUX) || defined(__PX4_QURT) +#if defined(__PX4_QURT) + +__BEGIN_DECLS +extern void qurt_log(const char *fmt, ...); +__END_DECLS + +#define PX4_DBG(...) qurt_log(__VA_ARGS__) +#define PX4_DEBUG(...) qurt_log(__VA_ARGS__) +#define PX4_INFO(...) qurt_log(__VA_ARGS__) +#define PX4_WARN(...) qurt_log(__VA_ARGS__) +#define PX4_ERR(...) { qurt_log("ERROR file %s line %d:", __FILE__, __LINE__); qurt_log(__VA_ARGS__); } + +#elif defined(__PX4_LINUX) #if defined(__PX4_LINUX) #include diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index c077e21c8a..e5d2297e9a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -53,11 +53,13 @@ #include #include +#include #include #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 +#define PX4_MAX_TASKS 5 + struct task_entry { int pid; @@ -73,28 +75,28 @@ typedef struct { px4_main_t entry; int argc; - char *argv[]; - // strings are allocated after the + char * argv[]; + // strings are allocated after } pthdata_t; static void entry_adapter ( void *ptr ) { - printf("entry_adapter\n"); pthdata_t *data = (pthdata_t *) ptr; + PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]); - printf("data->entry = %p\n", data->entry); + PX4_DEBUG("data->entry = %p\n", data->entry); data->entry(data->argc, data->argv); free(ptr); - printf("after entry\n"); - printf("Before px4_task_exit\n"); + PX4_DEBUG("after entry\n"); + PX4_DEBUG("Before px4_task_exit\n"); px4_task_exit(0); - printf("After px4_task_exit\n"); + PX4_DEBUG("After px4_task_exit\n"); } void px4_systemreset(bool to_bootloader) { - printf("Called px4_system_reset\n"); + PX4_DEBUG("Called px4_system_reset\n"); } px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) @@ -107,7 +109,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; - printf("px4_task_spawn_cmd entry = %p\n", entry); + PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]); // Calculate argc while (p != (char *)0) { p = argv[argc]; @@ -116,20 +118,24 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int ++argc; len += strlen(p)+1; } - printf("arg %d %p\n", argc, argv); + PX4_DEBUG("arg %d %p\n", argc, argv); structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; + PX4_DEBUG("arg %d %p\n", argc, argv); // not safe to pass stack data to the thread creation taskdata = (pthdata_t *)malloc(structsize+len); + PX4_DEBUG("arg %d %p\n", argc, argv); offset = ((unsigned long)taskdata)+structsize; + PX4_DEBUG("arg %d %p\n", argc, argv); taskdata->entry = entry; taskdata->argc = argc; + PX4_DEBUG("arg %d %p\n", argc, argv); for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); offset+=strlen(argv[i])+1; @@ -142,11 +148,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskmap[i].pid = i+1; taskmap[i].name = name; taskmap[i].isused = true; - taskmap[i].sp = malloc(stack_size); + taskmap[i].sp = malloc(2048); break; } } - printf("TEST2\n"); + PX4_DEBUG("taskdata %p entry %p %d %p\n", taskdata, taskdata->entry, taskdata->argc, taskdata->argv[0]); thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); return i+1; @@ -154,7 +160,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int int px4_task_delete(px4_task_t id) { - printf("Called px4_task_delete\n"); + PX4_DEBUG("Called px4_task_delete\n"); return -EINVAL; } @@ -167,7 +173,7 @@ void px4_task_exit(int ret) int px4_task_kill(px4_task_t id, int sig) { - printf("Called px4_task_kill\n"); + PX4_DEBUG("Called px4_task_kill\n"); return -EINVAL; } @@ -176,16 +182,16 @@ void px4_show_tasks() int idx; int count = 0; - printf("Active Tasks:\n"); + PX4_DEBUG("Active Tasks:\n"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - printf(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_DEBUG(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } if (count == 0) - printf(" No running tasks\n"); + PX4_DEBUG(" No running tasks\n"); } diff --git a/src/platforms/qurt/px4_layer/work_thread.c b/src/platforms/qurt/px4_layer/work_thread.c index a57cf4b10a..3873034189 100644 --- a/src/platforms/qurt/px4_layer/work_thread.c +++ b/src/platforms/qurt/px4_layer/work_thread.c @@ -139,7 +139,7 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id) work_unlock(lock_id); if (!worker) { - printf("MESSED UP: worker = 0\n"); + PX4_WARN("MESSED UP: worker = 0\n"); } else worker(arg); diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp index 6827220888..d0a4de0dc6 100644 --- a/src/platforms/qurt/tests/hello/hello_example.cpp +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -1,4 +1,3 @@ - /**************************************************************************** * * Copyright (C) 2015 Mark Charlebois. All rights reserved. @@ -40,6 +39,7 @@ */ #include "hello_example.h" +#include #include #include @@ -52,7 +52,7 @@ int HelloExample::main() int i=0; while (!appState.exitRequested() && i<5) { - printf(" Doing work...\n"); + PX4_DEBUG(" Doing work...\n"); ++i; } diff --git a/src/platforms/qurt/tests/hello/hello_main.cpp b/src/platforms/qurt/tests/hello/hello_main.cpp index 69e8c21ec0..d18c016753 100644 --- a/src/platforms/qurt/tests/hello/hello_main.cpp +++ b/src/platforms/qurt/tests/hello/hello_main.cpp @@ -38,6 +38,7 @@ * @author Mark Charlebois */ #include +#include #include #include "hello_example.h" #include @@ -46,10 +47,10 @@ int PX4_MAIN(int argc, char **argv) { px4::init(argc, argv, "hello"); - printf("hello\n"); + PX4_DEBUG("hello\n"); HelloExample hello; hello.main(); - printf("goodbye\n"); + PX4_DEBUG("goodbye\n"); return 0; } diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index 71f30cd8dd..7c935e1ab4 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -38,6 +38,7 @@ * @author Mark Charlebois */ #include "hello_example.h" +#include #include #include #include @@ -52,11 +53,11 @@ extern "C" __EXPORT int hello_main(int argc, char *argv[]); static void usage() { - printf("usage: hello {start|stop|status}\n"); + PX4_DEBUG("usage: hello {start|stop|status}\n"); } int hello_main(int argc, char *argv[]) { - printf("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); + PX4_DEBUG("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); if (argc < 2) { usage(); return 1; @@ -64,8 +65,9 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { + PX4_DEBUG("Starting\n"); if (HelloExample::appState.isRunning()) { - printf("already running\n"); + PX4_DEBUG("already running\n"); /* this is not an error */ return 0; } @@ -73,7 +75,7 @@ int hello_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("hello", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 16000, PX4_MAIN, (char* const*)argv); @@ -87,10 +89,10 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (HelloExample::appState.isRunning()) { - printf("is running\n"); + PX4_DEBUG("is running\n"); } else { - printf("not started\n"); + PX4_DEBUG("not started\n"); } return 0; From 2f434eb39545cb8db549de1773185d67719eedea Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 11 May 2015 16:14:06 -0700 Subject: [PATCH 228/258] POSIX: fixups for px4_log.h change After merge from qurt branch, fixups for posix build Signed-off-by: Mark Charlebois --- src/drivers/device/sim.cpp | 2 +- src/drivers/device/vdev_posix.cpp | 1 + src/platforms/px4_log.h | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp index 75127d0e2d..4e952409eb 100644 --- a/src/drivers/device/sim.cpp +++ b/src/drivers/device/sim.cpp @@ -40,7 +40,7 @@ * that is supplied. Should we just depend on the bus knowing? */ -#include +#include #include #include #include diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index d3b54c3db5..fdd2e73210 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -37,6 +37,7 @@ * POSIX-like API for virtual character device */ +#include #include #include #include "device.h" diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index e0d3cff8d4..6fd8387326 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -59,6 +59,7 @@ __END_DECLS #endif #define PX4_DBG(...) +#define PX4_DEBUG(...) #define PX4_INFO(...) warnx(__VA_ARGS__) #define PX4_WARN(...) warnx(__VA_ARGS__) #define PX4_ERR(...) { warnx("ERROR file %s line %d:", __FILE__, __LINE__); warnx(__VA_ARGS__); } From d0bf4ab449e5c7eaf9192417caf451ab4bef1c97 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 11 May 2015 19:25:11 -0700 Subject: [PATCH 229/258] Simulator: refactored mavlink additions QuRT does not support UDP so moved the mavlink specific code to a new file that is not built for the qurt target Signed-off-by: Mark Charlebois --- src/modules/simulator/module.mk | 4 + src/modules/simulator/simulator.cpp | 332 +----------------- src/modules/simulator/simulator.h | 41 +-- src/modules/simulator/simulator_mavlink.cpp | 361 ++++++++++++++++++++ 4 files changed, 384 insertions(+), 354 deletions(-) create mode 100644 src/modules/simulator/simulator_mavlink.cpp diff --git a/src/modules/simulator/module.mk b/src/modules/simulator/module.mk index 0555c5dbd0..80f619477d 100644 --- a/src/modules/simulator/module.mk +++ b/src/modules/simulator/module.mk @@ -39,6 +39,10 @@ MODULE_COMMAND = simulator SRCS = simulator.cpp +ifneq ($(PX4_TARGET_OS), qurt) +SRCS += simulator_mavlink.cpp +endif + INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index e712234f1b..1920724fdf 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -56,9 +56,6 @@ static px4_task_t g_sim_task = -1; Simulator *Simulator::_instance = NULL; -static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; - Simulator *Simulator::getInstance() { return _instance; @@ -101,167 +98,6 @@ int Simulator::start(int argc, char *argv[]) return ret; } -void Simulator::poll_topics() { - // copy new data if available - bool updated; - orb_check(_actuator_outputs_sub, &updated); - if(updated) { - orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); - } - - orb_check(_vehicle_attitude_sub, &updated); - if(updated) { - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); - } -} - -void Simulator::send_data() { - // check if it's time to send new data - hrt_abstime time_now = hrt_absolute_time(); - if (time_now - _time_last >= (hrt_abstime)(_interval * 1000)) { - _time_last = time_now; - mavlink_message_t msg; - pack_actuator_message(&msg); - send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200); - // can add more messages here, can also setup different timings - } -} - -void Simulator::pack_actuator_message(mavlink_message_t *msg) { - // pack message and send - mavlink_servo_output_raw_t actuator_msg; - - actuator_msg.time_usec = hrt_absolute_time(); - actuator_msg.port = 8; // hardcoded for now - actuator_msg.servo1_raw = _actuators.output[0]; - actuator_msg.servo2_raw = _actuators.output[1]; - actuator_msg.servo3_raw = _actuators.output[2]; - actuator_msg.servo4_raw = _actuators.output[3]; - actuator_msg.servo5_raw = _actuators.output[4]; - actuator_msg.servo6_raw = _actuators.output[5]; - actuator_msg.servo7_raw = _actuators.output[6]; - actuator_msg.servo8_raw = _actuators.output[7]; - - // encode the message - mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg); -} - -void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { - uint8_t payload_len = mavlink_message_lengths[msgid]; - - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - /* header */ - buf[0] = MAVLINK_STX; - buf[1] = payload_len; - /* no idea which numbers should be here*/ - buf[2] = 100; - buf[3] = 1; - buf[4] = component_ID; - buf[5] = msgid; - - /* payload */ - memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len); - - /* checksum */ - uint16_t checksum; - crc_init(&checksum); - crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); - crc_accumulate(mavlink_message_crcs[msgid], &checksum); - - buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); - buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - - ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen); - if (len <= 0) { - PX4_WARN("Failed sending mavlink message"); - } -} - -void Simulator::fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu) { - hrt_abstime timestamp = hrt_absolute_time(); - sensor->timestamp = timestamp; - sensor->gyro_raw[0] = imu->xgyro * 1000.0f; - sensor->gyro_raw[1] = imu->ygyro * 1000.0f; - sensor->gyro_raw[2] = imu->zgyro * 1000.0f; - sensor->gyro_rad_s[0] = imu->xgyro; - sensor->gyro_rad_s[1] = imu->ygyro; - sensor->gyro_rad_s[2] = imu->zgyro; - - sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2; - sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2; - sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2; - sensor->accelerometer_m_s2[0] = imu->xacc; - sensor->accelerometer_m_s2[1] = imu->yacc; - sensor->accelerometer_m_s2[2] = imu->zacc; - sensor->accelerometer_mode = 0; // TODO what is this? - sensor->accelerometer_range_m_s2 = 32.7f; // int16 - sensor->accelerometer_timestamp = timestamp; - sensor->timestamp = timestamp; - - sensor->adc_voltage_v[0] = 0.0f; - sensor->adc_voltage_v[1] = 0.0f; - sensor->adc_voltage_v[2] = 0.0f; - - sensor->magnetometer_raw[0] = imu->xmag * 1000.0f; - sensor->magnetometer_raw[1] = imu->ymag * 1000.0f; - sensor->magnetometer_raw[2] = imu->zmag * 1000.0f; - sensor->magnetometer_ga[0] = imu->xmag; - sensor->magnetometer_ga[1] = imu->ymag; - sensor->magnetometer_ga[2] = imu->zmag; - sensor->magnetometer_range_ga = 32.7f; // int16 - sensor->magnetometer_mode = 0; // TODO what is this - sensor->magnetometer_cuttoff_freq_hz = 50.0f; - sensor->magnetometer_timestamp = timestamp; - - sensor->baro_pres_mbar = imu->abs_pressure; - sensor->baro_alt_meter = imu->pressure_alt; - sensor->baro_temp_celcius = imu->temperature; - sensor->baro_timestamp = timestamp; - - sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa - sensor->differential_pressure_timestamp = timestamp; -} - -void Simulator::fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) { - manual->timestamp = hrt_absolute_time(); - manual->x = man_msg->x / 1000.0f; - manual->y = man_msg->y / 1000.0f; - manual->r = man_msg->r / 1000.0f; - manual->z = man_msg->z / 1000.0f; -} - -void Simulator::handle_message(mavlink_message_t *msg) { - switch(msg->msgid) { - case MAVLINK_MSG_ID_HIGHRES_IMU: - mavlink_highres_imu_t imu; - mavlink_msg_highres_imu_decode(msg, &imu); - fill_sensors_from_imu_msg(&_sensor, &imu); - - // publish message - if(_sensor_combined_pub < 0) { - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); - } else { - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); - } - break; - - case MAVLINK_MSG_ID_MANUAL_CONTROL: - - mavlink_manual_control_t man_ctrl_sp; - mavlink_msg_manual_control_decode(msg, &man_ctrl_sp); - fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp); - - // publish message - if(_manual_control_sp_pub < 0) { - _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); - } - break; - } -} - void Simulator::publishSensorsCombined() { struct baro_report baro; @@ -327,160 +163,6 @@ void Simulator::publishSensorsCombined() { } } -#ifndef __PX4_QURT - -void *Simulator::sending_trampoline(void *) { - _instance->send(); - return 0; // why do I have to put this??? -} - -void Simulator::send() { - px4_pollfd_struct_t fds[1]; - fds[0].fd = _actuator_outputs_sub; - fds[0].events = POLLIN; - - _time_last = hrt_absolute_time(); - - while(true) { - // wait for up to 100ms for data - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - //timed out - if (pret == 0) - continue; - - // this is undesirable but not much we can do - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - // sleep a bit before next try - usleep(100000); - continue; - } - - if (fds[0].revents & POLLIN) { - // got new data to read, update all topics - poll_topics(); - send_data(); - } - } -} - -void Simulator::updateSamples() -{ - struct baro_report baro; - memset(&baro,0,sizeof(baro)); - baro.pressure = 120000.0f; - - // acceleration report - struct accel_report accel; - memset(&accel,0,sizeof(accel)); - accel.z = 9.81f; - accel.range_m_s2 = 80.0f; - - // gyro report - struct gyro_report gyro; - memset(&gyro, 0 ,sizeof(gyro)); - - // mag report - struct mag_report mag; - memset(&mag, 0 ,sizeof(mag)); - - // init publishers - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - // subscribe to topics - _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - // try to setup udp socket for communcation with simulator - memset((char *)&_myaddr, 0, sizeof(_myaddr)); - _myaddr.sin_family = AF_INET; - _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); - _myaddr.sin_port = htons(_port); - - if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - PX4_WARN("create socket failed\n"); - return; - } - - if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { - PX4_WARN("bind failed\n"); - return; - } - - // create a thread for sending data to the simulator - pthread_t sender_thread; - - // initialize threads - pthread_attr_t sender_thread_attr; - pthread_attr_init(&sender_thread_attr); - pthread_attr_setstacksize(&sender_thread_attr, 1000); - - struct sched_param param; - (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 30; - (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); - pthread_attr_destroy(&sender_thread_attr); - - struct pollfd socket_fds; - socket_fds.fd = _fd; - socket_fds.events = POLLIN; - - int len = 0; - // wait for new mavlink messages to arrive - while (true) { - - int socket_pret = ::poll(&socket_fds, (size_t)1, 100); - - //timed out - if (socket_pret == 0) - continue; - - // this is undesirable but not much we can do - if (socket_pret < 0) { - PX4_WARN("poll error %d, %d", socket_pret, errno); - // sleep a bit before next try - usleep(100000); - continue; - } - - if (socket_fds.revents & POLLIN) { - len = recvfrom(_fd, _buf, _buflen, 0, (struct sockaddr *)&_srcaddr, &_addrlen); - if (len > 0) { - mavlink_message_t msg; - mavlink_status_t status; - for (int i = 0; i < len; ++i) - { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) - { - // have a message, handle it - handle_message(&msg); - } - } - } - } - - // publish these messages so that attitude estimator does not complain - hrt_abstime time_last = hrt_absolute_time(); - baro.timestamp = time_last; - accel.timestamp = time_last; - gyro.timestamp = time_last; - mag.timestamp = time_last; - // publish the sensor values - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } -} -#endif - static void usage() { PX4_WARN("Usage: simulator {start -[sc] |stop}"); @@ -502,19 +184,7 @@ int simulator_main(int argc, char *argv[]) { int ret = 0; if (argc == 3 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0) { - if (g_sim_task >= 0) { - warnx("Simulator already started"); - return 0; - } - g_sim_task = px4_task_spawn_cmd("Simulator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - Simulator::start, - argv); - } - else if (strcmp(argv[2], "-p") == 0) { + if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) { if (g_sim_task >= 0) { warnx("Simulator already started"); return 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index fe2e466c69..93f5327e83 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -51,8 +51,8 @@ #include #include #include -#include #ifndef __PX4_QURT +#include #include #endif @@ -165,18 +165,18 @@ private: _accel(1), _mpu(1), _baro(1), - _sensor_combined_pub(-1), + _sensor_combined_pub(-1) +#ifndef __PX4_QURT + , _manual_control_sp_pub(-1), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), _sensor{}, _manual_control_sp{}, _actuators{}, - _attitude{}, - _interval(1000) - { - _buf = new unsigned char [_buflen]; - } + _attitude{} +#endif + {} ~Simulator() { _instance=NULL; } #ifndef __PX4_QURT @@ -196,6 +196,12 @@ private: orb_advert_t _gyro_pub; orb_advert_t _mag_pub; orb_advert_t _sensor_combined_pub; + + // class methods + void publishSensorsCombined(); + +#ifndef __PX4_QURT + // uORB publisher handlers orb_advert_t _manual_control_sp_pub; // uORB subscription handlers @@ -208,29 +214,18 @@ private: struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; - // udp socket data - struct sockaddr_in _myaddr; + int _fd; + unsigned char _buf[200]; + hrt_abstime _time_last; struct sockaddr_in _srcaddr; socklen_t _addrlen = sizeof(_srcaddr); - int _fd; - const int _buflen = 200; - const int _port = 14550; - unsigned char *_buf; - hrt_abstime _time_last; - int _interval; - - // class methods - int setup_udp_socket(); - void do_subscriptions(); void poll_topics(); - void publishSensorsCombined(); - void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu); - void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg); void handle_message(mavlink_message_t *msg); void send_data(); - void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void pack_actuator_message(mavlink_message_t *msg); + void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); static void *sending_trampoline(void *); void send(); +#endif }; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp new file mode 100644 index 0000000000..a5aea30cd6 --- /dev/null +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 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. + * + ****************************************************************************/ + +#include +#include +#include "simulator.h" +#include "errno.h" + +using namespace simulator; + +#define SEND_INTERVAL 1000 +#define UDP_PORT 14550 + +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + +void Simulator::pack_actuator_message(mavlink_message_t *msg) { + // pack message and send + mavlink_servo_output_raw_t actuator_msg; + + actuator_msg.time_usec = hrt_absolute_time(); + actuator_msg.port = 8; // hardcoded for now + actuator_msg.servo1_raw = _actuators.output[0]; + actuator_msg.servo2_raw = _actuators.output[1]; + actuator_msg.servo3_raw = _actuators.output[2]; + actuator_msg.servo4_raw = _actuators.output[3]; + actuator_msg.servo5_raw = _actuators.output[4]; + actuator_msg.servo6_raw = _actuators.output[5]; + actuator_msg.servo7_raw = _actuators.output[6]; + actuator_msg.servo8_raw = _actuators.output[7]; + + // encode the message + mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg); +} + +void Simulator::send_data() { + // check if it's time to send new data + hrt_abstime time_now = hrt_absolute_time(); + if (time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { + _time_last = time_now; + mavlink_message_t msg; + pack_actuator_message(&msg); + send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200); + // can add more messages here, can also setup different timings + } +} + +static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) { + manual->timestamp = hrt_absolute_time(); + manual->x = man_msg->x / 1000.0f; + manual->y = man_msg->y / 1000.0f; + manual->r = man_msg->r / 1000.0f; + manual->z = man_msg->z / 1000.0f; +} + +void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu) { + hrt_abstime timestamp = hrt_absolute_time(); + sensor->timestamp = timestamp; + sensor->gyro_raw[0] = imu->xgyro * 1000.0f; + sensor->gyro_raw[1] = imu->ygyro * 1000.0f; + sensor->gyro_raw[2] = imu->zgyro * 1000.0f; + sensor->gyro_rad_s[0] = imu->xgyro; + sensor->gyro_rad_s[1] = imu->ygyro; + sensor->gyro_rad_s[2] = imu->zgyro; + + sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2; + sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2; + sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2; + sensor->accelerometer_m_s2[0] = imu->xacc; + sensor->accelerometer_m_s2[1] = imu->yacc; + sensor->accelerometer_m_s2[2] = imu->zacc; + sensor->accelerometer_mode = 0; // TODO what is this? + sensor->accelerometer_range_m_s2 = 32.7f; // int16 + sensor->accelerometer_timestamp = timestamp; + sensor->timestamp = timestamp; + + sensor->adc_voltage_v[0] = 0.0f; + sensor->adc_voltage_v[1] = 0.0f; + sensor->adc_voltage_v[2] = 0.0f; + + sensor->magnetometer_raw[0] = imu->xmag * 1000.0f; + sensor->magnetometer_raw[1] = imu->ymag * 1000.0f; + sensor->magnetometer_raw[2] = imu->zmag * 1000.0f; + sensor->magnetometer_ga[0] = imu->xmag; + sensor->magnetometer_ga[1] = imu->ymag; + sensor->magnetometer_ga[2] = imu->zmag; + sensor->magnetometer_range_ga = 32.7f; // int16 + sensor->magnetometer_mode = 0; // TODO what is this + sensor->magnetometer_cuttoff_freq_hz = 50.0f; + sensor->magnetometer_timestamp = timestamp; + + sensor->baro_pres_mbar = imu->abs_pressure; + sensor->baro_alt_meter = imu->pressure_alt; + sensor->baro_temp_celcius = imu->temperature; + sensor->baro_timestamp = timestamp; + + sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa + sensor->differential_pressure_timestamp = timestamp; +} + +void Simulator::handle_message(mavlink_message_t *msg) { + switch(msg->msgid) { + case MAVLINK_MSG_ID_HIGHRES_IMU: + mavlink_highres_imu_t imu; + mavlink_msg_highres_imu_decode(msg, &imu); + fill_sensors_from_imu_msg(&_sensor, &imu); + + // publish message + if(_sensor_combined_pub < 0) { + _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); + } else { + orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); + } + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + + mavlink_manual_control_t man_ctrl_sp; + mavlink_msg_manual_control_decode(msg, &man_ctrl_sp); + fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp); + + // publish message + if(_manual_control_sp_pub < 0) { + _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); + } + break; + } +} + +void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { + uint8_t payload_len = mavlink_message_lengths[msgid]; + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* no idea which numbers should be here*/ + buf[2] = 100; + buf[3] = 1; + buf[4] = component_ID; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen); + if (len <= 0) { + PX4_WARN("Failed sending mavlink message"); + } +} + +void Simulator::poll_topics() { + // copy new data if available + bool updated; + orb_check(_actuator_outputs_sub, &updated); + if(updated) { + orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); + } + + orb_check(_vehicle_attitude_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude); + } +} + +void *Simulator::sending_trampoline(void *) { + _instance->send(); + return 0; // why do I have to put this??? +} + +void Simulator::send() { + px4_pollfd_struct_t fds[1]; + fds[0].fd = _actuator_outputs_sub; + fds[0].events = POLLIN; + + _time_last = hrt_absolute_time(); + + while(true) { + // wait for up to 100ms for data + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + //timed out + if (pret == 0) + continue; + + // this is undesirable but not much we can do + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (fds[0].revents & POLLIN) { + // got new data to read, update all topics + poll_topics(); + send_data(); + } + } +} + +void Simulator::updateSamples() +{ + // udp socket data + struct sockaddr_in _myaddr; + const int _port = UDP_PORT; + + struct baro_report baro; + memset(&baro,0,sizeof(baro)); + baro.pressure = 120000.0f; + + // acceleration report + struct accel_report accel; + memset(&accel,0,sizeof(accel)); + accel.z = 9.81f; + accel.range_m_s2 = 80.0f; + + // gyro report + struct gyro_report gyro; + memset(&gyro, 0 ,sizeof(gyro)); + + // mag report + struct mag_report mag; + memset(&mag, 0 ,sizeof(mag)); + + // init publishers + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + // subscribe to topics + _actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); + _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + // try to setup udp socket for communcation with simulator + memset((char *)&_myaddr, 0, sizeof(_myaddr)); + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(_port); + + if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_WARN("create socket failed\n"); + return; + } + + if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_WARN("bind failed\n"); + return; + } + + // create a thread for sending data to the simulator + pthread_t sender_thread; + + // initialize threads + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, 1000); + + struct sched_param param; + (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 30; + (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); + pthread_attr_destroy(&sender_thread_attr); + + struct pollfd socket_fds; + socket_fds.fd = _fd; + socket_fds.events = POLLIN; + + int len = 0; + // wait for new mavlink messages to arrive + while (true) { + + int socket_pret = ::poll(&socket_fds, (size_t)1, 100); + + //timed out + if (socket_pret == 0) + continue; + + // this is undesirable but not much we can do + if (socket_pret < 0) { + PX4_WARN("poll error %d, %d", socket_pret, errno); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (socket_fds.revents & POLLIN) { + len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + if (len > 0) { + mavlink_message_t msg; + mavlink_status_t status; + for (int i = 0; i < len; ++i) + { + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) + { + // have a message, handle it + handle_message(&msg); + } + } + } + } + + // publish these messages so that attitude estimator does not complain + hrt_abstime time_last = hrt_absolute_time(); + baro.timestamp = time_last; + accel.timestamp = time_last; + gyro.timestamp = time_last; + mag.timestamp = time_last; + // publish the sensor values + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } +} From 14cbd240fff0120c5a6f1f4dd74438a6d302b675 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 11 May 2015 19:38:03 -0700 Subject: [PATCH 230/258] QuRT: added qurt_log Implement as a printf for now Signed-off-by: Mark Charlebois --- src/platforms/qurt/px4_layer/px4_qurt_impl.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index f878a8d140..48303ec651 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -130,3 +130,13 @@ size_t strnlen(const char *s, size_t maxlen) return i; } + +void qurt_log(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vprintf(fmt, args); + printf("\n"); +} + From 3db5f3bb3b9eba407e6db198d533cd775cd6ee36 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 12 May 2015 11:37:28 -0700 Subject: [PATCH 231/258] QuRT: toolchain changes Reworking toolchain and main.cpp for QuRT to a final link can be done and the apps.h file is autogenerated. Signed-off-by: Mark Charlebois --- Tools/qurt_apps.py | 9 ++-- makefiles/firmware_qurt.mk | 2 +- makefiles/qurt/config_qurt_default.mk | 50 +++++++++---------- makefiles/qurt_elf.mk | 5 +- makefiles/toolchain_hexagon.mk | 42 ++++++++++++---- .../posix/tests/hrt_test/hrt_test.cpp | 16 +++--- .../tests/hrt_test/hrt_test_start_posix.cpp | 10 ++-- src/platforms/qurt/main.cpp | 16 +++--- src/platforms/qurt/px4_layer/module.mk | 12 ++++- .../qurt/px4_layer/px4_qurt_impl.cpp | 31 ++++++------ 10 files changed, 109 insertions(+), 84 deletions(-) diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py index ef2972de3d..540a564aa5 100755 --- a/Tools/qurt_apps.py +++ b/Tools/qurt_apps.py @@ -44,8 +44,7 @@ print print """ #include #include - -#define __EXPORT +#include #include #include @@ -84,14 +83,14 @@ map apps = app_map(); static void list_builtins(void) { - cout << "Builtin Commands:" << endl; + printf("Builtin Commands:\\n"); for (map::iterator it=apps.begin(); it!=apps.end(); ++it) - cout << '\t' << it->first << endl; + printf("\\t%s\\n", (it->first).c_str()); } static int shutdown_main(int argc, char *argv[]) { - cout << "Shutting down" << endl; + printf("Shutting down\\n"); exit(0); } diff --git a/makefiles/firmware_qurt.mk b/makefiles/firmware_qurt.mk index 6385517214..41128fa60f 100644 --- a/makefiles/firmware_qurt.mk +++ b/makefiles/firmware_qurt.mk @@ -58,6 +58,6 @@ HEXAGON_TOOLS_ROOT = /opt/6.4.05 V_ARCH = v5 HEXAGON_CLANG_BIN = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT)) SIM = $(HEXAGON_CLANG_BIN)/hexagon-sim -SIMFLAGS+= -m$(V_ARCH) --timing +SIMFLAGS+= -m$(V_ARCH) sim: $(SIM) $(SIMFLAGS) Build/qurt_default.build/mainapp diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 2e2323f4f0..7fab719582 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -10,18 +10,18 @@ # # Board support modules # -MODULES += drivers/device -MODULES += drivers/blinkm -MODULES += drivers/hil -MODULES += drivers/led -MODULES += drivers/rgbled -MODULES += modules/sensors +#MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/led +#MODULES += drivers/rgbled +#MODULES += modules/sensors #MODULES += drivers/ms5611 # # System commands # -MODULES += systemcmds/param +#MODULES += systemcmds/param # # General system control @@ -31,8 +31,8 @@ MODULES += systemcmds/param # # Estimation modules (EKF/ SO3 / other filters) # -MODULES += modules/attitude_estimator_ekf -MODULES += modules/ekf_att_pos_estimator +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator # # Vehicle Control @@ -42,37 +42,37 @@ MODULES += modules/ekf_att_pos_estimator # # Library modules # -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/uORB +#MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +#MODULES += modules/uORB #MODULES += modules/dataman #MODULES += modules/sdlog2 -MODULES += modules/simulator -MODULES += modules/commander +#MODULES += modules/simulator +#MODULES += modules/commander # # Libraries # -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/geo -MODULES += lib/geo_lookup -MODULES += lib/conversion +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion # # QuRT port # MODULES += platforms/qurt/px4_layer -MODULES += platforms/posix/drivers/accelsim -MODULES += platforms/posix/drivers/gyrosim -MODULES += platforms/posix/drivers/adcsim -MODULES += platforms/posix/drivers/barosim +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim # # Unit tests # MODULES += platforms/qurt/tests/hello -MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/vcdev_test #MODULES += platforms/posix/tests/hrt_test -MODULES += platforms/posix/tests/wqueue +#MODULES += platforms/posix/tests/wqueue diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 57c6434e1d..0634013451 100644 --- a/makefiles/qurt_elf.mk +++ b/makefiles/qurt_elf.mk @@ -56,10 +56,8 @@ $(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) -MAIN = $(PX4_BASE)/src/platforms/qurt/main.cpp $(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) - $(PX4_BASE)/Tools/qurt_apps.py > apps.h - $(call LINK,$@, -I. $(MAIN) $(PRODUCT_SHARED_LIB)) + $(call LINK,$@, $(PRODUCT_SHARED_LIB)) # # Utility rules @@ -70,4 +68,3 @@ clean: $(MODULE_CLEANS) @$(ECHO) %% cleaning $(Q) $(REMOVE) $(PRODUCT_ELF) $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) - diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 62863a908f..11564918c8 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -38,6 +38,7 @@ # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT = /opt/6.4.05 +HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 #V_ARCH = v4 V_ARCH = v5 CROSSDEV = hexagon- @@ -45,6 +46,9 @@ HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT)) HEXAGON_CLANG_BIN = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT)) HEXAGON_LIB_DIR = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib HEXAGON_ISS_DIR = $(HEXAGON_TOOLS_ROOT)/qc/lib/iss +TOOLSLIB = $(HEXAGON_TOOLS_ROOT)/dinkumware/lib/$(V_ARCH)/G0 +QCTOOLSLIB = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0 +QURTLIB = $(HEXAGON_SDK_ROOT)/lib/common/qurt/ADSP$(V_ARCH)MP/lib CC = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang @@ -56,6 +60,24 @@ NM = $(HEXAGON_BIN)/$(CROSSDEV)nm OBJCOPY = $(HEXAGON_BIN)/$(CROSSDEV)objcopy OBJDUMP = $(HEXAGON_BIN)/$(CROSSDEV)objdump +QURTLIBS = \ + $(TOOLSLIB)/init.o \ + $(QURTLIB)/crt0.o \ + $(TOOLSLIB)/libc.a \ + $(TOOLSLIB)/libqcc.a \ + $(QCTOOLSLIB)/libhexagon.a \ + $(QURTLIB)/libqurt.a \ + $(QURTLIB)/libqurtkernel.a \ + $(QURTLIB)/libqurtcfs.a \ + $(QURTLIB)/libqube_compat.a \ + $(QURTLIB)/libtimer.a \ + $(QURTLIB)/libposix.a \ + $(TOOLSLIB)/libstdc++.a \ + $(QURTLIB)/../examples/cust_config.o \ + $(TOOLSLIB)/fini.o + + + # Check if the right version of the toolchain is available # CROSSDEV_VER_SUPPORTED = 6.4.05 @@ -68,7 +90,7 @@ endif # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup -MAXOPTIMIZATION ?= -Os +MAXOPTIMIZATION ?= -O0 # Base CPU flags for each of the supported architectures. # @@ -91,6 +113,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -I$(PX4_BASE)/../dspal/include \ -I$(PX4_BASE)/../dspal/sys \ -I$(PX4_BASE)/mavlink/include/mavlink \ + -I$(QURTLIB)/..//include \ -Wno-error=shadow # optimisation flags @@ -179,7 +202,7 @@ endif HEXAGON_LIB_PATH = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0 LIB_HEXAGON = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a -EXTRA_LIBS += $(PX4_BASE)../dspal_libs/libdspal.a +#EXTRA_LIBS += $(PX4_BASE)../dspal_libs/libdspal.a # Flags we pass to the assembler # @@ -190,7 +213,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script # Flags we pass to the linker # -LDFLAGS += \ +LDFLAGS += -g -nostdlib --section-start .start=0x1d000000\ $(EXTRALDFLAGS) \ $(addprefix -L,$(LIB_DIRS)) @@ -240,7 +263,7 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - echo $(Q) $(LD) -Ur -o $1 $2 + @echo $(Q) $(LD) -Ur -o $1 $2 $(Q) $(LD) -Ur -o $1 $2 endef @@ -250,8 +273,8 @@ endef define PRELINKF @$(ECHO) "PRELINKF: $1" @$(MKDIR) -p $(dir $1) - echo $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 - $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 + @echo $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 + $(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2 endef # $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 @@ -287,10 +310,7 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - echo $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) - $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(EXTRA_LIBS) - -# $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) - + @echo $(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group + $(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group endef diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index f5291b7dd7..8dd1f4bde3 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -53,7 +53,7 @@ static int update_interval = 1; static void timer_expired(void *arg) { static int i = 0; - printf("Test\n"); + PX4_INFO("Test\n"); if (i < 5) { i++; hrt_call_after(&t1, update_interval, timer_expired, (void *)0); @@ -67,24 +67,24 @@ int HRTTest::main() hrt_abstime t = hrt_absolute_time(); usleep(1000000); hrt_abstime elt = hrt_elapsed_time(&t); - printf("Elapsed time %llu in 1 sec (usleep)\n", (unsigned long long)elt); - printf("Start time %llu\n", (unsigned long long)t); + PX4_INFO("Elapsed time %llu in 1 sec (usleep)\n", (unsigned long long)elt); + PX4_INFO("Start time %llu\n", (unsigned long long)t); t = hrt_absolute_time(); sleep(1); elt = hrt_elapsed_time(&t); - printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); - printf("Start time %llu\n", (unsigned long long)t); + PX4_INFO("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt); + PX4_INFO("Start time %llu\n", (unsigned long long)t); memset(&t1, 0, sizeof(t1)); - printf("HRT_CALL %d\n", hrt_called(&t1)); + PX4_INFO("HRT_CALL %d\n", hrt_called(&t1)); hrt_call_after(&t1, update_interval, timer_expired, (void *)0); sleep(2); - printf("HRT_CALL - %d\n", hrt_called(&t1)); + PX4_INFO("HRT_CALL - %d\n", hrt_called(&t1)); hrt_cancel(&t1); - printf("HRT_CALL + %d\n", hrt_called(&t1)); + PX4_INFO("HRT_CALL + %d\n", hrt_called(&t1)); return 0; } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp index 9d521364a8..241df590ee 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp @@ -49,14 +49,14 @@ extern "C" __EXPORT int hrttest_main(int argc, char *argv[]); int hrttest_main(int argc, char *argv[]) { if (argc < 2) { - printf("usage: hrttest_main {start|stop|status}\n"); + PX4_WARN("usage: hrttest_main {start|stop|status}\n"); return 1; } if (!strcmp(argv[1], "start")) { if (HRTTest::appState.isRunning()) { - printf("already running\n"); + PX4_INFO("already running\n"); /* this is not an error */ return 0; } @@ -78,15 +78,15 @@ int hrttest_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (HRTTest::appState.isRunning()) { - printf("is running\n"); + PX4_INFO("is running\n"); } else { - printf("not started\n"); + PX4_INFO("not started\n"); } return 0; } - printf("usage: hrttest_main {start|stop|status}\n"); + PX4_WARN("usage: hrttest_main {start|stop|status}\n"); return 1; } diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/main.cpp index 58d37da24b..ae90d13e51 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/main.cpp @@ -37,12 +37,9 @@ * @author Mark Charlebois */ -#include -#include #include -#include #include -#include +//#include //using namespace std; @@ -52,7 +49,6 @@ #include "px4_middleware.h" static const char *commands = -"x\n" "hello start" #if 0 "uorb start\n" @@ -79,15 +75,15 @@ static const char *commands = static void run_cmd(const vector &appargs) { // command is appargs[0] string command = appargs[0]; - printf("Looking for %s\n", command.c_str()); + //printf("Looking for %s\n", command.c_str()); if (apps.find(command) != apps.end()) { const char *arg[2+1]; unsigned int i = 0; - printf("size = %d\n", appargs.size()); + //printf("size = %d\n", appargs.size()); while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - printf(" arg = '%s'\n", arg[i]); + //printf(" arg = '%s'\n", arg[i]); ++i; } arg[i] = (char *)0; @@ -97,7 +93,7 @@ static void run_cmd(const vector &appargs) { } else { - cout << "Invalid command" << endl; + //cout << "Invalid command" << endl; list_builtins(); } } @@ -159,8 +155,10 @@ extern void init_once(void); int main(int argc, char **argv) { + printf("In main\n"); px4::init_once(); px4::init(argc, argv, "mainapp"); process_commands(commands); for (;;) {} } + diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 7d377e8bdd..5ee11aa2ad 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -35,6 +35,15 @@ # NuttX / uORB adapter library # +SRCDIR=$(dir $(MODULE_MK)) + +apps.h: $(PX4_BASE)/Tools/qurt_apps.py + $(PX4_BASE)/Tools/qurt_apps.py > $@ + +# Force creation of apps.h +main_.cpp: $(SRCDIR)/../main.cpp apps.h + cp $(SRCDIR)/../main.cpp $@ + SRCS = \ px4_qurt_impl.cpp \ px4_qurt_tasks.cpp \ @@ -49,6 +58,7 @@ SRCS = \ sq_addlast.c \ sq_remfirst.c \ sq_addafter.c \ - dq_rem.c + dq_rem.c \ + main_.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 48303ec651..ad971c08be 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -46,7 +46,9 @@ #include #include #include +#include #include "systemlib/param/param.h" +#include __BEGIN_DECLS @@ -58,6 +60,14 @@ unsigned int sleep(unsigned int sec) { return 0; } extern void hrt_init(void); +void qurt_log(const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + printf(fmt, args); + printf("n"); +} + __END_DECLS extern struct wqueue_s gwork[NWORKERS]; @@ -71,12 +81,7 @@ void init_once(void) { work_queues_init(); hrt_init(); -} - -void init(int argc, char *argv[], const char *app_name) -{ - printf("App name: %s\n", app_name); - + // Create high priority worker thread g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", SCHED_DEFAULT, @@ -95,6 +100,11 @@ void init(int argc, char *argv[], const char *app_name) } +void init(int argc, char *argv[], const char *app_name) +{ + PX4_DEBUG("App name: %s\n", app_name); +} + } /** Retrieve from the data manager store */ @@ -131,12 +141,3 @@ size_t strnlen(const char *s, size_t maxlen) return i; } -void qurt_log(const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - vprintf(fmt, args); - printf("\n"); -} - From 3a79679e2de1060fe4f2b360b1f1a862f063df47 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 12 May 2015 23:15:58 +0200 Subject: [PATCH 232/258] get manual control setpoint from PIXHAWK --- src/modules/simulator/simulator_mavlink.cpp | 54 ++++++++++++++++++--- 1 file changed, 46 insertions(+), 8 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a5aea30cd6..7061e46505 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -40,6 +40,7 @@ using namespace simulator; #define SEND_INTERVAL 1000 #define UDP_PORT 14550 +#define PIXHAWK_DEVICE "/dev/ttyACM0" static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; @@ -308,29 +309,49 @@ void Simulator::updateSamples() pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); - struct pollfd socket_fds; - socket_fds.fd = _fd; - socket_fds.events = POLLIN; + // setup serial connection to autopilot (used to get manual controls) + int serial_fd = open(PIXHAWK_DEVICE, O_RDWR); + + if (serial_fd < 0) { + PX4_WARN("failed to open %s\n", PIXHAWK_DEVICE); + } + + // tell the device to stream some messages + char command[] = "\nsh /etc/init.d/rc.usb\n"; + int w = ::write(serial_fd, command, sizeof(command)); + + if (w <= 0) { + PX4_WARN("failed to send streaming command to %s\n", PIXHAWK_DEVICE); + } + + char serial_buf[1024]; + + struct pollfd fds[2]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + fds[1].fd = serial_fd; + fds[1].events = POLLIN; int len = 0; // wait for new mavlink messages to arrive while (true) { - int socket_pret = ::poll(&socket_fds, (size_t)1, 100); + int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100); //timed out - if (socket_pret == 0) + if (pret == 0) continue; // this is undesirable but not much we can do - if (socket_pret < 0) { - PX4_WARN("poll error %d, %d", socket_pret, errno); + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); // sleep a bit before next try usleep(100000); continue; } - if (socket_fds.revents & POLLIN) { + // got data from simulator + if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); if (len > 0) { mavlink_message_t msg; @@ -346,6 +367,23 @@ void Simulator::updateSamples() } } + // got data from PIXHAWK + if (fds[1].revents & POLLIN) { + len = ::read(serial_fd, serial_buf, sizeof(serial_buf)); + if (len > 0) { + mavlink_message_t msg; + mavlink_status_t status; + for (int i = 0; i < len; ++i) + { + if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) + { + // have a message, handle it + handle_message(&msg); + } + } + } + } + // publish these messages so that attitude estimator does not complain hrt_abstime time_last = hrt_absolute_time(); baro.timestamp = time_last; From 9686f8004ebe7a9e9004a3eb047a44665218be24 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 12 May 2015 23:55:32 +0200 Subject: [PATCH 233/258] send pwm outputs to simulator --- src/modules/simulator/simulator_mavlink.cpp | 53 +++++++++++++++------ 1 file changed, 39 insertions(+), 14 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 7061e46505..42c7ab6bc6 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -35,10 +35,11 @@ #include #include "simulator.h" #include "errno.h" +#include using namespace simulator; -#define SEND_INTERVAL 1000 +#define SEND_INTERVAL 20 #define UDP_PORT 14550 #define PIXHAWK_DEVICE "/dev/ttyACM0" @@ -46,22 +47,46 @@ static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; void Simulator::pack_actuator_message(mavlink_message_t *msg) { - // pack message and send - mavlink_servo_output_raw_t actuator_msg; + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + // for now we only support quadrotors + unsigned n = 4; + + for (unsigned i = 0; i < 8; i++) { + if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { + if (i < n) { + // scale PWM out 900..2100 us to 0..1 for rotors */ + out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + // scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + } + + } else { + // send 0 when disarmed and for disabled channels */ + out[i] = 0.0f; + } + } + + mavlink_hil_controls_t actuator_msg; actuator_msg.time_usec = hrt_absolute_time(); - actuator_msg.port = 8; // hardcoded for now - actuator_msg.servo1_raw = _actuators.output[0]; - actuator_msg.servo2_raw = _actuators.output[1]; - actuator_msg.servo3_raw = _actuators.output[2]; - actuator_msg.servo4_raw = _actuators.output[3]; - actuator_msg.servo5_raw = _actuators.output[4]; - actuator_msg.servo6_raw = _actuators.output[5]; - actuator_msg.servo7_raw = _actuators.output[6]; - actuator_msg.servo8_raw = _actuators.output[7]; + actuator_msg.roll_ailerons = out[0]; + actuator_msg.pitch_elevator = out[1]; + actuator_msg.yaw_rudder = out[2]; + actuator_msg.throttle = out[3]; + actuator_msg.aux1 = out[4]; + actuator_msg.aux2 = out[5]; + actuator_msg.aux3 = out[6]; + actuator_msg.aux4 = out[7]; + actuator_msg.mode = 0; // need to put something here + actuator_msg.nav_mode = 0; // encode the message - mavlink_msg_servo_output_raw_encode(1, 100, msg, &actuator_msg); + mavlink_msg_hil_controls_encode(1, MAVLINK_MSG_ID_HIL_CONTROLS, msg, &actuator_msg); } void Simulator::send_data() { @@ -71,7 +96,7 @@ void Simulator::send_data() { _time_last = time_now; mavlink_message_t msg; pack_actuator_message(&msg); - send_mavlink_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg, 200); + send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); // can add more messages here, can also setup different timings } } From cf27fc59c7115948e18d62ee46c728a8643dd170 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 13 May 2015 10:31:52 +0200 Subject: [PATCH 234/258] read hil sensor message instead of highres imu message --- src/modules/simulator/simulator_mavlink.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 42c7ab6bc6..d468dd6661 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -109,7 +109,7 @@ static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, manual->z = man_msg->z / 1000.0f; } -void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres_imu_t *imu) { +void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { hrt_abstime timestamp = hrt_absolute_time(); sensor->timestamp = timestamp; sensor->gyro_raw[0] = imu->xgyro * 1000.0f; @@ -156,9 +156,9 @@ void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_highres void Simulator::handle_message(mavlink_message_t *msg) { switch(msg->msgid) { - case MAVLINK_MSG_ID_HIGHRES_IMU: - mavlink_highres_imu_t imu; - mavlink_msg_highres_imu_decode(msg, &imu); + case MAVLINK_MSG_ID_HIL_SENSOR: + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); fill_sensors_from_imu_msg(&_sensor, &imu); // publish message From 884f62878d5222cf235b94b8b4792aa6c26031c8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 13 May 2015 16:21:52 -0700 Subject: [PATCH 235/258] QuRT: pthread API now working The use of std::map and static initialization was an issue. The code was refactored to not use static initialization. Signed-off-by: Mark Charlebois --- Tools/posix_apps.py | 2 +- Tools/qurt_apps.py | 33 +++- makefiles/qurt/config_qurt_default.mk | 6 +- makefiles/qurt_elf.mk | 10 +- makefiles/toolchain_hexagon.mk | 5 - src/platforms/px4_log.h | 1 + src/platforms/qurt/{ => px4_layer}/main.cpp | 37 ++-- src/platforms/qurt/px4_layer/module.mk | 9 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 49 +++-- .../qurt/px4_layer/px4_qurt_tasks.cpp | 174 ++++++++++++------ 10 files changed, 208 insertions(+), 118 deletions(-) rename src/platforms/qurt/{ => px4_layer}/main.cpp (85%) diff --git a/Tools/posix_apps.py b/Tools/posix_apps.py index 375e71fc2d..00e11fcbbb 100755 --- a/Tools/posix_apps.py +++ b/Tools/posix_apps.py @@ -71,7 +71,7 @@ static map app_map(void); static map app_map(void) { - map apps; + static map apps; """ for app in apps: print '\tapps["'+app+'"] = '+app+'_main;' diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py index 540a564aa5..e5d75344be 100755 --- a/Tools/qurt_apps.py +++ b/Tools/qurt_apps.py @@ -61,27 +61,28 @@ for app in apps: print """ static int shutdown_main(int argc, char *argv[]); static int list_tasks_main(int argc, char *argv[]); +static int list_files_main(int argc, char *argv[]); +static int list_devices_main(int argc, char *argv[]); +static int list_topics_main(int argc, char *argv[]); } -static map app_map(void); - -static map app_map(void) +void init_app_map(map &apps) { - map apps; """ for app in apps: print '\tapps["'+app+'"] = '+app+'_main;' print '\tapps["shutdown"] = shutdown_main;' print '\tapps["list_tasks"] = list_tasks_main;' +print '\tapps["list_files"] = list_files_main;' +print '\tapps["list_devices"] = list_devices_main;' +print '\tapps["list_topics"] = list_topics_main;' + print """ - return apps; } -map apps = app_map(); - -static void list_builtins(void) +void list_builtins(map &apps) { printf("Builtin Commands:\\n"); for (map::iterator it=apps.begin(); it!=apps.end(); ++it) @@ -100,5 +101,21 @@ static int list_tasks_main(int argc, char *argv[]) return 0; } +static int list_devices_main(int argc, char *argv[]) +{ + px4_show_devices(); + return 0; +} + +static int list_topics_main(int argc, char *argv[]) +{ + px4_show_topics(); + return 0; +} +static int list_files_main(int argc, char *argv[]) +{ + px4_show_files(); + return 0; +} """ diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 7fab719582..5e30743576 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -10,7 +10,7 @@ # # Board support modules # -#MODULES += drivers/device +MODULES += drivers/device #MODULES += drivers/blinkm #MODULES += drivers/hil #MODULES += drivers/led @@ -42,9 +42,9 @@ # # Library modules # -#MODULES += modules/systemlib +MODULES += modules/systemlib #MODULES += modules/systemlib/mixer -#MODULES += modules/uORB +MODULES += modules/uORB #MODULES += modules/dataman #MODULES += modules/sdlog2 #MODULES += modules/simulator diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 0634013451..95a3fc315a 100644 --- a/makefiles/qurt_elf.mk +++ b/makefiles/qurt_elf.mk @@ -56,8 +56,14 @@ $(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) -$(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) - $(call LINK,$@, $(PRODUCT_SHARED_LIB)) +$(WORK_DIR)apps.cpp: $(PX4_BASE)/Tools/qurt_apps.py + $(PX4_BASE)/Tools/qurt_apps.py > $@ + +$(WORK_DIR)apps.o: $(WORK_DIR)apps.cpp + $(call COMPILEXX,$<, $@) + +$(WORK_DIR)mainapp: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) + $(call LINK,$@, $^) # # Utility rules diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 11564918c8..42144e9177 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -199,11 +199,6 @@ ifeq (1,$(V_dynamic)) CXX_FLAGS += -fpic -D__V_DYNAMIC__ endif -HEXAGON_LIB_PATH = $(HEXAGON_TOOLS_ROOT)/gnu/hexagon/lib/$(V_ARCH)/G0 -LIB_HEXAGON = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0/libhexagon.a - -#EXTRA_LIBS += $(PX4_BASE)../dspal_libs/libdspal.a - # Flags we pass to the assembler # AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 6fd8387326..7d6f602083 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -44,6 +44,7 @@ __BEGIN_DECLS extern void qurt_log(const char *fmt, ...); __END_DECLS +#define qurt_log(...) {printf(__VA_ARGS__); printf("\n");} #define PX4_DBG(...) qurt_log(__VA_ARGS__) #define PX4_DEBUG(...) qurt_log(__VA_ARGS__) #define PX4_INFO(...) qurt_log(__VA_ARGS__) diff --git a/src/platforms/qurt/main.cpp b/src/platforms/qurt/px4_layer/main.cpp similarity index 85% rename from src/platforms/qurt/main.cpp rename to src/platforms/qurt/px4_layer/main.cpp index ae90d13e51..6a4493a292 100644 --- a/src/platforms/qurt/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -37,16 +37,18 @@ * @author Mark Charlebois */ -#include +#include +#include +#include #include -//#include +#include +#include +#include -//using namespace std; +using namespace std; -//typedef int (*px4_main_t)(int argc, char *argv[]); - -#include "apps.h" -#include "px4_middleware.h" +extern void init_app_map(map &apps); +extern void list_builtins(map &apps); static const char *commands = "hello start" @@ -71,30 +73,24 @@ static const char *commands = #endif ; - -static void run_cmd(const vector &appargs) { +static void run_cmd(map &apps, const vector &appargs) { // command is appargs[0] string command = appargs[0]; - //printf("Looking for %s\n", command.c_str()); if (apps.find(command) != apps.end()) { const char *arg[2+1]; unsigned int i = 0; - //printf("size = %d\n", appargs.size()); while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - //printf(" arg = '%s'\n", arg[i]); + printf(" arg = '%s'\n", arg[i]); ++i; } arg[i] = (char *)0; - //printf("BEFORE argc = %d %s %s %p\n", i, arg[0], arg[1], arg[2]); apps[command](i,(char **)arg); - //printf("AFTER argc = %d %s %s %p\n", i, arg[0], arg[1], arg[2]); } else { - //cout << "Invalid command" << endl; - list_builtins(); + list_builtins(apps); } } @@ -106,7 +102,7 @@ void eat_whitespace(const char *&b, int &i) i=0; } -static void process_commands(const char *cmds) +static void process_commands(map &apps, const char *cmds) { vector appargs; int i=0; @@ -126,7 +122,7 @@ static void process_commands(const char *cmds) // If we have a command to run if (appargs.size() > 0) { - run_cmd(appargs); + run_cmd(apps, appargs); } appargs.clear(); if (b[i] == '\n') { @@ -145,6 +141,7 @@ static void process_commands(const char *cmds) eat_whitespace(b, ++i); continue; } + printf("ch %c\n", b[i]); ++i; } } @@ -156,9 +153,11 @@ extern void init_once(void); int main(int argc, char **argv) { printf("In main\n"); + map apps; + init_app_map(apps); px4::init_once(); px4::init(argc, argv, "mainapp"); - process_commands(commands); + process_commands(apps, commands); for (;;) {} } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 5ee11aa2ad..92f1c6abfb 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -37,13 +37,6 @@ SRCDIR=$(dir $(MODULE_MK)) -apps.h: $(PX4_BASE)/Tools/qurt_apps.py - $(PX4_BASE)/Tools/qurt_apps.py > $@ - -# Force creation of apps.h -main_.cpp: $(SRCDIR)/../main.cpp apps.h - cp $(SRCDIR)/../main.cpp $@ - SRCS = \ px4_qurt_impl.cpp \ px4_qurt_tasks.cpp \ @@ -59,6 +52,6 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main_.cpp + main.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index ad971c08be..3ff09b3d4f 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -47,19 +48,28 @@ #include #include #include +#include #include "systemlib/param/param.h" -#include __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported long PX4_TICKS_PER_SEC = 1000; -void usleep(useconds_t usec) {} -unsigned int sleep(unsigned int sec) { return 0; } +void usleep(useconds_t usec) { + qurt_timer_sleep(usec); +} + +unsigned int sleep(unsigned int sec) +{ + for (unsigned int i=0; i< sec; i++) + qurt_timer_sleep(1000000); + return 0; +} extern void hrt_init(void); +#if 0 void qurt_log(const char *fmt, ...) { va_list args; @@ -67,11 +77,15 @@ void qurt_log(const char *fmt, ...) printf(fmt, args); printf("n"); } +#endif + +extern int _posix_init(void); __END_DECLS extern struct wqueue_s gwork[NWORKERS]; + namespace px4 { @@ -79,25 +93,11 @@ void init_once(void); void init_once(void) { + // Required for QuRT + _posix_init(); + work_queues_init(); hrt_init(); - - // Create high priority worker thread - g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 2000, - work_hpthread, - (char* const*)NULL); - - // Create low priority worker thread - g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low", - SCHED_DEFAULT, - SCHED_PRIORITY_MIN, - 2000, - work_lpthread, - (char* const*)NULL); - } void init(int argc, char *argv[], const char *app_name) @@ -141,3 +141,12 @@ size_t strnlen(const char *s, size_t maxlen) return i; } +int ioctl(int a, int b, unsigned long c) +{ + return -1; +} + +int write(int a, char const* b, int c) +{ + return -1; +} diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index e5d2297e9a..95eea3191b 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -33,10 +33,11 @@ ****************************************************************************/ /** - * @file px4_linux_tasks.c + * @file px4_posix_tasks.c * Implementation of existing task API for Linux */ +#include #include #include #include @@ -53,20 +54,16 @@ #include #include -#include -#include #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 5 - +#define PX4_MAX_TASKS 100 struct task_entry { - int pid; + pthread_t pid; std::string name; bool isused; task_entry() : isused(false) {} - void *sp; }; static task_entry taskmap[PX4_MAX_TASKS]; @@ -75,31 +72,32 @@ typedef struct { px4_main_t entry; int argc; - char * argv[]; - // strings are allocated after + char *argv[]; + // strings are allocated after the } pthdata_t; -static void entry_adapter ( void *ptr ) +static void *entry_adapter ( void *ptr ) { - pthdata_t *data = (pthdata_t *) ptr; - PX4_DEBUG("entry_adapter %p %p entry %p %d %p\n", ptr, data, data->entry, data->argc, data->argv[0]); + pthdata_t *data; + data = (pthdata_t *) ptr; - PX4_DEBUG("data->entry = %p\n", data->entry); + PX4_DBG("entry_adapter"); data->entry(data->argc, data->argv); free(ptr); - PX4_DEBUG("after entry\n"); - PX4_DEBUG("Before px4_task_exit\n"); + PX4_DBG("Before px4_task_exit"); px4_task_exit(0); - PX4_DEBUG("After px4_task_exit\n"); + PX4_DBG("After px4_task_exit"); + + return NULL; } void px4_systemreset(bool to_bootloader) { - PX4_DEBUG("Called px4_system_reset\n"); + PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const *argv) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) { int rv; int argc = 0; @@ -109,7 +107,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; - PX4_DEBUG("px4_task_spawn_cmd entry = %p %p %s\n", entry, argv, argv[0]); + printf("Creating %s\n", name); + pthread_t task; + pthread_attr_t attr; + struct sched_param param; + // Calculate argc while (p != (char *)0) { p = argv[argc]; @@ -118,24 +120,18 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int ++argc; len += strlen(p)+1; } - PX4_DEBUG("arg %d %p\n", argc, argv); structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); pthdata_t *taskdata; - PX4_DEBUG("arg %d %p\n", argc, argv); // not safe to pass stack data to the thread creation taskdata = (pthdata_t *)malloc(structsize+len); - PX4_DEBUG("arg %d %p\n", argc, argv); offset = ((unsigned long)taskdata)+structsize; - PX4_DEBUG("arg %d %p\n", argc, argv); taskdata->entry = entry; taskdata->argc = argc; - PX4_DEBUG("arg %d %p\n", argc, argv); for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); offset+=strlen(argv[i])+1; @@ -143,38 +139,123 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; + rv = pthread_attr_init(&attr); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); + return (rv < 0) ? rv : -rv; + } +#if 0 + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); + return (rv < 0) ? rv : -rv; + } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); + return (rv < 0) ? rv : -rv; + } +#endif + + param.sched_priority = priority; + + rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { + PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); + return (rv < 0) ? rv : -rv; + } + + rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { + + if (rv == EPERM) { + //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); + rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { + PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); + return (rv < 0) ? rv : -rv; + } + } + else { + return (rv < 0) ? rv : -rv; + } + } + for (i=0; ientry, taskdata->argc, taskdata->argv[0]); - thread_create(entry_adapter, taskmap[i].sp, i+1, (void *) taskdata); - - return i+1; + if (i>=PX4_MAX_TASKS) { + return -ENOSPC; + } + return i; } int px4_task_delete(px4_task_t id) { - PX4_DEBUG("Called px4_task_delete\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_WARN("Called px4_task_delete"); + + if (id < PX4_MAX_TASKS && taskmap[id].isused) + pid = taskmap[id].pid; + else + return -EINVAL; + + // If current thread then exit, otherwise cancel + if (pthread_self() == pid) { + taskmap[id].isused = false; + pthread_exit(0); + } else { + rv = pthread_cancel(pid); + } + + taskmap[id].isused = false; + + return rv; } void px4_task_exit(int ret) { - thread_stop(); + int i; + pthread_t pid = pthread_self(); - // Free stack + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) { + PX4_ERR("px4_task_exit: self task not found!"); + } + else { + PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); + } + + //pthread_exit((void *)(unsigned long)ret); } int px4_task_kill(px4_task_t id, int sig) { - PX4_DEBUG("Called px4_task_kill\n"); - return -EINVAL; + int rv = 0; + pthread_t pid; + PX4_DBG("Called px4_task_kill %d", sig); + + if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + pid = taskmap[id].pid; + else + return -EINVAL; + + // If current thread then exit, otherwise cancel + rv = pthread_kill(pid, sig); + + return rv; } void px4_show_tasks() @@ -182,26 +263,15 @@ void px4_show_tasks() int idx; int count = 0; - PX4_DEBUG("Active Tasks:\n"); + PX4_INFO("Active Tasks:"); for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_DEBUG(" %-10s %d\n", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s %u", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } if (count == 0) - PX4_DEBUG(" No running tasks\n"); + PX4_INFO(" No running tasks"); } - -// STUBS - -extern "C" { -void hrt_sleep(unsigned long) -{ -} - -} -int ioctl(int d, int request, unsigned long foo) { return 0; } -int write(int a, char const*b, int c) { return c; } From 8e346a06fb2a7c95e63d8295c00e83d5049cc333 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 13 May 2015 18:03:08 -0700 Subject: [PATCH 236/258] QuRT: enable uORB, and simulator uORB, the simulator and simulated devices now run Signed-off-by: Mark Charlebois --- makefiles/qurt/config_qurt_default.mk | 24 +++++++++---------- makefiles/qurt_elf.mk | 3 ++- .../tests/hrt_test/hrt_test_start_posix.cpp | 1 + .../tests/vcdev_test/vcdevtest_example.cpp | 12 ++++++---- src/platforms/qurt/px4_layer/main.cpp | 12 +++++----- .../qurt/px4_layer/px4_qurt_tasks.cpp | 3 --- .../qurt/tests/hello/hello_example.cpp | 2 +- src/platforms/qurt/tests/hello/hello_main.cpp | 4 ++-- .../qurt/tests/hello/hello_start_qurt.cpp | 10 ++++---- 9 files changed, 36 insertions(+), 35 deletions(-) diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 5e30743576..5d7c0ccaf9 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -13,7 +13,7 @@ MODULES += drivers/device #MODULES += drivers/blinkm #MODULES += drivers/hil -#MODULES += drivers/led +MODULES += drivers/led #MODULES += drivers/rgbled #MODULES += modules/sensors #MODULES += drivers/ms5611 @@ -47,32 +47,32 @@ MODULES += modules/systemlib MODULES += modules/uORB #MODULES += modules/dataman #MODULES += modules/sdlog2 -#MODULES += modules/simulator +MODULES += modules/simulator #MODULES += modules/commander # # Libraries # -#MODULES += lib/mathlib -#MODULES += lib/mathlib/math/filter +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter #MODULES += lib/geo #MODULES += lib/geo_lookup -#MODULES += lib/conversion +MODULES += lib/conversion # # QuRT port # MODULES += platforms/qurt/px4_layer -#MODULES += platforms/posix/drivers/accelsim -#MODULES += platforms/posix/drivers/gyrosim -#MODULES += platforms/posix/drivers/adcsim -#MODULES += platforms/posix/drivers/barosim +MODULES += platforms/posix/drivers/accelsim +MODULES += platforms/posix/drivers/gyrosim +MODULES += platforms/posix/drivers/adcsim +MODULES += platforms/posix/drivers/barosim # # Unit tests # MODULES += platforms/qurt/tests/hello -#MODULES += platforms/posix/tests/vcdev_test -#MODULES += platforms/posix/tests/hrt_test -#MODULES += platforms/posix/tests/wqueue +MODULES += platforms/posix/tests/vcdev_test +MODULES += platforms/posix/tests/hrt_test +MODULES += platforms/posix/tests/wqueue diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 95a3fc315a..53cc8d8287 100644 --- a/makefiles/qurt_elf.mk +++ b/makefiles/qurt_elf.mk @@ -56,11 +56,12 @@ $(PRODUCT_SHARED_PRELINK): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(PRODUCT_SHARED_LIB): $(PRODUCT_SHARED_PRELINK) $(call LINK_A,$@,$(PRODUCT_SHARED_PRELINK)) -$(WORK_DIR)apps.cpp: $(PX4_BASE)/Tools/qurt_apps.py +$(WORK_DIR)apps.cpp: $(PX4_BASE)/Tools/qurt_apps.py $(PX4_BASE)/Tools/qurt_apps.py > $@ $(WORK_DIR)apps.o: $(WORK_DIR)apps.cpp $(call COMPILEXX,$<, $@) + mv $(WORK_DIR)apps.cpp $(WORK_DIR)apps.cpp_sav $(WORK_DIR)mainapp: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) $(call LINK,$@, $^) diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp index 241df590ee..2544804496 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp @@ -37,6 +37,7 @@ * @author Mark Charlebois */ #include "hrt_test.h" +#include #include #include #include diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index 0721ae035a..dc3111a40a 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -190,16 +190,20 @@ int VCDevExample::main() px4_close(fd); } else if (i > 0) { - if (ret == 0) + if (ret == 0) { PX4_INFO("==== Nothing to read - PASS\n"); - else + } + else { PX4_INFO("==== poll returned %d\n", ret); + } } else if (i == 0) { - if (ret == 1) + if (ret == 1) { PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); - else + } + else { PX4_INFO("==== %d to read - FAIL\n", ret); + } } ++i; diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 6a4493a292..f2a957499b 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -51,14 +51,17 @@ extern void init_app_map(map &apps); extern void list_builtins(map &apps); static const char *commands = -"hello start" -#if 0 +"hello start\n" "uorb start\n" "simulator start -s\n" "barosim start\n" "adcsim start\n" "accelsim start\n" "gyrosim start\n" +"list_devices\n" +"list_topics\n" +"list_tasks" +#if 0 "param set CAL_GYRO0_ID 2293760\n" "param set CAL_ACC0_ID 1310720\n" "param set CAL_ACC1_ID 1376256\n" @@ -68,8 +71,6 @@ static const char *commands = "sensors start\n" "hil mode_pwm\n" "commander start\n" -"list_devices\n" -"list_topics\n" #endif ; @@ -82,7 +83,7 @@ static void run_cmd(map &apps, const vector &appargs) unsigned int i = 0; while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); - printf(" arg = '%s'\n", arg[i]); + //printf(" arg = '%s'\n", arg[i]); ++i; } arg[i] = (char *)0; @@ -141,7 +142,6 @@ static void process_commands(map &apps, const char *cmds) eat_whitespace(b, ++i); continue; } - printf("ch %c\n", b[i]); ++i; } } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 95eea3191b..7490f1528c 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -81,12 +81,9 @@ static void *entry_adapter ( void *ptr ) pthdata_t *data; data = (pthdata_t *) ptr; - PX4_DBG("entry_adapter"); data->entry(data->argc, data->argv); free(ptr); - PX4_DBG("Before px4_task_exit"); px4_task_exit(0); - PX4_DBG("After px4_task_exit"); return NULL; } diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp index d0a4de0dc6..1cfcf463e1 100644 --- a/src/platforms/qurt/tests/hello/hello_example.cpp +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -52,7 +52,7 @@ int HelloExample::main() int i=0; while (!appState.exitRequested() && i<5) { - PX4_DEBUG(" Doing work...\n"); + PX4_DEBUG(" Doing work..."); ++i; } diff --git a/src/platforms/qurt/tests/hello/hello_main.cpp b/src/platforms/qurt/tests/hello/hello_main.cpp index d18c016753..35e72e8b36 100644 --- a/src/platforms/qurt/tests/hello/hello_main.cpp +++ b/src/platforms/qurt/tests/hello/hello_main.cpp @@ -47,10 +47,10 @@ int PX4_MAIN(int argc, char **argv) { px4::init(argc, argv, "hello"); - PX4_DEBUG("hello\n"); + PX4_DEBUG("hello"); HelloExample hello; hello.main(); - PX4_DEBUG("goodbye\n"); + PX4_DEBUG("goodbye"); return 0; } diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index 7c935e1ab4..e4180307ce 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -53,11 +53,10 @@ extern "C" __EXPORT int hello_main(int argc, char *argv[]); static void usage() { - PX4_DEBUG("usage: hello {start|stop|status}\n"); + PX4_DEBUG("usage: hello {start|stop|status}"); } int hello_main(int argc, char *argv[]) { - PX4_DEBUG("argc = %d %s %s %p\n", argc, argv[0], argv[1], argv[2]); if (argc < 2) { usage(); return 1; @@ -65,9 +64,8 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - PX4_DEBUG("Starting\n"); if (HelloExample::appState.isRunning()) { - PX4_DEBUG("already running\n"); + PX4_DEBUG("already running"); /* this is not an error */ return 0; } @@ -89,10 +87,10 @@ int hello_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (HelloExample::appState.isRunning()) { - PX4_DEBUG("is running\n"); + PX4_DEBUG("is running"); } else { - PX4_DEBUG("not started\n"); + PX4_DEBUG("not started"); } return 0; From b2c12ff522b28bf662a5f49d7c19f73baec596d8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 13 May 2015 18:49:00 -0700 Subject: [PATCH 237/258] QuRT: added stub for inclusion of libdspal.a libdspal.a is still incomplete and so is not yet used Signed-off-by: Mark Charlebois --- makefiles/toolchain_hexagon.mk | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 42144e9177..a220baa594 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -49,6 +49,7 @@ HEXAGON_ISS_DIR = $(HEXAGON_TOOLS_ROOT)/qc/lib/iss TOOLSLIB = $(HEXAGON_TOOLS_ROOT)/dinkumware/lib/$(V_ARCH)/G0 QCTOOLSLIB = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0 QURTLIB = $(HEXAGON_SDK_ROOT)/lib/common/qurt/ADSP$(V_ARCH)MP/lib +#DSPAL = $(PX4_BASE)/../dspal_libs/libdspal.a CC = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang @@ -62,22 +63,23 @@ OBJDUMP = $(HEXAGON_BIN)/$(CROSSDEV)objdump QURTLIBS = \ $(TOOLSLIB)/init.o \ - $(QURTLIB)/crt0.o \ $(TOOLSLIB)/libc.a \ $(TOOLSLIB)/libqcc.a \ - $(QCTOOLSLIB)/libhexagon.a \ + $(TOOLSLIB)/libstdc++.a \ + $(QURTLIB)/crt0.o \ $(QURTLIB)/libqurt.a \ $(QURTLIB)/libqurtkernel.a \ $(QURTLIB)/libqurtcfs.a \ $(QURTLIB)/libqube_compat.a \ $(QURTLIB)/libtimer.a \ $(QURTLIB)/libposix.a \ - $(TOOLSLIB)/libstdc++.a \ $(QURTLIB)/../examples/cust_config.o \ + $(QCTOOLSLIB)/libhexagon.a \ $(TOOLSLIB)/fini.o + # Check if the right version of the toolchain is available # CROSSDEV_VER_SUPPORTED = 6.4.05 From f6bf6c89ff6ab670c99bfd2e1123f2a8846469ca Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 14 May 2015 14:58:23 +0200 Subject: [PATCH 238/258] ported mc_pos_controller --- .../mc_pos_control/mc_pos_control_main.cpp | 51 +++++++++++-------- 1 file changed, 29 insertions(+), 22 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fe6db135f8..11784037bb 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -49,9 +49,11 @@ * @author Anton Babushkin */ -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -59,6 +61,7 @@ #include #include #include +#include #include #include @@ -384,7 +387,7 @@ MulticopterPositionControl::~MulticopterPositionControl() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -761,9 +764,9 @@ void MulticopterPositionControl::control_auto(float dt) orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); //Make sure that the position setpoint is valid - if (!isfinite(_pos_sp_triplet.current.lat) || - !isfinite(_pos_sp_triplet.current.lon) || - !isfinite(_pos_sp_triplet.current.alt)) { + if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || + !PX4_ISFINITE(_pos_sp_triplet.current.lon) || + !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } } @@ -881,7 +884,7 @@ void MulticopterPositionControl::control_auto(float dt) _pos_sp = pos_sp_s.edivide(scale); /* update yaw setpoint if needed */ - if (isfinite(_pos_sp_triplet.current.yaw)) { + if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } @@ -933,14 +936,14 @@ MulticopterPositionControl::task_main() R.identity(); /* wakeup source */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { @@ -1410,11 +1413,9 @@ MulticopterPositionControl::task_main() reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; } - warnx("stopped"); mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; - _exit(0); } int @@ -1427,7 +1428,7 @@ MulticopterPositionControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, - (main_t)&MulticopterPositionControl::task_main_trampoline, + (px4_main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1441,46 +1442,52 @@ MulticopterPositionControl::start() int mc_pos_control_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: mc_pos_control {start|stop|status}"); + warnx("usage: mc_pos_control {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { if (pos_control::g_control != nullptr) { - errx(1, "already running"); + warnx("already running"); + return 1; } pos_control::g_control = new MulticopterPositionControl; if (pos_control::g_control == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != pos_control::g_control->start()) { delete pos_control::g_control; pos_control::g_control = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (pos_control::g_control == nullptr) { - errx(1, "not running"); + warnx("not running"); } delete pos_control::g_control; pos_control::g_control = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (pos_control::g_control) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } From cfa0073c351c8ed2000f0d627e70d0b4748c8f23 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 14 May 2015 14:58:46 +0200 Subject: [PATCH 239/258] build mc_pos_control --- makefiles/posix/config_posix_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index d5c8d5be55..78b40d3e87 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -34,6 +34,7 @@ MODULES += modules/ekf_att_pos_estimator # # Vehicle Control # +MODULES += modules/mc_pos_control MODULES += modules/mc_att_control # From abe61a3d7e8fc680397c910987179f44b1e0dde4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 14 May 2015 08:32:51 -0700 Subject: [PATCH 240/258] Added missing return on error When mc_pos_control_main.cpp was ported to posix one error condition retuned 0 instead of 1. Signed-off-by: Mark Charlebois --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 11784037bb..14d860f29e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1473,6 +1473,7 @@ int mc_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (pos_control::g_control == nullptr) { warnx("not running"); + return 1; } delete pos_control::g_control; From a3a0d0612cf9e63b8b3c180b09e307334cce2609 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 14 May 2015 09:45:03 -0700 Subject: [PATCH 241/258] QuRT: enabled more modules rgbled is now enabled. Saving parameters causes a crash so those commands are not enabled. Signed-off-by: Mark Charlebois --- makefiles/qurt/config_qurt_default.mk | 10 +++++----- src/platforms/qurt/px4_layer/main.cpp | 9 +++++---- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 5d7c0ccaf9..8285ef28a8 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -11,17 +11,17 @@ # Board support modules # MODULES += drivers/device -#MODULES += drivers/blinkm -#MODULES += drivers/hil +MODULES += drivers/blinkm +MODULES += drivers/hil MODULES += drivers/led -#MODULES += drivers/rgbled +MODULES += drivers/rgbled #MODULES += modules/sensors #MODULES += drivers/ms5611 # # System commands # -#MODULES += systemcmds/param +MODULES += systemcmds/param # # General system control @@ -43,7 +43,7 @@ MODULES += drivers/led # Library modules # MODULES += modules/systemlib -#MODULES += modules/systemlib/mixer +MODULES += modules/systemlib/mixer MODULES += modules/uORB #MODULES += modules/dataman #MODULES += modules/sdlog2 diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index f2a957499b..3f42c1c301 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -60,16 +60,17 @@ static const char *commands = "gyrosim start\n" "list_devices\n" "list_topics\n" -"list_tasks" +"list_tasks\n" +"param show *\n" +"rgbled start\n" #if 0 +"hil mode_pwm" "param set CAL_GYRO0_ID 2293760\n" "param set CAL_ACC0_ID 1310720\n" "param set CAL_ACC1_ID 1376256\n" "param set CAL_MAG0_ID 196608\n" -"rgbled start\n" -"mavlink start -d /tmp/ttyS0\n" "sensors start\n" -"hil mode_pwm\n" +"mavlink start -d /tmp/ttyS0\n" "commander start\n" #endif ; From 4a84215a8f73a307d38621ed8e8e7e8cd3421d3d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 15 May 2015 17:49:20 +0200 Subject: [PATCH 242/258] fix mavlink message sending, make thread priority default --- src/modules/simulator/simulator.h | 2 +- src/modules/simulator/simulator_mavlink.cpp | 23 +++++++++------------ 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 93f5327e83..60b60f72f8 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -223,7 +223,7 @@ private: void poll_topics(); void handle_message(mavlink_message_t *msg); void send_data(); - void pack_actuator_message(mavlink_message_t *msg); + void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); static void *sending_trampoline(void *); void send(); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d468dd6661..9b6efb5102 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -46,7 +46,7 @@ using namespace simulator; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -void Simulator::pack_actuator_message(mavlink_message_t *msg) { +void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8]; const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; @@ -71,8 +71,6 @@ void Simulator::pack_actuator_message(mavlink_message_t *msg) { } } - mavlink_hil_controls_t actuator_msg; - actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; actuator_msg.pitch_elevator = out[1]; @@ -84,18 +82,15 @@ void Simulator::pack_actuator_message(mavlink_message_t *msg) { actuator_msg.aux4 = out[7]; actuator_msg.mode = 0; // need to put something here actuator_msg.nav_mode = 0; - - // encode the message - mavlink_msg_hil_controls_encode(1, MAVLINK_MSG_ID_HIL_CONTROLS, msg, &actuator_msg); } void Simulator::send_data() { // check if it's time to send new data hrt_abstime time_now = hrt_absolute_time(); - if (time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { + if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { _time_last = time_now; - mavlink_message_t msg; - pack_actuator_message(&msg); + mavlink_hil_controls_t msg; + pack_actuator_message(msg); send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); // can add more messages here, can also setup different timings } @@ -186,7 +181,9 @@ void Simulator::handle_message(mavlink_message_t *msg) { } void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { + component_ID = 0; uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -195,12 +192,12 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 buf[1] = payload_len; /* no idea which numbers should be here*/ buf[2] = 100; - buf[3] = 1; + buf[3] = 0; buf[4] = component_ID; buf[5] = msgid; /* payload */ - memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len); + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len); /* checksum */ uint16_t checksum; @@ -211,7 +208,7 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen); + ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen); if (len <= 0) { PX4_WARN("Failed sending mavlink message"); } @@ -329,7 +326,7 @@ void Simulator::updateSamples() (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 30; + param.sched_priority = SCHED_PRIORITY_DEFAULT; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); From 9f391b18675cfec3954043ddb868dc47deec9a6e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 15 May 2015 12:56:18 -0700 Subject: [PATCH 243/258] NuttX: fixes for NuttX build In the upstream tree ringbuffer.h includes the method implementations in the header file which causes multiple definitions in the link for other targets. Changed so ringbuffer.cpp is build separately for other platforms and is included by ringbuffer.h on NuttX. uORB changes do not link without uORBTest_UnitTest.cpp enabled for the NuttX build. px4_getopt was not exported and wasn't visible in NuttX build. The makefiles were restored to be as close as possible to upstream so the NuttX build builtin's work again. The code will have to be refactored after the merge. Signed-off-by: Mark Charlebois --- Makefile | 145 ++++++++++++- makefiles/firmware.mk | 193 +++++++++++++++++- makefiles/nuttx.mk | 2 +- src/drivers/device/module.mk | 5 +- src/drivers/device/ringbuffer.cpp | 2 +- src/drivers/device/ringbuffer.h | 6 + src/examples/publisher/publisher_example.cpp | 2 + .../commander/state_machine_helper.cpp | 2 +- src/modules/uORB/module.mk | 1 + src/modules/uORB/uORBTest_UnitTest.cpp | 1 + src/platforms/common/px4_getopt.c | 3 +- .../nuttx/px4_layer/px4_nuttx_tasks.c | 15 -- 12 files changed, 340 insertions(+), 37 deletions(-) diff --git a/Makefile b/Makefile index 63bb6cec92..4a0f13cd19 100644 --- a/Makefile +++ b/Makefile @@ -97,8 +97,132 @@ upload: endif endif +ifeq ($(PX4_TARGET_OS),nuttx) +# +# Built products +# +DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) +STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) + +all: $(DESIRED_FIRMWARES) + +# +# Copy FIRMWARES into the image directory. +# +# XXX copying the .bin files is a hack to work around the PX4IO uploader +# not supporting .px4 files, and it should be deprecated onced that +# is taken care of. +# +$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 + @$(ECHO) %% Copying $@ + $(Q) $(COPY) $< $@ + $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) + +# +# Generate FIRMWARES. +# +.PHONY: $(FIRMWARES) +$(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) +$(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ +$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: generateuorbtopicheaders checksubmodules + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ + -f $(PX4_MK_DIR)firmware.mk \ + CONFIG=$(config) \ + WORK_DIR=$(work_dir) \ + $(FIRMWARE_GOAL) + +# +# Make FMU firmwares depend on the corresponding IO firmware. +# +# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency +# and forces the _default config in all cases. There has to be a better way to do this... +# +FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) +define FMU_DEP +$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 +endef +FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) +$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) + +# +# Build the NuttX export archives. +# +# Note that there are no explicit dependencies extended from these +# archives. If NuttX is updated, the user is expected to rebuild the +# archives/build area manually. Likewise, when the 'archives' target is +# invoked, all archives are always rebuilt. +# +# XXX Should support fetching/unpacking from a separate directory to permit +# downloads of the prebuilt archives as well... +# +NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) +.PHONY: archives +archives: checksubmodules $(NUTTX_ARCHIVES) + +# We cannot build these parallel; note that we also force -j1 for the +# sub-make invocations. +ifneq ($(filter archives,$(MAKECMDGOALS)),) +.NOTPARALLEL: +endif + +J?=1 + +$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) +$(ARCHIVE_DIR)%.export: configuration = nsh +$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(board) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) + @$(ECHO) %% Exporting NuttX for $(board) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MKDIR) -p $(dir $@) + $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ + $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) + +# +# The user can run the NuttX 'menuconfig' tool for a single board configuration with +# make BOARDS= menuconfig +# +ifeq ($(MAKECMDGOALS),menuconfig) +ifneq ($(words $(BOARDS)),1) +$(error BOARDS must specify exactly one board for the menuconfig goal) +endif +BOARD = $(BOARDS) +menuconfig: $(NUTTX_SRC) + @$(ECHO) %% Configuring NuttX for $(BOARD) + $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) + $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) + @$(ECHO) %% Running menuconfig for $(BOARD) + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + @$(ECHO) %% Saving configuration file + $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig +else +menuconfig: + @$(ECHO) "" + @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" + @$(ECHO) "" + +endif + +$(NUTTX_SRC): checksubmodules + +$(UAVCAN_DIR): + $(Q) (./Tools/check_submodules.sh) + +endif + ifeq ($(PX4_TARGET_OS),nuttx) -include $(PX4_BASE)makefiles/firmware_nuttx.mk +# TODO +# Move the above nuttx specific rules into $(PX4_BASE)makefiles/firmware_nuttx.mk endif ifeq ($(PX4_TARGET_OS),posix) include $(PX4_BASE)makefiles/firmware_posix.mk @@ -107,6 +231,16 @@ ifeq ($(PX4_TARGET_OS),qurt) include $(PX4_BASE)makefiles/firmware_qurt.mk endif + +.PHONY: checksubmodules +checksubmodules: + $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) + +.PHONY: updatesubmodules +updatesubmodules: + $(Q) (git submodule init) + $(Q) (git submodule update) + MSG_DIR = $(PX4_BASE)msg UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb @@ -117,15 +251,6 @@ TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src -.PHONY: checksubmodules -checksubmodules: - $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) - -.PHONY: updatesubmodules -updatesubmodules: - $(Q) (git submodule init) - $(Q) (git submodule update) - .PHONY: generateuorbtopicheaders generateuorbtopicheaders: checksubmodules @$(ECHO) "Generating uORB topic headers" diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index b58f23cbc2..f5f5d8188d 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -213,7 +213,7 @@ endef MODULE_MKFILES := $(foreach module,$(MODULES),$(call MODULE_SEARCH,$(module))) MISSING_MODULES := $(subst MISSING_,,$(filter MISSING_%,$(MODULE_MKFILES))) ifneq ($(MISSING_MODULES),) -$(error Can't find module(s): $(MISSING_MODULES)) +$(error Cant find module(s): $(MISSING_MODULES)) endif # Make a list of the object files we expect to build from modules @@ -273,7 +273,7 @@ endef LIBRARY_MKFILES := $(foreach library,$(LIBRARIES),$(call LIBRARY_SEARCH,$(library))) MISSING_LIBRARIES := $(subst MISSING_,,$(filter MISSING_%,$(LIBRARY_MKFILES))) ifneq ($(MISSING_LIBRARIES),) -$(error Can't find library(s): $(MISSING_LIBRARIES)) +$(error Cant find library(s): $(MISSING_LIBRARIES)) endif # Make a list of the archive files we expect to build from libraries @@ -311,6 +311,132 @@ $(LIBRARY_CLEANS): LIBRARY_MK=$(mkfile) \ clean +ifeq ($(PX4_TARGET_OS),nuttx) +################################################################################ +# ROMFS generation +################################################################################ + +ifneq ($(ROMFS_ROOT),) +ifeq ($(wildcard $(ROMFS_ROOT)),) +$(error ROMFS_ROOT specifies a directory that does not exist) +endif + +# +# Note that there is no support for more than one root directory or constructing +# a root from several templates. That would be a nice feature. +# + +# Add dependencies on anything in the ROMFS root directory +ROMFS_FILES += $(wildcard \ + $(ROMFS_ROOT)/* \ + $(ROMFS_ROOT)/*/* \ + $(ROMFS_ROOT)/*/*/* \ + $(ROMFS_ROOT)/*/*/*/* \ + $(ROMFS_ROOT)/*/*/*/*/* \ + $(ROMFS_ROOT)/*/*/*/*/*/*) +ifeq ($(ROMFS_FILES),) +$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files) +endif +ROMFS_DEPS += $(ROMFS_FILES) + +# Extra files that may be copied into the ROMFS /extras directory +# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional +ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES)) +ROMFS_DEPS += $(ROMFS_EXTRA_FILES) + +ROMFS_IMG = romfs.img +ROMFS_SCRATCH = romfs_scratch +ROMFS_CSRC = $(ROMFS_IMG:.img=.c) +ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) +LIBS += $(ROMFS_OBJ) +LINK_DEPS += $(ROMFS_OBJ) + +# Remove all comments from startup and mixer files +ROMFS_PRUNER = $(PX4_BASE)/Tools/px_romfs_pruner.py + +# Turn the ROMFS image into an object file +$(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) + $(call BIN_TO_OBJ,$<,$@,romfs_img) + +# Generate the ROMFS image from the root +$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) + @$(ECHO) "ROMFS: $@" + $(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol" + +# Construct the ROMFS scratch root from the canonical root +$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) + $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) +# delete all files in ROMFS_SCRATCH which start with a . or end with a ~ + $(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~ +ifneq ($(ROMFS_EXTRA_FILES),) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras + $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras +endif + $(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH) + +EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) + +endif + +################################################################################ +# Builtin command list generation +################################################################################ + +# +# Builtin commands can be generated by the configuration, in which case they +# must refer to commands that already exist, or indirectly generated by modules +# when they are built. +# +# The configuration supplies builtin command information in the BUILTIN_COMMANDS +# variable. Applications make empty files in $(WORK_DIR)/builtin_commands whose +# filename contains the same information. +# +# In each case, the command information consists of four fields separated with a +# period. These fields are the command's name, its thread priority, its stack size +# and the name of the function to call when starting the thread. +# +BUILTIN_CSRC = $(WORK_DIR)builtin_commands.c + +# command definitions from modules (may be empty at Makefile parsing time...) +MODULE_COMMANDS = $(subst COMMAND.,,$(notdir $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*))) + +# We must have at least one pre-defined builtin command in order to generate +# any of this. +# +ifneq ($(BUILTIN_COMMANDS),) + +# (BUILTIN_PROTO,,) +define BUILTIN_PROTO + $(ECHO) 'extern int $(word 4,$1)(int argc, char *argv[]);' >> $2; +endef + +# (BUILTIN_DEF,,) +define BUILTIN_DEF + $(ECHO) ' {"$(word 1,$1)", $(word 2,$1), $(word 3,$1), $(word 4,$1)},' >> $2; +endef + +# Don't generate until modules have updated their command files +$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(MODULE_MKFILES) $(BUILTIN_COMMAND_FILES) + @$(ECHO) "CMDS: $@" + $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ + $(Q) $(ECHO) '#include ' >> $@ + $(Q) $(ECHO) '#include ' >> $@ + $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@)) + $(Q) $(ECHO) 'const struct builtin_s g_builtins[] = {' >> $@ + $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) + $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@)) + $(Q) $(ECHO) ' {NULL, 0, 0, NULL}' >> $@ + $(Q) $(ECHO) '};' >> $@ + $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS) $(MODULE_COMMANDS));' >> $@ + +SRCS += $(BUILTIN_CSRC) + +EXTRA_CLEANS += $(BUILTIN_CSRC) + +endif +endif ################################################################################ # Default SRCS generation @@ -329,9 +455,22 @@ SRCS += $(EMPTY_SRC) endif ################################################################################ -# Generic Build rules +# Build rules ################################################################################ +ifeq ($(PX4_TARGET_OS),nuttx) +# +# What we're going to build. +# +PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 +PRODUCT_BIN = $(WORK_DIR)firmware.bin +PRODUCT_ELF = $(WORK_DIR)firmware.elf +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml + +.PHONY: firmware +firmware: $(PRODUCT_BUNDLE) +endif + # # Object files we will generate from sources # @@ -352,13 +491,59 @@ $(filter %.cpp.o,$(OBJS)): $(WORK_DIR)%.cpp.o: %.cpp $(GLOBAL_DEPS) $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(call ASSEMBLE,$<,$@) +ifeq ($(PX4_TARGET_OS),nuttx) # +# Built product rules +# + +$(PRODUCT_BUNDLE): $(PRODUCT_BIN) + @$(ECHO) %% Generating $@ +ifdef GEN_PARAM_XML + $(Q) $(PYTHON) $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --parameter_xml $(PRODUCT_PARAMXML) \ + --image $< > $@ +else + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --image $< > $@ +endif + +$(PRODUCT_BIN): $(PRODUCT_ELF) + $(call SYM_TO_BIN,$<,$@) + +$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES) + $(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS)) + +# +# Utility rules +# + +.PHONY: upload +upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN) + $(Q) $(MAKE) -f $(PX4_MK_DIR)/upload.mk \ + METHOD=serial \ + CONFIG=$(CONFIG) \ + BOARD=$(BOARD) \ + BUNDLE=$(PRODUCT_BUNDLE) \ + BIN=$(PRODUCT_BIN) + +.PHONY: clean +clean: $(MODULE_CLEANS) + @$(ECHO) %% cleaning + $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_ELF) + $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS) + $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR) +endif + # Include the OS specific build rules # The rules must define the "firmware" make target # ifeq ($(PX4_TARGET_OS),nuttx) -include $(MK_DIR)/nuttx_romfs.mk +# TODO +# Move above nuttx specific rules to $(MK_DIR)/nuttx_romfs.mk endif ifeq ($(PX4_TARGET_OS),posix) include $(MK_DIR)/posix_elf.mk diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index a151e7d917..4ca1dc2ac6 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -34,7 +34,7 @@ # building firmware. # -#MODULES += platforms/nuttx/px4_layer +MODULES += platforms/nuttx/px4_layer platforms/common # # Check that the NuttX archive for the selected board is available. diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index c206a5037c..a9e964ef94 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -41,8 +41,7 @@ SRCS = \ cdev.cpp \ i2c_nuttx.cpp \ pio.cpp \ - spi.cpp \ - ringbuffer.cpp + spi.cpp else SRCS = \ device_posix.cpp \ @@ -51,5 +50,5 @@ SRCS = \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ - ringbuffer.cpp + ringbuffer.cpp endif diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index 0da2467fd6..1790289338 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -237,7 +237,7 @@ RingBuffer::force(double val) // FIXME - clang crashes on this get() call #ifdef __PX4_QURT #define __PX4_SBCAP my_sync_bool_compare_and_swap -static bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c) +static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c) { if (*a == b) { *a = c; diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 876bb3a8f2..1c65c02135 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -172,3 +172,9 @@ private: RingBuffer operator=(const RingBuffer&); }; +#ifdef __PX4_NUTTX +// Not sure why NuttX requires these to be defined in the header file +// but on other targets it causes a problem with multiple definitions +// at link time +#include "ringbuffer.cpp" +#endif diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 7ce1533125..d034d33d6e 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -51,6 +51,8 @@ PublisherExample::PublisherExample() : { } +px4::AppState PublisherExample::appState; + int PublisherExample::main() { px4::Rate loop_rate(10); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a2829be829..7d973abdcb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -361,7 +361,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } -#ifdef PX4_NUTTX +#ifdef __PX4_NUTTX static transition_result_t disable_publication(const int mavlink_fd) { transition_result_t ret; diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 88f1d6807a..31a2b24e4a 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -41,6 +41,7 @@ MODULE_STACKSIZE = 2048 ifeq ($(PX4_TARGET_OS),nuttx) SRCS = uORBDevices_nuttx.cpp \ + uORBTest_UnitTest.cpp \ uORBManager_nuttx.cpp else diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp index c1ea73959f..722f538976 100644 --- a/src/modules/uORB/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -35,6 +35,7 @@ #include "uORBCommon.hpp" #include #include +#include uORBTest::UnitTest &uORBTest::UnitTest::instance() { diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c index 3078351727..5f76065bb0 100644 --- a/src/platforms/common/px4_getopt.c +++ b/src/platforms/common/px4_getopt.c @@ -121,7 +121,7 @@ static int reorder(int argc, char **argv, const char *options) // Argv is changed to put all options and option args at the beginning, // followed by non-options. // -int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const char **myoptarg) +__EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const char **myoptarg) { char *p; char c; @@ -149,4 +149,3 @@ int px4_getopt(int argc, char *argv[], const char *options, int *myoptind, const } return -1; } - diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c index 7aa96bb5c0..e9b722de4c 100644 --- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c +++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c @@ -72,21 +72,6 @@ px4_systemreset(bool to_bootloader) while(true); } -static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); - -void px4_killall() -{ -// printf("Sending SIGUSR1 to all processes now\n"); - - /* iterate through all tasks and send kill signal */ - sched_foreach(kill_task, NULL); -} - -static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) -{ - kill(tcb->pid, SIGUSR1); -} - int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) { int pid; From dcb55ff38df5d9bce9de1391a5044b3451d193e7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 18 May 2015 09:58:49 -0700 Subject: [PATCH 244/258] Changed isfinite to PX4_ISFINITE There are cross platform issues with the isfinite call that are handled by PX4_ISFINITE Signed-off-by: Mark Charlebois --- src/modules/commander/gyro_calibration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index a0d1de3907..817cbcdb0e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -228,9 +228,9 @@ int do_gyro_calibration(int mavlink_fd) /* maximum allowable calibration error in radians */ const float maxoff = 0.0055f; - if (!isfinite(worker_data.gyro_scale[0].x_offset) || - !isfinite(worker_data.gyro_scale[0].y_offset) || - !isfinite(worker_data.gyro_scale[0].z_offset) || + if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || + !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) || + !PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) || fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) { From 466db74d299c188dc53819a236c6803064522933 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 18 May 2015 12:21:53 -0700 Subject: [PATCH 245/258] QuRT: Added define so pthread functions are enabled Added -D__QDSP6_DINKUM_PTHREAD_TYPES__ to makefiles/toolchain_hexagon.mk so the pthreads functions are properly defined. Signed-off-by: Mark Charlebois --- makefiles/toolchain_hexagon.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index a220baa594..7135158aa8 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -107,6 +107,7 @@ endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -D__PX4_QURT -D__PX4_POSIX \ -D__EXPORT= \ + -D__QDSP6_DINKUM_PTHREAD_TYPES__ \ -Dnoreturn_function= \ -Drestrict= \ -I$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/include \ From 791d780bb898e6f1ce840e3aa0245dbd99524b65 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 18 May 2015 13:29:20 -0700 Subject: [PATCH 246/258] BAROSIM: Fixed error in transfer function The transfer function would previously return error if the receive buffer length was 0. This appears to be a valid condition for requesting a measurmement be taken but no data returned. Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/barosim/baro.cpp | 12 ++++++------ src/platforms/posix/drivers/barosim/baro_sim.cpp | 9 +++++++-- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 751ffd43db..4a247f85bd 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -280,7 +280,7 @@ BAROSIM::init() /* do temperature first */ if (OK != measure()) { ret = -EIO; - PX4_WARN("temp measure failed"); + PX4_ERR("temp measure failed"); break; } @@ -288,14 +288,14 @@ BAROSIM::init() if (OK != collect()) { ret = -EIO; - PX4_WARN("temp collect failed"); + PX4_ERR("temp collect failed"); break; } /* now do a pressure measurement */ if (OK != measure()) { ret = -EIO; - PX4_WARN("pressure collect failed"); + PX4_ERR("pressure collect failed"); break; } @@ -303,7 +303,7 @@ BAROSIM::init() if (OK != collect()) { ret = -EIO; - PX4_WARN("pressure collect failed"); + PX4_ERR("pressure collect failed"); break; } @@ -316,7 +316,7 @@ BAROSIM::init() &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == (orb_advert_t)(-1)) { - PX4_WARN("failed to create sensor_baro publication"); + PX4_ERR("failed to create sensor_baro publication"); } //PX4_WARN("sensor_baro publication %ld", _baro_topic); @@ -728,7 +728,7 @@ BAROSIM::collect() orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } else { - PX4_ERR("BAROSIM::collect _baro_topic not initialized"); + PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 42d52b4fef..7c267f6aaa 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -201,15 +201,20 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len, { // TODO add Simulation data connection so calls retrieve // data from the simulator - - if (send_len != 1 || send[0] != 0 || recv_len != 3) { + if (recv_len == 0) { + PX4_DEBUG("BARO_SIM measurement requested"); + } + else if (send_len != 1 || send[0] != 0 || recv_len != 3) { + PX4_WARN("BARO_SIM::transfer invalid param %u %u %u\n", send_len, send[0], recv_len); return 1; } else { Simulator *sim = Simulator::getInstance(); if (sim == NULL) { + PX4_ERR("Error BARO_SIM::transfer no simulator \n"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample\n"); sim->getBaroSample(recv, recv_len); } return 0; From ffdc9d629cfa78223a23d193bc244b0cd2486e20 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 09:19:24 -0700 Subject: [PATCH 247/258] POSIX: Improved logging The warnx and warn calls map to PX4_WARN. Calls to errx or err genrtate a compile error. The px4_log.h file implements a new log format: For DEBUG and INFO: For ERROR and WARN: (file filepath line linenum) The verbosity can be changed by setting the macro to use either linux_log or linux_log_verbose in px4_log.h Signed-off-by: Mark Charlebois --- src/drivers/device/device_posix.cpp | 2 +- src/drivers/device/sim.cpp | 8 +- src/drivers/device/vdev.cpp | 57 +++++++----- src/drivers/device/vdev_posix.cpp | 20 ++-- src/drivers/led/led.cpp | 2 +- src/modules/simulator/simulator.cpp | 14 +-- src/modules/simulator/simulator_mavlink.cpp | 4 +- src/modules/systemlib/err.h | 10 ++ src/modules/systemlib/module.mk | 5 +- .../posix/drivers/accelsim/accelsim.cpp | 2 +- src/platforms/posix/drivers/barosim/baro.cpp | 91 +++++++++---------- .../posix/drivers/barosim/baro_sim.cpp | 6 +- .../posix/drivers/gyrosim/gyrosim.cpp | 4 +- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 2 +- .../posix/px4_layer/px4_posix_tasks.cpp | 23 ++++- src/platforms/px4_log.h | 29 +++--- .../qurt/px4_layer/px4_qurt_tasks.cpp | 2 +- 17 files changed, 161 insertions(+), 120 deletions(-) diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 34382263ac..771bee2471 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -83,7 +83,7 @@ Device::log(const char *fmt, ...) { va_list ap; - printf("[%s] ", _name); + PX4_INFO("[%s] ", _name); va_start(ap, fmt); vprintf(fmt, ap); va_end(ap); diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp index 4e952409eb..787a3826c9 100644 --- a/src/drivers/device/sim.cpp +++ b/src/drivers/device/sim.cpp @@ -63,7 +63,7 @@ SIM::SIM(const char *name, _devname(devname) { - PX4_DBG("SIM::SIM name = %s devname = %s", name, devname); + PX4_DEBUG("SIM::SIM name = %s devname = %s", name, devname); // fill in _device_id fields for a SIM device _device_id.devid_s.bus_type = DeviceBusType_SIM; _device_id.devid_s.bus = bus; @@ -99,16 +99,16 @@ int SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { if (send_len > 0) { - PX4_DBG("SIM: sending %d bytes", send_len); + PX4_DEBUG("SIM: sending %d bytes", send_len); } if (recv_len > 0) { - PX4_DBG("SIM: receiving %d bytes", recv_len); + PX4_DEBUG("SIM: receiving %d bytes", recv_len); // TODO - write data to recv; } - PX4_DBG("I2C SIM: transfer_4 on %s", _devname); + PX4_DEBUG("I2C SIM: transfer_4 on %s", _devname); return PX4_OK; } diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 9efdb63e8a..50fe6ed697 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -84,12 +84,14 @@ VDev::VDev(const char *name, _registered(false), _open_count(0) { + PX4_DEBUG("VDev::VDev"); for (unsigned i = 0; i < _max_pollwaiters; i++) _pollset[i] = nullptr; } VDev::~VDev() { + PX4_DEBUG("VDev::~VDev"); if (_registered) unregister_driver(_devname); } @@ -97,6 +99,7 @@ VDev::~VDev() int VDev::register_class_devname(const char *class_devname) { + PX4_DEBUG("VDev::register_class_devname"); if (class_devname == nullptr) { return -EINVAL; } @@ -121,6 +124,7 @@ VDev::register_class_devname(const char *class_devname) int VDev::register_driver(const char *name, void *data) { + PX4_DEBUG("VDev::register_driver"); int ret = -ENOSPC; if (name == NULL || data == NULL) @@ -136,7 +140,7 @@ VDev::register_driver(const char *name, void *data) for (int i=0;iname) == 0)) { delete devmap[i]; devmap[i] = NULL; - debug("Unregistered DEV %s", name); + PX4_DEBUG("Unregistered DEV %s", name); ret = PX4_OK; break; } @@ -167,12 +172,13 @@ VDev::unregister_driver(const char *name) int VDev::unregister_class_devname(const char *class_devname, unsigned class_instance) { + PX4_DEBUG("VDev::unregister_class_devname"); char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); for (int i=0;iname,name) == 0) { delete devmap[i]; - debug("Unregistered class DEV %s", name); + PX4_DEBUG("Unregistered class DEV %s", name); devmap[i] = NULL; return PX4_OK; } @@ -183,6 +189,8 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc int VDev::init() { + PX4_DEBUG("VDev::init"); + // base class init first int ret = Device::init(); @@ -209,9 +217,9 @@ out: int VDev::open(file_t *filep) { + PX4_DEBUG("VDev::open"); int ret = PX4_OK; - debug("VDev::open"); lock(); /* increment the open count */ _open_count++; @@ -233,14 +241,14 @@ VDev::open(file_t *filep) int VDev::open_first(file_t *filep) { - debug("VDev::open_first"); + PX4_DEBUG("VDev::open_first"); return PX4_OK; } int VDev::close(file_t *filep) { - debug("VDev::close"); + PX4_DEBUG("VDev::close"); int ret = PX4_OK; lock(); @@ -265,36 +273,37 @@ VDev::close(file_t *filep) int VDev::close_last(file_t *filep) { - debug("VDev::close_last"); + PX4_DEBUG("VDev::close_last"); return PX4_OK; } ssize_t VDev::read(file_t *filep, char *buffer, size_t buflen) { - debug("VDev::read"); + PX4_DEBUG("VDev::read"); return -ENOSYS; } ssize_t VDev::write(file_t *filep, const char *buffer, size_t buflen) { - debug("VDev::write"); + PX4_DEBUG("VDev::write"); return -ENOSYS; } off_t VDev::seek(file_t *filep, off_t offset, int whence) { + PX4_DEBUG("VDev::seek"); return -ENOSYS; } int VDev::ioctl(file_t *filep, int cmd, unsigned long arg) { + PX4_DEBUG("VDev::ioctl"); int ret = -ENOTTY; - debug("VDev::ioctl"); switch (cmd) { /* fetch a pointer to the driver's private data */ @@ -311,7 +320,7 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) break; case DEVIOCGDEVICEID: ret = (int)_device_id.devid; - printf("IOCTL DEVIOCGDEVICEID %d\n", ret); + PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); default: break; } @@ -322,8 +331,8 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) int VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) { + PX4_DEBUG("VDev::Poll %s", setup ? "setup" : "teardown"); int ret = PX4_OK; - debug("VDev::Poll %s", setup ? "setup" : "teardown"); /* * Lock against pollnotify() (and possibly other callers) @@ -336,7 +345,7 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) * benefit. */ fds->priv = (void *)filep; - debug("VDev::poll: fds->priv = %p", filep); + PX4_DEBUG("VDev::poll: fds->priv = %p", filep); /* * Handle setup requests. @@ -371,7 +380,7 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) void VDev::poll_notify(pollevent_t events) { - debug("VDev::poll_notify events = %0x", events); + PX4_DEBUG("VDev::poll_notify events = %0x", events); /* lock against poll() as well as other wakeups */ lock(); @@ -386,14 +395,14 @@ VDev::poll_notify(pollevent_t events) void VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) { - debug("VDev::poll_notify_one"); + PX4_DEBUG("VDev::poll_notify_one"); int value; sem_getvalue(fds->sem, &value); /* update the reported event set */ fds->revents |= fds->events & events; - debug(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value); + PX4_DEBUG(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value); /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ @@ -404,7 +413,7 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) pollevent_t VDev::poll_state(file_t *filep) { - debug("VDev::poll_notify"); + PX4_DEBUG("VDev::poll_notify"); /* by default, no poll events to report */ return 0; } @@ -415,7 +424,7 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds) /* * Look for a free slot. */ - debug("VDev::store_poll_waiter"); + PX4_DEBUG("VDev::store_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -432,7 +441,7 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds) int VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { - debug("VDev::remove_poll_waiter"); + PX4_DEBUG("VDev::remove_poll_waiter"); for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -442,13 +451,13 @@ VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) } } - puts("poll: bad fd state"); + PX4_WARN("poll: bad fd state"); return -EINVAL; } VDev *VDev::getDev(const char *path) { - printf("VDev::getDev\n"); + PX4_DEBUG("VDev::getDev"); int i=0; for (; iname, "/dev/", 5) == 0) { printf(" %s\n", devmap[i]->name); @@ -475,7 +484,7 @@ void VDev::showDevices() void VDev::showTopics() { int i=0; - printf("Devices:\n"); + PX4_INFO("Devices:\n"); for (; iname, "/obj/", 5) == 0) { printf(" %s\n", devmap[i]->name); @@ -486,7 +495,7 @@ void VDev::showTopics() void VDev::showFiles() { int i=0; - printf("Files:\n"); + PX4_INFO("Files:\n"); for (; iname, "/obj/", 5) != 0 && strncmp(devmap[i]->name, "/dev/", 5) != 0) { diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index fdd2e73210..0e94e88dfe 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -71,7 +71,7 @@ static void *timer_handler(void *data) usleep(td->ts.tv_nsec/1000); sem_post(&(td->sem)); - PX4_DEBUG("timer_handler: Timer expired\n"); + PX4_DEBUG("timer_handler: Timer expired"); return 0; } @@ -87,7 +87,7 @@ inline bool valid_fd(int fd) int px4_open(const char *path, int flags, ...) { - printf("px4_open\n"); + PX4_DEBUG("px4_open"); VDev *dev = VDev::getDev(path); int ret = 0; int i; @@ -103,7 +103,7 @@ int px4_open(const char *path, int flags, ...) va_end(p); // Create the file - warnx("Creating virtual file %s\n", path); + PX4_DEBUG("Creating virtual file %s", path); dev = VFile::createFile(path, mode); } if (dev) { @@ -136,7 +136,7 @@ int px4_close(int fd) int ret; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_close fd = %d\n", fd); + PX4_DEBUG("px4_close fd = %d", fd); ret = dev->close(filemap[fd]); filemap[fd] = NULL; } @@ -155,7 +155,7 @@ ssize_t px4_read(int fd, void *buffer, size_t buflen) int ret; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_read fd = %d\n", fd); + PX4_DEBUG("px4_read fd = %d", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); } else { @@ -173,7 +173,7 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen) int ret; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_write fd = %d\n", fd); + PX4_DEBUG("px4_write fd = %d", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); } else { @@ -188,7 +188,7 @@ ssize_t px4_write(int fd, const void *buffer, size_t buflen) int px4_ioctl(int fd, int cmd, unsigned long arg) { - PX4_DEBUG("px4_ioctl fd = %d\n", fd); + PX4_DEBUG("px4_ioctl fd = %d", fd); int ret = 0; if (valid_fd(fd)) { VDev *dev = (VDev *)(filemap[fd]->vdev); @@ -212,7 +212,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) unsigned int i; struct timespec ts; - PX4_DEBUG("Called px4_poll timeout = %d\n", timeout); + PX4_DEBUG("Called px4_poll timeout = %d", timeout); sem_init(&sem, 0, 0); // For each fd @@ -226,7 +226,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) if (valid_fd(fds[i].fd)) { VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; - PX4_DEBUG("px4_poll: VDev->poll(setup) %d\n", fds[i].fd); + PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); if (ret < 0) @@ -270,7 +270,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) if (valid_fd(fds[i].fd)) { VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; - PX4_DEBUG("px4_poll: VDev->poll(teardown) %d\n", fds[i].fd); + PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); if (ret < 0) diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 428a542f15..69c72b5b53 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -89,7 +89,7 @@ LED::~LED() int LED::init() { - printf("Initializing LED\n"); + debug("LED::init"); #ifdef __PX4_NUTTX CDev::init(); #else diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 1920724fdf..83e3fd8d76 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -81,7 +81,7 @@ int Simulator::start(int argc, char *argv[]) int ret = 0; _instance = new Simulator(); if (_instance) { - PX4_INFO("Simulator started\n"); + PX4_INFO("Simulator started"); drv_led_start(); if (argv[2][1] == 's') { #ifndef __PX4_QURT @@ -92,7 +92,7 @@ int Simulator::start(int argc, char *argv[]) } } else { - PX4_WARN("Simulator creation failed\n"); + PX4_WARN("Simulator creation failed"); ret = 1; } return ret; @@ -150,7 +150,7 @@ void Simulator::publishSensorsCombined() { gyro.timestamp = time_last; mag.timestamp = time_last; // publish the sensor values - //printf("Publishing SensorsCombined\n"); + //PX4_DEBUG("Publishing SensorsCombined\n"); orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &sensors); orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro); @@ -225,14 +225,14 @@ bool static _led_state[2] = { false , false }; __EXPORT void led_init() { - PX4_DBG("LED_INIT\n"); + PX4_DEBUG("LED_INIT"); } __EXPORT void led_on(int led) { if (led == 1 || led == 0) { - PX4_DBG("LED%d_ON", led); + PX4_DEBUG("LED%d_ON", led); _led_state[led] = true; } } @@ -241,7 +241,7 @@ __EXPORT void led_off(int led) { if (led == 1 || led == 0) { - PX4_DBG("LED%d_OFF", led); + PX4_DEBUG("LED%d_OFF", led); _led_state[led] = false; } } @@ -251,7 +251,7 @@ __EXPORT void led_toggle(int led) if (led == 1 || led == 0) { _led_state[led] = !_led_state[led]; - PX4_DBG("LED%d_TOGGLE: %s\n", led, _led_state[led] ? "ON" : "OFF"); + PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); } } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 9b6efb5102..ed4a6343fe 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -335,7 +335,7 @@ void Simulator::updateSamples() int serial_fd = open(PIXHAWK_DEVICE, O_RDWR); if (serial_fd < 0) { - PX4_WARN("failed to open %s\n", PIXHAWK_DEVICE); + PX4_WARN("failed to open %s", PIXHAWK_DEVICE); } // tell the device to stream some messages @@ -343,7 +343,7 @@ void Simulator::updateSamples() int w = ::write(serial_fd, command, sizeof(command)); if (w <= 0) { - PX4_WARN("failed to send streaming command to %s\n", PIXHAWK_DEVICE); + PX4_WARN("failed to send streaming command to %s", PIXHAWK_DEVICE); } char serial_buf[1024]; diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h index af90c25600..05401128ef 100644 --- a/src/modules/systemlib/err.h +++ b/src/modules/systemlib/err.h @@ -65,6 +65,7 @@ #ifndef _SYSTEMLIB_ERR_H #define _SYSTEMLIB_ERR_H +#include #include #include "visibility.h" @@ -72,6 +73,14 @@ __BEGIN_DECLS __EXPORT const char *getprogname(void); +#ifdef __PX4_POSIX + +#define err(...) ERROR +#define errx(...) ERROR +#define warn(...) PX4_WARN(__VA_ARGS__) +#define warnx(...) PX4_WARN(__VA_ARGS__) + +#else __EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); __EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); __EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4))); @@ -84,6 +93,7 @@ __EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(print __EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0))); __EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2))); __EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); +#endif __END_DECLS diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 0e023cf1b1..cb599e2052 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -35,7 +35,7 @@ # System utility library # -SRCS = err.c \ +SRCS = \ perf_counter.c \ param/param.c \ conversions.c \ @@ -55,7 +55,8 @@ SRCS = err.c \ circuit_breaker_params.c ifeq ($(PX4_TARGET_OS),nuttx) -SRCS += up_cxxinitialize.c +SRCS += err.c \ + up_cxxinitialize.c endif ifneq ($(PX4_TARGET_OS),qurt) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 42e9eb0b1c..f431231706 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -1331,7 +1331,7 @@ info() return 1; } - PX4_DBG("state @ %p", g_dev); + PX4_DEBUG("state @ %p", g_dev); //g_dev->print_info(); return 0; diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 4a247f85bd..13021444c0 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -250,7 +250,7 @@ int BAROSIM::init() { int ret; - PX4_WARN("BAROSIM::init"); + debug("BAROSIM::init"); ret = VDev::init(); if (ret != OK) { @@ -401,59 +401,58 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { + case SENSORIOCSPOLLRATE: + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; return OK; + } - /* external signalling not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* zero would be bad */ - case 0: - return -EINVAL; + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* check against maximum rate */ + if ((long)ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) + return -EINVAL; - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if ((long)ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start_cycle(); - - return OK; - } + return OK; } } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 7c267f6aaa..734c2bc286 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -205,16 +205,16 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len, PX4_DEBUG("BARO_SIM measurement requested"); } else if (send_len != 1 || send[0] != 0 || recv_len != 3) { - PX4_WARN("BARO_SIM::transfer invalid param %u %u %u\n", send_len, send[0], recv_len); + PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; } else { Simulator *sim = Simulator::getInstance(); if (sim == NULL) { - PX4_ERR("Error BARO_SIM::transfer no simulator \n"); + PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } - PX4_DEBUG("BARO_SIM::transfer getting sample\n"); + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getBaroSample(recv, recv_len); } return 0; diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index e0d50ce25d..b253ab3b0d 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -669,11 +669,11 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } else if (cmd & DIR_READ) { - PX4_DBG("Reading %u bytes from register %u", len-1, reg); + PX4_DEBUG("Reading %u bytes from register %u", len-1, reg); memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); } else { - PX4_DBG("Writing %u bytes to register %u", len-1, reg); + PX4_DEBUG("Writing %u bytes to register %u", len-1, reg); if (recv) memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); } diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index 232da9cb2d..37f51ff607 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -350,7 +350,7 @@ ToneAlarm::start_note(unsigned note) // Silence warning of unused var do_something(period); - PX4_DBG("ToneAlarm::start_note %u", period); + PX4_DEBUG("ToneAlarm::start_note %u", period); } void diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index c09845b79a..5a36313aed 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -83,9 +83,9 @@ static void *entry_adapter ( void *ptr ) data->entry(data->argc, data->argv); free(ptr); - PX4_DBG("Before px4_task_exit"); + PX4_DEBUG("Before px4_task_exit"); px4_task_exit(0); - PX4_DBG("After px4_task_exit"); + PX4_DEBUG("After px4_task_exit"); return NULL; } @@ -231,7 +231,7 @@ void px4_task_exit(int ret) PX4_ERR("px4_task_exit: self task not found!"); } else { - PX4_DBG("px4_task_exit: %s", taskmap[i].name.c_str()); + PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } pthread_exit((void *)(unsigned long)ret); @@ -241,7 +241,7 @@ int px4_task_kill(px4_task_t id, int sig) { int rv = 0; pthread_t pid; - PX4_DBG("Called px4_task_kill %d", sig); + PX4_DEBUG("Called px4_task_kill %d", sig); if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) pid = taskmap[id].pid; @@ -271,3 +271,18 @@ void px4_show_tasks() PX4_INFO(" No running tasks"); } + +__BEGIN_DECLS +const char *getprogname() +{ + pthread_t pid = pthread_self(); + for (int i=0; i + #define qurt_log(...) {printf(__VA_ARGS__); printf("\n");} #define PX4_DBG(...) qurt_log(__VA_ARGS__) #define PX4_DEBUG(...) qurt_log(__VA_ARGS__) @@ -52,18 +54,23 @@ __END_DECLS #define PX4_ERR(...) { qurt_log("ERROR file %s line %d:", __FILE__, __LINE__); qurt_log(__VA_ARGS__); } #elif defined(__PX4_LINUX) +#include -#if defined(__PX4_LINUX) -#include -#else -#include -#endif - -#define PX4_DBG(...) -#define PX4_DEBUG(...) -#define PX4_INFO(...) warnx(__VA_ARGS__) -#define PX4_WARN(...) warnx(__VA_ARGS__) -#define PX4_ERR(...) { warnx("ERROR file %s line %d:", __FILE__, __LINE__); warnx(__VA_ARGS__); } +#define linux_log(level, ...) { \ + printf("%-5s ", level);\ + printf(__VA_ARGS__);\ + printf("\n");\ +} +#define linux_log_verbose(level, ...) { \ + printf("%-5s ", level);\ + printf(__VA_ARGS__);\ + printf(" (file %s line %d)\n", __FILE__, __LINE__);\ +} +//#define PX4_DEBUG(...) { } +#define PX4_DEBUG(...) { linux_log("DEBUG", __VA_ARGS__); } +#define PX4_INFO(...) { linux_log("INFO", __VA_ARGS__); } +#define PX4_WARN(...) { linux_log_verbose("WARN", __VA_ARGS__); } +#define PX4_ERR(...) { linux_log_verbose("ERROR", __VA_ARGS__); } #elif defined(__PX4_ROS) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 7490f1528c..bbd1366ec2 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -264,7 +264,7 @@ void px4_show_tasks() for (idx=0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { - PX4_INFO(" %-10s %u", taskmap[idx].name.c_str(), taskmap[idx].pid); + PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } From 0f5cb756926b1d0156dfee5abe5adb5415a3ede2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 10:41:15 -0700 Subject: [PATCH 248/258] ROS: Fixes for ROS build The ROS build included some files that used isfinite vs PX4_ISFINITE. The AppState class also needed to be supported for ROS. Signed-off-by: Mark Charlebois --- src/examples/subscriber/subscriber_example.cpp | 10 +++++----- .../mc_att_control_multiplatform/mc_att_control.cpp | 8 ++++---- .../mc_pos_control_multiplatform/mc_pos_control.cpp | 8 ++++---- src/platforms/px4_defines.h | 2 ++ src/platforms/px4_nodehandle.h | 9 ++++++--- src/platforms/ros/geo.cpp | 8 ++++---- 6 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index ae58f634ce..c4af0757b5 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -45,7 +45,7 @@ using namespace px4; void rc_channels_callback_function(const px4_rc_channels &msg) { - PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); + PX4_INFO("I heard: [%lu]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : @@ -84,21 +84,21 @@ SubscriberExample::SubscriberExample() : */ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { - PX4_INFO("rc_channels_callback (method): [%llu]", + PX4_INFO("rc_channels_callback (method): [%lu]", msg.data().timestamp_last_valid); - PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", + PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%lu]", _sub_rc_chan->data().timestamp_last_valid); } void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { - PX4_INFO("vehicle_attitude_callback (method): [%llu]", + PX4_INFO("vehicle_attitude_callback (method): [%lu]", msg.data().timestamp); } void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { - PX4_INFO("parameter_update_callback (method): [%llu]", + PX4_INFO("parameter_update_callback (method): [%lu]", msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 2ad89e606c..f5341155b8 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -236,10 +236,10 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti control_attitude_rates(dt); /* publish actuator controls */ - _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.data().timestamp = px4::get_time_micros(); if (!_actuators_0_circuit_breaker_enabled) { diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index b596226336..c14a4b7627 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -537,7 +537,7 @@ MulticopterPositionControl::control_auto(float dt) _pos_sp = pos_sp_s.edivide(scale); /* update yaw setpoint if needed */ - if (isfinite(_pos_sp_triplet->data().current.yaw)) { + if (PX4_ISFINITE(_pos_sp_triplet->data().current.yaw)) { _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; } @@ -554,9 +554,9 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) { /* Make sure that the position setpoint is valid */ - if (!isfinite(_pos_sp_triplet->data().current.lat) || - !isfinite(_pos_sp_triplet->data().current.lon) || - !isfinite(_pos_sp_triplet->data().current.alt)) { + if (!PX4_ISFINITE(_pos_sp_triplet->data().current.lat) || + !PX4_ISFINITE(_pos_sp_triplet->data().current.lon) || + !PX4_ISFINITE(_pos_sp_triplet->data().current.alt)) { _pos_sp_triplet->data().current.valid = false; } } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index c17b39e4c5..f350a26326 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -66,6 +66,8 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) +#define PX4_ISFINITE(x) std::isfinite(x) + #elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) /* * Building for NuttX or POSIX diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 259dab8005..540cab5231 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -42,7 +42,6 @@ #include "px4_subscriber.h" #include "px4_publisher.h" #include "px4_middleware.h" -#include "px4_posix.h" #include "px4_app.h" #if defined(__PX4_ROS) @@ -64,10 +63,11 @@ class NodeHandle : private ros::NodeHandle { public: - NodeHandle() : + NodeHandle(AppState &a) : ros::NodeHandle(), _subs(), - _pubs() + _pubs(), + _appState(a) {} ~NodeHandle() @@ -138,6 +138,9 @@ public: protected: std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ + + AppState &_appState; + }; #else //Building for NuttX class __EXPORT NodeHandle diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 04094de8bb..2f277d7f16 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -55,7 +55,7 @@ __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; } @@ -85,7 +85,7 @@ __EXPORT float _wrap_pi(float bearing) __EXPORT float _wrap_2pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; } @@ -115,7 +115,7 @@ __EXPORT float _wrap_2pi(float bearing) __EXPORT float _wrap_180(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; } @@ -145,7 +145,7 @@ __EXPORT float _wrap_180(float bearing) __EXPORT float _wrap_360(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; } From e2d175b3e531bb5b8daedeeafcb5ae1c9c77e042 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 10:47:17 -0700 Subject: [PATCH 249/258] Added back include of px4_posix.h for non-ROS builds Signed-off-by: Mark Charlebois --- src/platforms/px4_nodehandle.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 540cab5231..b0deef16c1 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -52,6 +52,7 @@ #include #else /* includes when building for NuttX */ +#include #include #endif #include From ce96329f954bf722a26a1f504c8bc51d6bedcf4a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 11:04:39 -0700 Subject: [PATCH 250/258] Resolve printing uint64 types Using %llu or %lu will break one of the build targets. The "right way" to print a uint64_t is via the PRIU64 macro defined in C99. This wasn't defined for the NuttX compiler so it was added to px4_defines.h for NuttX. Signed-off-by: Mark Charlebois --- src/examples/subscriber/subscriber_example.cpp | 10 +++++----- src/platforms/px4_defines.h | 4 ++++ 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index c4af0757b5..77bfcb8d3e 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -45,7 +45,7 @@ using namespace px4; void rc_channels_callback_function(const px4_rc_channels &msg) { - PX4_INFO("I heard: [%lu]", msg.data().timestamp_last_valid); + PX4_INFO("I heard: [%" PRIu64 "]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : @@ -84,21 +84,21 @@ SubscriberExample::SubscriberExample() : */ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { - PX4_INFO("rc_channels_callback (method): [%lu]", + PX4_INFO("rc_channels_callback (method): [%" PRIu64 "]", msg.data().timestamp_last_valid); - PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%lu]", + PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%" PRIu64 "]", _sub_rc_chan->data().timestamp_last_valid); } void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { - PX4_INFO("vehicle_attitude_callback (method): [%lu]", + PX4_INFO("vehicle_attitude_callback (method): [%" PRIu64 "]", msg.data().timestamp); } void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { - PX4_INFO("parameter_update_callback (method): [%lu]", + PX4_INFO("parameter_update_callback (method): [%" PRIu64 "]", msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index f350a26326..db54f8c53e 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -105,6 +105,10 @@ typedef param_t px4_param_t; #define PX4_ISFINITE(x) isfinite(x) +#ifndef PRIu64 +#define PRIu64 "llu" +#endif + /* * POSIX Specific defines */ From 60080dd9e78da5c109a65170bab06124d0c0f045 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 11:10:06 -0700 Subject: [PATCH 251/258] QuRT: fixed print of INT32_t type In QuRT, this is a long but the variable was being printed with "%d" Need to cast variable as long and use "%ld" Signed-off-by: Mark Charlebois --- src/systemcmds/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index b1044d71ea..2e0f1c1047 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -297,7 +297,7 @@ do_show_index(const char *index, bool used_index) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &ii)) { - printf("%d\n", ii); + printf("%ld\n", (long)ii); } break; From 95de7c2e94a6acf45725e201172ca7856503e46e Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 11:14:40 -0700 Subject: [PATCH 252/258] ROS: Fixes for print of uint64_t type Changed printf to use PRIu64 Signed-off-by: Mark Charlebois --- src/examples/publisher/publisher_example.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index d034d33d6e..a39f0ecd25 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -64,19 +64,19 @@ int PublisherExample::main() /* Publish example message */ px4_rc_channels rc_channels_msg; rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid); + PX4_INFO("rc: %" PRIu64, rc_channels_msg.data().timestamp_last_valid); _rc_channels_pub->publish(rc_channels_msg); /* Publish example message */ px4_vehicle_attitude v_att_msg; v_att_msg.data().timestamp = px4::get_time_micros(); - PX4_INFO("att: %llu", v_att_msg.data().timestamp); + PX4_INFO("att: %" PRIu64, v_att_msg.data().timestamp); _v_att_pub->publish(v_att_msg); /* Publish example message */ px4_parameter_update parameter_update_msg; parameter_update_msg.data().timestamp = px4::get_time_micros(); - PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp); + PX4_INFO("param update: %" PRIu64, parameter_update_msg.data().timestamp); _parameter_update_pub->publish(parameter_update_msg); } From eaef0db7d6c90f4060006f2bb45008b45c1da5c9 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 11:53:19 -0700 Subject: [PATCH 253/258] Logging fixes and enhancements Made the logging macros generic so they can be used for multiple targets. Fixed toolchain_native.mk so err.h is included from src/systemlib for posix. Reduced debug output for uORB. Signed-off-by: Mark Charlebois --- makefiles/toolchain_native.mk | 1 + src/modules/uORB/uORBDevices_posix.cpp | 2 +- src/platforms/px4_log.h | 48 ++++++++----------- .../qurt/px4_layer/px4_qurt_tasks.cpp | 20 ++------ 4 files changed, 27 insertions(+), 44 deletions(-) diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index 7c42ee4ba1..0b8223bfd0 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -122,6 +122,7 @@ endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -D__PX4_LINUX -D__PX4_POSIX \ -Dnoreturn_function= \ + -I$(PX4_BASE)/src/modules/systemlib \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/posix/include \ -I$(PX4_BASE)/mavlink/include/mavlink \ diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index e3530926e1..c80824d39b 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -443,7 +443,7 @@ uORB::DeviceMaster::DeviceMaster(Flavor f) : _flavor(f) { // enable debug() calls - _debug_enabled = true; + //_debug_enabled = true; } diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 2dd849f24a..07438a4ec4 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -38,39 +38,35 @@ #pragma once -#if defined(__PX4_QURT) +#define __px4_log_omit(level, ...) { } -__BEGIN_DECLS -extern void qurt_log(const char *fmt, ...); -__END_DECLS - -#include - -#define qurt_log(...) {printf(__VA_ARGS__); printf("\n");} -#define PX4_DBG(...) qurt_log(__VA_ARGS__) -#define PX4_DEBUG(...) qurt_log(__VA_ARGS__) -#define PX4_INFO(...) qurt_log(__VA_ARGS__) -#define PX4_WARN(...) qurt_log(__VA_ARGS__) -#define PX4_ERR(...) { qurt_log("ERROR file %s line %d:", __FILE__, __LINE__); qurt_log(__VA_ARGS__); } - -#elif defined(__PX4_LINUX) -#include - -#define linux_log(level, ...) { \ +#define __px4_log(level, ...) { \ printf("%-5s ", level);\ printf(__VA_ARGS__);\ printf("\n");\ } -#define linux_log_verbose(level, ...) { \ +#define __px4_log_verbose(level, ...) { \ printf("%-5s ", level);\ printf(__VA_ARGS__);\ printf(" (file %s line %d)\n", __FILE__, __LINE__);\ } + +#if defined(__PX4_QURT) +#include + +#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); +#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); +#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); +#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); + +#elif defined(__PX4_LINUX) +#include + //#define PX4_DEBUG(...) { } -#define PX4_DEBUG(...) { linux_log("DEBUG", __VA_ARGS__); } -#define PX4_INFO(...) { linux_log("INFO", __VA_ARGS__); } -#define PX4_WARN(...) { linux_log_verbose("WARN", __VA_ARGS__); } -#define PX4_ERR(...) { linux_log_verbose("ERROR", __VA_ARGS__); } +#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__); +#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__); +#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__); +#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__); #elif defined(__PX4_ROS) @@ -80,7 +76,6 @@ __END_DECLS #define PX4_ERR(...) ROS_WARN(__VA_ARGS__) #elif defined(__PX4_NUTTX) - #include #define PX4_DBG(...) @@ -90,9 +85,6 @@ __END_DECLS #else -#define PX4_DBG(...) -#define PX4_WARN(...) -#define PX4_INFO(...) -#define PX4_ERR(...) +#error "Target platform unknown" #endif diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index bbd1366ec2..d688f1104a 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -104,7 +104,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned long structsize; char * p = (char *)argv; - printf("Creating %s\n", name); + PX4_DEBUG("Creating %s\n", name); pthread_t task; pthread_attr_t attr; struct sched_param param; @@ -128,7 +128,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int taskdata->argc = argc; for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); offset+=strlen(argv[i])+1; @@ -165,17 +165,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); if (rv != 0) { - if (rv == EPERM) { - //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); - if (rv != 0) { - PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); - return (rv < 0) ? rv : -rv; - } - } - else { - return (rv < 0) ? rv : -rv; - } + return (rv < 0) ? rv : -rv; } for (i=0; i Date: Tue, 19 May 2015 12:06:58 -0700 Subject: [PATCH 254/258] POSIX: Converted poll to px4_poll A new poll command was added in accelerometer_calibration.cpp that needed to be converted to a px4_poll. Signed-off-by: Mark Charlebois --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1015dfb1b9..1155631ea3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -574,7 +574,7 @@ int do_level_calibration(int mavlink_fd) { param_set(roll_offset_handler, &zero); param_set(pitch_offset_handler, &zero); - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = att_sub; fds[0].events = POLLIN; @@ -593,7 +593,7 @@ int do_level_calibration(int mavlink_fd) { start = hrt_absolute_time(); // average attitude for 5 seconds while(hrt_elapsed_time(&start) < cal_time * 1000000) { - poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); roll_mean += att.roll; pitch_mean += att.pitch; From 7301b59d14a70bb9a03afe5d195ca0f4262dd557 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 13:36:13 -0700 Subject: [PATCH 255/258] Unit tests: Fixed unit test build Unit tests now work. The linux build was failing saving params because it was using the changes for QuRT that fake out the filesystem. Signed-off-by: Mark Charlebois --- src/modules/systemlib/bson/tinybson.c | 19 +++++++++++++++---- src/modules/systemlib/param/param.c | 16 ++++++++++++---- unittests/CMakeLists.txt | 1 + 3 files changed, 28 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 5908ace6c7..2378a2d033 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -54,13 +54,24 @@ #define CODER_CHECK(_c) do { if (_c->dead) { debug("coder dead"); return -1; }} while(0) #define CODER_KILL(_c, _reason) do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0) +#ifdef __PX4_QURT +#define BSON_READ px4_read +#define BSON_WRITE px4_write +#define BSON_FSYNC px4_fsync +#else +#define BSON_READ read +#define BSON_WRITE write +#define BSON_FSYNC fsync +#endif + static int read_x(bson_decoder_t decoder, void *p, size_t s) { CODER_CHECK(decoder); - if (decoder->fd > -1) - return (px4_read(decoder->fd, p, s) == (int)s) ? 0 : -1; + if (decoder->fd > -1) { + return (BSON_READ(decoder->fd, p, s) == (int)s) ? 0 : -1; + } if (decoder->buf != NULL) { /* staged operations to avoid integer overflow for corrupt data */ @@ -302,7 +313,7 @@ write_x(bson_encoder_t encoder, const void *p, size_t s) CODER_CHECK(encoder); if (encoder->fd > -1) - return (px4_write(encoder->fd, p, s) == (int)s) ? 0 : -1; + return (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1; /* do we need to extend the buffer? */ while ((encoder->bufpos + s) > encoder->bufsize) { @@ -409,7 +420,7 @@ bson_encoder_fini(bson_encoder_t encoder) } /* sync file */ - px4_fsync(encoder->fd); + BSON_FSYNC(encoder->fd); return 0; } diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index ca0b7296c3..09e9e88870 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -70,6 +70,14 @@ # define debug(fmt, args...) do { } while(0) #endif +#ifdef __PX4_QURT +#define PARAM_OPEN px4_open +#define PARAM_CLOSE px4_close +#else +#define PARAM_OPEN open +#define PARAM_CLOSE close +#endif + /** * Array of static parameter info. */ @@ -707,7 +715,7 @@ param_save_default(void) const char *filename = param_get_default_file(); /* write parameters to temp file */ - fd = px4_open(filename, O_WRONLY | O_CREAT, 0x777); + fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("failed to open param file: %s", filename); @@ -720,7 +728,7 @@ param_save_default(void) warnx("failed to write parameters to file: %s", filename); } - px4_close(fd); + PARAM_CLOSE(fd); return res; } @@ -732,7 +740,7 @@ int param_load_default(void) { warnx("param_load_default\n"); - int fd_load = px4_open(param_get_default_file(), O_RDONLY); + int fd_load = PARAM_OPEN(param_get_default_file(), O_RDONLY); if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ @@ -745,7 +753,7 @@ param_load_default(void) } int result = param_load(fd_load); - px4_close(fd_load); + PARAM_CLOSE(fd_load); if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 96c2f995f7..ae87cfca7a 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -33,6 +33,7 @@ add_definitions(-Dmain_t=int) add_definitions(-DERROR=-1) add_definitions(-DOK=0) add_definitions(-D_UNIT_TEST=) +add_definitions(-D__PX4_POSIX) add_definitions(-D__PX4_LINUX) # check From 30895c2dfd61eb24b03081a456ea11142483feec Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 13:37:59 -0700 Subject: [PATCH 256/258] POSIX: fixed printf to use PX4_INFO Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 50fe6ed697..58db85169e 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -476,7 +476,7 @@ void VDev::showDevices() PX4_INFO("Devices:\n"); for (; iname, "/dev/", 5) == 0) { - printf(" %s\n", devmap[i]->name); + PX4_INFO(" %s\n", devmap[i]->name); } } } @@ -487,7 +487,7 @@ void VDev::showTopics() PX4_INFO("Devices:\n"); for (; iname, "/obj/", 5) == 0) { - printf(" %s\n", devmap[i]->name); + PX4_INFO(" %s\n", devmap[i]->name); } } } @@ -499,7 +499,7 @@ void VDev::showFiles() for (; iname, "/obj/", 5) != 0 && strncmp(devmap[i]->name, "/dev/", 5) != 0) { - printf(" %s\n", devmap[i]->name); + PX4_INFO(" %s\n", devmap[i]->name); } } } From 0d19a50b1b43b40b5928f6acf364886187d2f2dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 May 2015 22:59:27 +0200 Subject: [PATCH 257/258] Add POSIX build to autobuild suite --- .travis.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 0405ba560f..10cd8dfa65 100644 --- a/.travis.yml +++ b/.travis.yml @@ -68,11 +68,15 @@ script: - make archives - ccache -s - echo -en 'travis_fold:end:script.2\\r' - - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r' + - echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.3\\r' - make -j4 - ccache -s - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 + - echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.4\\r' + - make posix -j4 + - ccache -s + - echo -en 'travis_fold:end:script.4\\r' after_script: - git clone git://github.com/PX4/CI-Tools.git From a02b99ec592ff88393e989e5ab548eabdcb56f8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 May 2015 23:02:16 +0200 Subject: [PATCH 258/258] Travis: Build POSIX before unit tests --- .travis.yml | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/.travis.yml b/.travis.yml index 10cd8dfa65..9df5239df7 100644 --- a/.travis.yml +++ b/.travis.yml @@ -60,23 +60,23 @@ env: script: - ccache -z - arm-none-eabi-gcc --version - - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r' - - make tests - - cat src/modules/systemlib/mixer/mixer_multirotor.generated.h - - echo -en 'travis_fold:end:script.1\\r' - - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r' - - make archives - - ccache -s - - echo -en 'travis_fold:end:script.2\\r' - - echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.3\\r' - - make -j4 - - ccache -s - - echo -en 'travis_fold:end:script.3\\r' - - zip Firmware.zip Images/*.px4 - - echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.4\\r' + - echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.1\\r' - make posix -j4 - ccache -s + - echo -en 'travis_fold:end:script.1\\r' + - echo 'Running Tests..' && echo -en 'travis_fold:start:script.2\\r' + - make tests + - cat src/modules/systemlib/mixer/mixer_multirotor.generated.h + - echo -en 'travis_fold:end:script.2\\r' + - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.3\\r' + - make archives + - ccache -s + - echo -en 'travis_fold:end:script.3\\r' + - echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.4\\r' + - make -j4 + - ccache -s - echo -en 'travis_fold:end:script.4\\r' + - zip Firmware.zip Images/*.px4 after_script: - git clone git://github.com/PX4/CI-Tools.git