diff --git a/.gitignore b/.gitignore index bd835e271f..cd3f8f26c2 100644 --- a/.gitignore +++ b/.gitignore @@ -48,3 +48,5 @@ unittests/build .vagrant *.pretty xcode +src/platforms/posix/px4_messages/ +src/platforms/qurt/px4_messages/ diff --git a/.travis.yml b/.travis.yml index 0405ba560f..9df5239df7 100644 --- a/.travis.yml +++ b/.travis.yml @@ -60,18 +60,22 @@ env: script: - ccache -z - arm-none-eabi-gcc --version - - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\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.1\\r' - - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r' + - 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.2\\r' - - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r' + - 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.3\\r' + - echo -en 'travis_fold:end:script.4\\r' - zip Firmware.zip Images/*.px4 after_script: diff --git a/Makefile b/Makefile index 201187e021..4a0f13cd19 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,6 +97,7 @@ upload: endif endif +ifeq ($(PX4_TARGET_OS),nuttx) # # Built products # @@ -209,6 +210,7 @@ menuconfig: @$(ECHO) "" @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" @$(ECHO) "" + endif $(NUTTX_SRC): checksubmodules @@ -216,6 +218,20 @@ $(NUTTX_SRC): checksubmodules $(UAVCAN_DIR): $(Q) (./Tools/check_submodules.sh) +endif + +ifeq ($(PX4_TARGET_OS),nuttx) +# 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 +endif +ifeq ($(PX4_TARGET_OS),qurt) +include $(PX4_BASE)makefiles/firmware_qurt.mk +endif + + .PHONY: checksubmodules checksubmodules: $(Q) ($(PX4_BASE)/Tools/check_submodules.sh) @@ -229,7 +245,7 @@ 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 @@ -255,6 +271,21 @@ 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 + +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/Tools/generate_listener.py b/Tools/generate_listener.py new file mode 100755 index 0000000000..e52d3cd26f --- /dev/null +++ b/Tools/generate_listener.py @@ -0,0 +1,150 @@ +#!/usr/bin/python + +import glob +import sys + +# 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_elements = [] + + +for index,m in enumerate(raw_messages): + 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(("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_elements.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 +#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\tbool updated;" + print "\t\tfor(uint32_t i = 0;i +#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 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) +{ + static 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) +{ + 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; +} + +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/Tools/posix_run.sh b/Tools/posix_run.sh new file mode 100755 index 0000000000..fdeefb4a29 --- /dev/null +++ b/Tools/posix_run.sh @@ -0,0 +1,19 @@ +#!/bin/bash + +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" + exit 1 +fi + +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/posix_default.build/mainapp posix-configs/posixtest/init/rc.S diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py new file mode 100755 index 0000000000..e5d75344be --- /dev/null +++ b/Tools/qurt_apps.py @@ -0,0 +1,121 @@ +#!/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 +#include + +#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 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[]); +} + + +void init_app_map(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 """ +} + +void list_builtins(map &apps) +{ + printf("Builtin Commands:\\n"); + for (map::iterator it=apps.begin(); it!=apps.end(); ++it) + printf("\\t%s\\n", (it->first).c_str()); +} + +static int shutdown_main(int argc, char *argv[]) +{ + printf("Shutting down\\n"); + 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; +} + +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/README.txt b/makefiles/README.txt index 8b84e4c40d..9578793957 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_posix.mk + + Called by firmware.mk to build POSIX (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". + +posix.mk + + Called by ../Makefile to set POSIX specific parameters if + PX4_TARGET_OS is set to "posix". 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 ebe7a09c20..f5f5d8188d 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 @@ -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,7 @@ $(LIBRARY_CLEANS): LIBRARY_MK=$(mkfile) \ clean +ifeq ($(PX4_TARGET_OS),nuttx) ################################################################################ # ROMFS generation ################################################################################ @@ -434,6 +435,7 @@ SRCS += $(BUILTIN_CSRC) EXTRA_CLEANS += $(BUILTIN_CSRC) +endif endif ################################################################################ @@ -456,6 +458,7 @@ endif # Build rules ################################################################################ +ifeq ($(PX4_TARGET_OS),nuttx) # # What we're going to build. # @@ -466,6 +469,7 @@ PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml .PHONY: firmware firmware: $(PRODUCT_BUNDLE) +endif # # Object files we will generate from sources @@ -487,6 +491,7 @@ $(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 # @@ -530,7 +535,22 @@ clean: $(MODULE_CLEANS) $(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) +# TODO +# Move above nuttx specific rules to $(MK_DIR)/nuttx_romfs.mk +endif +ifeq ($(PX4_TARGET_OS),posix) +include $(MK_DIR)/posix_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_nuttx.mk b/makefiles/firmware_nuttx.mk new file mode 100644 index 0000000000..77bb2bace0 --- /dev/null +++ b/makefiles/firmware_nuttx.mk @@ -0,0 +1,151 @@ +# +# 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) + diff --git a/makefiles/firmware_posix.mk b/makefiles/firmware_posix.mk new file mode 100644 index 0000000000..6acec4e733 --- /dev/null +++ b/makefiles/firmware_posix.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_qurt.mk b/makefiles/firmware_qurt.mk new file mode 100644 index 0000000000..41128fa60f --- /dev/null +++ b/makefiles/firmware_qurt.mk @@ -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. +# + +# +# 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) + +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) +sim: + $(SIM) $(SIMFLAGS) Build/qurt_default.build/mainapp 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/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/nuttx.mk b/makefiles/nuttx.mk index bf07441408..4ca1dc2ac6 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -34,6 +34,8 @@ # building firmware. # +MODULES += platforms/nuttx/px4_layer platforms/common + # # 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_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/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_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/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/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/nuttx_romfs.mk b/makefiles/nuttx_romfs.mk new file mode 100644 index 0000000000..42aeb3b8d4 --- /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 --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) + diff --git a/makefiles/posix.mk b/makefiles/posix.mk new file mode 100644 index 0000000000..13cb97f0db --- /dev/null +++ b/makefiles/posix.mk @@ -0,0 +1,40 @@ +# +# 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/common \ + platforms/posix/px4_layer + diff --git a/makefiles/posix/board_posix.mk b/makefiles/posix/board_posix.mk new file mode 100644 index 0000000000..93146b6a2e --- /dev/null +++ b/makefiles/posix/board_posix.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the POSIX port of PX4 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = NATIVE +CONFIG_BOARD = POSIXTEST + +include $(PX4_MK_DIR)/toolchain_native.mk diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk new file mode 100644 index 0000000000..78b40d3e87 --- /dev/null +++ b/makefiles/posix/config_posix_default.mk @@ -0,0 +1,78 @@ +# +# Makefile for the POSIXTEST *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/blinkm +MODULES += drivers/hil +MODULES += drivers/rgbled +MODULES += drivers/led +MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param +MODULES += systemcmds/mixer +MODULES += systemcmds/topic_listener + +# +# 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_pos_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/posix/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/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 + diff --git a/makefiles/posix_elf.mk b/makefiles/posix_elf.mk new file mode 100644 index 0000000000..edb1e32d76 --- /dev/null +++ b/makefiles/posix_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 POSIX 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/posix/main.cpp +$(WORK_DIR)mainapp: $(PRODUCT_SHARED_LIB) + $(PX4_BASE)/Tools/posix_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/qurt.mk b/makefiles/qurt.mk new file mode 100644 index 0000000000..612dd0bd16 --- /dev/null +++ b/makefiles/qurt.mk @@ -0,0 +1,39 @@ +# +# 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 + 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..8285ef28a8 --- /dev/null +++ b/makefiles/qurt/config_qurt_default.mk @@ -0,0 +1,78 @@ +# +# 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/led +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 + +# +# 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 + +# +# Unit tests +# +MODULES += platforms/qurt/tests/hello +MODULES += platforms/posix/tests/vcdev_test +MODULES += platforms/posix/tests/hrt_test +MODULES += platforms/posix/tests/wqueue + diff --git a/makefiles/qurt/qurt_eigen.patch b/makefiles/qurt/qurt_eigen.patch new file mode 100644 index 0000000000..9ea57403ba --- /dev/null +++ b/makefiles/qurt/qurt_eigen.patch @@ -0,0 +1,17 @@ +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 +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 { diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk new file mode 100644 index 0000000000..53cc8d8287 --- /dev/null +++ b/makefiles/qurt_elf.mk @@ -0,0 +1,77 @@ +# +# 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)) + +$(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,$@, $^) + +# +# 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 351b9f1b71..f14dfe531b 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -33,6 +33,13 @@ # Path and tool setup # +export PX4_TARGET_OS ?= nuttx + +# PX4_TARGET_OS can be nuttx, posix, or qurt +ifeq ($(PX4_TARGET_OS),) +$(error Use: make PX4_TARGET_OS= where is nuttx, posix, or qurt) +endif + # # Some useful paths. # @@ -47,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)/ diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 3b9fefb3ef..6c4e64f54c 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 # @@ -138,7 +138,6 @@ ARCHWARNINGS = -Wall \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ - -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ -Wmissing-declarations \ diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk new file mode 100644 index 0000000000..7135158aa8 --- /dev/null +++ b/makefiles/toolchain_hexagon.mk @@ -0,0 +1,314 @@ +# +# 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 +HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 +#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)) +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 +#DSPAL = $(PX4_BASE)/../dspal_libs/libdspal.a + + +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 + +QURTLIBS = \ + $(TOOLSLIB)/init.o \ + $(TOOLSLIB)/libc.a \ + $(TOOLSLIB)/libqcc.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 \ + $(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 +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 ?= -O0 + +# Base CPU flags for each of the supported architectures. +# +ARCHCPUFLAGS = -m$(V_ARCH) + + +# 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 -D__PX4_POSIX \ + -D__EXPORT= \ + -D__QDSP6_DINKUM_PTHREAD_TYPES__ \ + -Dnoreturn_function= \ + -Drestrict= \ + -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 \ + -I$(PX4_BASE)/../dspal/sys \ + -I$(PX4_BASE)/mavlink/include/mavlink \ + -I$(QURTLIB)/..//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-unused-parameter \ + -Wno-unused-function \ + -Wno-unused-variable \ + -Wno-gnu-array-member-paren-init \ + -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 + +# Flags we pass to the assembler +# +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ + $(EXTRADEFINES) \ + $(EXTRAAFLAGS) + +LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script +# Flags we pass to the linker +# +LDFLAGS += -g -nostdlib --section-start .start=0x1d000000\ + $(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) + +# 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) + @echo $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $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 +# +#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold +define PRELINK + @$(ECHO) "PRELINK: $1" + @$(MKDIR) -p $(dir $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 +# +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) $(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/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk new file mode 100644 index 0000000000..0b8223bfd0 --- /dev/null +++ b/makefiles/toolchain_native.mk @@ -0,0 +1,357 @@ +# +# Copyright (C) 2012-2014 PX4 Development Team. All rights reuint32_tserved. +# +# 2005 Modified for clang and GCC on POSIX: +# 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. +# + +# +# 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 + +ifneq ($(USE_GCC),1) + +HAVE_CLANG35:=$(shell clang-3.5 -dumpversion 2>/dev/null) + +# Clang will report 4.2.1 as GCC version +HAVE_CLANG:=$(shell clang -dumpversion) + +#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.2 +ifeq ($(HAVE_CLANG),4.2.1) +USE_GCC=0 +CLANGVER= +endif +endif + +# If no version of clang was found +ifeq ($(HAVE_CLANG35),) +ifeq ($(HAVE_CLANG),) +$(error Clang not found. Try make USE_GCC=1) +endif +endif +endif # USE_GCC is not 1 + +ifeq ($(USE_GCC),1) +# GCC Options: +CC = gcc +CXX = g++ +CPP = gcc -E + +# GCC Version +DEV_VER_SUPPORTED = 4.8.1 4.8.2 4.9.1 + +else +# Clang options +CC = clang$(CLANGVER) +CXX = clang++$(CLANGVER) +CPP = clang$(CLANGVER) -E + +# Clang GCC reported version +DEV_VER_SUPPORTED = 4.2.1 +endif + +#LD = ld.gold +LD = ld.bfd +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 -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 \ + -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++0x -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 \ + -Wno-packed \ + -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 +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+ +# -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 \ + -Wno-error=unused-local-typedefs \ + -Wno-error=enum-compare \ + -Wno-error=float-equal +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 -lrt + +# 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) + +LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script +# 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 +# +#$(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 + +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 + +# 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) + $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) + +endef + 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/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S new file mode 100644 index 0000000000..896062639e --- /dev/null +++ b/posix-configs/posixtest/init/rc.S @@ -0,0 +1,16 @@ +uorb start +simulator start -s +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 -d /tmp/ttyS0 +sensors start +hil mode_pwm +commander start +list_devices diff --git a/posix-configs/posixtest/scripts/ld.script b/posix-configs/posixtest/scripts/ld.script new file mode 100644 index 0000000000..32478e1e14 --- /dev/null +++ b/posix-configs/posixtest/scripts/ld.script @@ -0,0 +1,46 @@ +/**************************************************************************** + * ld.script + * + * 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 + * 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. + * + ****************************************************************************/ + +SECTIONS +{ + /* + * Construction data for parameters. + */ + __param : ALIGN(8) { + __param_start = .; + KEEP(*(__param*)) + __param_end = .; + } +} 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..3fbf3f9ddf 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 @@ -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/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 53d98ba9a1..1777e7f27c 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 @@ -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/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 cfa59eb3ae..185565d8b6 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -93,7 +93,7 @@ * */ -#include +#include #include #include @@ -106,7 +106,7 @@ #include #include -#include +#include #include #include @@ -140,7 +140,7 @@ public: virtual int init(); virtual int probe(); virtual int setMode(int mode); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); static const char *const script_names[]; @@ -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, 100000), + 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), @@ -316,6 +320,7 @@ BlinkM::init() ret = I2C::init(); if (ret != OK) { + warnx("I2C init failed"); return ret; } @@ -357,7 +362,7 @@ BlinkM::probe() } int -BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) +BlinkM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = ENOTTY; @@ -942,6 +947,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) { @@ -958,38 +967,43 @@ blinkm_main(int argc, char *argv[]) } if (!strcmp(argv[1], "start")) { - if (g_blinkm != nullptr) - errx(1, "already started"); + if (g_blinkm != nullptr) { + warnx("already started"); + return 1; + } g_blinkm = new BlinkM(i2cdevice, blinkmadr); - if (g_blinkm == nullptr) - errx(1, "new failed"); + if (g_blinkm == nullptr) { + warnx("new failed"); + return 1; + } if (OK != g_blinkm->init()) { delete g_blinkm; g_blinkm = nullptr; - errx(1, "no BlinkM found"); + warnx("init failed"); + return 1; } - exit(0); + return 0; } if (g_blinkm == nullptr) { fprintf(stderr, "not started\n"); blinkm_usage(); - exit(0); + return 0; } if (!strcmp(argv[1], "systemstate")) { g_blinkm->setMode(1); - exit(0); + return 0; } if (!strcmp(argv[1], "ledoff")) { g_blinkm->setMode(0); - exit(0); + return 0; } @@ -997,18 +1011,22 @@ blinkm_main(int argc, char *argv[]) for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) fprintf(stderr, " %s\n", BlinkM::script_names[i]); fprintf(stderr, " \n"); - exit(0); + return 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"); + int fd = px4_open(BLINKM0_DEVICE_PATH, 0); + if (fd < 0) { + warn("can't open BlinkM device"); + return 1; + } g_blinkm->setMode(0); - if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) - exit(0); + if (px4_ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) + return 0; + + px4_close(fd); blinkm_usage(); - exit(0); + return 0; } 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 c7d4f1ce47..2b2b5ccd7a 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 266642cbbf..6a718fdd24 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/cdev.cpp b/src/drivers/device/cdev.cpp index 6db0400373..3bc05b0c73 100644 --- a/src/drivers/device/cdev.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.h b/src/drivers/device/device.h index 4d4bed8358..e145635b37 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_POSIX) +#include "vdev.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.cpp b/src/drivers/device/device_nuttx.cpp similarity index 100% rename from src/drivers/device/device.cpp rename to src/drivers/device/device_nuttx.cpp diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h new file mode 100644 index 0000000000..a3a0640d2d --- /dev/null +++ b/src/drivers/device/device_nuttx.h @@ -0,0 +1,563 @@ +/**************************************************************************** + * + * 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 +#include + +#include + +/** + * Namespace encapsulating all device framework classes, functions and data. + */ +namespace device __EXPORT +{ + +typedef struct file file_t; + +/** + * 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(file_t *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(file_t *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(file_t *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(file_t *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(file_t *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(file_t *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(file_t *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(file_t *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(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 filp Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(file_t *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(file_t *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(px4_pollfd_struct_t *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_posix.cpp b/src/drivers/device/device_posix.cpp new file mode 100644 index 0000000000..771bee2471 --- /dev/null +++ b/src/drivers/device/device_posix.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 +#include +#include + +namespace device +{ + +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; +} + +Device::~Device() +{ + sem_destroy(&_lock); +} + +int +Device::init() +{ + int ret = OK; + + return ret; +} + +void +Device::log(const char *fmt, ...) +{ + va_list ap; + + PX4_INFO("[%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); + } +} + +int +Device::dev_read(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::dev_write(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::dev_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..e6a4dffc83 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(i2c_msg_s *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; - struct i2c_dev_s *_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_posix.h" +#endif 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/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/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp new file mode 100644 index 0000000000..473307b1cc --- /dev/null +++ b/src/drivers/device/i2c_posix.cpp @@ -0,0 +1,263 @@ +/**************************************************************************** + * + * 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" +#ifdef __PX4_LINUX +#include +#include +#endif +#include +#include +#include + +#define PX4_SIMULATE_I2C 1 +static int simulate = PX4_SIMULATE_I2C; + +namespace device +{ + +I2C::I2C(const char *name, + const char *devname, + int bus, + uint16_t address) : + // base class + VDev(name, devname), + // public + // protected + _retries(0), + // private + _bus(bus), + _address(address), + _fd(-1) +{ + 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; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; +} + +I2C::~I2C() +{ + if (_fd >= 0) { + ::close(_fd); + _fd = -1; + } +} + +int +I2C::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 will create device node, etc + ret = VDev::init(); + + if (ret != PX4_OK) { + debug("VDev::init failed"); + return ret; + } + + _fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY); + if (_fd < 0) { + debug("px4_open failed of device %s", get_devname()); + return PX4_ERROR; + } + +#ifdef __PX4_QURT + simulate = true; +#endif + + if (simulate) { + _fd = 10000; + } + 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; +} + +int +I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + struct i2c_msg msgv[2]; + unsigned msgs; + struct i2c_rdwr_ioctl_data packets; + 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; + + if (send_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = 0; + 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].buf = recv; + msgv[msgs].len = recv_len; + msgs++; + } + + if (msgs == 0) + return -EINVAL; + + packets.msgs = msgv; + packets.nmsgs = msgs; + + if (simulate) { + //warnx("I2C SIM: transfer_4 on %s", get_devname()); + ret = PX4_OK; + } + else { + ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + if (ret < 0) { + warnx("I2C transfer failed"); + return 1; + } + } + + /* success */ + if (ret == PX4_OK) + break; + + } while (retry_count++ < _retries); + + return ret; +} + +int +I2C::transfer(struct i2c_msg *msgv, unsigned msgs) +{ + struct i2c_rdwr_ioctl_data packets; + 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 { + packets.msgs = msgv; + packets.nmsgs = msgs; + + if (simulate) { + warnx("I2C SIM: transfer_2 on %s", get_devname()); + ret = PX4_OK; + } + else { + ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + } + if (ret < 0) { + warnx("I2C transfer failed"); + return 1; + } + + /* success */ + if (ret == PX4_OK) + break; + + } while (retry_count++ < _retries); + + return ret; +} + +int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; + + switch (cmd) { + case I2C_RDWR: + warnx("Use I2C::transfer, not ioctl"); + return 0; + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } +} + +ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) +{ + if (simulate) { + // FIXME no idea what this should be + warnx ("2C SIM I2C::read"); + return 0; + } + + return ::read(_fd, buffer, buflen); +} + +ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) +{ + if (simulate) { + warnx ("2C SIM I2C::write"); + return buflen; + } + + return ::write(_fd, buffer, buflen); +} + +} // namespace device diff --git a/src/drivers/device/i2c_posix.h b/src/drivers/device/i2c_posix.h new file mode 100644 index 0000000000..c05100ae43 --- /dev/null +++ b/src/drivers/device/i2c_posix.h @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 "vdev.h" + +#include +#ifdef __PX4_LINUX +#include +#include +#endif +#include + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on I2C + */ +class __EXPORT I2C : public VDev +{ + +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 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 + */ + I2C(const char *name, + const char *devname, + int bus, + uint16_t address); + virtual ~I2C(); + + virtual int init(); + + 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. + * + * 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(struct i2c_msg *msgv, unsigned msgs); + +private: + uint16_t _address; + int _fd; + std::string _dname; + + I2C(const device::I2C&); + I2C operator=(const device::I2C&); +}; + +} // namespace device + +#endif /* _DEVICE_I2C_H */ diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 8c716d9cdb..a9e964ef94 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -35,8 +35,20 @@ # Build the device driver framework. # -SRCS = cdev.cpp \ - device.cpp \ - i2c.cpp \ +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = \ + device_nuttx.cpp \ + cdev.cpp \ + i2c_nuttx.cpp \ pio.cpp \ - spi.cpp + spi.cpp +else +SRCS = \ + device_posix.cpp \ + vdev.cpp \ + vfile.cpp \ + vdev_posix.cpp \ + i2c_posix.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..1790289338 --- /dev/null +++ b/src/drivers/device/ringbuffer.cpp @@ -0,0 +1,400 @@ +/**************************************************************************** + * + * 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)); +} + +// FIXME - clang crashes on this get() call +#ifdef __PX4_QURT +#define __PX4_SBCAP my_sync_bool_compare_and_swap +static inline 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) +{ + 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 (!__PX4_SBCAP(&_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 b26e2e7c83..1c65c02135 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,9 @@ 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/%u (%u/%u @ %p)\n", - name, - _num_items, - _num_items * _item_size, - _head, - _tail, - _buf); -} +#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/drivers/device/sim.cpp b/src/drivers/device/sim.cpp new file mode 100644 index 0000000000..787a3826c9 --- /dev/null +++ b/src/drivers/device/sim.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 +#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) +{ + + 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; + _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) { + PX4_ERR("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) { + PX4_DEBUG("SIM: sending %d bytes", send_len); + } + + if (recv_len > 0) { + PX4_DEBUG("SIM: receiving %d bytes", recv_len); + + // TODO - write data to recv; + } + + PX4_DEBUG("I2C SIM: transfer_4 on %s", _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..139967f6e8 --- /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. + */ + virtual 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/spi.h b/src/drivers/device/spi.h index 1d98376898..2f44f3cafa 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 { @@ -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 new file mode 100644 index 0000000000..58db85169e --- /dev/null +++ b/src/drivers/device/vdev.cpp @@ -0,0 +1,524 @@ +/**************************************************************************** + * + * 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 vcdev.cpp + * + * Virtual character device base class. + */ + +#include "px4_posix.h" +#include "vdev.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 30 +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. + */ + +VDev::VDev(const char *name, + const char *devname) : + // base class + Device(name), + // public + // protected + _pub_blocked(false), + // private + _devname(devname), + _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); +} + +int +VDev::register_class_devname(const char *class_devname) +{ + PX4_DEBUG("VDev::register_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); + if (ret == OK) break; + class_instance++; + } + + if (class_instance == 4) { + return ret; + } + return class_instance; +} + +int +VDev::register_driver(const char *name, void *data) +{ + PX4_DEBUG("VDev::register_driver"); + 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; + PX4_DEBUG("Unregistered DEV %s", name); + ret = PX4_OK; + break; + } + } + return ret; +} + +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]; + PX4_DEBUG("Unregistered class DEV %s", name); + devmap[i] = NULL; + return PX4_OK; + } + } + return -EINVAL; +} + +int +VDev::init() +{ + PX4_DEBUG("VDev::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 +VDev::open(file_t *filep) +{ + PX4_DEBUG("VDev::open"); + int ret = PX4_OK; + + lock(); + /* increment the open count */ + _open_count++; + + if (_open_count == 1) { + + /* first-open callback may decline the open */ + ret = open_first(filep); + + if (ret != PX4_OK) + _open_count--; + } + + unlock(); + + return ret; +} + +int +VDev::open_first(file_t *filep) +{ + PX4_DEBUG("VDev::open_first"); + return PX4_OK; +} + +int +VDev::close(file_t *filep) +{ + PX4_DEBUG("VDev::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(filep); + + } else { + ret = -EBADF; + } + + unlock(); + + return ret; +} + +int +VDev::close_last(file_t *filep) +{ + PX4_DEBUG("VDev::close_last"); + return PX4_OK; +} + +ssize_t +VDev::read(file_t *filep, char *buffer, size_t buflen) +{ + PX4_DEBUG("VDev::read"); + return -ENOSYS; +} + +ssize_t +VDev::write(file_t *filep, const char *buffer, size_t buflen) +{ + 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; + + switch (cmd) { + + /* fetch a pointer to the driver's private data */ + case DIOC_GETPRIV: + *(void **)(uintptr_t)arg = (void *)this; + ret = PX4_OK; + break; + case DEVIOCSPUBBLOCK: + _pub_blocked = (arg != 0); + ret = PX4_OK; + break; + case DEVIOCGPUBBLOCK: + ret = _pub_blocked; + break; + case DEVIOCGDEVICEID: + ret = (int)_device_id.devid; + PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); + default: + break; + } + + return ret; +} + +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; + + /* + * Lock against pollnotify() (and possibly other callers) + */ + lock(); + + if (setup) { + /* + * Save the file pointer in the pollfd for the subclass' + * benefit. + */ + fds->priv = (void *)filep; + PX4_DEBUG("VDev::poll: fds->priv = %p", filep); + + /* + * 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(filep); + + /* 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 +VDev::poll_notify(pollevent_t events) +{ + PX4_DEBUG("VDev::poll_notify events = %0x", events); + + /* lock against poll() as well as other wakeups */ + lock(); + + for (unsigned i = 0; i < _max_pollwaiters; i++) + if (nullptr != _pollset[i]) + poll_notify_one(_pollset[i], events); + + unlock(); +} + +void +VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +{ + PX4_DEBUG("VDev::poll_notify_one"); + int value; + sem_getvalue(fds->sem, &value); + + /* update the reported event set */ + fds->revents |= fds->events & events; + + 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 */ + if ((fds->revents != 0) && (value <= 0)) + sem_post(fds->sem); +} + +pollevent_t +VDev::poll_state(file_t *filep) +{ + PX4_DEBUG("VDev::poll_notify"); + /* by default, no poll events to report */ + return 0; +} + +int +VDev::store_poll_waiter(px4_pollfd_struct_t *fds) +{ + /* + * Look for a free slot. + */ + PX4_DEBUG("VDev::store_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (nullptr == _pollset[i]) { + + /* save the pollfd */ + _pollset[i] = fds; + + return PX4_OK; + } + } + + return -ENOMEM; +} + +int +VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) +{ + PX4_DEBUG("VDev::remove_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (fds == _pollset[i]) { + + _pollset[i] = nullptr; + return PX4_OK; + + } + } + + PX4_WARN("poll: bad fd state"); + return -EINVAL; +} + +VDev *VDev::getDev(const char *path) +{ + PX4_DEBUG("VDev::getDev"); + int i=0; + for (; iname, path); + //} + if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) { + return (VDev *)(devmap[i]->cdev); + } + } + return NULL; +} + +void VDev::showDevices() +{ + int i=0; + PX4_INFO("Devices:\n"); + for (; iname, "/dev/", 5) == 0) { + PX4_INFO(" %s\n", devmap[i]->name); + } + } +} + +void VDev::showTopics() +{ + int i=0; + PX4_INFO("Devices:\n"); + for (; iname, "/obj/", 5) == 0) { + PX4_INFO(" %s\n", devmap[i]->name); + } + } +} + +void VDev::showFiles() +{ + int i=0; + PX4_INFO("Files:\n"); + for (; iname, "/obj/", 5) != 0 && + strncmp(devmap[i]->name, "/dev/", 5) != 0) { + PX4_INFO(" %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; +} + +} // namespace device + diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h new file mode 100644 index 0000000000..7d5b8811b2 --- /dev/null +++ b/src/drivers/device/vdev.h @@ -0,0 +1,535 @@ +/**************************************************************************** + * + * 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 +{ + +struct file_t { + int fd; + int flags; + mode_t mode; + void *priv; + void *vdev; + + 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) {} +}; + +/** + * Fundamental base class for all physical drivers (I2C, SPI). + * + * This class provides the basic driver template for I2C and SPI devices + */ +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(); + + /** + * 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 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. + */ + virtual 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. + */ + virtual int dev_ioctl(unsigned operation, unsigned &arg); + + /* + device bus types for DEVID + */ + enum DeviceBusType { + DeviceBusType_UNKNOWN = 0, + DeviceBusType_I2C = 1, + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, + DeviceBusType_SIM = 4, + }; + + /* + 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 + */ + 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() { + debug("lock"); + do {} while (sem_wait(&_lock) != 0); + } + + /** + * Release the driver lock. + */ + void unlock() { + debug("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 virtual character device + */ +class __EXPORT VDev : public Device +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + */ + VDev(const char *name, const char *devname); + + /** + * Destructor + */ + virtual ~VDev(); + + virtual int init(); + + /** + * Perform a poll setup/teardown operation. Based on NuttX implementation. + * + * This is handled internally and should not normally be overridden. + * + * @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(file_t *filep, 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 filep Pointer to the NuttX file structure. + * @return OK if the open is allowed, -errno otherwise. + */ + virtual int open(file_t *filep); + + /** + * 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 filep Pointer to the NuttX file structure. + * @return OK if the close was successful, -errno otherwise. + */ + virtual int close(file_t *filep); + + /** + * Perform a read from the device. + * + * The default implementation returns -ENOSYS. + * + * @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(file_t *filep, char *buffer, size_t buflen); + + /** + * Perform a write to the device. + * + * The default implementation returns -ENOSYS. + * + * @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(file_t *filep, const char *buffer, size_t buflen); + + /** + * Perform a logical seek operation on the device. + * + * The default implementation returns -ENOSYS. + * + * @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(file_t *filep, 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 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(file_t *filep, int cmd, unsigned long arg); + + static VDev *getDev(const char *path); + static void showFiles(void); + static void showDevices(void); + static void showTopics(void); + static const char *devList(unsigned int *next); + static const char *topicList(unsigned int *next); + +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 filep The file that's interested. + * @return The current set of poll events. + */ + virtual pollevent_t poll_state(file_t *filep); + + /** + * 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 filep Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(file_t *filep); + + /** + * 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 filep Pointer to the NuttX file structure. + * @return OK if the open should return OK, -errno otherwise. + */ + virtual int close_last(file_t *filep); + + /** + * 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 */ + VDev(const VDev&); + //VDev operator=(const VDev&); +}; + +#if 0 +/** + * Abstract class for character device accessed via PIO + */ +class __EXPORT VPIO : 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; +}; +#endif + +} // 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/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp new file mode 100644 index 0000000000..0e94e88dfe --- /dev/null +++ b/src/drivers/device/vdev_posix.cpp @@ -0,0 +1,322 @@ +/**************************************************************************** + * + * 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 vdev_posix.cpp + * + * POSIX-like API for virtual character device + */ + +#include +#include +#include +#include "device.h" +#include "vfile.h" + +#include +#include +#include +#include +#include + +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)); + + PX4_DEBUG("timer_handler: Timer expired"); + return 0; +} + +#define PX4_MAX_FD 100 +static device::file_t *filemap[PX4_MAX_FD] = {}; + +int px4_errno; + +inline bool valid_fd(int fd) +{ + return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); +} + +int px4_open(const char *path, int flags, ...) +{ + PX4_DEBUG("px4_open"); + VDev *dev = VDev::getDev(path); + int ret = 0; + int i; + mode_t mode; + + 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); + mode = va_arg(p, mode_t); + va_end(p); + + // Create the file + PX4_DEBUG("Creating virtual file %s", path); + dev = VFile::createFile(path, mode); + } + if (dev) { + for (i=0; iopen(filemap[i]); + } + else { + ret = -ENOENT; + } + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + return -1; + } + PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); + return filemap[i]->fd; +} + +int px4_close(int fd) +{ + int ret; + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_close fd = %d", fd); + ret = dev->close(filemap[fd]); + filemap[fd] = NULL; + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + return ret; +} + +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", fd); + ret = dev->read(filemap[fd], (char *)buffer, buflen); + } + else { + ret = -EINVAL; + } + 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; + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_write fd = %d", fd); + ret = dev->write(filemap[fd], (const char *)buffer, buflen); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + return ret; +} + +int px4_ioctl(int fd, int cmd, unsigned long arg) +{ + PX4_DEBUG("px4_ioctl fd = %d", fd); + int ret = 0; + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + ret = dev->ioctl(filemap[fd], cmd, arg); + } + else { + ret = -EINVAL; + } + if (ret < 0) { + px4_errno = -ret; + } + + return (ret == 0) ? PX4_OK : PX4_ERROR; +} + +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; + + PX4_DEBUG("Called px4_poll timeout = %d", timeout); + sem_init(&sem, 0, 0); + + // For each fd + for (i=0; ivdev);; + PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + + if (ret < 0) + break; + } + } + + 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; ivdev);; + PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], false); + + if (ret < 0) + break; + + if (fds[i].revents) + count += 1; + } + } + } + +cleanup: + sem_destroy(&sem); + + return count; +} + +int px4_fsync(int fd) +{ + return 0; +} + +void px4_show_devices() +{ + VDev::showDevices(); +} + +void px4_show_topics() +{ + VDev::showTopics(); +} + +void px4_show_files() +{ + VDev::showFiles(); +} + +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/drivers/device/vfile.cpp b/src/drivers/device/vfile.cpp new file mode 100644 index 0000000000..2d2d81c816 --- /dev/null +++ b/src/drivers/device/vfile.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 vdev_file.cpp + * Virtual file + * + * @author Mark Charlebois + */ + +#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/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_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/drivers/drv_device.h b/src/drivers/drv_device.h index 19d55cef38..3477dde9fd 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) @@ -65,5 +65,8 @@ */ #define DEVIOCGDEVICEID _DEVICEIOC(2) +#ifdef __PX4_POSIX +#define DIOC_GETPRIV SIOCDEVPRIVATE +#endif #endif /* _DRV_DEVICE_H */ diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index f18c8162d7..a0faab8098 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_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_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \ + !defined(CONFIG_ARCH_BOARD_POSIXTEST) # error No CONFIG_ARCH_BOARD_xxxx set #endif /* 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_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/drivers/drv_led.h b/src/drivers/drv_led.h index f3e8164714..dbcfde91fa 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include @@ -52,9 +53,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_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_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_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/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/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_sensor.h b/src/drivers/drv_sensor.h index 467dace082..a98bcafc7a 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 @@ -51,15 +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_ACC_DEVTYPE_LSM303D 0x11 -#define DRV_ACC_DEVTYPE_BMA180 0x12 -#define DRV_ACC_DEVTYPE_MPU6000 0x13 -#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_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_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 @@ -69,7 +74,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/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/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/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/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index ae75d3a14a..db016b363a 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 e3706cf671..965c0e3e5d 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 @@ -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..0e706efc21 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -48,7 +48,10 @@ * driver. Use instead the normal FMU or IO driver. */ -#include +#include +#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/module.mk b/src/drivers/hil/module.mk index f1fc49fb3d..77213b4be7 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -38,5 +38,5 @@ MODULE_COMMAND = hil SRCS = hil.cpp - MAXOPTIMIZATION = -Os + diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5bf88da3df..92ffa80665 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..29680a279f 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..a1a5b080c5 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/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 82c18b5c14..f036987d7d 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..69c72b5b53 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -37,9 +37,11 @@ * LED driver. */ -#include +#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() { + debug("LED::init"); +#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/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 b05df6a6af..e0863f46cd 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..a20bf7c7c6 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/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..d1484a2f01 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -41,7 +41,7 @@ * */ -#include +#include #include #include @@ -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/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 9880544ecb..9dd7cc7048 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/module.mk b/src/drivers/ms5611/module.mk index ee74058fc1..7ff49d7354 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_posix.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 c946e38cb8..1d6239467d 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,7 @@ 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)); +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_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index ca651409f9..730fd9b968 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -38,13 +38,14 @@ */ /* XXX trim includes */ -#include +#include +#include #include #include #include #include -#include +//#include #include #include @@ -70,16 +71,20 @@ 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); +#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. @@ -98,7 +103,7 @@ private: /** * Read the MS5611 PROM * - * @return OK if the PROM reads successfully. + * @return PX4_OK if the PROM reads successfully. */ int _read_prom(); @@ -111,7 +116,13 @@ 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", +#ifdef __PX4_NUTTX +nullptr, bus, 0, 400000 +#else +"/dev/MS5611_I2C", bus, 0 +#endif +), _prom(prom) { } @@ -128,7 +139,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 +150,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 +162,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; @@ -171,19 +182,20 @@ MS5611_I2C::ioctl(unsigned operation, unsigned &arg) return ret; } +#ifdef __PX4_NUTTX int 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,15 +208,16 @@ 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; } +#endif int @@ -254,7 +267,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 +277,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.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp similarity index 99% rename from src/drivers/ms5611/ms5611.cpp rename to src/drivers/ms5611/ms5611_nuttx.cpp index ef94d03633..9d0e436eeb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.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_posix.cpp b/src/drivers/ms5611/ms5611_posix.cpp new file mode 100644 index 0000000000..9e99d926e2 --- /dev/null +++ b/src/drivers/ms5611/ms5611_posix.cpp @@ -0,0 +1,1274 @@ +/**************************************************************************** + * + * 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 + +#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, + MS5611_BUS_SIM_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::VDev +{ +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) : + VDev("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; + warnx("MS5611::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(MS5611_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(MS5611_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 +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 ((long)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 VDev::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) && + ((long)_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)) { + 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)) { + 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 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 +#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 +#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])) + +bool start_bus(struct ms5611_bus_option &bus); +struct ms5611_bus_option &find_bus(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(); + +/** + * 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; + 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(enum MS5611_BUS busid) +{ + uint8_t i; + bool started = false; + + for (i=0; iprint_info(); + } + } + return 0; +} + +/** + * Calculate actual MSL pressure given current altitude + */ +int +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 = px4_open(bus.devpath, O_RDONLY); + + 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)) { + 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'"); + warnx("options:"); + warnx(" -X (external I2C bus)"); + warnx(" -I (intternal I2C bus)"); + warnx(" -S (Simulation bus)"); +} + +} // namespace + +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 */ + int myoptind = 1; + const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "XIS", &myoptind, &myoptarg)) != EOF) { + printf("ch = %d\n", ch); + switch (ch) { + case 'X': + busid = MS5611_BUS_I2C_EXTERNAL; + break; + case 'I': + busid = MS5611_BUS_I2C_INTERNAL; + break; + case 'S': + busid = MS5611_BUS_SIM_EXTERNAL; + break; + default: + ms5611::usage(); + return 1; + } + } + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) + ret = ms5611::start(busid); + + /* + * Test the driver/device. + */ + else if (!strcmp(verb, "test")) + ret = ms5611::test(busid); + + /* + * Reset the driver. + */ + else if (!strcmp(verb, "reset")) + ret = ms5611::reset(busid); + + /* + * Print driver information. + */ + else if (!strcmp(verb, "info")) + ret = ms5611::info(); + + /* + * Perform MSL pressure calibration given an altitude in metres + */ + else if (!strcmp(verb, "calibrate")) { + if (argc < 2) + errx(1, "missing altitude"); + + long altitude = strtol(argv[optind+1], nullptr, 10); + + ret = ms5611::calibrate(altitude, busid); + } + else { + ms5611::usage(); + warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); + return 1; + } + return ret; +} diff --git a/src/drivers/ms5611/ms5611_sim.cpp b/src/drivers/ms5611/ms5611_sim.cpp new file mode 100644 index 0000000000..ab7656b9c3 --- /dev/null +++ b/src/drivers/ms5611/ms5611_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 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); + + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); +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", "/dev/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; +} + +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; +} diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 554a1d6596..7ec7718157 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -38,13 +38,13 @@ */ /* XXX trim includes */ -#include +#include #include #include #include #include -#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 b343a3e30a..f35957caae 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..e876ac75ce 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 @@ -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 176f5b241d..6de4f993a8 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 @@ -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/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/module.mk b/src/drivers/rgbled/module.mk index c287e35f31..924624cebb 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -3,6 +3,7 @@ # MODULE_COMMAND = rgbled + SRCS = rgbled.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 512d6318da..6491253017 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -40,7 +40,8 @@ * @author Anton Babushkin */ -#include +#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, "no RGB led on bus #%d", i2cdevice); + warnx("no RGB led on bus #%d", i2cdevice); + 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, "no RGB led on bus #%d", i2cdevice); + warnx("no RGB led on bus #%d", i2cdevice); + 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/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index ea7178076b..aa5e6dcc0b 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/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..d6e78fd821 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 @@ -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_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_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 ab67bd2532..027511df20 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 @@ -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_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_task_spawn_cmd("flow_position_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, 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..139a182b22 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 @@ -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_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_task_spawn_cmd("matlab_csv_serial", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index e7ab9eb5af..a39f0ecd25 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -44,37 +44,39 @@ using namespace px4; PublisherExample::PublisherExample() : - _n(), + _n(appState), _rc_channels_pub(_n.advertise()), _v_att_pub(_n.advertise()), _parameter_update_pub(_n.advertise()) { } +px4::AppState PublisherExample::appState; + int PublisherExample::main() { px4::Rate loop_rate(10); - while (px4::ok()) { + while (!appState.exitRequested()) { loop_rate.sleep(); _n.spinOnce(); /* 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); } diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 0a66d3edca..2841320734 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::AppState appState; protected: px4::NodeHandle _n; px4::Publisher *_rc_channels_pub; diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index 4ff2cebfbc..2379d7aa68 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -68,12 +68,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/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 6e99939d14..7440a07eb4 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 @@ -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/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..887ea86309 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_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 5a23dc80b8..77bfcb8d3e 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -45,11 +45,11 @@ 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: [%" PRIu64 "]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : - _n(), + _n(_appState), _p_sub_interv("SUB_INTERV", PARAM_SUB_INTERV_DEFAULT), _p_test_float("SUB_TESTF", PARAM_SUB_TESTF_DEFAULT) { @@ -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): [%" PRIu64 "]", 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: [%" PRIu64 "]", _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): [%" PRIu64 "]", 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): [%" PRIu64 "]", msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 9eb5dd2a0a..4fdd236366 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; + AppState _appState; void rc_channels_callback(const px4_rc_channels &msg); void vehicle_attitude_callback(const px4_vehicle_attitude &msg); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 757e8ec521..424e49e9c1 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -68,12 +68,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/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" { diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index f595467b36..30b9d9ec28 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -43,7 +43,7 @@ */ #include -#include +#include #include #include #include @@ -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/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/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index 0ae545f1a5..4b1273c7a1 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_POSIX) #define M_PI_F 3.14159265358979323846f #endif 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/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; diff --git a/src/lib/version/version.h b/src/lib/version/version.h index b79fee182e..ddf13822df 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_POSIXTEST +#define HW_ARCH "LINUXTEST" +#endif #endif /* VERSION_H_ */ 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 fb7cda8c42..6f22786a3d 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,9 @@ * @author Thomas Gubler */ -#include +#include +#include +#include #include #include #include @@ -49,9 +51,6 @@ #include #include #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); } /** @@ -114,12 +113,13 @@ 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[]) { if (argc < 2) { usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -127,39 +127,39 @@ 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; - 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, 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; } /* @@ -298,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 */ @@ -525,7 +525,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 (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)); @@ -535,7 +535,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } if (last_data > 0 && raw.timestamp - last_data > 30000) { - warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data); + warnx("sensor data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); } last_data = raw.timestamp; @@ -572,8 +572,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; - if (isfinite(att.q[0]) && isfinite(att.q[1]) - && isfinite(att.q[2]) && isfinite(att.q[3])) { + 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/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 13a9fa5f6f..30eb2a96e0 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 */ 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 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/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 82bcbc66ff..68a61488f9 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..987b930d28 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 @@ -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/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/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 96e7757da0..9d38e8179a 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -40,7 +40,8 @@ * @author Johan Jansen */ -#include +#include +#include #include #include #include @@ -76,7 +77,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) { @@ -89,7 +90,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)); @@ -100,7 +101,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, @@ -110,7 +111,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -120,7 +121,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) { @@ -133,7 +134,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)); @@ -144,7 +145,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, @@ -156,7 +157,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 */ @@ -177,7 +178,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, } out: - close(fd); + px4_close(fd); return success; } @@ -187,7 +188,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) { @@ -200,7 +201,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)); @@ -211,7 +212,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, @@ -221,7 +222,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -231,7 +232,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) { @@ -242,7 +243,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - close(fd); + px4_close(fd); return success; } @@ -261,7 +262,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_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index f4fd88fa22..1155631ea3 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -127,11 +127,13 @@ #include "calibration_routines.h" #include "commander_helper.h" +#include +#include #include #include #include #include -#include +//#include #include #include #include @@ -191,16 +193,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_ERROR_RESET_CAL_MSG, s); @@ -279,14 +281,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, CAL_QGC_FAILED_MSG, "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) { @@ -385,7 +387,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; } - close(worker_data.subs[i]); + px4_close(worker_data.subs[i]); } } @@ -434,7 +436,7 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc /* combine board rotation with offset rotation */ board_rotation = board_rotation_offset * board_rotation; - 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]; @@ -449,7 +451,7 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc /* 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) { @@ -572,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; @@ -591,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; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 837a3f1e83..f2f4f0c64f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -41,10 +41,13 @@ #include "calibration_routines.h" #include "commander_helper.h" +#include +#include #include +#include #include #include -#include +#include #include #include #include @@ -91,17 +94,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, "[cal] airspeed offset zero failed"); } - close(fd); + px4_close(fd); } int cancel_sub = calibrate_cancel_subscribe(); @@ -133,11 +136,11 @@ int do_airspeed_calibration(int mavlink_fd) } /* 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); @@ -158,16 +161,16 @@ 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 = 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, "[cal] airspeed offset update failed"); } - close(fd_scale); + px4_close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { @@ -206,11 +209,11 @@ int do_airspeed_calibration(int mavlink_fd) } /* 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); @@ -270,7 +273,7 @@ int do_airspeed_calibration(int mavlink_fd) normal_return: calibrate_cancel_unsubscribe(cancel_sub); - close(diff_pres_sub); + px4_close(diff_pres_sub); // This give a chance for the log messages to go out of the queue before someone else stomps on then sleep(1); diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index e854c9aa7e..68e094f7c4 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -38,7 +38,11 @@ * @author Lorenz Meier */ +#include +#include +#include #include +#include #include #include #include @@ -54,9 +58,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) @@ -245,7 +246,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = accel_sub; fds[0].events = POLLIN; @@ -261,7 +262,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_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_combined), accel_sub, &sensor); @@ -527,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)); @@ -553,4 +554,4 @@ bool calibrate_cancel_check(int mavlink_fd, int cancel_sub) } return false; -} \ No newline at end of file +} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7e29dbd94d..298cd0289b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -42,9 +42,12 @@ * @author Anton Babushkin */ -#include +#include +#include +#include #include #include +#include #include #include #include @@ -53,8 +56,10 @@ #include #include #include -#include +//#include +#ifndef __PX4_QURT #include +#endif #include #include #include @@ -254,6 +259,7 @@ int commander_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -261,11 +267,11 @@ 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; - daemon_task = task_spawn_cmd("commander", + daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 3400, @@ -276,13 +282,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; @@ -294,18 +301,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")) { @@ -324,9 +331,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"); @@ -338,25 +346,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) @@ -366,7 +374,6 @@ void usage(const char *reason) } fprintf(stderr, "usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); - exit(1); } void print_status() @@ -545,7 +552,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; @@ -928,7 +935,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 */ @@ -2593,8 +2600,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)); @@ -2605,7 +2614,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; @@ -2613,7 +2622,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) { @@ -2662,13 +2671,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/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c0f8561fda..b902952ae2 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -41,6 +41,8 @@ * */ +#include +#include #include #include #include @@ -117,7 +119,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,24 +132,24 @@ 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; } - return OK; + return PX4_OK; } 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) @@ -158,7 +160,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; @@ -236,22 +238,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; } @@ -259,7 +261,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); @@ -271,11 +273,11 @@ int led_init() void led_deinit() { if (leds >= 0) { - close(leds); + px4_close(leds); } if (rgbleds >= 0) { - close(rgbleds); + px4_close(rgbleds); } } @@ -284,7 +286,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) @@ -292,7 +294,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) @@ -300,7 +302,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) @@ -309,7 +311,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) @@ -318,7 +320,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) @@ -327,7 +329,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); } unsigned battery_get_n_cells() { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 5f43fd77cd..40d84ff008 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/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 fc6398bd68..817cbcdb0e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -42,10 +42,13 @@ #include "calibration_routines.h" #include "commander_helper.h" +#include +#include #include +#include #include #include -#include +#include #include #include #include @@ -82,7 +85,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) 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; @@ -97,7 +100,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) return calibrate_return_cancelled; } - int poll_ret = poll(&fds[0], max_gyros, 1000); + int poll_ret = px4_poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -180,11 +183,11 @@ 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); + 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) { mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); @@ -225,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) { @@ -251,7 +254,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 (res == OK) { @@ -273,15 +276,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)&worker_data.gyro_scale[s]); - close(fd); + 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/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 04e66a5cb6..10d32b0995 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -42,11 +42,14 @@ #include "calibration_routines.h" #include "calibration_messages.h" +#include +#include #include +#include #include #include #include -#include +#include #include #include #include @@ -119,16 +122,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_ERROR_RESET_CAL_MSG, cur_mag); @@ -136,7 +139,7 @@ int do_mag_calibration(int mavlink_fd) /* calibrate range */ if (result == OK) { - result = ioctl(fd, MAGIOCCALIBRATE, fd); + result = px4_ioctl(fd, MAGIOCCALIBRATE, fd); if (result != OK) { mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag); @@ -145,7 +148,7 @@ int do_mag_calibration(int mavlink_fd) } } - close(fd); + px4_close(fd); } // Calibrate all mags at the same time @@ -274,7 +277,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } // 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) { @@ -283,7 +286,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta 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]); } } @@ -443,7 +446,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag &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_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag); result = calibrate_return_error; } @@ -467,14 +470,14 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag // 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_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag); result = calibrate_return_error; } if (result == calibrate_return_ok) { - if (ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) { + if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) { mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag); result = calibrate_return_error; } @@ -485,7 +488,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag mscale.y_offset = sphere_y[cur_mag]; mscale.z_offset = sphere_z[cur_mag]; - if (ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) { + if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag); result = calibrate_return_error; } @@ -493,7 +496,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag // Mag device no longer needed if (fd_mag >= 0) { - close(fd_mag); + px4_close(fd_mag); } if (result == calibrate_return_ok) { diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 5331428c23..c16a606711 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -54,5 +54,7 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os +ifeq ($(PX4_TARGET_OS),nuttx) EXTRACXXFLAGS = -Wframe-larger-than=2200 +endif 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/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index cdcc12043e..fc05cce6e9 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) { @@ -255,8 +258,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) { @@ -356,6 +361,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 */ @@ -378,77 +498,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/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp new file mode 100644 index 0000000000..2d3b78ddaf --- /dev/null +++ b/src/modules/commander/state_machine_helper_posix.cpp @@ -0,0 +1,634 @@ +/**************************************************************************** + * + * 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 "state_machine_helper.h" +#include "commander_helper.h" +#include "PreflightCheck.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, 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, 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 +}; + +// 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) { + 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 + 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) +{ + /* 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) { + checkAirspeed = true; + } + + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); +} diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b442b74303..e454568825 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 @@ -129,6 +131,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 static const char *k_data_manager_device_path = "/fs/microsd/dataman"; /* The data manager work queues */ @@ -668,7 +671,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); @@ -794,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; } @@ -828,31 +831,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(); @@ -862,8 +874,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/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..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 @@ -43,7 +43,11 @@ #include "AttitudePositionEstimatorEKF.h" #include "estimator_22states.h" -#include +#include +#include +#include +#include +#include #include #include #include @@ -228,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) { @@ -240,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); @@ -252,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); @@ -281,7 +285,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 +494,8 @@ void AttitudePositionEstimatorEKF::task_main() _filter_start_time = hrt_absolute_time(); if (!_ekf) { - errx(1, "OUT OF MEM!"); + warnx("OUT OF MEM!"); + return; } /* @@ -517,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; @@ -535,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) { @@ -696,10 +701,8 @@ void AttitudePositionEstimatorEKF::task_main() _task_running = false; - warnx("exiting.\n"); - _estimator_task = -1; - _exit(0); + return; } void AttitudePositionEstimatorEKF::initializeGPS() @@ -1035,11 +1038,11 @@ 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, - (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, + (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); if (_estimator_task < 0) { @@ -1155,7 +1158,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 (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } @@ -1167,9 +1170,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 (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 +1181,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 (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]; @@ -1528,25 +1531,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 +1565,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/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/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 967402fff7..5d56dbaae3 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 @@ -48,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() : @@ -2392,9 +2389,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 (PX4_ISFINITE(storedStates[i][bestStoreIndex])) { statesForFusion[i] = storedStates[i][bestStoreIndex]; - } else if (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 +2403,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 (PX4_ISFINITE(states[i])) { statesForFusion[i] = states[i]; } else { ret++; @@ -2630,7 +2627,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } - if (!isfinite(val)) { + if (!PX4_ISFINITE(val)) { ekf_debug("constrain: non-finite!"); } @@ -2710,7 +2707,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 +2919,21 @@ bool AttPosEKF::StatesNaN() { bool err = false; // check all integrators - if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !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 (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !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 (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !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 +2943,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 (!PX4_ISFINITE(KH[i][j])) { current_ekf_state.KHNaN = true; err = true; @@ -2954,7 +2951,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // intermediate result used for covariance updates - if (!isfinite(KHP[i][j])) { + if (!PX4_ISFINITE(KHP[i][j])) { current_ekf_state.KHPNaN = true; err = true; @@ -2962,7 +2959,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // intermediate result used for covariance updates - if (!isfinite(P[i][j])) { + if (!PX4_ISFINITE(P[i][j])) { current_ekf_state.covarianceNaN = true; err = true; @@ -2970,7 +2967,7 @@ bool AttPosEKF::StatesNaN() { } // covariance matrix } - if (!isfinite(Kfusion[i])) { + if (!PX4_ISFINITE(Kfusion[i])) { current_ekf_state.kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); @@ -2978,7 +2975,7 @@ bool AttPosEKF::StatesNaN() { goto out; } // Kalman gains - if (!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/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index bd14863247..06559de979 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..fb6f74f357 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 @@ -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_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d9ccd8bac8..dbf40230ba 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -40,7 +40,7 @@ * @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..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 @@ -51,7 +51,7 @@ * @author Thomas Gubler */ -#include +#include #include #include #include @@ -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/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/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/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 30c2d2b956..857c83f392 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_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 233bc2d323..999bcbf008 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -371,7 +371,11 @@ MavlinkFTP::_workList(PayloadHeader* payload) // 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); @@ -380,7 +384,11 @@ MavlinkFTP::_workList(PayloadHeader* payload) 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; @@ -790,7 +798,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); + 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_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e5cddfafb6..30d2a64164 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -39,7 +39,8 @@ * @author Anton Babushkin */ -#include +#include +#include #include #include #include @@ -50,7 +51,9 @@ #include #include #include +#ifndef __PX4_POSIX #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), @@ -141,7 +149,9 @@ Mavlink::Mavlink() : _forwarding_on(false), _passing_on(false), _ftp_on(false), +#ifndef __PX4_POSIX _uart_fd(-1), +#endif _baudrate(57600), _datarate(1000), _datarate_events(500), @@ -176,7 +186,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(); @@ -217,7 +229,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; } @@ -401,6 +414,7 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) } } +#ifndef __PX4_POSIX int Mavlink::get_uart_fd(unsigned index) { @@ -418,6 +432,7 @@ Mavlink::get_uart_fd() { return _uart_fd; } +#endif // __PX4_POSIX int Mavlink::get_instance_id() @@ -436,7 +451,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: @@ -555,6 +574,7 @@ int Mavlink::get_component_id() return mavlink_system.compid; } +#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 */ @@ -610,7 +630,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; @@ -625,7 +645,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; } @@ -641,7 +661,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; } @@ -649,7 +669,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; } @@ -659,7 +679,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 */ @@ -702,6 +726,8 @@ Mavlink::enable_flow_control(bool enabled) return ret; } +#endif + int Mavlink::set_hil_enabled(bool hil_enabled) { @@ -733,7 +759,13 @@ Mavlink::get_free_tx_buf() * flow control if it continues to be full */ int buf_free = 0; + +#ifndef __PX4_POSIX + +// 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: @@ -747,6 +779,7 @@ Mavlink::get_free_tx_buf() enable_flow_control(false); } } +#endif return buf_free; } @@ -801,8 +834,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_POSIX /* 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(); @@ -812,6 +846,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); } @@ -851,8 +886,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_POSIX /* 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(); @@ -862,6 +898,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); } @@ -1204,38 +1241,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': @@ -1243,13 +1284,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; } @@ -1305,6 +1346,7 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); +#ifndef __PX4_POSIX struct termios uart_config_original; /* default values for arguments */ @@ -1314,6 +1356,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); @@ -1328,7 +1371,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 */ @@ -1336,10 +1380,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(); @@ -1570,14 +1618,16 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(_receive_thread, NULL); +#ifndef __PX4_POSIX /* 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(); @@ -1632,11 +1682,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 @@ -1667,7 +1717,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); @@ -1757,11 +1807,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; @@ -1776,7 +1828,7 @@ int mavlink_main(int argc, char *argv[]) { if (argc < 2) { usage(); - exit(1); + return 1; } if (!strcmp(argv[1], "start")) { @@ -1785,7 +1837,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(); @@ -1798,7 +1850,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 90b84061f6..4d2a833a0d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -42,7 +42,11 @@ #pragma once #include +#ifdef __PX4_NUTTX #include +#else +#include +#endif #include #include #include @@ -61,7 +65,11 @@ #include "mavlink_parameters.h" #include "mavlink_ftp.h" +#ifdef __PX4_NUTTX class Mavlink +#else +class Mavlink : public device::VDev +#endif { public: @@ -105,9 +113,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. @@ -183,12 +193,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(); @@ -314,7 +326,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 @@ -372,7 +386,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); @@ -404,7 +420,11 @@ private: */ 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::file_t *filp, int cmd, unsigned long arg); +#endif /** * Main mavlink task. diff --git a/src/modules/mavlink/mavlink_main_posix.cpp b/src/modules/mavlink/mavlink_main_posix.cpp new file mode 100644 index 0000000000..9ac7707cfd --- /dev/null +++ b/src/modules/mavlink/mavlink_main_posix.cpp @@ -0,0 +1,1818 @@ +/**************************************************************************** + * + * 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 +#ifndef __PX4_QURT +#include +#endif +#include +#include /* isinf / isnan checks */ + +#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 "/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. + +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() : + VDev("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), +#ifndef __PX4_QURT + _uart_fd(-1), +#endif + _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: + warnx("instance ID is out of range"); + px4_task_exit(1); + 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); + } + + if (_mavlink_instances) + 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); + } + } + } +} + +#ifndef __PX4_QURT +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; +} +#endif // __PX4_QURT + +int +Mavlink::get_instance_id() +{ + return _instance_id; +} + +mavlink_channel_t +Mavlink::get_channel() +{ + return _channel; +} + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +int +Mavlink::ioctl(device::file_t *filp, 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; +} + +#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 */ + 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; +} + +#endif + +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; + +#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: + * 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); + } + } +#endif + + return buf_free; +} + +void +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. */ + 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] = (component_ID == 0) ? mavlink_system.compid : 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); + +#ifndef __PX4_QURT + /* 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); + } +#endif + + 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); + +#ifndef __PX4_QURT + /* 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); + } +#endif + + 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; + + /* 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 = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'b': + _baudrate = strtoul(myoptarg, NULL, 10); + + if (_baudrate < 9600 || _baudrate > 921600) { + warnx("invalid baud rate '%s'", myoptarg); + err_flag = true; + } + + break; + + case 'r': + _datarate = strtoul(myoptarg, NULL, 10); + + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { + warnx("invalid data rate '%s'", myoptarg); + err_flag = true; + } + + break; + + case 'd': + _device_name = myoptarg; + break; + +// case 'e': +// mavlink_link_termination_allowed = true; +// break; + + case 'm': + if (strcmp(myoptarg, "custom") == 0) { + _mode = MAVLINK_MODE_CUSTOM; + + } else if (strcmp(myoptarg, "camera") == 0) { + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(myoptarg, "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); + +#ifndef __PX4_QURT + 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; + } +#endif + + /* 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)) { + warnx("msg buf:"); + return 1; + } + + /* 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); + +#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); + + 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%llu us ago\n", (unsigned long long)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. + warnx("mavlink for device %s is not running", device_name); + return 0; + } + + } else { + warnx("usage: mavlink stream [-d device] -s stream -r rate"); + return 1; + } + + 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_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3883cb84a9..e62d223e0c 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 @@ -997,7 +1000,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_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 { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 40adc6b054..3c4fa76a0c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -41,7 +41,8 @@ */ /* XXX trim includes */ -#include +#include +#include #include #include #include @@ -59,9 +60,10 @@ #include #include #include -#include +#ifndef __PX4_POSIX #include #include +#endif #include #include #include @@ -989,16 +991,16 @@ 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 = 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) { 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 { @@ -1484,6 +1486,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { +#ifndef __PX4_POSIX int uart_fd = _mavlink->get_uart_fd(); const int timeout = 500; @@ -1506,7 +1509,7 @@ MavlinkReceiver::receive_thread(void *arg) 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)) { + 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); } @@ -1526,6 +1529,7 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->count_rxbytes(nread); } } +#endif return NULL; } @@ -1572,9 +1576,11 @@ MavlinkReceiver::receive_start(Mavlink *parent) pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); +#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); +#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 e0a18cce70..c37b791b04 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -90,7 +90,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/mavlink/module.mk b/src/modules/mavlink/module.mk index e82b8bd935..7b2905f6b7 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -35,17 +35,18 @@ # MAVLink protocol to uORB interface process # -MODULE_COMMAND = mavlink -SRCS += mavlink_main.cpp \ - mavlink.c \ - mavlink_receiver.cpp \ +MODULE_COMMAND = mavlink + +SRCS += mavlink.c \ + mavlink_main.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_receiver.cpp \ + mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink 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 34bfa46140..47df1b4698 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,10 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include +#include +#include +#include +#include #include #include #include @@ -404,7 +407,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 +724,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 (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; } @@ -757,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; @@ -765,7 +768,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) @@ -858,10 +861,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] = (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; @@ -893,10 +896,8 @@ MulticopterAttitudeControl::task_main() perf_end(_loop_perf); } - warnx("exit"); - _control_task = -1; - _exit(0); + return; } int @@ -905,11 +906,11 @@ 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, - (main_t)&MulticopterAttitudeControl::task_main_trampoline, + (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -923,43 +924,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; } } 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 9e9c8ad5ff..f5341155b8 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(_appState), /* parameters */ _params_handles({ @@ -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_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index 1242c7fe37..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,6 +94,8 @@ private: px4::NodeHandle _n; + px4::AppState _appState; + struct { px4::ParameterFloat roll_p; px4::ParameterFloat roll_rate_p; 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 e5ea4d1047..b6ad4fd547 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) { @@ -1427,11 +1430,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 @@ -1440,11 +1441,11 @@ 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, - (main_t)&MulticopterPositionControl::task_main_trampoline, + (px4_main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -1458,46 +1459,53 @@ 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"); + return 1; } 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; } } 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 e0e086999d..c14a4b7627 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(_appState), /* parameters */ _params_handles({ @@ -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/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 54c6de155c..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,6 +107,8 @@ protected: px4::NodeHandle _n; + px4::AppState _appState; + struct { px4::ParameterFloat thr_min; px4::ParameterFloat thr_max; 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/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 b1bd4e540e..8e501fd8ef 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 042f926d3a..e9733b40ce 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 @@ -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/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 3d8c78cf12..a33ded28ab 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..5660bcc194 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 @@ -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/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 ac004f2127..a0722ac4d6 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 f14599a247..5bb56be794 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 a7ac74c33e..7e1bd439ed 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 a8009da414..887f53b6e2 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 9d28490907..11f336a286 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/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/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)"' diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 37234ca5d3..00c1d7be40 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -42,7 +42,8 @@ * @author Ban Siesta */ -#include +#include +#include #include #include #include @@ -112,7 +113,7 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" -#define PX4_EPOCH_SECS 1234567890ULL +#define PX4_EPOCH_SECS 1234567890L /** * Logging rate. @@ -305,7 +306,7 @@ sdlog2_usage(const char *reason) fprintf(stderr, "%s\n", reason); } - errx(1, "usage: sdlog2 {start|stop|status|on|off} [-r ] [-b ] -e -a -t -x\n" + warnx("usage: sdlog2 {start|stop|status|on|off} [-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" @@ -326,6 +327,7 @@ int sdlog2_main(int argc, char *argv[]) { if (argc < 2) { sdlog2_usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -333,17 +335,17 @@ 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; - deamon_task = task_spawn_cmd("sdlog2", + deamon_task = px4_task_spawn_cmd("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, 3000, sdlog2_thread_main, (char * const *)argv); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { @@ -352,7 +354,7 @@ int sdlog2_main(int argc, char *argv[]) } main_thread_should_exit = true; - exit(0); + return 0; } if (!thread_running) { @@ -388,7 +390,7 @@ int sdlog2_main(int argc, char *argv[]) } sdlog2_usage("unrecognized command"); - exit(1); + return 1; } bool get_log_time_utc_tt(struct tm *tt, bool boot_time) { @@ -505,7 +507,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); @@ -553,7 +555,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); @@ -634,7 +636,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) { @@ -683,7 +686,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 */ @@ -709,7 +712,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 */ @@ -1016,7 +1019,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; } @@ -1024,7 +1028,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 */ @@ -1042,7 +1047,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; @@ -1665,8 +1671,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; @@ -1955,7 +1961,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"); @@ -1963,7 +1969,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]; @@ -1974,7 +1980,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; } } @@ -1984,7 +1990,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() @@ -1992,19 +1998,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)); @@ -2012,7 +2019,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/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index b36d56d6d7..67acd94bf7 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/module.mk b/src/modules/sensors/module.mk index f37bc93277..d92ebc00eb 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = sensors MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" -SRCS = sensors.cpp \ +SRCS = sensors.cpp \ sensor_params.c MODULE_STACKSIZE = 1200 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1bebea2060..6a127df0ea 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 edfd5ffb82..c3ac138ad5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -46,7 +46,10 @@ * @author Anton Babushkin */ -#include +#include +#include +#include +#include #include #include @@ -59,7 +62,8 @@ #include #include -#include +#include +//#include #include #include @@ -111,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 @@ -124,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 @@ -223,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 */ @@ -655,7 +659,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); @@ -684,7 +688,7 @@ Sensors::parameters_update() tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; /* handle blowup in the scaling factor calculation */ - if (!isfinite(tmpScaleFactor) || + 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])); @@ -830,18 +834,18 @@ Sensors::parameters_update() /* set px4flow rotation */ int flowfd; - flowfd = open(PX4FLOW0_DEVICE_PATH, 0); + flowfd = px4_open(PX4FLOW0_DEVICE_PATH, 0); if (flowfd >= 0) { - int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + int flowret = px4_ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); if (flowret) { warnx("flow rotation could not be set"); - close(flowfd); + px4_close(flowfd); return ERROR; } - close(flowfd); + px4_close(flowfd); } get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); @@ -861,22 +865,22 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); int barofd; - barofd = open(BARO0_DEVICE_PATH, 0); + barofd = px4_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)); + int baroret = px4_ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (baroret) { warnx("qnh could not be set"); - close(barofd); + px4_close(barofd); return ERROR; } - close(barofd); + px4_close(barofd); } return OK; @@ -887,7 +891,7 @@ Sensors::accel_init() { int fd; - fd = open(ACCEL0_DEVICE_PATH, 0); + fd = px4_open(ACCEL0_DEVICE_PATH, 0); if (fd < 0) { warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); @@ -896,12 +900,12 @@ Sensors::accel_init() } else { /* set the accel internal sampling rate to default rate */ - ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); + px4_ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); /* set the driver to poll at default rate */ - ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); + px4_close(fd); } return OK; @@ -912,7 +916,7 @@ Sensors::gyro_init() { int fd; - fd = open(GYRO0_DEVICE_PATH, 0); + fd = px4_open(GYRO0_DEVICE_PATH, 0); if (fd < 0) { warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); @@ -921,10 +925,10 @@ Sensors::gyro_init() } else { /* set the gyro internal sampling rate to default rate */ - ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); + px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); /* set the driver to poll at default rate */ - ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); } @@ -937,7 +941,7 @@ Sensors::mag_init() int fd; int ret; - fd = open(MAG0_DEVICE_PATH, 0); + fd = px4_open(MAG0_DEVICE_PATH, 0); if (fd < 0) { warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); @@ -947,19 +951,19 @@ Sensors::mag_init() /* try different mag sampling rates */ - ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 150); if (ret == OK) { /* set the pollrate accordingly */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); + px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); } else { - ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + 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 */ - ioctl(fd, SENSORIOCSPOLLRATE, 100); + px4_ioctl(fd, SENSORIOCSPOLLRATE, 100); } else { warnx("FATAL: mag sampling rate could not be set"); @@ -967,7 +971,7 @@ Sensors::mag_init() } } - close(fd); + px4_close(fd); return OK; } @@ -977,7 +981,7 @@ Sensors::baro_init() { int fd; - fd = open(BARO0_DEVICE_PATH, 0); + fd = px4_open(BARO0_DEVICE_PATH, 0); if (fd < 0) { warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); @@ -985,9 +989,9 @@ Sensors::baro_init() } /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); + px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); - close(fd); + px4_close(fd); return OK; } @@ -996,7 +1000,7 @@ int Sensors::adc_init() { - _fd_adc = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + _fd_adc = px4_open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); if (_fd_adc < 0) { warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); @@ -1358,7 +1362,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; @@ -1376,12 +1380,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &device_id)); if (failed) { - close(fd); + px4_close(fd); continue; } /* if the calibration is for this device, apply it */ - if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { + 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)); @@ -1400,7 +1404,7 @@ Sensors::parameter_update_poll(bool forced) warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); } else { /* apply new scaling and offsets */ - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); } else { @@ -1415,7 +1419,7 @@ Sensors::parameter_update_poll(bool forced) gyro_count++; } - close(fd); + px4_close(fd); } /* run through all accel sensors */ @@ -1424,7 +1428,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; @@ -1442,12 +1446,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &device_id)); if (failed) { - close(fd); + px4_close(fd); continue; } /* if the calibration is for this device, apply it */ - if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { + 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)); @@ -1466,7 +1470,7 @@ Sensors::parameter_update_poll(bool forced) warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ - res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); } else { @@ -1481,7 +1485,7 @@ Sensors::parameter_update_poll(bool forced) accel_count++; } - close(fd); + px4_close(fd); } /* run through all mag sensors */ @@ -1490,7 +1494,7 @@ Sensors::parameter_update_poll(bool forced) res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - int fd = open(str, 0); + int fd = px4_open(str, 0); if (fd < 0) { /* the driver is not running, abort */ @@ -1515,12 +1519,12 @@ Sensors::parameter_update_poll(bool forced) failed = failed || (OK != param_get(param_find(str), &device_id)); if (failed) { - close(fd); + px4_close(fd); continue; } /* if the calibration is for this device, apply it */ - if (device_id == ioctl(fd, DEVIOCGDEVICEID, 0)) { + 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)); @@ -1537,7 +1541,7 @@ Sensors::parameter_update_poll(bool forced) (void)sprintf(str, "CAL_MAG%u_ROT", i); - if (ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { + if (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { /* mag is internal */ _mag_rotation[s] = _board_rotation; /* reset param to -1 to indicate internal mag */ @@ -1589,7 +1593,7 @@ Sensors::parameter_update_poll(bool forced) warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); } else { /* apply new scaling and offsets */ - res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); } else { @@ -1604,10 +1608,10 @@ Sensors::parameter_update_poll(bool forced) mag_count++; } - close(fd); + px4_close(fd); } - int fd = open(AIRSPEED0_DEVICE_PATH, 0); + int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); /* this sensor is optional, abort without error */ @@ -1617,11 +1621,11 @@ Sensors::parameter_update_poll(bool forced) 1.0f, }; - if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { warn("WARNING: failed to set scale / offsets for airspeed sensor"); } - close(fd); + px4_close(fd); } /* do not output this for now, as its covered in preflight checks */ @@ -1687,7 +1691,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* 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)); + int ret = px4_read(_fd_adc, &buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { @@ -1968,7 +1972,7 @@ Sensors::rc_poll() _rc.channels[i] *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (!isfinite(_rc.channels[i])) { + if (!PX4_ISFINITE(_rc.channels[i])) { _rc.channels[i] = 0.0f; } } @@ -2170,7 +2174,7 @@ Sensors::task_main() _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* 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; @@ -2181,7 +2185,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. */ @@ -2240,7 +2244,7 @@ Sensors::task_main() exit_immediate: _sensors_task = -1; - _exit(ret); + px4_task_exit(ret); } int @@ -2249,11 +2253,11 @@ 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, - (main_t)&Sensors::task_main_trampoline, + (px4_main_t)&Sensors::task_main_trampoline, nullptr); /* wait until the task is up and running or has failed */ @@ -2271,46 +2275,53 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { if (argc < 2) { - errx(1, "usage: sensors {start|stop|status}"); + warnx("usage: sensors {start|stop|status}"); + return 0; } if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) { - errx(0, "already running"); + warnx("already running"); + return 0; } sensors::g_sensors = new Sensors; if (sensors::g_sensors == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; sensors::g_sensors = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (sensors::g_sensors == nullptr) { - errx(1, "not running"); + warnx("not running"); + return 1; } delete sensors::g_sensors; sensors::g_sensors = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { - errx(0, "is running"); + warnx("is running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/simulator/module.mk b/src/modules/simulator/module.mk new file mode 100644 index 0000000000..80f619477d --- /dev/null +++ b/src/modules/simulator/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# 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 + +ifneq ($(PX4_TARGET_OS), qurt) +SRCS += simulator_mavlink.cpp +endif + +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 new file mode 100644 index 0000000000..83e3fd8d76 --- /dev/null +++ b/src/modules/simulator/simulator.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +#include "simulator.h" + +using namespace simulator; + +static px4_task_t g_sim_task = -1; + +Simulator *Simulator::_instance = NULL; + +Simulator *Simulator::getInstance() +{ + return _instance; +} + +bool Simulator::getMPUReport(uint8_t *buf, int len) +{ + return _mpu.copyData(buf, len); +} + +bool Simulator::getRawAccelReport(uint8_t *buf, int len) +{ + return _accel.copyData(buf, len); +} + +bool Simulator::getBaroSample(uint8_t *buf, int len) +{ + return _baro.copyData(buf, len); +} + +int Simulator::start(int argc, char *argv[]) +{ + int ret = 0; + _instance = new Simulator(); + if (_instance) { + PX4_INFO("Simulator started"); + drv_led_start(); + if (argv[2][1] == 's') { +#ifndef __PX4_QURT + _instance->updateSamples(); +#endif + } else { + _instance->publishSensorsCombined(); + } + } + else { + PX4_WARN("Simulator creation failed"); + ret = 1; + } + 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 + //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); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + else { + usleep(1000000-delta); + } + } +} + +static void usage() +{ + PX4_WARN("Usage: simulator {start -[sc] |stop}"); + PX4_WARN("Simulate raw sensors: simulator start -s"); + 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[]) +{ + int ret = 0; + if (argc == 3 && strcmp(argv[1], "start") == 0) { + if (strcmp(argv[2], "-s") == 0 || 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; + } + } + else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_sim_task < 0) { + PX4_WARN("Simulator not running"); + } + else { + px4_task_delete(g_sim_task); + g_sim_task = -1; + } + } + else { + usage(); + ret = -EINVAL; + } + + return ret; +} + +} + +bool static _led_state[2] = { false , false }; + +__EXPORT void led_init() +{ + PX4_DEBUG("LED_INIT"); +} + +__EXPORT void led_on(int led) +{ + if (led == 1 || led == 0) + { + PX4_DEBUG("LED%d_ON", led); + _led_state[led] = true; + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1 || led == 0) + { + PX4_DEBUG("LED%d_OFF", led); + _led_state[led] = false; + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1 || led == 0) + { + _led_state[led] = !_led_state[led]; + PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); + + } +} + diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h new file mode 100644 index 0000000000..60b60f72f8 --- /dev/null +++ b/src/modules/simulator/simulator.h @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifndef __PX4_QURT +#include +#include +#endif + +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: + Report(int readers) : + _max_readers(readers), + _report_len(sizeof(RType)) + { + sem_init(&_lock, 0, _max_readers); + } + + ~Report() {}; + + 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() { sem_wait(&_lock); } + void read_unlock() { sem_post(&_lock); } + void write_lock() + { + 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 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[]); + + bool getRawAccelReport(uint8_t *buf, int len); + bool getMPUReport(uint8_t *buf, int len); + bool getBaroSample(uint8_t *buf, int len); +private: + Simulator() : + _accel(1), + _mpu(1), + _baro(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{} +#endif + {} + ~Simulator() { _instance=NULL; } + +#ifndef __PX4_QURT + void updateSamples(); +#endif + + 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; + 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 + 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; + + int _fd; + unsigned char _buf[200]; + hrt_abstime _time_last; + struct sockaddr_in _srcaddr; + socklen_t _addrlen = sizeof(_srcaddr); + + void poll_topics(); + void handle_message(mavlink_message_t *msg); + void send_data(); + 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(); +#endif +}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp new file mode 100644 index 0000000000..ed4a6343fe --- /dev/null +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -0,0 +1,421 @@ +/**************************************************************************** + * + * 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" +#include + +using namespace simulator; + +#define SEND_INTERVAL 20 +#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; + +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; + + // 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; + } + } + + actuator_msg.time_usec = hrt_absolute_time(); + 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; +} + +void Simulator::send_data() { + // check if it's time to send new data + hrt_abstime time_now = hrt_absolute_time(); + if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { + _time_last = time_now; + 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 + } +} + +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_hil_sensor_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_HIL_SENSOR: + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_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) { + 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]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* no idea which numbers should be here*/ + buf[2] = 100; + buf[3] = 0; + 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, packet_len, 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; + (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); + + // 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", 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", 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 pret = ::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; + } + + // 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; + 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); + } + } + } + } + + // 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; + 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); + } +} diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 49403c98bd..2378a2d033 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 @@ -53,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 (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 */ @@ -301,7 +313,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 (BSON_WRITE(encoder->fd, p, s) == (int)s) ? 0 : -1; /* do we need to extend the buffer? */ while ((encoder->bufpos + s) > encoder->bufsize) { @@ -408,7 +420,7 @@ bson_encoder_fini(bson_encoder_t encoder) } /* sync file */ - fsync(encoder->fd); + BSON_FSYNC(encoder->fd); return 0; } 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/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..fe08da71fd 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -41,16 +41,16 @@ * @author Lorenz Meier * @author Petri Tanskanen */ -#include -#include +#include +//#include #include #include #include -#include +//#include -#include +//#include #include @@ -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/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/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/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/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 24f4e42076..1f66971c15 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -42,7 +42,8 @@ #include "mcu_version.h" -#include +#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 4d9d60c2d8..369e6807d1 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/module.mk b/src/modules/systemlib/module.mk index f2499bbb13..cb599e2052 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -35,17 +35,13 @@ # System utility library # -SRCS = err.c \ - hx_stream.c \ +SRCS = \ perf_counter.c \ param/param.c \ - bson/tinybson.c \ conversions.c \ cpuload.c \ getopt_long.c \ - up_cxxinitialize.c \ pid/pid.c \ - systemlib.c \ airspeed.c \ system_params.c \ mavlink_log.c \ @@ -53,9 +49,19 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ + mcu_version.c \ + bson/tinybson.c \ circuit_breaker.cpp \ - circuit_breaker_params.c \ - mcu_version.c + circuit_breaker_params.c + +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS += err.c \ + up_cxxinitialize.c +endif + +ifneq ($(PX4_TARGET_OS),qurt) +SRCS += hx_stream.c +endif MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 0548a9f7db..a866c843d7 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -43,7 +43,7 @@ * */ -#include +#include #include #include #include @@ -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; @@ -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 690224753f..09e9e88870 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -41,7 +41,9 @@ * and background parameter saving. */ -#include +//#include +#include +#include #include #include #include @@ -68,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. */ @@ -705,7 +715,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 = PARAM_OPEN(filename, O_WRONLY | O_CREAT, 0x777); if (fd < 0) { warn("failed to open param file: %s", filename); @@ -718,7 +728,7 @@ param_save_default(void) warnx("failed to write parameters to file: %s", filename); } - close(fd); + PARAM_CLOSE(fd); return res; } @@ -729,7 +739,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 = PARAM_OPEN(param_get_default_file(), O_RDONLY); if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ @@ -742,7 +753,7 @@ param_load_default(void) } int result = param_load(fd_load); - close(fd_load); + PARAM_CLOSE(fd_load); if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 07c6dd6bf2..04a37a229a 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -382,6 +382,7 @@ union param_value_u { void *p; int32_t i; float f; + long long x; }; /** @@ -391,7 +392,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_POSIX + __attribute__((aligned(16))); +#else + ; +#endif param_type_t type; union param_value_u val; }; diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 950577f003..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. */ @@ -386,7 +390,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 +399,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 +415,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; } @@ -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/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..fc28d17416 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -37,9 +37,11 @@ * RC calibration check */ -#include +#include +#include #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/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.h b/src/modules/systemlib/systemlib.h index 2f24215a9f..29b6eb5cef 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -43,29 +43,11 @@ #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, - main_t entry, - char * const argv[]); - enum MULT_PORTS { MULT_0_US2_RXTX = 0, MULT_1_US2_FLOW, diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index c78f295708..41707360e4 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -38,11 +38,12 @@ * Included Files ************************************************************************************/ -#include +#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/modules/uORB/module.mk b/src/modules/uORB/module.mk index 71ad09130c..31a2b24e4a 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -39,9 +39,21 @@ MODULE_COMMAND = uorb MODULE_STACKSIZE = 2048 -SRCS = uORB.cpp \ - objects_common.cpp \ +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = uORBDevices_nuttx.cpp \ + uORBTest_UnitTest.cpp \ + uORBManager_nuttx.cpp + +else +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/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/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/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index d531c6b0bc..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..c80824d39b --- /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..147ce5fbb3 --- /dev/null +++ b/src/modules/uORB/uORBMain.cpp @@ -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. + * + ****************************************************************************/ + +#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 = uORBTest::UnitTest::instance(); + return t.test(); + } + + /* + * Test the latency. + */ + if (!strcmp(argv[1], "latency_test")) { + + 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")) { + 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(); + return -EINVAL; +} 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..722f538976 --- /dev/null +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -0,0 +1,290 @@ +/**************************************************************************** + * + * 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 +#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 */ + 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 argv[]) +{ + 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 new file mode 100644 index 0000000000..f19c95451d --- /dev/null +++ b/src/modules/uORB/uORBTest_UnitTest.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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" +#include + +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: + + // Singleton pattern + static uORBTest::UnitTest &instance(); + ~UnitTest() {} + int test(); + template int latency_test(orb_id_t T, bool print); + int info(); + +private: + 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; + bool pubsubtest_print; + 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); + + 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, + 1500, + (px4_main_t)&uORBTest::UnitTest::pubsubtest_threadEntry, + args); + + /* 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); + } + + px4_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_ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f04ab9f17c..75e8eeff95 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include +#include #include #include @@ -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/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 fdb4de4343..11aa88a5f3 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 @@ -862,7 +862,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/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..5f76065bb0 --- /dev/null +++ b/src/platforms/common/px4_getopt.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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. +// +__EXPORT 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/nuttx/px4_layer/module.mk b/src/platforms/nuttx/px4_layer/module.mk new file mode 100644 index 0000000000..1738063a56 --- /dev/null +++ b/src/platforms/nuttx/px4_layer/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. +# +############################################################################ + +# +# NuttX porting layer +# + +SRCS = px4_nuttx_tasks.c + +MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/systemlib.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c similarity index 81% rename from src/modules/systemlib/systemlib.c rename to src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c index 280c30023d..e9b722de4c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c @@ -33,11 +33,11 @@ ****************************************************************************/ /** - * @file systemlib.c - * Implementation of commonly used low-level system-call like functions. + * @file px4_nuttx_tasks.c + * Implementation of existing task API for NuttX */ -#include +#include #include #include #include @@ -52,13 +52,13 @@ #include -#include "systemlib.h" +#include // 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) +px4_systemreset(bool to_bootloader) { if (to_bootloader) { stm32_pwr_enablebkp(); @@ -66,29 +66,13 @@ systemreset(bool to_bootloader) /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } - up_systemreset(); /* lock up here */ - while (true); + while(true); } -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); -} - -static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) -{ - 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 px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) { int pid; @@ -112,3 +96,8 @@ int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size return pid; } + +int px4_task_delete(int pid) +{ + return task_delete(pid); +} diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp new file mode 100644 index 0000000000..f431231706 --- /dev/null +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -0,0 +1,1390 @@ +/**************************************************************************** + * + * 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 + * 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 accelsim.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 "/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 + +#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 accelsim_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::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. + */ + //void print_info(); + + /** + * dump register values + */ + void print_registers(); + +protected: + friend class ACCELSIM_mag; + + 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: + + 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::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, 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 = false; + + _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) { + PX4_WARN("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) { + PX4_WARN("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) { + PX4_WARN("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 == (orb_advert_t)(-1)) { + PX4_WARN("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::file_t *filp, 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::file_t *filp, 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::file_t *filp, 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(filp, SENSORIOCSPOLLRATE, 1600); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, 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; + + case ACCELIOCSELFTEST: + return OK; + + default: + /* give it to the superclass */ + return VDev::ioctl(filp, cmd, arg); + } +} + +int +ACCELSIM::mag_ioctl(device::file_t *filp, 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(filp, 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; + + case MAGIOCSELFTEST: + return OK; + default: + /* give it to the superclass */ + return VDev::ioctl(filp, 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 */ + + // 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++; + + /* 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::file_t *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +ACCELSIM_mag::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->mag_ioctl(filp, 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 accelsim +{ + +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) { + PX4_WARN( "already started"); + return 0; + } + + /* create the driver */ + g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating ACCELSIM obj"); + goto fail; + } + + if (OK != g_dev->init()) { + PX4_ERR("failed init of ACCELSIM obj"); + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); + + if (fd < 0) { + PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + goto fail; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("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) { + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + } + } + else + { + PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); + } + + px4_close(fd); + px4_close(fd_mag); + + return 0; +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_ERR("driver start failed"); + return 1; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_DEBUG("state @ %p", g_dev); + //g_dev->print_info(); + + return 0; +} + +void +usage() +{ + PX4_WARN("Usage: accelsim 'start', 'info'"); + PX4_WARN("options:"); + PX4_WARN(" -R rotation"); +} + +} // namespace + +int +accelsim_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 = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + default: + accelsim::usage(); + return 0; + } + } + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) + ret = accelsim::start(rotation); + + /* + * Print driver information. + */ + else if (!strcmp(verb, "info")) + ret = accelsim::info(); + + else { + accelsim::usage(); + return 1; + } + return ret; +} diff --git a/src/platforms/posix/drivers/accelsim/module.mk b/src/platforms/posix/drivers/accelsim/module.mk new file mode 100644 index 0000000000..dc980c5cc8 --- /dev/null +++ b/src/platforms/posix/drivers/accelsim/module.mk @@ -0,0 +1,7 @@ +# +# Simulated accel/mag driver +# + +MODULE_COMMAND = accelsim +SRCS = accelsim.cpp + diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp new file mode 100644 index 0000000000..fa6690ac21 --- /dev/null +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * 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 adcsim.cpp + * + * Driver for the ADCSIM. + * + * This is a designed for simulating sampling things like voltages + * and so forth. + */ + +#include +#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::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::file_t *filp); + virtual int close_last(device::file_t *filp); + +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 */ + + /** 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("adcsim", ADCSIM0_DEVICE_PATH), + _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), + _channel_count(0), + _samples(nullptr) +{ + //_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::file_t *filp, int cmd, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t +ADCSIM::read(device::file_t *filp, 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::file_t *filp) +{ + /* get fresh data */ + _tick(); + + /* and schedule regular updates */ + hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + + return 0; +} + +int +ADCSIM::close_last(device::file_t *filp) +{ + 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 adcsim_main(int argc, char *argv[]); + +namespace +{ +ADCSIM *g_adc; + +int +test(void) +{ + + int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + if (fd < 0) { + PX4_ERR("can't open ADCSIM device"); + return 1; + } + + for (unsigned i = 0; i < 50; i++) { + adc_msg_s data[12]; + ssize_t count = px4_read(fd, data, sizeof(data)); + + if (count < 0) { + PX4_ERR("read error"); + return 1; + } + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + PX4_INFO("%d: %lu ", data[j].am_channel, (unsigned long)data[j].am_data); + } + + usleep(500000); + } + + px4_close(fd); + return 0; +} +} + +int +adcsim_main(int argc, char *argv[]) +{ + int ret = 0; + + if (g_adc == nullptr) { + /* XXX this hardcodes the default channel set for POSIXTEST - should be configurable */ + g_adc = new ADCSIM((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); + + if (g_adc == nullptr) { + PX4_ERR("couldn't allocate the ADCSIM driver"); + return 1; + } + + if (g_adc->init() != OK) { + delete g_adc; + PX4_ERR("ADCSIM init failed"); + return 1; + } + } + + if (argc > 1) { + if (!strcmp(argv[1], "test")) + ret = test(); + } + + return ret; +} diff --git a/src/platforms/posix/drivers/adcsim/module.mk b/src/platforms/posix/drivers/adcsim/module.mk new file mode 100644 index 0000000000..a8ad55effc --- /dev/null +++ b/src/platforms/posix/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 = adcsim + +SRCS = adcsim.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp new file mode 100644 index 0000000000..ea9d92025e --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -0,0 +1,404 @@ +/**************************************************************************** + * + * 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 + * @author Mark Charlebois (simulator) + * + * Driver for the Simulated Airspeed Sensor based on Eagle Tree Airspeed V3. + */ + +#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 + +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")), + _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)); +} + +Airspeed::~Airspeed() +{ + /* 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 +Airspeed::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) + PX4_WARN("uORB started?"); + } + + ret = OK; + +out: + return ret; +} + +int +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 + be reduced + */ + _retries = 4; + int ret = measure(); + + // drop back to 2 retries once initialised + _retries = 2; + return ret; +} + +int +Airspeed::ioctl(device::file_t *filp, 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(filp, cmd, arg); + } +} + +ssize_t +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); + 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 +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)&Airspeed::cycle_trampoline, this, 1); +} + +void +Airspeed::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +Airspeed::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 +Airspeed::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 +Airspeed::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + PX4_INFO("poll interval: %u ticks", _measure_ticks); + _reports->print_info("report queue"); +} + +void +Airspeed::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 new file mode 100644 index 0000000000..13021444c0 --- /dev/null +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -0,0 +1,1189 @@ +/**************************************************************************** + * + * 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 + +#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::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. + */ + 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; + debug("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; + PX4_ERR("temp measure failed"); + break; + } + + usleep(BAROSIM_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + PX4_ERR("temp collect failed"); + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + PX4_ERR("pressure collect failed"); + break; + } + + usleep(BAROSIM_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + PX4_ERR("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)) { + PX4_ERR("failed to create sensor_baro publication"); + } + //PX4_WARN("sensor_baro publication %ld", _baro_topic); + + } while (0); + +out: + return ret; +} + +ssize_t +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); + 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::file_t *filp, 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(filp, 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 { + PX4_WARN("BAROSIM::collect _baro_topic not initialized"); + } + } + + 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); + PX4_INFO("poll interval: %u ticks", _measure_ticks); + _reports->print_info("report queue"); + 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)); + + 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); +} + +/** + * 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, "/dev/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) { + PX4_ERR("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; + PX4_ERR("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; + PX4_ERR("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) { + PX4_ERR("can't open baro device"); + return false; + } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + px4_close(fd); + PX4_ERR("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) { + PX4_ERR("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) { + PX4_ERR("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)) { + PX4_ERR("immediate read failed"); + return 1; + } + + 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)) { + 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)) { + PX4_ERR("failed to set 2Hz poll rate"); + px4_close(fd); + return 1; + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + px4_pollfd_struct_t fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = px4_poll(&fds, 1, 2000); + + if (ret != 1) { + PX4_WARN("timed out waiting for sensor data"); + } + + /* now go get it */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("periodic read failed"); + px4_close(fd); + return 1; + } + + 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); + PX4_INFO("PASS"); + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + struct barosim_bus_option &bus = bus_options[0]; + int fd; + + fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { + PX4_ERR("failed "); + return 1; + } + + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return 1; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("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) { + 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)) { + PX4_ERR("failed to set poll rate"); + return 1; + } + + /* average a few measurements */ + pressure = 0.0f; + + for (unsigned i = 0; i < 20; i++) { + px4_pollfd_struct_t fds; + int ret; + ssize_t sz; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = px4_poll(&fds, 1, 1000); + + if (ret != 1) { + PX4_ERR("timed out waiting for sensor data"); + return 1; + } + + /* now go get it */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("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 */ + + PX4_INFO("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); + + p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); + + 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) { + PX4_WARN("BAROIOCSMSLPRESSURE"); + return 1; + } + + px4_close(fd); + return 0; +} + +void +usage() +{ + PX4_WARN("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) { + PX4_WARN("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/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp new file mode 100644 index 0000000000..734c2bc286 --- /dev/null +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * 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 +#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", "/dev/BARO_SIM", bus, 0) +{ +} + +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 + 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", send_len, send[0], recv_len); + return 1; + } + else { + Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { + PX4_ERR("Error BARO_SIM::transfer no simulator"); + return -ENODEV; + } + PX4_DEBUG("BARO_SIM::transfer getting sample"); + sim->getBaroSample(recv, recv_len); + } + return 0; +} diff --git a/src/platforms/posix/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h new file mode 100644 index 0000000000..8eaa0c21f5 --- /dev/null +++ b/src/platforms/posix/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/posix/drivers/barosim/module.mk b/src/platforms/posix/drivers/barosim/module.mk new file mode 100644 index 0000000000..5a4cb4ebb9 --- /dev/null +++ b/src/platforms/posix/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 diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp new file mode 100644 index 0000000000..b253ab3b0d --- /dev/null +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -0,0 +1,1925 @@ +/**************************************************************************** + * + * 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::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); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + +protected: + friend class GYROSIM_gyro; + + 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; + 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; + + // 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) + + uint8_t _regdata[108]; +}; + +/* + 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::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, 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), + _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) { + PX4_WARN("VDev setup failed"); + return ret; + } + + /* allocate basic report buffers */ + _accel_reports = new RingBuffer(2, sizeof(accel_report)); + if (_accel_reports == nullptr) { + PX4_WARN("_accel_reports creation failed"); + goto out; + } + + _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); + if (_gyro_reports == nullptr) { + PX4_WARN("_gyro_reports creation failed"); + goto out; + } + + if (reset() != OK) { + PX4_WARN("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) { + PX4_WARN("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) { + PX4_WARN("ADVERT FAIL"); + } + +out: + return ret; +} + +int GYROSIM::reset() +{ + return OK; +} + +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) + { + PX4_DEBUG("Reading %u bytes from register %u", len-1, reg); + memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); + } + else { + PX4_DEBUG("Writing %u bytes to register %u", len-1, reg); + if (recv) + memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + } + 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::file_t *filp, 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() +{ + return OK; + + 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() +{ + return OK; + + 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; +} + +ssize_t +GYROSIM::gyro_read(device::file_t *filp, 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::file_t *filp, 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(filp, SENSORIOCSPOLLRATE, 1000); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, 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(filp, cmd, arg); + } +} + +int +GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCGPOLLRATE: + case SENSORIOCRESET: + return ioctl(filp, 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(filp, 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 (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"); + PX4_WARN("checked_next: %u", _checked_next); + for (uint8_t i=0; igyro_read(filp, buffer, buflen); +} + +int +GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, 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(); +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 */ + PX4_WARN("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) { + px4_close(fd); + goto fail; + } + + px4_close(fd); + return 0; +fail: + + if (*g_dev_ptr != nullptr) { + delete (*g_dev_ptr); + *g_dev_ptr = nullptr; + } + + PX4_WARN("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 */ + PX4_WARN("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) { + PX4_ERR("%s open failed (try 'gyrosim start')", path_accel); + return 1; + } + + /* get the driver */ + int fd_gyro = px4_open(path_gyro, O_RDONLY); + + if (fd_gyro < 0) { + PX4_ERR("%s open failed", path_gyro); + return 1; + } + + /* reset to manual polling */ + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + PX4_ERR("reset to manual polling"); + return 1; + } + + /* do a simple demand read */ + sz = read(fd, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) { + PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); + PX4_ERR("immediate acc read failed"); + return 1; + } + + PX4_INFO("single read"); + PX4_INFO("time: %lld", (long long)a_report.timestamp); + PX4_INFO("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + PX4_INFO("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + PX4_INFO("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + PX4_INFO("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); + PX4_INFO("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); + PX4_INFO("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); + PX4_INFO("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)) { + PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); + PX4_ERR("immediate gyro read failed"); + return 1; + } + + PX4_INFO("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + PX4_INFO("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + PX4_INFO("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + PX4_INFO("gyro x: \t%d\traw", (int)g_report.x_raw); + PX4_INFO("gyro y: \t%d\traw", (int)g_report.y_raw); + PX4_INFO("gyro z: \t%d\traw", (int)g_report.z_raw); + PX4_INFO("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)); + + PX4_INFO("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); + PX4_INFO("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + + + /* XXX add poll-rate tests here too */ + + px4_close(fd); + reset(); + PX4_INFO("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) { + PX4_ERR("reset failed"); + return 1; + } + + + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + goto reset_fail; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + goto reset_fail; + } + + px4_close(fd); + return 0; + +reset_fail: + px4_close(fd); + return 1; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_INFO("state @ %p", *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) { + PX4_ERR("driver not running"); + return 1; + } + + PX4_INFO("regdump @ %p", *g_dev_ptr); + (*g_dev_ptr)->print_registers(); + + return 0; +} + +void +usage() +{ + PX4_WARN("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'"); + PX4_WARN("options:"); + PX4_WARN(" -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 { + gyrosim::usage(); + ret = 1; + } + + return ret; +} diff --git a/src/platforms/posix/drivers/gyrosim/module.mk b/src/platforms/posix/drivers/gyrosim/module.mk new file mode 100644 index 0000000000..7bd37c2e2a --- /dev/null +++ b/src/platforms/posix/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 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..37f51ff607 --- /dev/null +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -0,0 +1,818 @@ +/**************************************************************************** + * + * 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) { + PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + return 1; + } + + ret = px4_ioctl(fd, TONE_SET_ALARM, tune); + px4_close(fd); + + if (ret != 0) { + PX4_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) { + PX4_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) { + PX4_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) { + PX4_WARN("couldn't allocate the ToneAlarm driver"); + return 1; + } + + if (g_dev->init() != OK) { + delete g_dev; + PX4_WARN("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) { + PX4_WARN("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) { + PX4_WARN("not enough memory memory"); + return 1; + } + // FIXME - Make GCC happy + if (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; + } + PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); + ret = 1; + } + } + + return ret; +} + diff --git a/src/platforms/posix/include/arch/board/board.h b/src/platforms/posix/include/arch/board/board.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/platforms/posix/include/board_config.h b/src/platforms/posix/include/board_config.h new file mode 100644 index 0000000000..a03b8c3e20 --- /dev/null +++ b/src/platforms/posix/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/posix/include/crc32.h b/src/platforms/posix/include/crc32.h new file mode 100644 index 0000000000..bf828e3e0e --- /dev/null +++ b/src/platforms/posix/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/posix/include/queue.h b/src/platforms/posix/include/queue.h new file mode 100644 index 0000000000..4d95541e02 --- /dev/null +++ b/src/platforms/posix/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/posix/main.cpp b/src/platforms/posix/main.cpp new file mode 100644 index 0000000000..f756749d52 --- /dev/null +++ b/src/platforms/posix/main.cpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 + +namespace px4 { +void init_once(void); +} + +using namespace std; + +typedef int (*px4_main_t)(int argc, char *argv[]); + +#include "apps.h" +#include "px4_middleware.h" + +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]; + + unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { + arg[i] = (char *)appargs[i].c_str(); + ++i; + } + arg[i] = (char *)0; + cout << "Running: " << command << "\n"; + apps[command](i,(char **)arg); + } + else + { + cout << "Invalid command: " << 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) +{ + px4::init_once(); + + px4::init(argc, argv, "mainapp"); + + // 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; + + while(1) { + cout << "Enter a command and its args:" << endl; + getline (cin,mystr); + process_line(mystr); + mystr = ""; + } +} diff --git a/src/platforms/posix/px4_layer/dq_addlast.c b/src/platforms/posix/px4_layer/dq_addlast.c new file mode 100644 index 0000000000..3ef08abd05 --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/dq_rem.c b/src/platforms/posix/px4_layer/dq_rem.c new file mode 100644 index 0000000000..db20762c75 --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/dq_remfirst.c b/src/platforms/posix/px4_layer/dq_remfirst.c new file mode 100644 index 0000000000..e87acc3382 --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c new file mode 100644 index 0000000000..f5c351b187 --- /dev/null +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -0,0 +1,408 @@ +/**************************************************************************** + * + * 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 +#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); + +// 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. + */ +hrt_abstime hrt_absolute_time(void) +{ + struct timespec ts; + + 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) +{ + hrt_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; + hrt_unlock(); + // 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) +{ + //printf("hrt_init\n"); + sq_init(&callout_queue); + sem_init(&_hrt_lock, 0, 1); + memset(&_hrt_work, 0, sizeof(_hrt_work)); +} + +static void +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)) { + 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"); +} + +/** + * 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. + * + * 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; + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + + //printf("hrt_call_reschedule\n"); + + /* + * 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 */ + ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + 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 + 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); + hrt_unlock(); +} + +/* + * 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) +{ + //printf("hrt_call_after\n"); + 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 + +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/posix/px4_layer/lib_crc32.c b/src/platforms/posix/px4_layer/lib_crc32.c new file mode 100644 index 0000000000..4ba6fbf6df --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk new file mode 100644 index 0000000000..2df90461da --- /dev/null +++ b/src/platforms/posix/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_posix_impl.cpp \ + px4_posix_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/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp new file mode 100644 index 0000000000..3f1916a51a --- /dev/null +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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_impl.cpp + * + * PX4 Middleware Wrapper Linux Implementation + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "systemlib/param/param.h" + +__BEGIN_DECLS + +long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); + +extern void hrt_init(void); + +__END_DECLS + +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/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp new file mode 100644 index 0000000000..5a36313aed --- /dev/null +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -0,0 +1,288 @@ +/**************************************************************************** + * + * 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_posix_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 +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); + PX4_DEBUG("Before px4_task_exit"); + px4_task_exit(0); + PX4_DEBUG("After px4_task_exit"); + + return NULL; +} + +void +px4_systemreset(bool to_bootloader) +{ + 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[]) +{ + 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; + + rv = pthread_attr_init(&attr); + if (rv != 0) { + 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) { + 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; + } + + 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) +{ + 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) +{ + int i; + pthread_t pid = pthread_self(); + + // 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_DEBUG("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) +{ + int rv = 0; + pthread_t pid; + PX4_DEBUG("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() +{ + int idx; + int count = 0; + + PX4_INFO("Active Tasks:"); + for (idx=0; idx < PX4_MAX_TASKS; idx++) + { + if (taskmap[idx].isused) { + PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid); + count++; + } + } + if (count == 0) + PX4_INFO(" No running tasks"); + +} + +__BEGIN_DECLS +const char *getprogname() +{ + pthread_t pid = pthread_self(); + 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/posix/px4_layer/sq_addafter.c b/src/platforms/posix/px4_layer/sq_addafter.c new file mode 100644 index 0000000000..5d47feba0f --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/sq_addlast.c b/src/platforms/posix/px4_layer/sq_addlast.c new file mode 100644 index 0000000000..faa07efb5c --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/sq_remfirst.c b/src/platforms/posix/px4_layer/sq_remfirst.c new file mode 100644 index 0000000000..f81c18dc2e --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/work_cancel.c b/src/platforms/posix/px4_layer/work_cancel.c new file mode 100644 index 0000000000..6f737877d9 --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/work_lock.h b/src/platforms/posix/px4_layer/work_lock.h new file mode 100644 index 0000000000..f77f228b36 --- /dev/null +++ b/src/platforms/posix/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/posix/px4_layer/work_queue.c b/src/platforms/posix/px4_layer/work_queue.c new file mode 100644 index 0000000000..46ca359842 --- /dev/null +++ b/src/platforms/posix/px4_layer/work_queue.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * 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 "work_lock.h" + +#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. + */ + + 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 */ + + work_unlock(qid); + return PX4_OK; +} + +#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/px4_layer/work_thread.c b/src/platforms/posix/px4_layer/work_thread.c new file mode 100644 index 0000000000..1128a80944 --- /dev/null +++ b/src/platforms/posix/px4_layer/work_thread.c @@ -0,0 +1,327 @@ +/**************************************************************************** + * 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 "work_lock.h" + +#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 + ****************************************************************************/ +sem_t _work_lock[NWORKERS]; + +/**************************************************************************** + * 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, int lock_id) +{ + volatile FAR struct work_s *work; + worker_t worker; + FAR void *arg; + uint64_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; + + work_lock(lock_id); + + 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 = 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 */ + + (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! + */ + + work_unlock(lock_id); + 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. + */ + + work_lock(lock_id); + 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 = USEC_PER_TICK*(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. + */ + work_unlock(lock_id); + + 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 + * + * 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], 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], 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], USRWORK); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_USRWORK */ + +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/hello/hello_example.cpp b/src/platforms/posix/tests/hello/hello_example.cpp new file mode 100644 index 0000000000..a30aeb57bc --- /dev/null +++ b/src/platforms/posix/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/posix/tests/hello/hello_example.h b/src/platforms/posix/tests/hello/hello_example.h new file mode 100644 index 0000000000..a4ae517056 --- /dev/null +++ b/src/platforms/posix/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/posix/tests/hello/hello_main.cpp b/src/platforms/posix/tests/hello/hello_main.cpp new file mode 100644 index 0000000000..69e8c21ec0 --- /dev/null +++ b/src/platforms/posix/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/posix/tests/hello/hello_start_posix.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp new file mode 100644 index 0000000000..8dde729a6e --- /dev/null +++ b/src/platforms/posix/tests/hello/hello_start_posix.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_posix.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) { + PX4_WARN("usage: hello {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (HelloExample::appState.isRunning()) { + PX4_INFO("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()) { + PX4_INFO("is running\n"); + + } else { + PX4_INFO("not started\n"); + } + + return 0; + } + + PX4_WARN("usage: hello_main {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/posix/tests/hello/module.mk b/src/platforms/posix/tests/hello/module.mk new file mode 100644 index 0000000000..294c0ad7fc --- /dev/null +++ b/src/platforms/posix/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_posix.cpp \ + hello_example.cpp + diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp new file mode 100644 index 0000000000..8dd1f4bde3 --- /dev/null +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -0,0 +1,90 @@ + +/**************************************************************************** + * + * 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.cpp + * Test High Resolution Timers in Linux + * + * @author Mark Charlebois + */ + +#include +#include +#include "hrt_test.h" +#include +#include + +px4::AppState HRTTest::appState; + +static struct hrt_call t1; +static int update_interval = 1; + +static void timer_expired(void *arg) +{ + static int i = 0; + PX4_INFO("Test\n"); + if (i < 5) { + i++; + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); + } +} + +int HRTTest::main() +{ + appState.setRunning(true); + + hrt_abstime t = hrt_absolute_time(); + usleep(1000000); + hrt_abstime elt = hrt_elapsed_time(&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); + 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)); + + PX4_INFO("HRT_CALL %d\n", hrt_called(&t1)); + + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); + sleep(2); + PX4_INFO("HRT_CALL - %d\n", hrt_called(&t1)); + hrt_cancel(&t1); + PX4_INFO("HRT_CALL + %d\n", hrt_called(&t1)); + + return 0; +} diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.h b/src/platforms/posix/tests/hrt_test/hrt_test.h new file mode 100644 index 0000000000..c4c97be6d2 --- /dev/null +++ b/src/platforms/posix/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/posix/tests/hrt_test/hrt_test_main.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_main.cpp new file mode 100644 index 0000000000..9a01690e38 --- /dev/null +++ b/src/platforms/posix/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 hrt_test_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, "hrt_test"); + + printf("starting\n"); + HRTTest test; + test.main(); + + printf("goodbye\n"); + 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 new file mode 100644 index 0000000000..2544804496 --- /dev/null +++ b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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_posix.cpp + * + * @author Mark Charlebois + */ +#include "hrt_test.h" +#include +#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) { + PX4_WARN("usage: hrttest_main {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (HRTTest::appState.isRunning()) { + PX4_INFO("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()) { + PX4_INFO("is running\n"); + + } else { + PX4_INFO("not started\n"); + } + + return 0; + } + + PX4_WARN("usage: hrttest_main {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/posix/tests/hrt_test/module.mk b/src/platforms/posix/tests/hrt_test/module.mk new file mode 100644 index 0000000000..e1bd89ef7f --- /dev/null +++ b/src/platforms/posix/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_posix.cpp \ + hrt_test.cpp + diff --git a/src/platforms/posix/tests/vcdev_test/module.mk b/src/platforms/posix/tests/vcdev_test/module.mk new file mode 100644 index 0000000000..81920c860c --- /dev/null +++ b/src/platforms/posix/tests/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_posix.cpp \ + vcdevtest_example.cpp + diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp new file mode 100644 index 0000000000..dc3111a40a --- /dev/null +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -0,0 +1,214 @@ + +/**************************************************************************** + * + * 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 +#include +#include "vcdevtest_example.h" +#include +#include +#include +#include + +px4::AppState VCDevExample::appState; + +using namespace device; + +#define TESTDEV "/dev/vdevex" + +static int writer_main(int argc, char *argv[]) +{ + char buf[1] = { '1' }; + + int fd = px4_open(TESTDEV, PX4_F_RDONLY); + if (fd < 0) { + PX4_INFO("--- Open failed %d %d", fd, px4_errno); + return -px4_errno; + } + + int ret; + int i=0; + while (i<3) { + // Wait for 3 seconds + PX4_INFO("--- Sleeping for 4 sec\n"); + ret = sleep(4); + if (ret < 0) { + PX4_INFO("--- sleep failed %d %d\n", ret, errno); + return ret; + } + + PX4_INFO("--- writing to fd\n"); + ret = px4_write(fd, buf, 1); + ++i; + } + px4_close(fd); + return ret; +} + +class VCDevNode : public VDev { +public: + VCDevNode() : VDev("vcdevtest", TESTDEV) {}; + + ~VCDevNode() {} + + virtual ssize_t write(device::file_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); + + return buflen; +} + +VCDevExample::~VCDevExample() { + if (_node) { + delete _node; + _node = 0; + } +} + +static int test_pub_block(int fd, unsigned long blocked) +{ + int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked); + if (ret < 0) { + PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); + return -px4_errno; + } + + ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0); + if (ret < 0) { + PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); + return -px4_errno; + } + PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); + + return 0; +} + +int VCDevExample::main() +{ + appState.setRunning(true); + + _node = new VCDevNode(); + + if (_node == 0) { + PX4_INFO("Failed to allocate VCDevNode\n"); + return -ENOMEM; + } + + if (_node->init() != PX4_OK) { + PX4_INFO("Failed to init VCDevNode\n"); + return 1; + } + + int fd = px4_open(TESTDEV, PX4_F_RDONLY); + + if (fd < 0) { + 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) { + PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); + return -px4_errno; + } + PX4_INFO("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]; + + // 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 (!appState.exitRequested() && i<13) { + PX4_INFO("=====================\n"); + PX4_INFO("==== sleeping 2 sec ====\n"); + sleep(2); + + fds[0].fd = fd; + fds[0].events = POLLIN; + fds[0].revents = 0; + + PX4_INFO("==== Calling Poll\n"); + ret = px4_poll(fds, 1, 1000); + PX4_INFO("==== Done poll\n"); + if (ret < 0) { + PX4_INFO("==== poll failed %d %d\n", ret, px4_errno); + px4_close(fd); + } + else if (i > 0) { + if (ret == 0) { + PX4_INFO("==== Nothing to read - PASS\n"); + } + else { + PX4_INFO("==== poll returned %d\n", ret); + } + } + else if (i == 0) { + if (ret == 1) { + PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL"); + } + else { + PX4_INFO("==== %d to read - FAIL\n", ret); + } + + } + ++i; + } + px4_close(fd); + + return 0; +} diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h new file mode 100644 index 0000000000..4898210dff --- /dev/null +++ b/src/platforms/posix/tests/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::AppState appState; /* track requests to terminate app */ + +private: + VCDevNode *_node; +}; diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_main.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_main.cpp new file mode 100644 index 0000000000..6d8d31a4c1 --- /dev/null +++ b/src/platforms/posix/tests/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/posix/tests/vcdev_test/vcdevtest_start_posix.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp new file mode 100644 index 0000000000..5ed9269b2d --- /dev/null +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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_posix.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "vcdevtest_example.h" +#include +#include +#include +#include + +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::appState.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::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (VCDevExample::appState.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/posix/tests/wqueue/module.mk b/src/platforms/posix/tests/wqueue/module.mk new file mode 100644 index 0000000000..4c3b6550c6 --- /dev/null +++ b/src/platforms/posix/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_posix.cpp \ + wqueue_test.cpp + diff --git a/src/platforms/posix/tests/wqueue/wqueue_main.cpp b/src/platforms/posix/tests/wqueue/wqueue_main.cpp new file mode 100644 index 0000000000..8dd5cc9707 --- /dev/null +++ b/src/platforms/posix/tests/wqueue/wqueue_main.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 +#include "wqueue_test.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "wqueue_test"); + + PX4_INFO("wqueue hello\n"); + WQueueTest wq; + wq.main(); + + 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 new file mode 100644 index 0000000000..2479020097 --- /dev/null +++ b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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_posix.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "wqueue_test.h" +#include +#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) { + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (WQueueTest::appState.isRunning()) { + PX4_INFO("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()) { + PX4_INFO("is running\n"); + + } else { + PX4_INFO("not started\n"); + } + + return 0; + } + + PX4_INFO("usage: wqueue_test {start|stop|status}\n"); + return 1; +} diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.cpp b/src/platforms/posix/tests/wqueue/wqueue_test.cpp new file mode 100644 index 0000000000..6c9ececc14 --- /dev/null +++ b/src/platforms/posix/tests/wqueue/wqueue_test.cpp @@ -0,0 +1,106 @@ + +/**************************************************************************** + * + * 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 +#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() +{ + static int iter = 0; + printf("done lp work\n"); + 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"); + if (iter > 5) + _hpwork_done = true; + ++iter; + + // requeue + work_queue(HPWORK, &_hpwork, (worker_t)&hp_worker_cb, this, 1000); +} + +int WQueueTest::main() +{ + appState.setRunning(true); + + //Put work on HP work queue + 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, 1000); + + + // Wait for work to finsh + while (!appState.exitRequested() && !(_hpwork_done && _lpwork_done)) { + printf(" Sleeping for 2 sec...\n"); + sleep(2); + } + + return 0; +} diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.h b/src/platforms/posix/tests/wqueue/wqueue_test.h new file mode 100644 index 0000000000..6db3fc1e25 --- /dev/null +++ b/src/platforms/posix/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_adc.h b/src/platforms/px4_adc.h new file mode 100644 index 0000000000..8420f51971 --- /dev/null +++ b/src/platforms/px4_adc.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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 px4_adc.h + * + * ADC header depending on the build target + */ + +#pragma once + +#include + +#if defined(__PX4_ROS) +#error "ADC not supported in ROS" +#elif defined(__PX4_NUTTX) +/* + * Building for NuttX + */ +#include +#elif defined(__PX4_POSIX) + +// 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) */ +} __attribute__ ((packed)); + +// Example settings +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#define ADCSIM0_DEVICE_PATH "/dev/adc0" +#else +#error "No target platform defined" +#endif diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h new file mode 100644 index 0000000000..9aa285e228 --- /dev/null +++ b/src/platforms/px4_app.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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_app.h + * + * PX4 app template classes, functions and defines. Apps need to call their + * main function PX4_MAIN. + */ + +#pragma once + +namespace px4 { + +class AppState { +public: + ~AppState() {} + +#if defined(__PX4_ROS) + AppState() {} + + bool exitRequested() { return !ros::ok(); } + void requestExit() { ros::shutdown(); } +#else + AppState() : _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: + 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 + +#ifdef PX4_MAIN +extern int PX4_MAIN(int argc, char *argv[]); +#endif + +#endif + diff --git a/src/platforms/px4_common.h b/src/platforms/px4_common.h new file mode 100644 index 0000000000..384b1bca65 --- /dev/null +++ b/src/platforms/px4_common.h @@ -0,0 +1,7 @@ +#pragma once + +#ifdef __PX4_QURT +#include +size_t strnlen(const char *s, size_t maxlen); + +#endif diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h new file mode 100644 index 0000000000..e724411080 --- /dev/null +++ b/src/platforms/px4_config.h @@ -0,0 +1,60 @@ + +/**************************************************************************** + * + * 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 + Configuration flags used in code. + */ + +#pragma once + +#if defined(__PX4_NUTTX) + +#include + +#elif defined (__PX4_POSIX) + +#define CONFIG_NFILE_STREAMS 1 +#define CONFIG_SCHED_WORKQUEUE 1 +#define CONFIG_SCHED_HPWORK 1 +#define CONFIG_SCHED_LPWORK 1 +#define CONFIG_ARCH_BOARD_POSIXTEST 1 + +/** time in ms between checks for work in work queues **/ +#define CONFIG_SCHED_WORKPERIOD 50000 + +#define CONFIG_SCHED_INSTRUMENTATION 1 +#define CONFIG_MAX_TASKS 32 + +#endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 996e56efb9..db54f8c53e 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 @@ -45,6 +48,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) /* @@ -58,59 +63,19 @@ /* 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) -#define OK 0 -#define ERROR -1 +#define PX4_ISFINITE(x) std::isfinite(x) -//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_POSIX) /* - * Building for NuttX + * Building for NuttX or POSIX */ #include /* 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; @@ -118,6 +83,15 @@ 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,9 +99,121 @@ typedef param_t px4_param_t; (c) == '\r' || (c) == '\f' || c== '\v') #endif +#define _PX4_IOC(x,y) _IOC(x,y) + +#define px4_statfs_buf_f_bavail_t int + +#define PX4_ISFINITE(x) isfinite(x) + +#ifndef PRIu64 +#define PRIu64 "llu" #endif -/* Defines for all platforms */ +/* + * POSIX Specific defines + */ +#elif defined(__PX4_POSIX) + +// 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 */ +//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 +extern long PX4_TICKS_PER_SEC; +__END_DECLS + +#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 + +#endif + + +/* + * Defines for ROS and Linux + */ +#if defined(__PX4_ROS) || defined(__PX4_POSIX) +#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 +#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 + +#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 + +/* + *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_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 + diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h new file mode 100644 index 0000000000..2b72f37c5d --- /dev/null +++ b/src/platforms/px4_i2c.h @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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; + +#elif defined(__PX4_POSIX) +#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; + +// 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 - 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; } + +#ifdef __PX4_QURT + +struct i2c_msg +{ + 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 */ +}; + +// 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 +#else +#error "No target platform defined" +#endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 0e98783fda..83bb60d796 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,66 @@ #include #include +#elif defined(__PX4_POSIX) && !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 +#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_log.h b/src/platforms/px4_log.h new file mode 100644 index 0000000000..07438a4ec4 --- /dev/null +++ b/src/platforms/px4_log.h @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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_log.h + * Platform dependant logging/debug + */ + +#pragma once + +#define __px4_log_omit(level, ...) { } + +#define __px4_log(level, ...) { \ + printf("%-5s ", level);\ + printf(__VA_ARGS__);\ + printf("\n");\ +} +#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(...) __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) + +#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 + +#error "Target platform unknown" + +#endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index 735d34234a..786614fc9d 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -54,12 +54,19 @@ __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; } +#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 + */ #endif class Rate diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 83a3e422dd..b0deef16c1 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 */ @@ -51,6 +52,7 @@ #include #else /* includes when building for NuttX */ +#include #include #endif #include @@ -62,10 +64,11 @@ class NodeHandle : private ros::NodeHandle { public: - NodeHandle() : + NodeHandle(AppState &a) : ros::NodeHandle(), _subs(), - _pubs() + _pubs(), + _appState(a) {} ~NodeHandle() @@ -136,15 +139,19 @@ public: protected: std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ + + AppState &_appState; + }; #else //Building for NuttX class __EXPORT NodeHandle { public: - NodeHandle() : + NodeHandle(AppState &a) : _subs(), _pubs(), - _sub_min_interval(nullptr) + _sub_min_interval(nullptr), + _appState(a) {} ~NodeHandle() @@ -262,7 +269,7 @@ public: */ void spin() { - while (ok()) { + while (!_appState.exitRequested()) { const int timeout_ms = 100; /* Only continue in the loop if the nodehandle has subscriptions */ @@ -272,10 +279,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(); } } @@ -287,6 +294,8 @@ protected: SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ + AppState &_appState; + /** * Check if this is the smallest interval so far and update _sub_min_interval */ diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h new file mode 100644 index 0000000000..67c9a47963 --- /dev/null +++ b/src/platforms/px4_posix.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + + +#ifdef __PX4_NUTTX + +#define PX4_F_RDONLY 1 +#define PX4_F_WRONLY 2 + +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 +#define px4_fsync _GLOBAL fsync + +#elif defined(__PX4_POSIX) + +#define PX4_F_RDONLY O_RDONLY +#define PX4_F_WRONLY O_WRONLY +#define PX4_F_CREAT O_CREAT + +typedef short pollevent_t; + +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; + +__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 int px4_fsync(int fd); + +__END_DECLS +#else +#error "No TARGET OS Provided" +#endif + +__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); +__EXPORT const char * px4_get_topic_names(unsigned int *handle); +__END_DECLS diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h new file mode 100644 index 0000000000..17397ee7ac --- /dev/null +++ b/src/platforms/px4_spi.h @@ -0,0 +1,35 @@ +#pragma once + +#ifdef __PX4_NUTTX +#include +#elif defined(__PX4_POSIX) +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 new file mode 100644 index 0000000000..e7eda424bf --- /dev/null +++ b/src/platforms/px4_tasks.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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; + +/** 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) +#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; + +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; + +/** 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[]); + +/** 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); + +/** Show a list of running tasks **/ +__EXPORT void px4_show_tasks(void); + +__END_DECLS + diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h new file mode 100644 index 0000000000..0c9b7f24c9 --- /dev/null +++ b/src/platforms/px4_time.h @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +#if defined(__PX4_LINUX) || defined(__PX4_NUTTX) + +#define px4_clock_gettime clock_gettime +#define px4_clock_settime clock_settime + +#elif defined(__PX4_QURT) + +#include + +#define CLOCK_REALTIME 1 + +__BEGIN_DECLS + +#if 0 +#if !defined(__cplusplus) +struct timespec +{ + time_t tv_sec; + 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); + +__EXPORT int usleep(useconds_t usec); +__EXPORT unsigned int sleep(unsigned int sec); + +__END_DECLS +#endif diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h new file mode 100644 index 0000000000..b9ae6b4b6c --- /dev/null +++ b/src/platforms/px4_workqueue.h @@ -0,0 +1,144 @@ +/**************************************************************************** + * 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 + +#if defined(__PX4_ROS) +#error "Work queue not supported on ROS" +#elif defined(__PX4_NUTTX) +#include +#include +#include +#elif defined(__PX4_POSIX) || defined(__PX4_QURT) + +#include +#include + +__BEGIN_DECLS + +#define HPWORK 0 +#define LPWORK 1 +#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 */ + +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_queues_init() + * + * Description: + * Initialize the work queues. + * + ****************************************************************************/ +void work_queues_init(void); + +/**************************************************************************** + * 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, struct work_s *work, worker_t worker, 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, struct work_s *work); + +uint32_t 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 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/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 new file mode 100644 index 0000000000..59f1809e2a --- /dev/null +++ b/src/platforms/qurt/include/poll.h @@ -0,0 +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/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/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); + 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/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..3edf34f64c --- /dev/null +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * 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 +#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); + +// 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. + */ +hrt_abstime hrt_absolute_time(void) +{ + struct timespec ts; + + // FIXME - clock_gettime unsupported 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) +{ + hrt_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; + hrt_unlock(); + // 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) +{ + //printf("hrt_init\n"); + sq_init(&callout_queue); + sem_init(&_hrt_lock, 0, 1); + memset(&_hrt_work, 0, sizeof(_hrt_work)); +} + +static void +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)) { + 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"); +} + +/** + * 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. + * + * 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; + uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX*1000); + + //printf("hrt_call_reschedule\n"); + + /* + * 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 */ + ticks = USEC2TICK(HRT_INTERVAL_MIN*1000); + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + 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 + 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); + hrt_unlock(); +} + +/* + * 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) +{ + //printf("hrt_call_after\n"); + 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 + +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/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/main.cpp b/src/platforms/qurt/px4_layer/main.cpp new file mode 100644 index 0000000000..3f42c1c301 --- /dev/null +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -0,0 +1,164 @@ +/**************************************************************************** + * + * 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 +#include +#include + +using namespace std; + +extern void init_app_map(map &apps); +extern void list_builtins(map &apps); + +static const char *commands = +"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\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" +"sensors start\n" +"mavlink start -d /tmp/ttyS0\n" +"commander start\n" +#endif +; + +static void run_cmd(map &apps, const vector &appargs) { + // command is appargs[0] + string command = appargs[0]; + if (apps.find(command) != apps.end()) { + const char *arg[2+1]; + + 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]); + ++i; + } + arg[i] = (char *)0; + apps[command](i,(char **)arg); + } + else + { + list_builtins(apps); + } +} + +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(map &apps, const char *cmds) +{ + vector appargs; + int i=0; + const char *b = cmds; + bool found_first_char = false; + char arg[20]; + + // Eat leading whitespace + eat_whitespace(b, i); + + for(;;) { + // End of command line + if (b[i] == '\n' || b[i] == '\0') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs.push_back(arg); + + // If we have a command to run + if (appargs.size() > 0) { + run_cmd(apps, appargs); + } + appargs.clear(); + if (b[i] == '\n') { + eat_whitespace(b, ++i); + continue; + } + else { + break; + } + } + // End of arg + else if (b[i] == ' ') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs.push_back(arg); + eat_whitespace(b, ++i); + continue; + } + ++i; + } +} + +namespace px4 { +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(apps, commands); + for (;;) {} +} + diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk new file mode 100644 index 0000000000..92f1c6abfb --- /dev/null +++ b/src/platforms/qurt/px4_layer/module.mk @@ -0,0 +1,57 @@ +############################################################################ +# +# 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 +# + +SRCDIR=$(dir $(MODULE_MK)) + +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 \ + 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 new file mode 100644 index 0000000000..3ff09b3d4f --- /dev/null +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -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_linux_impl.cpp + * + * PX4 Middleware Wrapper Linux Implementation + */ + +#include +#include +#include +#include +#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; + +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; + va_start(args, fmt); + printf(fmt, args); + printf("n"); +} +#endif + +extern int _posix_init(void); + +__END_DECLS + +extern struct wqueue_s gwork[NWORKERS]; + + +namespace px4 +{ + +void init_once(void); + +void init_once(void) +{ + // Required for QuRT + _posix_init(); + + work_queues_init(); + hrt_init(); +} + +void init(int argc, char *argv[], const char *app_name) +{ + PX4_DEBUG("App name: %s\n", 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; +} + +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 new file mode 100644 index 0000000000..d688f1104a --- /dev/null +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * 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_posix_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 +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); + px4_task_exit(0); + + return NULL; +} + +void +px4_systemreset(bool to_bootloader) +{ + 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[]) +{ + int rv; + int argc = 0; + int i; + unsigned int len = 0; + unsigned long offset; + unsigned long structsize; + char * p = (char *)argv; + + PX4_DEBUG("Creating %s\n", name); + 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; + + 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) { + + 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; + 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) +{ + int i; + pthread_t pid = pthread_self(); + + // 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_DEBUG("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) +{ + int rv = 0; + pthread_t pid; + PX4_DEBUG("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() +{ + int idx; + int count = 0; + + PX4_INFO("Active Tasks:"); + for (idx=0; idx < PX4_MAX_TASKS; idx++) + { + if (taskmap[idx].isused) { + PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), taskmap[idx].pid); + count++; + } + } + if (count == 0) + PX4_INFO(" No running tasks"); + +} diff --git a/src/platforms/qurt/px4_layer/queue.c b/src/platforms/qurt/px4_layer/queue.c new file mode 100644 index 0000000000..eecbd98830 --- /dev/null +++ b/src/platforms/qurt/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; +} + + 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_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 new file mode 100644 index 0000000000..1746359b63 --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_queue.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * 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 "work_lock.h" + +#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. + */ + + 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 */ + + work_unlock(qid); + 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..3873034189 --- /dev/null +++ b/src/platforms/qurt/px4_layer/work_thread.c @@ -0,0 +1,328 @@ +/**************************************************************************** + * 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 "work_lock.h" + +#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 + ****************************************************************************/ +sem_t _work_lock[NWORKERS]; + +/**************************************************************************** + * 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, int lock_id) +{ + volatile FAR struct work_s *work; + worker_t worker; + FAR void *arg; + uint64_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; + + work_lock(lock_id); + + 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 = 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 */ + + (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! + */ + + work_unlock(lock_id); + if (!worker) { + PX4_WARN("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. + */ + + work_lock(lock_id); + 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 = USEC_PER_TICK*(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. + */ + work_unlock(lock_id); + + 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 + * + * 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], 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], 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], USRWORK); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +#endif /* CONFIG_SCHED_USRWORK */ + +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/qurt/tests/hello/hello_example.cpp b/src/platforms/qurt/tests/hello/hello_example.cpp new file mode 100644 index 0000000000..1cfcf463e1 --- /dev/null +++ b/src/platforms/qurt/tests/hello/hello_example.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 +#include + +px4::AppState HelloExample::appState; + +int HelloExample::main() +{ + appState.setRunning(true); + + int i=0; + while (!appState.exitRequested() && i<5) { + + PX4_DEBUG(" Doing work..."); + ++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..35e72e8b36 --- /dev/null +++ b/src/platforms/qurt/tests/hello/hello_main.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 +#include "hello_example.h" +#include + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "hello"); + + PX4_DEBUG("hello"); + HelloExample hello; + hello.main(); + + 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 new file mode 100644 index 0000000000..e4180307ce --- /dev/null +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 +#include + +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() +{ + PX4_DEBUG("usage: hello {start|stop|status}"); +} +int hello_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (HelloExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("hello", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char* const*)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + HelloExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (HelloExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + 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..6bed2aea67 --- /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_qurt.cpp \ + hello_example.cpp + 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; } 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/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 2f5ed3265d..686a50f436 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 @@ -57,23 +58,31 @@ */ 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 void usage(const char *reason); +static int load(const char *devname, const char *fname); int mixer_main(int argc, char *argv[]) { - if (argc < 2) + if (argc < 2) { usage("missing command"); - - if (!strcmp(argv[1], "load")) { - if (argc < 4) - usage("missing device or filename"); - - load(argv[2], argv[3]); + return 1; } - usage("unrecognised command"); + if (!strcmp(argv[1], "load")) { + if (argc < 4) { + usage("missing device or filename"); + return 1; + } + + int ret = load(argv[2], argv[3]); + if(ret !=0) { + warnx("failed to load mixer"); + return 1; + } + } + usage("Unknown command"); + return 0; } static void @@ -84,31 +93,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; } 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 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 45fb2fd830..2e0f1c1047 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -39,7 +39,8 @@ * Parameter tool. */ -#include +#include +#include #include #include @@ -58,16 +59,16 @@ __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_index(const char *index, bool used_index); 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[]) @@ -75,34 +76,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()); } } @@ -115,15 +116,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; } } @@ -133,40 +136,42 @@ 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); } } @@ -189,66 +194,73 @@ param_main(int argc, char *argv[]) } } - 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); + int fd = px4_open(param_file_name, O_WRONLY | O_CREAT, 0x777); 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); - close(fd); + px4_close(fd); 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); + int fd = px4_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); + px4_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); + int fd = px4_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); + px4_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 @@ -257,8 +269,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 @@ -287,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; @@ -330,7 +340,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++; @@ -358,7 +369,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; } @@ -373,7 +384,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: @@ -381,10 +392,10 @@ do_show_print(void *arg, param_t param) return; } - printf("\n", param); + printf("\n", (unsigned long)param); } -static void +static int do_set(const char *name, const char *val, bool fail_on_not_found) { int32_t i; @@ -394,7 +405,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: ", @@ -417,9 +429,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); } } @@ -449,13 +461,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; @@ -465,7 +478,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; } /* @@ -486,7 +500,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; } } @@ -514,7 +528,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) { @@ -523,10 +538,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) { @@ -538,17 +553,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; @@ -567,9 +579,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; } 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..de4d77d5fd 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 @@ -65,7 +65,7 @@ int reboot_main(int argc, char *argv[]) } } - systemreset(to_bootloader); + px4_systemreset(to_bootloader); } 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..da73f68964 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -36,7 +36,7 @@ * Included Files ****************************************************************************/ -#include +#include #include @@ -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"); } } 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_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; 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/topic_listener/module.mk b/src/systemcmds/topic_listener/module.mk new file mode 100644 index 0000000000..4048c6b318 --- /dev/null +++ b/src/systemcmds/topic_listener/module.mk @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build the topic listener tool. +# + +./topic_listener.cpp : $(PX4_BASE)/Tools/generate_listener.py + $(PX4_BASE)/Tools/generate_listener.py $(PX4_BASE) > $@ + +MODULE_COMMAND = listener +SRCS = topic_listener.cpp + +MODULE_STACKSIZE = 1800 + +MAXOPTIMIZATION = -Os 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 diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 84078c3e5d..ae87cfca7a 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,8 @@ 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 add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)