diff --git a/.gitignore b/.gitignore index cd3f8f26c2..029a45346f 100644 --- a/.gitignore +++ b/.gitignore @@ -49,4 +49,5 @@ unittests/build *.pretty xcode src/platforms/posix/px4_messages/ +src/platforms/posix-arm/px4_messages/ src/platforms/qurt/px4_messages/ diff --git a/Makefile b/Makefile index 0075022892..1532f7ed35 100644 --- a/Makefile +++ b/Makefile @@ -33,7 +33,7 @@ # Top-level Makefile for building PX4 firmware images. # -TARGETS := nuttx posix qurt +TARGETS := nuttx posix posix-arm qurt EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) ifneq ($(EXPLICIT_TARGET),) export PX4_TARGET_OS=$(EXPLICIT_TARGET) @@ -241,6 +241,9 @@ endif ifeq ($(PX4_TARGET_OS),posix) include $(PX4_BASE)makefiles/firmware_posix.mk endif +ifeq ($(PX4_TARGET_OS),posix-arm) +include $(PX4_BASE)makefiles/firmware_posix.mk +endif ifeq ($(PX4_TARGET_OS),qurt) include $(PX4_BASE)makefiles/firmware_qurt.mk endif @@ -285,7 +288,7 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -nuttx posix qurt: +nuttx posix posix-arm qurt: ifeq ($(GOALS),) make PX4_TARGET_OS=$@ $(GOALS) else diff --git a/Tools/posix_apps.py b/Tools/posix_apps.py index cbe5204ee6..b7ddbc210c 100755 --- a/Tools/posix_apps.py +++ b/Tools/posix_apps.py @@ -47,6 +47,7 @@ print(""" #include #include +#include using namespace std; @@ -63,6 +64,7 @@ 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 int sleep_main(int argc, char *argv[]); } @@ -80,6 +82,7 @@ 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('\tapps["sleep"] = sleep_main;') print(""" return apps; } @@ -121,5 +124,14 @@ static int list_files_main(int argc, char *argv[]) px4_show_files(); return 0; } +static int sleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + cout << "Usage: sleep " << endl; + return 1; + } + sleep(atoi(argv[1])); + return 0; +} """) diff --git a/Tools/posix_run.sh b/Tools/posix_run.sh index fdeefb4a29..3a20dc3766 100755 --- a/Tools/posix_run.sh +++ b/Tools/posix_run.sh @@ -1,19 +1,5 @@ #!/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 +mkdir -p Build/posix_default.build/rootfs/fs/microsd +mkdir -p Build/posix_default.build/rootfs/eeprom +cd Build/posix_default.build && ./mainapp ../../posix-configs/posixtest/init/rc.S diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f5f5d8188d..43cbfb59c0 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -548,6 +548,9 @@ endif ifeq ($(PX4_TARGET_OS),posix) include $(MK_DIR)/posix_elf.mk endif +ifeq ($(PX4_TARGET_OS),posix-arm) +include $(MK_DIR)/posix_elf.mk +endif ifeq ($(PX4_TARGET_OS),qurt) include $(MK_DIR)/qurt_elf.mk endif diff --git a/makefiles/nuttx/board_aerocore.mk b/makefiles/nuttx/board_aerocore.mk index 6f4b932666..71a0f264d0 100644 --- a/makefiles/nuttx/board_aerocore.mk +++ b/makefiles/nuttx/board_aerocore.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = AEROCORE -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4-stm32f4discovery.mk b/makefiles/nuttx/board_px4-stm32f4discovery.mk index fe761ba444..2477f55858 100644 --- a/makefiles/nuttx/board_px4-stm32f4discovery.mk +++ b/makefiles/nuttx/board_px4-stm32f4discovery.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = PX4_STM32F4DISCOVERY -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4fmu-v1.mk b/makefiles/nuttx/board_px4fmu-v1.mk index 4d692e31ab..45156efcdc 100644 --- a/makefiles/nuttx/board_px4fmu-v1.mk +++ b/makefiles/nuttx/board_px4fmu-v1.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = PX4FMU_V1 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4fmu-v2.mk b/makefiles/nuttx/board_px4fmu-v2.mk index e9a2985b7f..7c3e00c5d0 100644 --- a/makefiles/nuttx/board_px4fmu-v2.mk +++ b/makefiles/nuttx/board_px4fmu-v2.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM4F CONFIG_BOARD = PX4FMU_V2 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4io-v1.mk b/makefiles/nuttx/board_px4io-v1.mk index 1872a4124f..66731a5694 100644 --- a/makefiles/nuttx/board_px4io-v1.mk +++ b/makefiles/nuttx/board_px4io-v1.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM3 CONFIG_BOARD = PX4IO_V1 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/nuttx/board_px4io-v2.mk b/makefiles/nuttx/board_px4io-v2.mk index 50a4068fb2..94c3318199 100644 --- a/makefiles/nuttx/board_px4io-v2.mk +++ b/makefiles/nuttx/board_px4io-v2.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = CORTEXM3 CONFIG_BOARD = PX4IO_V2 -include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk +include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk similarity index 97% rename from makefiles/toolchain_gnu-arm-eabi.mk rename to makefiles/nuttx/toolchain_gnu-arm-eabi.mk index 6c4e64f54c..3c2898cb0c 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk @@ -125,6 +125,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # note - requires corresponding support in NuttX INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a) + # Language-specific flags # ARCHCFLAGS = -std=gnu99 @@ -262,7 +264,8 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + #$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 endef # Update the archive $1 with the files in $2 diff --git a/makefiles/posix-arm.mk b/makefiles/posix-arm.mk new file mode 100644 index 0000000000..13cb97f0db --- /dev/null +++ b/makefiles/posix-arm.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-arm/board_eagle.mk b/makefiles/posix-arm/board_eagle.mk new file mode 100644 index 0000000000..94dd049367 --- /dev/null +++ b/makefiles/posix-arm/board_eagle.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the POSIX port of PX4 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXA8 +CONFIG_BOARD = EAGLE + +include $(PX4_MK_DIR)/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk diff --git a/makefiles/posix-arm/config_eagle_default.mk b/makefiles/posix-arm/config_eagle_default.mk new file mode 100644 index 0000000000..30dada7bbd --- /dev/null +++ b/makefiles/posix-arm/config_eagle_default.mk @@ -0,0 +1,85 @@ +# +# 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/navigator +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 +MODULES += modules/controllib + +# +# 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 +MODULES += platforms/posix/drivers/gpssim + +# +# Unit tests +# +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# muorb fastrpc changes. +# +#MODULES += $(PX4_BASE)../muorb_krait diff --git a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk new file mode 100644 index 0000000000..a5964e07d9 --- /dev/null +++ b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk @@ -0,0 +1,329 @@ +# +# Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +# +# Definitions for a generic GNU ARM-EABI toolchain +# + +#$(info TOOLCHAIN arm-linux-gnueabihf) + +# Toolchain commands. Normally only used inside this file. +# +CROSSDEV = arm-linux-gnueabihf- + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +# Check if the right version of the toolchain is available +# +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.2 4.8.4 4.9.3 +CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) + +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 ?= -O3 + +# Base CPU flags for each of the supported architectures. +# +ARCHCPUFLAGS_CORTEXA8 = -mtune=cortex-a8 \ + -mthumb-interwork \ + -march=armv7-a \ + -mfloat-abi=hard \ + -mfpu=neon + +ARCHCPUFLAGS_CORTEXM4F = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + +ARCHCPUFLAGS_CORTEXM4 = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfloat-abi=soft + +ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m \ + -mfloat-abi=soft + +# Enabling stack checks if OS was build with them +# +TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h +TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1 +ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;) +ifeq ("$(ENABLE_STACK_CHECKS)","0") +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +else +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +endif + +# Pick the right set of flags for the architecture. +# +ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH)) +ifeq ($(ARCHCPUFLAGS),) +$(error Must set CONFIG_ARCH to one of CORTEXA8 CORTEXM4F, CORTEXM4 or CORTEXM3) +endif + +# 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 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ + +# Generic warnings +# +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 \ + -Wno-error=logical-op \ + -Wdouble-promotion \ + -Wlogical-op \ + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=double-promotion \ + -fno-strength-reduce \ + -Wno-error=unused-value + +ARCHOPTIMIZATION += -fno-strength-reduce + +# -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 + +# 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 += ${PX4_BASE}../muorb_krait/lib/libmuorb.so +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)) + +# 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/posix/DISABLE_config_posix_muorb_test.mk b/makefiles/posix/DISABLE_config_posix_muorb_test.mk new file mode 100644 index 0000000000..34fa623d86 --- /dev/null +++ b/makefiles/posix/DISABLE_config_posix_muorb_test.mk @@ -0,0 +1,78 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# 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 + +# +# posix 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 + +# +# muorb fastrpc changes. +# +#MODULES += $(PX4_BASE)../muorb_krait + +# +# Unit tests +# +MODULES += platforms/posix/tests/muorb +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + diff --git a/makefiles/posix/board_posix.mk b/makefiles/posix/board_posix.mk index 93146b6a2e..9982680f92 100644 --- a/makefiles/posix/board_posix.mk +++ b/makefiles/posix/board_posix.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = NATIVE CONFIG_BOARD = POSIXTEST -include $(PX4_MK_DIR)/toolchain_native.mk +include $(PX4_MK_DIR)/posix/toolchain_native.mk diff --git a/makefiles/posix/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index 48de2636e0..30dada7bbd 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -69,6 +69,7 @@ MODULES += platforms/posix/drivers/adcsim MODULES += platforms/posix/drivers/barosim MODULES += platforms/posix/drivers/tonealrmsim MODULES += platforms/posix/drivers/airspeedsim +MODULES += platforms/posix/drivers/gpssim # # Unit tests @@ -78,3 +79,7 @@ MODULES += platforms/posix/drivers/airspeedsim #MODULES += platforms/posix/tests/hrt_test #MODULES += platforms/posix/tests/wqueue +# +# muorb fastrpc changes. +# +#MODULES += $(PX4_BASE)../muorb_krait diff --git a/makefiles/toolchain_native.mk b/makefiles/posix/toolchain_native.mk similarity index 99% rename from makefiles/toolchain_native.mk rename to makefiles/posix/toolchain_native.mk index 0b8223bfd0..fb730cd9fa 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -210,6 +210,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \ # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) #EXTRA_LIBS += $(LIBM) +#EXTRA_LIBS += ${PX4_BASE}../muorb_krait/lib/libmuorb.so EXTRA_LIBS += -pthread -lm -lrt # Flags we pass to the C compiler diff --git a/makefiles/qurt/DISABLE_config_qurt_muorb_test.mk b/makefiles/qurt/DISABLE_config_qurt_muorb_test.mk new file mode 100644 index 0000000000..1afa954f5b --- /dev/null +++ b/makefiles/qurt/DISABLE_config_qurt_muorb_test.mk @@ -0,0 +1,77 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# 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/muorb +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# sources for muorb over fastrpc +# +MODULES += $(PX4_BASE)/../muorb_qurt/ diff --git a/makefiles/qurt/board_qurt.mk b/makefiles/qurt/board_qurt.mk index 6d145f1c33..1038a90b76 100644 --- a/makefiles/qurt/board_qurt.mk +++ b/makefiles/qurt/board_qurt.mk @@ -8,4 +8,4 @@ CONFIG_ARCH = HEXAGON CONFIG_BOARD = QURTTEST -include $(PX4_MK_DIR)/toolchain_hexagon.mk +include $(PX4_MK_DIR)/qurt/toolchain_hexagon.mk diff --git a/makefiles/qurt/config_qurt_default.mk b/makefiles/qurt/config_qurt_default.mk index 8285ef28a8..398fe05194 100644 --- a/makefiles/qurt/config_qurt_default.mk +++ b/makefiles/qurt/config_qurt_default.mk @@ -15,7 +15,7 @@ MODULES += drivers/blinkm MODULES += drivers/hil MODULES += drivers/led MODULES += drivers/rgbled -#MODULES += modules/sensors +MODULES += modules/sensors #MODULES += drivers/ms5611 # @@ -76,3 +76,7 @@ MODULES += platforms/posix/tests/vcdev_test MODULES += platforms/posix/tests/hrt_test MODULES += platforms/posix/tests/wqueue +# +# sources for muorb over fastrpc +# +#MODULES += $(PX4_BASE)/../muorb_qurt/ diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk similarity index 86% rename from makefiles/toolchain_hexagon.mk rename to makefiles/qurt/toolchain_hexagon.mk index 7135158aa8..8f44a84946 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -39,7 +39,6 @@ # 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)) @@ -60,8 +59,10 @@ AR = $(HEXAGON_BIN)/$(CROSSDEV)ar rcs NM = $(HEXAGON_BIN)/$(CROSSDEV)nm OBJCOPY = $(HEXAGON_BIN)/$(CROSSDEV)objcopy OBJDUMP = $(HEXAGON_BIN)/$(CROSSDEV)objdump +HEXAGON_GCC = $(HEXAGON_BIN)/$(CROSSDEV)gcc QURTLIBS = \ + $(QCTOOLSLIB)/libdl.a \ $(TOOLSLIB)/init.o \ $(TOOLSLIB)/libc.a \ $(TOOLSLIB)/libqcc.a \ @@ -77,7 +78,8 @@ QURTLIBS = \ $(QCTOOLSLIB)/libhexagon.a \ $(TOOLSLIB)/fini.o - +DYNAMIC_LIBS = \ + -Wl,$(TOOLSLIB)/pic/libstdc++.a # Check if the right version of the toolchain is available @@ -96,7 +98,7 @@ MAXOPTIMIZATION ?= -O0 # Base CPU flags for each of the supported architectures. # -ARCHCPUFLAGS = -m$(V_ARCH) +ARCHCPUFLAGS = -m$(V_ARCH) -G0 # Set the board flags @@ -117,6 +119,8 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -I$(PX4_BASE)/../dspal/sys \ -I$(PX4_BASE)/mavlink/include/mavlink \ -I$(QURTLIB)/..//include \ + -I$(HEXAGON_SDK_ROOT)/inc \ + -I$(HEXAGON_SDK_ROOT)/inc/stddef \ -Wno-error=shadow # optimisation flags @@ -127,7 +131,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ -fomit-frame-pointer \ -funsafe-math-optimizations \ -ffunction-sections \ - -fdata-sections + -fdata-sections \ + -fpic # enable precise stack overflow tracking # note - requires corresponding support in NuttX @@ -198,10 +203,6 @@ CXXFLAGS = $(ARCHCXXFLAGS) \ $(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__ \ @@ -211,7 +212,16 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script # Flags we pass to the linker # -LDFLAGS += -g -nostdlib --section-start .start=0x1d000000\ +LDFLAGS += -g -mv5 -nostdlib -mG0lib -G0 -fpic -shared \ + -nostartfiles \ + -Wl,-Bsymbolic \ + -Wl,--wrap=malloc \ + -Wl,--wrap=calloc \ + -Wl,--wrap=free \ + -Wl,--wrap=realloc \ + -Wl,--wrap=memalign \ + -Wl,--wrap=__stack_chk_fail \ + -lc \ $(EXTRALDFLAGS) \ $(addprefix -L,$(LIB_DIRS)) @@ -230,21 +240,31 @@ DEP_INCLUDES = $(subst .o,.d,$(OBJS)) # Compile C source $1 to object $2 # as a side-effect, generate a dependency file # -define COMPILE +define COMPILENOSHARED @$(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 +# Compile C source $1 to object $2 for use in shared library +# 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) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 +endef + +# Compile C++ source $1 to $2 for use in shared library # 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 + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 endef # Assemble $1 into $2 @@ -299,8 +319,7 @@ endef 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 + $(HEXAGON_GCC) $(LDFLAGS) -fPIC -shared -nostartfiles -o $1 -Wl,--whole-archive $2 -Wl,--no-whole-archive $(LIBS) $(DYNAMIC_LIBS) endef # Link the objects in $2 into the application $1 @@ -308,7 +327,6 @@ endef 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 + $(LD) --section-start .start=0x1d000000 -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group --dynamic-linker= -E --force-dynamic endef diff --git a/makefiles/qurt_elf.mk b/makefiles/qurt_elf.mk index 53cc8d8287..281a1603d2 100644 --- a/makefiles/qurt_elf.mk +++ b/makefiles/qurt_elf.mk @@ -40,11 +40,13 @@ # # What we're going to build. # + +EXTRALDFLAGS = -Wl,-soname=libdspal_client.so PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a PRODUCT_SHARED_PRELINK = $(WORK_DIR)firmware.o .PHONY: firmware -firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)mainapp +firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)libdspal_client.so $(WORK_DIR)mainapp # # Built product rules @@ -63,7 +65,13 @@ $(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) +$(WORK_DIR)libdspal_client.so: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) + $(call LINK_SO,$@, $^) + +$(WORK_DIR)dspal_stub.o: $(PX4_BASE)/src/platforms/qurt/dspal/dspal_stub.c + $(call COMPILENOSHARED,$^, $@) + +$(WORK_DIR)mainapp: $(WORK_DIR)dspal_stub.o $(call LINK,$@, $^) # diff --git a/posix-configs/posixtest/init/rc.S b/posix-configs/posixtest/init/rc.S index 896062639e..9a15db2f01 100644 --- a/posix-configs/posixtest/init/rc.S +++ b/posix-configs/posixtest/init/rc.S @@ -1,9 +1,11 @@ uorb start simulator start -s +sleep 2 barosim start adcsim start accelsim start gyrosim start +tone_alarm start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1310720 param set CAL_ACC1_ID 1376256 diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index 771bee2471..7d83842266 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -85,10 +85,11 @@ Device::log(const char *fmt, ...) PX4_INFO("[%s] ", _name); va_start(ap, fmt); - vprintf(fmt, ap); + PX4_INFO( fmt, ap ); + //vprintf(fmt, ap); va_end(ap); - printf("\n"); - fflush(stdout); + //printf("\n"); + //fflush(stdout); } void @@ -97,12 +98,14 @@ Device::debug(const char *fmt, ...) va_list ap; if (_debug_enabled) { - printf("<%s> ", _name); + PX4_INFO("<%s> ", _name); + //printf("<%s> ", _name); va_start(ap, fmt); - vprintf(fmt, ap); + //vprintf(fmt, ap); + PX4_INFO(fmt, ap); va_end(ap); - printf("\n"); - fflush(stdout); + //printf("\n"); + //fflush(stdout); } } diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index 473307b1cc..aa031d9e42 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -69,7 +69,7 @@ I2C::I2C(const char *name, _address(address), _fd(-1) { - warnx("I2C::I2C name = %s devname = %s", name, devname); + //warnx("I2C::I2C name = %s devname = %s", name, devname); // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; _device_id.devid_s.bus = bus; @@ -81,7 +81,9 @@ I2C::I2C(const char *name, I2C::~I2C() { if (_fd >= 0) { +#ifndef __PX4_QURT ::close(_fd); +#endif _fd = -1; } } @@ -116,6 +118,7 @@ I2C::init() _fd = 10000; } else { +#ifndef __PX4_QURT // Open the actual I2C device and map to the virtual dev name _fd = ::open(get_devname(), O_RDWR); if (_fd < 0) { @@ -123,6 +126,7 @@ I2C::init() px4_errno = errno; return PX4_ERROR; } +#endif } return ret; @@ -246,8 +250,11 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) warnx ("2C SIM I2C::read"); return 0; } - +#ifndef __PX4_QURT return ::read(_fd, buffer, buflen); +#else + return 0; +#endif } ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) @@ -257,7 +264,11 @@ ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) return buflen; } +#ifndef __PX4_QURT return ::write(_fd, buffer, buflen); +#else + return buflen; +#endif } } // namespace device diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 8618626ba8..89a3da3e0a 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -64,7 +64,7 @@ private: px4_dev_t() {} }; -#define PX4_MAX_DEV 50 +#define PX4_MAX_DEV 100 static px4_dev_t *devmap[PX4_MAX_DEV]; /* diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 11f3da9b49..1deb131bc3 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -108,6 +108,7 @@ public: int set_mode(Mode mode); int set_pwm_rate(unsigned rate); + int _task; private: static const unsigned _max_actuators = 4; @@ -115,7 +116,6 @@ private: Mode _mode; int _update_rate; int _current_update_rate; - int _task; int _t_actuators; int _t_armed; orb_advert_t _t_outputs; @@ -165,15 +165,15 @@ HIL *g_hil; HIL::HIL() : #ifdef __PX4_NUTTX - CDev( + CDev #else - VDev( + VDev #endif - "hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), + ("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/), + _task(-1), _mode(MODE_NONE), _update_rate(50), _current_update_rate(0), - _task(-1), _t_actuators(-1), _t_armed(-1), _t_outputs(0), @@ -228,8 +228,9 @@ HIL::init() ret = VDev::init(); #endif - if (ret != OK) + if (ret != OK) { return ret; + } // XXX already claimed with CDEV ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ @@ -346,9 +347,9 @@ HIL::task_main() /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs, insist on the first group output */ - _t_outputs = orb_advertise(ORB_ID(actuator_outputs), - &outputs); + _t_outputs = orb_advertise(ORB_ID(actuator_outputs), &outputs); px4_pollfd_struct_t fds[2]; fds[0].fd = _t_actuators; @@ -407,21 +408,18 @@ HIL::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { - /* get controls - must always do this to avoid spinning */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { - /* do mixing */ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && PX4_ISFINITE(outputs.output[i]) && @@ -467,9 +465,9 @@ HIL::task_main() int HIL::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -674,7 +672,7 @@ enum PortMode { PORT2_16PWM, }; -PortMode g_port_mode; +static PortMode g_port_mode = PORT_MODE_UNDEFINED; int hil_new_mode(PortMode new_mode) @@ -739,31 +737,6 @@ hil_new_mode(PortMode new_mode) return OK; } -int -hil_start(void) -{ - int ret = OK; - - if (g_hil == nullptr) { - - g_hil = new HIL; - - if (g_hil == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_hil->init(); - - if (ret != OK) { - delete g_hil; - g_hil = nullptr; - } - } - } - - return ret; -} - int test(void) { @@ -829,17 +802,19 @@ hil_main(int argc, char *argv[]) const char *verb; int ret = OK; - if (hil_start() != OK) { - warnx("failed to start the HIL driver"); - return 1; - } - if (argc < 2) { usage(); return -EINVAL; } verb = argv[1]; + if (g_hil == nullptr) { + g_hil = new HIL; + if (g_hil == nullptr) { + return -ENOMEM; + } + } + /* * Mode switches. */ @@ -872,10 +847,9 @@ hil_main(int argc, char *argv[]) return OK; /* switch modes */ - return hil_new_mode(new_mode); + ret = hil_new_mode(new_mode); } - - if (!strcmp(verb, "test")) { + else if (!strcmp(verb, "test")) { ret = test(); } @@ -887,5 +861,14 @@ hil_main(int argc, char *argv[]) usage(); ret = -EINVAL; } + + if ( ret == OK && g_hil->_task == -1 ) { + ret = g_hil->init(); + if (ret != OK) { + warnx("failed to start the HIL driver"); + delete g_hil; + g_hil = nullptr; + } + } return ret; } diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b9bf402afa..e48b0b5c89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -132,7 +132,7 @@ static sem_t g_sys_state_mutex; /* The data manager store file handle and file name */ static int g_fd = -1, g_task_fd = -1; -static const char *default_device_path = "/fs/microsd/dataman"; +static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman"; static char *k_data_manager_device_path = NULL; /* The data manager work queues */ diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 36d2f2ff6a..44127f586e 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -571,7 +571,7 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workTruncateFile(PayloadHeader* payload) { char file[kMaxDataLength]; - const char temp_file[] = "/fs/microsd/.trunc.tmp"; + const char temp_file[] = PX4_ROOTFSDIR"/fs/microsd/.trunc.tmp"; strncpy(file, _data_as_cstring(payload), kMaxDataLength); payload->size = 0; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a28c3bd0b3..1a117fb8a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -163,6 +163,7 @@ Mavlink::Mavlink() : mavlink_link_termination_allowed(false), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), + _udp_initialised(false), _flow_control_enabled(true), _last_write_success_time(0), _last_write_try_time(0), @@ -173,6 +174,9 @@ Mavlink::Mavlink() : _rate_tx(0.0f), _rate_txerr(0.0f), _rate_rx(0.0f), + _socket_fd(-1), + _protocol(SERIAL), + _network_port(14556), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -325,6 +329,20 @@ Mavlink::get_instance_for_device(const char *device_name) return nullptr; } +Mavlink * +Mavlink::get_instance_for_network_port(unsigned long port) +{ + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + if (inst->_network_port == port) { + return inst; + } + } + + return nullptr; +} + int Mavlink::destroy_all_instances() { @@ -672,6 +690,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * _is_usb_uart = true; } +#ifdef __PX4_LINUX + /* Put in raw mode */ + cfmakeraw(&uart_config); +#endif + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERR SET CONF %s\n", uart_name); ::close(_uart_fd); @@ -801,20 +824,21 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID 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; + if (get_protocol() == SERIAL) { + /* check if there is space in the buffer, let it overflow else */ + unsigned buf_free = get_free_tx_buf(); + 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]; @@ -840,11 +864,21 @@ 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); + size_t ret = -1; #ifndef __PX4_POSIX /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); + if (get_protocol() == SERIAL) { + ret = ::write(_uart_fd, buf, packet_len); + } +#else + if (get_protocol() == UDP) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + } else if (get_protocol() == TCP) { + // not implemented, but possible to do so + } +#endif - if (ret != (int) packet_len) { + if (ret != (size_t) packet_len) { count_txerr(); count_txerrbytes(packet_len); @@ -853,8 +887,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID count_txbytes(packet_len); } -#endif - pthread_mutex_unlock(&_send_mutex); } @@ -911,6 +943,35 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_unlock(&_send_mutex); } +void +Mavlink::init_udp() +{ +#ifdef __PX4_LINUX + PX4_INFO("Setting up UDP w/port %d\n",_network_port); + + memset((char *)&_myaddr, 0, sizeof(_myaddr)); + _myaddr.sin_family = AF_INET; + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + _myaddr.sin_port = htons(_network_port); + + if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + PX4_WARN("create socket failed\n"); + return; + } + + if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + PX4_WARN("bind failed\n"); + return; + } + + unsigned char inbuf[256]; + socklen_t addrlen = sizeof(_src_addr); + + // wait for client to connect to socket + recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen); +#endif +} + void Mavlink::handle_message(const mavlink_message_t *msg) { @@ -1305,8 +1366,10 @@ Mavlink::task_main(int argc, char *argv[]) bool err_flag = false; int myoptind = 1; const char *myoptarg = NULL; + char* eptr; + int temp_int_arg; - while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:u:m:fpvwx", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(myoptarg, NULL, 10); @@ -1330,6 +1393,18 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; + set_protocol(SERIAL); + break; + + case 'u': + temp_int_arg = strtoul(myoptarg, &eptr, 10); + if ( *eptr == '\0' ) { + _network_port = temp_int_arg; + set_protocol(UDP); + } else { + warnx("invalid data udp_port '%s'", myoptarg); + err_flag = true; + } break; // case 'e': @@ -1395,11 +1470,19 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + if (get_protocol() == SERIAL) { warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); - + } else if (get_protocol() == UDP) { + warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); + } /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); + /* init socket if necessary */ + if (get_protocol() == UDP) { + init_udp(); + } + #ifndef __PX4_POSIX struct termios uart_config_original; @@ -1537,7 +1620,10 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); - send_autopilot_capabilites(); + /* if the protocol is serial, we send the system version blindly */ + if (get_protocol() != SERIAL) { + send_autopilot_capabilites(); + } while (!_task_should_exit) { /* main loop */ @@ -1824,6 +1910,11 @@ Mavlink::stream_command(int argc, char *argv[]) const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; + unsigned short network_port = 0; + char* eptr; + int temp_int_arg; + bool provided_device = false; + bool provided_network_port = false; /* * Called via main with original argv @@ -1852,13 +1943,22 @@ Mavlink::stream_command(int argc, char *argv[]) i++; } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + provided_device = true; device_name = argv[i + 1]; i++; } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { stream_name = argv[i + 1]; i++; - + } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) { + provided_network_port = true; + temp_int_arg = strtoul(argv[i + 1], &eptr, 10); + if ( *eptr == '\0' ) { + network_port = temp_int_arg; + } else { + err_flag = true; + } + i++; } else { err_flag = true; } @@ -1867,7 +1967,16 @@ Mavlink::stream_command(int argc, char *argv[]) } if (!err_flag && rate >= 0.0f && stream_name != nullptr) { - Mavlink *inst = get_instance_for_device(device_name); + + Mavlink *inst = nullptr; + if (provided_device && !provided_network_port) { + inst = get_instance_for_device(device_name); + } else if (provided_network_port && !provided_device) { + inst = get_instance_for_network_port(network_port); + } else if (provided_device && provided_network_port) { + warnx("please provide either a device name or a network port"); + return 1; + } if (inst != nullptr) { inst->configure_stream_threadsafe(stream_name, rate); @@ -1876,12 +1985,17 @@ 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. - warnx("mavlink for device %s is not running", device_name); - return 0; + if (provided_device) { + warnx("mavlink for device %s is not running", device_name); + } else { + warnx("mavlink for network on port %hu is not running", network_port); + } + + return 1; } } else { - warnx("usage: mavlink stream [-d device] -s stream -r rate"); + warnx("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate"); return 1; } @@ -1890,7 +2004,7 @@ Mavlink::stream_command(int argc, char *argv[]) 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]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-u udp_port] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a658ca20ca..326182bbef 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -46,6 +46,8 @@ #include #else #include +#include +#include #endif #include #include @@ -65,6 +67,12 @@ #include "mavlink_parameters.h" #include "mavlink_ftp.h" +enum Protocol { + SERIAL = 0, + UDP, + TCP, +}; + #ifdef __PX4_NUTTX class Mavlink #else @@ -103,7 +111,9 @@ public: static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); + + static Mavlink *get_instance_for_network_port(unsigned long port); static int destroy_all_instances(); @@ -190,6 +200,11 @@ public: */ void set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; } + /** + * Set communication protocol for this mavlink instance + */ + void set_protocol(Protocol p) {_protocol = p;}; + /** * Get the manual input generation mode * @@ -305,6 +320,10 @@ public: unsigned get_system_type() { return _system_type; } + Protocol get_protocol() { return _protocol; }; + + unsigned short get_network_port() { return _network_port; } + protected: Mavlink *next; @@ -320,8 +339,8 @@ private: bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -364,6 +383,7 @@ private: char *_subscribe_to_stream; float _subscribe_to_stream_rate; + bool _udp_initialised; bool _flow_control_enabled; uint64_t _last_write_success_time; @@ -377,6 +397,15 @@ private: float _rate_txerr; float _rate_rx; +#ifdef __PX4_POSIX + struct sockaddr_in _myaddr; + struct sockaddr_in _src_addr; + +#endif + int _socket_fd; + Protocol _protocol; + unsigned short _network_port; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { @@ -439,6 +468,8 @@ private: */ void update_rate_mult(); + void init_udp(); + #ifdef __PX4_NUTTX static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); #else diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1b2689e6bb..ffdc0e0730 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -389,7 +389,7 @@ protected: } else if (write_err_count < write_err_threshold) { /* string to hold the path to the log */ char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_path[70] = ""; timespec ts; px4_clock_gettime(CLOCK_REALTIME, &ts); @@ -400,7 +400,7 @@ protected: // XXX we do not want to interfere here with the SD log app strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); - snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + snprintf(log_file_path, sizeof(log_file_path), PX4_ROOTFSDIR"/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); /* write first message */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 96764cc976..c9518872f1 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -49,8 +49,9 @@ #include #include #include +#include -#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" +#define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt" class Geofence : public control::SuperBlock { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c104f8162c..28bd482cc2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -257,7 +257,7 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: @@ -346,7 +346,7 @@ Navigator::task_main() if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { /* try to reopen the mavlink log device with specified interval */ mavlink_open_time = hrt_abstime() + mavlink_open_interval; - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); } static bool have_geofence_position_data = false; 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 8654a7cb11..53d6323e81 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -189,7 +189,7 @@ int position_estimator_inav_main(int argc, char *argv[]) static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { - FILE *f = fopen("/fs/microsd/inav.log", "a"); + FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index acc4771968..f35963d885 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -179,7 +179,7 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; static bool _gpstime_only = false; -#define MOUNTPOINT "/fs/microsd" +#define MOUNTPOINT PX4_ROOTFSDIR"/fs/microsd" static const char *mountpoint = MOUNTPOINT; static const char *log_root = MOUNTPOINT "/log"; static int mavlink_fd = -1; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bc1ae89906..0be31046b1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -868,7 +868,7 @@ Sensors::parameters_update() barofd = px4_open(BARO0_DEVICE_PATH, 0); if (barofd < 0) { - warnx("ERROR: no barometer foundon %s", BARO0_DEVICE_PATH); + warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); return ERROR; } else { diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 83e3fd8d76..0c02da247f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -71,11 +71,41 @@ bool Simulator::getRawAccelReport(uint8_t *buf, int len) return _accel.copyData(buf, len); } +bool Simulator::getMagReport(uint8_t *buf, int len) +{ + return _mag.copyData(buf, len); +} + bool Simulator::getBaroSample(uint8_t *buf, int len) { return _baro.copyData(buf, len); } +bool Simulator::getGPSSample(uint8_t *buf, int len) +{ + return _gps.copyData(buf, len); +} + +void Simulator::write_MPU_data(void *buf) { + _mpu.writeData(buf); +} + +void Simulator::write_accel_data(void *buf) { + _accel.writeData(buf); +} + +void Simulator::write_mag_data(void *buf) { + _mag.writeData(buf); +} + +void Simulator::write_baro_data(void *buf) { + _baro.writeData(buf); +} + +void Simulator::write_gps_data(void *buf) { + _gps.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index a95fa15731..b2ebc880cd 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -61,31 +62,63 @@ 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; + float temperature; + float x; + float y; + float z; +}; +#pragma pack(pop) + +#pragma pack(push, 1) +struct RawMagData { + float temperature; + float x; + float y; + float 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]; + float accel_x; + float accel_y; + float accel_z; + float temp; + float gyro_x; + float gyro_y; + float gyro_z; }; #pragma pack(pop) +#pragma pack(push, 1) struct RawBaroData { - uint8_t d[3]; + float pressure; + float altitude; + float temperature; }; +#pragma pack(pop) + +#pragma pack(push, 1) +struct RawGPSData { + int32_t lat; + int32_t lon; + int32_t alt; + uint16_t eph; + uint16_t epv; + uint16_t vel; + int16_t vn; + int16_t ve; + int16_t vd; + uint16_t cog; + uint8_t fix_type; + uint8_t satellites_visible; +}; +#pragma pack(pop) template class Report { public: Report(int readers) : + _readidx(0), _max_readers(readers), _report_len(sizeof(RType)) { @@ -158,23 +191,36 @@ public: static int start(int argc, char *argv[]); bool getRawAccelReport(uint8_t *buf, int len); + bool getMagReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); + bool getGPSSample(uint8_t *buf, int len); + + void write_MPU_data(void *buf); + void write_accel_data(void *buf); + void write_mag_data(void *buf); + void write_baro_data(void *buf); + void write_gps_data(void *buf); + private: Simulator() : _accel(1), _mpu(1), _baro(1), + _mag(1), + _gps(1), _sensor_combined_pub(nullptr) #ifndef __PX4_QURT , - _manual_control_sp_pub(nullptr), + _rc_channels_pub(nullptr), _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), + _manual_sub(-1), _sensor{}, - _manual_control_sp{}, + _rc_input{}, _actuators{}, - _attitude{} + _attitude{}, + _manual{} #endif {} ~Simulator() { _instance=NULL; } @@ -189,6 +235,8 @@ private: simulator::Report _accel; simulator::Report _mpu; simulator::Report _baro; + simulator::Report _mag; + simulator::Report _gps; // uORB publisher handlers orb_advert_t _accel_pub; @@ -202,29 +250,32 @@ private: #ifndef __PX4_QURT // uORB publisher handlers - orb_advert_t _manual_control_sp_pub; + orb_advert_t _rc_channels_pub; // uORB subscription handlers int _actuator_outputs_sub; int _vehicle_attitude_sub; + int _manual_sub; // uORB data containers struct sensor_combined_s _sensor; - struct manual_control_setpoint_s _manual_control_sp; + struct rc_input_values _rc_input; struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; + struct manual_control_setpoint_s _manual; int _fd; unsigned char _buf[200]; - hrt_abstime _time_last; struct sockaddr_in _srcaddr; socklen_t _addrlen = sizeof(_srcaddr); - void poll_topics(); + void poll_actuators(); void handle_message(mavlink_message_t *msg); - void send_data(); + void send_controls(); 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); + void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); + void update_gps(mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); void send(); #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index de24fb8626..e4403fd221 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - +#include #include #include #include "simulator.h" @@ -43,9 +43,15 @@ using namespace simulator; #define UDP_PORT 14550 #define PIXHAWK_DEVICE "/dev/ttyACM0" +#define PRESS_GROUND 101325.0f +#define DENSITY 1.2041f +#define GRAVITY 9.81f + static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +static int openUart(const char *uart_name, int baud); + void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8]; @@ -84,69 +90,91 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { 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 - } +void Simulator::send_controls() { + mavlink_hil_controls_t msg; + pack_actuator_message(msg); + send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); } -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; +static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) { + rc->timestamp_publication = hrt_absolute_time(); + rc->timestamp_last_signal = hrt_absolute_time(); + rc->channel_count = rc_channels->chancount; + rc->rssi = rc_channels->rssi; + + rc->values[0] = rc_channels->chan1_raw; + rc->values[1] = rc_channels->chan2_raw; + rc->values[2] = rc_channels->chan3_raw; + rc->values[3] = rc_channels->chan4_raw; + rc->values[4] = rc_channels->chan5_raw; + rc->values[5] = rc_channels->chan6_raw; + rc->values[6] = rc_channels->chan7_raw; + rc->values[7] = rc_channels->chan8_raw; + rc->values[8] = rc_channels->chan9_raw; + rc->values[9] = rc_channels->chan10_raw; + rc->values[10] = rc_channels->chan11_raw; + rc->values[11] = rc_channels->chan12_raw; + rc->values[12] = rc_channels->chan13_raw; + rc->values[13] = rc_channels->chan14_raw; + rc->values[14] = rc_channels->chan15_raw; + rc->values[15] = rc_channels->chan16_raw; + rc->values[16] = rc_channels->chan17_raw; + rc->values[17] = rc_channels->chan18_raw; } -static 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; +void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) { + // write sensor data to memory so that drivers can copy data from there + RawMPUData mpu; + mpu.accel_x = imu->xacc; + mpu.accel_y = imu->yacc; + mpu.accel_z = imu->zacc; + mpu.temp = imu->temperature; + mpu.gyro_x = imu->xgyro; + mpu.gyro_y = imu->ygyro; + mpu.gyro_z = 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; + write_MPU_data((void *)&mpu); - sensor->adc_voltage_v[0] = 0.0f; - sensor->adc_voltage_v[1] = 0.0f; - sensor->adc_voltage_v[2] = 0.0f; + RawAccelData accel; + accel.x = imu->xacc; + accel.y = imu->yacc; + accel.z = imu->zacc; - 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; + write_accel_data((void *)&accel); - sensor->baro_pres_mbar = imu->abs_pressure; - sensor->baro_alt_meter = imu->pressure_alt; - sensor->baro_temp_celcius = imu->temperature; - sensor->baro_timestamp = timestamp; + RawMagData mag; + mag.x = imu->xmag; + mag.y = imu->ymag; + mag.z = imu->zmag; + + write_mag_data((void *)&mag); + + RawBaroData baro; + // calculate air pressure from altitude (valid for low altitude) + baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt; + baro.altitude = imu->pressure_alt; + baro.temperature = imu->temperature; + + write_baro_data((void *)&baro); +} + +void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { + RawGPSData gps; + gps.lat = gps_sim->lat; + gps.lon = gps_sim->lon; + gps.alt = gps_sim->alt; + gps.eph = gps_sim->eph; + gps.epv = gps_sim->epv; + gps.vel = gps_sim->vel; + gps.vn = gps_sim->vn; + gps.ve = gps_sim->ve; + gps.vd = gps_sim->vd; + gps.cog = gps_sim->cog; + gps.fix_type = gps_sim->fix_type; + gps.satellites_visible = gps_sim->satellites_visible; + + write_gps_data((void *)&gps); - 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) { @@ -154,27 +182,26 @@ void Simulator::handle_message(mavlink_message_t *msg) { 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 == nullptr) { - _sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor); - } else { - orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor); - } + update_sensors(&_sensor, &imu); break; - case MAVLINK_MSG_ID_MANUAL_CONTROL: + case MAVLINK_MSG_ID_HIL_GPS: + mavlink_hil_gps_t gps_sim; + mavlink_msg_hil_gps_decode(msg, &gps_sim); + update_gps(&gps_sim); + break; - 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); + case MAVLINK_MSG_ID_RC_CHANNELS: + + mavlink_rc_channels_t rc_channels; + mavlink_msg_rc_channels_decode(msg, &rc_channels); + fill_rc_input_msg(&_rc_input, &rc_channels); // publish message - if(_manual_control_sp_pub == nullptr) { - _manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp); + if(_rc_channels_pub == nullptr) { + _rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input); } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp); + orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input); } break; } @@ -214,18 +241,13 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 } } -void Simulator::poll_topics() { - // copy new data if available +void Simulator::poll_actuators() { + // copy new actuator 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 *) { @@ -238,11 +260,11 @@ void Simulator::send() { fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; - _time_last = hrt_absolute_time(); + int pret; while(true) { // wait for up to 100ms for data - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); //timed out if (pret == 0) @@ -258,8 +280,8 @@ void Simulator::send() { if (fds[0].revents & POLLIN) { // got new data to read, update all topics - poll_topics(); - send_data(); + poll_actuators(); + send_controls(); } } } @@ -288,16 +310,6 @@ void Simulator::updateSamples() 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; @@ -328,11 +340,9 @@ void Simulator::updateSamples() /* 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); + int serial_fd = openUart(PIXHAWK_DEVICE, 115200); if (serial_fd < 0) { PX4_WARN("failed to open %s", PIXHAWK_DEVICE); @@ -355,10 +365,30 @@ void Simulator::updateSamples() fds[1].events = POLLIN; int len = 0; + + // wait for first data from simulator and respond with first controls + // this is important for the UDP communication to work + int pret = -1; + while (pret <= 0) { + pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100); + } + + if (fds[0].revents & POLLIN) { + len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + send_controls(); + } + + // subscribe to topics + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + + // got data from simulator, now activate the sending thread + pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); + pthread_attr_destroy(&sender_thread_attr); + // wait for new mavlink messages to arrive while (true) { - int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100); + pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100); //timed out if (pret == 0) @@ -405,17 +435,107 @@ void Simulator::updateSamples() } } } - - // 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); } } + +int openUart(const char *uart_name, int baud) +{ + /* 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 */ + int 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; + memset(&uart_config, 0, sizeof(uart_config)); + + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 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); + + /* 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; + } + + } + + // Make raw + cfmakeraw(&uart_config); + + if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR SET CONF %s\n", uart_name); + ::close(uart_fd); + return -1; + } + + return uart_fd; +} diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 5f2dc2df27..1bf986753d 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -695,7 +695,7 @@ param_reset_excludes(const char *excludes[], int num_excludes) param_notify_changes(); } -static const char *param_default_file = "/eeprom/parameters"; +static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters"; static char *param_user_file = NULL; int diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp new file mode 100644 index 0000000000..99247d951a --- /dev/null +++ b/src/modules/uORB/ORBMap.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + + +namespace uORB +{ + class DeviceNode; + class ORBMap; +} + +class uORB::ORBMap +{ +public: + struct Node { + struct Node *next; + const char * node_name; + uORB::DeviceNode *node; + }; + + ORBMap() : + _top(nullptr), + _end(nullptr) + { } + ~ORBMap() { + while (_top != nullptr) { + unlinkNext(_top); + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + _end = nullptr; + } + } + } + void insert(const char *node_name, uORB::DeviceNode*node) + { + Node **p; + if (_top == nullptr) + p = &_top; + else + p = &_end->next; + + *p = (Node *)malloc(sizeof(Node)); + if (_end) + _end = _end->next; + else { + _end = _top; + } + _end->next = nullptr; + _end->node_name = strdup(node_name); + _end->node = node; + } + + bool find(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + p = p->next; + } + return false; + } + + uORB::DeviceNode* get(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return p->node; + } + } + return nullptr; + } + + void unlinkNext(Node *a) + { + Node *b = a->next; + if (b != nullptr) { + if (_end == b) { + _end = a; + } + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + +private: + Node *_top; + Node *_end; +}; + diff --git a/src/modules/uORB/ORBSet.hpp b/src/modules/uORB/ORBSet.hpp new file mode 100644 index 0000000000..8b1e0d00fb --- /dev/null +++ b/src/modules/uORB/ORBSet.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +class ORBSet +{ +public: + struct Node { + struct Node *next; + const char * node_name; + }; + + ORBSet() : + _top(nullptr), + _end(nullptr) + { } + ~ORBSet() { + while (_top != nullptr) { + unlinkNext(_top); + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + } + } + } + void insert(const char *node_name) + { + Node **p; + if (_top == nullptr) + p = &_top; + else + p = &_end->next; + + *p = (Node *)malloc(sizeof(Node)); + if (_end) + _end = _end->next; + else { + _end = _top; + } + _end->next = nullptr; + _end->node_name = strdup(node_name); + } + + bool find(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + p = p->next; + } + return false; + } + + bool erase(const char *node_name) + { + Node *p = _top; + if (_top && (strcmp(_top->node_name, node_name) == 0)) { + p = _top->next; + free((void *)_top->node_name); + free(_top); + _top = p; + if (_top == nullptr) { + _end = nullptr; + } + return true; + } + while (p->next) { + if (strcmp(p->next->node_name, node_name) == 0) { + unlinkNext(p); + return true; + } + } + return nullptr; + } + +private: + + void unlinkNext(Node *a) + { + Node *b = a->next; + if (b != nullptr) { + if (_end == b) { + _end = a; + } + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + + Node *_top; + Node *_end; +}; + diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 31a2b24e4a..30a997b317 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -46,9 +46,16 @@ SRCS = uORBDevices_nuttx.cpp \ else SRCS = uORBDevices_posix.cpp \ - uORBTest_UnitTest.cpp \ uORBManager_posix.cpp endif + +ifeq ($(PX4_TARGET_OS),posix) +SRCS += uORBTest_UnitTest.cpp +endif +ifeq ($(PX4_TARGET_OS),posix-arm) +SRCS += uORBTest_UnitTest.cpp +endif + SRCS += objects_common.cpp \ Publication.cpp \ Subscription.cpp \ diff --git a/src/modules/uORB/uORBCommunicator.hpp b/src/modules/uORB/uORBCommunicator.hpp new file mode 100644 index 0000000000..e7db2e464f --- /dev/null +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -0,0 +1,166 @@ +/**************************************************************************** + * + * 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 _uORBCommunicator_hpp_ +#define _uORBCommunicator_hpp_ + +#include + +namespace uORBCommunicator +{ + class IChannel; + class IChannelRxHandler; +} + +/** + * Interface to enable remote subscriptions. The implementor of this interface + * shall manage the communication channel. It can be fastRPC or tcp or ip. + */ + +class uORBCommunicator::IChannel +{ +public: + + //========================================================================= + // INTERFACES FOR Control messages over a channel. + //========================================================================= + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription( const char *messageName ) = 0; + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ) = 0; + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message( const char *messageName, int32_t length, uint8_t* data) = 0; +}; + +/** + * Class passed to the communication link implement to provide callback for received + * messages over a channel. + */ +class uORBCommunicator::IChannelRxHandler +{ +public: + + /** + * Interface to process a received AddSubscription from remote. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; + + /** + * Interface to process a received control msg to remove subscription + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_remove_subscription( const char *messageName ) = 0; + + /** + * Interface to process the received data message. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_received_message( const char *messageName, int32_t length, uint8_t* data ) = 0; +}; + +#endif /* _uORBCommunicator_hpp_ */ diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index d3c30f0508..bebb35f989 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -36,10 +36,15 @@ #include #include #include +#include #include "uORBDevices_nuttx.hpp" #include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBCommunicator.hpp" #include +uORB::ORBMap uORB::DeviceMaster::_node_map; + uORB::DeviceNode::DeviceNode ( const struct orb_metadata *meta, @@ -53,7 +58,9 @@ uORB::DeviceNode::DeviceNode _last_update(0), _generation(0), _publisher(0), - _priority(priority) + _priority(priority), + _IsRemoteSubscriberPresent( false ), + _subscriber_count(0) { // enable debug() calls _debug_enabled = true; @@ -120,6 +127,8 @@ uORB::DeviceNode::open(struct file *filp) ret = CDev::open(filp); + add_internal_subscriber(); + if (ret != OK) delete sd; @@ -142,7 +151,9 @@ uORB::DeviceNode::close(struct file *filp) if (sd != nullptr) { hrt_cancel(&sd->update_call); + remove_internal_subscriber(); delete sd; + sd = nullptr; } } @@ -295,7 +306,19 @@ uORB::DeviceNode::publish errno = EIO; return ERROR; } - + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr ) + { + if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 ) + { + warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name ); + return ERROR; + } + } return OK; } @@ -421,6 +444,79 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg) node->update_deferred(); } +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::add_internal_subscriber() +{ + _subscriber_count++; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count > 0 ) + { + ch->add_subscription( _meta->o_name, 1 ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::remove_internal_subscriber() +{ + _subscriber_count--; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count == 0 ) + { + ch->remove_subscription( _meta->o_name ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz ) +{ + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher. + { + ch->send_message( _meta->o_name, _meta->o_size, _data); + } + + return OK; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_remove_subscription() +{ + return OK; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data) +{ + int16_t ret = -1; + if( length != (int32_t)(_meta->o_size) ) + { + warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size ); + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = 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; +} + uORB::DeviceMaster::DeviceMaster(Flavor f) : CDev((f == PUBSUB) ? "obj_master" : "param_master", (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), @@ -509,6 +605,11 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) free((void *)objname); free((void *)devpath); } + else + { + // add to the node map;. + _node_map.insert(nodepath, node); + } group_tries++; @@ -529,3 +630,14 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) return CDev::ioctl(filp, cmd, arg); } } + +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) +{ + uORB::DeviceNode* rc = nullptr; + if( _node_map.find( nodepath ) ) + { + rc = _node_map.get(nodepath); + } + return rc; +} + diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index 3d8c0f90ad..012e7893dd 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -35,6 +35,9 @@ #define _uORBDevices_nuttx_hpp_ #include +#include +#include +#include "ORBMap.hpp" #include "uORBCommon.hpp" @@ -119,6 +122,43 @@ public: const void *data ); + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription( int32_t rateInHz ); + + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); + + /** + * processed the received data message from remote. + */ + int16_t process_received_message( int32_t length, uint8_t* data ); + + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); + + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); + protected: virtual pollevent_t poll_state(struct file *filp); virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); @@ -147,6 +187,9 @@ private: // private class methods. return sd; } + bool _IsRemoteSubscriberPresent; + int32_t _subscriber_count; + /** * Perform a deferred update for a rate-limited subscriber. */ @@ -166,6 +209,10 @@ private: // private class methods. * @return True if the topic should appear updated to the subscriber */ bool appears_updated(SubscriberData *sd); + + // disable copy and assignment operators + DeviceNode( const DeviceNode& ); + DeviceNode& operator=( const DeviceNode& ); }; /** @@ -180,9 +227,11 @@ public: DeviceMaster(Flavor f); virtual ~DeviceMaster(); + static uORB::DeviceNode* GetDeviceNode( const char * node_name ); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: Flavor _flavor; + static ORBMap _node_map; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 9035a32bdc..95a2374611 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -35,11 +35,16 @@ #include #include #include +#include #include "uORBDevices_posix.hpp" #include "uORBUtils.hpp" +#include "uORBManager.hpp" +#include "uORBCommunicator.hpp" #include +std::map uORB::DeviceMaster::_node_map; + uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp) { @@ -60,7 +65,8 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, _last_update(0), _generation(0), _publisher(0), - _priority(priority) + _priority(priority), + _subscriber_count(0) { // enable debug() calls //_debug_enabled = true; @@ -85,7 +91,7 @@ uORB::DeviceNode::open(device::file_t *filp) lock(); if (_publisher == 0) { - _publisher = getpid(); + _publisher = px4_getpid(); ret = PX4_OK; } else { @@ -127,6 +133,8 @@ uORB::DeviceNode::open(device::file_t *filp) ret = VDev::open(filp); + add_internal_subscriber(); + if (ret != PX4_OK) { warnx("ERROR: VDev::open failed\n"); delete sd; @@ -145,7 +153,7 @@ uORB::DeviceNode::close(device::file_t *filp) { //warnx("uORB::DeviceNode::close fd = %d", filp->fd); /* is this the publisher closing? */ - if (getpid() == _publisher) { + if (px4_getpid() == _publisher) { _publisher = 0; } else { @@ -153,7 +161,9 @@ uORB::DeviceNode::close(device::file_t *filp) if (sd != nullptr) { hrt_cancel(&sd->update_call); + remove_internal_subscriber(); delete sd; + sd = nullptr; } } @@ -280,7 +290,7 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) } ssize_t -uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data ) { //warnx("uORB::DeviceNode::publish meta = %p", meta); @@ -310,6 +320,19 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v return ERROR; } + /* + * if the write is successful, send the data over the Multi-ORB link + */ + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr ) + { + if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 ) + { + warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", + __LINE__, meta->o_name ); + return ERROR; + } + } return PX4_OK; } @@ -437,6 +460,80 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg) node->update_deferred(); } +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::add_internal_subscriber() +{ + _subscriber_count++; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count > 0 ) + { + ch->add_subscription( _meta->o_name, 1 ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::DeviceNode::remove_internal_subscriber() +{ + _subscriber_count--; + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( ch != nullptr && _subscriber_count == 0 ) + { + ch->remove_subscription( _meta->o_name ); + } +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz ) +{ + // if there is already data in the node, send this out to + // the remote entity. + // send the data to the remote entity. + uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator(); + if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher. + { + ch->send_message( _meta->o_name, _meta->o_size, _data); + } + + return 0; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_remove_subscription() +{ + return 0; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data) +{ + int16_t ret = -1; + if( length != (int32_t)(_meta->o_size) ) + { + warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", + __LINE__, _meta->o_name, (int)length, (int)_meta->o_size ); + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = 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; +} + + uORB::DeviceMaster::DeviceMaster(Flavor f) : VDev((f == PUBSUB) ? "obj_master" : "param_master", (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), @@ -528,6 +625,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) free((void *)objname); free((void *)devpath); } + else + { + // add to the node map;. + _node_map[std::string(nodepath)] = node; + } + group_tries++; @@ -549,3 +652,13 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) +{ + uORB::DeviceNode* rc = nullptr; + std::string np(nodepath); + if( _node_map.find( np ) != _node_map.end() ) + { + rc = _node_map[np]; + } + return rc; +} diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index ce22b84321..4fe4d499bd 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -35,9 +35,10 @@ #define _uORBDevices_posix_hpp_ #include +#include +#include #include "uORBCommon.hpp" - namespace uORB { class DeviceNode; @@ -56,7 +57,44 @@ public: 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); + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data ); + + /** + * processes a request for add subscription from remote + * @param rateInHz + * Specifies the desired rate for the message. + * @return + * 0 = success + * otherwise failure. + */ + int16_t process_add_subscription( int32_t rateInHz ); + + /** + * processes a request to remove a subscription from remote. + */ + int16_t process_remove_subscription(); + + /** + * processed the received data message from remote. + */ + int16_t process_received_message( int32_t length, uint8_t* data ); + + /** + * Add the subscriber to the node's list of subscriber. If there is + * remote proxy to which this subscription needs to be sent, it will + * done via uORBCommunicator::IChannel interface. + * @param sd + * the subscriber to be added. + */ + void add_internal_subscriber(); + + /** + * Removes the subscriber from the list. Also notifies the remote + * if there a uORBCommunicator::IChannel instance. + * @param sd + * the Subscriber to be removed. + */ + void remove_internal_subscriber(); protected: virtual pollevent_t poll_state(device::file_t *filp); @@ -81,6 +119,8 @@ private: SubscriberData *filp_to_sd(device::file_t *filp); + int32_t _subscriber_count; + /** * Perform a deferred update for a rate-limited subscriber. */ @@ -100,6 +140,11 @@ private: * @return True if the topic should appear updated to the subscriber */ bool appears_updated(SubscriberData *sd); + + + // disable copy and assignment operators + DeviceNode( const DeviceNode& ); + DeviceNode& operator=( const DeviceNode& ); }; /** @@ -114,9 +159,12 @@ public: DeviceMaster(Flavor f); ~DeviceMaster(); + static uORB::DeviceNode* GetDeviceNode( const char *node_name ); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: Flavor _flavor; + static std::map _node_map; }; #endif /* _uORBDeviceNode_posix.hpp */ diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 147ce5fbb3..8b2930eaef 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -34,9 +34,12 @@ #include #include "uORBDevices.hpp" #include "uORB.h" -#include "uORBTest_UnitTest.hpp" #include "uORBCommon.hpp" +#ifndef __PX4_QURT +#include "uORBTest_UnitTest.hpp" +#endif + extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static uORB::DeviceMaster *g_dev = nullptr; @@ -85,6 +88,7 @@ uorb_main(int argc, char *argv[]) return OK; } +#ifndef __PX4_QURT /* * Test the driver/device. */ @@ -108,6 +112,7 @@ uorb_main(int argc, char *argv[]) return t.latency_test(ORB_ID(orb_test), true); } } +#endif /* * Print driver information. diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 05626de723..a4fb49c82c 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -35,7 +35,17 @@ #define _uORBManager_hpp_ #include "uORBCommon.hpp" +#include "uORBDevices.hpp" #include +#ifdef __PX4_NUTTX +#include "ORBSet.hpp" +#else +#include +#include +#define ORBSet std::set +#endif + +#include "uORBCommunicator.hpp" namespace uORB { @@ -47,7 +57,7 @@ namespace uORB * uORB nodes for each uORB topics and also implements the behavor of the * uORB Api's. */ -class uORB::Manager +class uORB::Manager : public uORBCommunicator::IChannelRxHandler { public: // public interfaces for this class. @@ -281,6 +291,27 @@ class uORB::Manager */ int orb_set_interval(int handle, unsigned interval) ; + /** + * Method to set the uORBCommunicator::IChannel instance. + * @param comm_channel + * The IChannel instance to talk to remote proxies. + * @note: + * Currently this call only supports the use of one IChannel + * Future extensions may include more than one IChannel's. + */ + void set_uorb_communicator(uORBCommunicator::IChannel* comm_channel); + + /** + * Gets the uORB Communicator instance. + */ + uORBCommunicator::IChannel* get_uorb_communicator( void ); + + /** + * Utility method to check if there is a remote subscriber present + * for a given topic + */ + bool is_remote_subscriber_present( const char * messageName ); + private: // class methods /** * Advertise a node; don't consider it an error if the node has @@ -316,10 +347,57 @@ class uORB::Manager private: // data members static Manager _Instance; + // the communicator channel instance. + uORBCommunicator::IChannel* _comm_channel; + ORBSet _remote_subscriber_topics; private: //class methods Manager(); + /** + * Interface to process a received AddSubscription from remote. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_add_subscription(const char * messageName, + int32_t msgRateInHz); + + /** + * Interface to process a received control msg to remove subscription + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_remove_subscription(const char * messageName); + + /** + * Interface to process the received data message. + * @param messageName + * This represents the uORB message Name; This message Name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully handled in the + * handler. + * otherwise = failure. + */ + virtual int16_t process_received_message(const char * messageName, + int32_t length, uint8_t* data); + }; #endif /* _uORBManager_hpp_ */ diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index e8784d593e..7b1bcf33d1 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -39,7 +39,6 @@ #include #include "uORBUtils.hpp" #include "uORBManager.hpp" -#include "uORBDevices.hpp" //========================= Static initializations ================= @@ -55,6 +54,7 @@ uORB::Manager* uORB::Manager::get_instance() //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager::Manager() +: _comm_channel( nullptr ) { } @@ -280,3 +280,103 @@ int uORB::Manager::node_open /* everything has been OK, we can return the handle now */ return fd; } + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel) +{ + _comm_channel = channel; + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); + } +} + +uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) +{ + return _comm_channel; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_add_subscription(const char * messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName ); + int16_t rc = 0; + _remote_subscriber_topics.insert( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + // get the node name. + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + if ( node == nullptr) { + warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", + __LINE__, messageName ); + } + else{ + // node is present. + node->process_add_subscription(msgRateInHz); + } + } else { + rc = -1; + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const char * messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName ); + int16_t rc = -1; + _remote_subscriber_topics.erase( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName); + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const char * messageName, + int32_t length, uint8_t* data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); + + int16_t rc = -1; + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", + __LINE__, messageName, nodepath ); + + } else { + // node is present. + node->process_received_message( length, data ); + rc = 0; + } + } + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present( const char * messageName ) +{ + return _remote_subscriber_topics.find( messageName ); +} diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 21187bf359..7cd0d68e9f 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -39,6 +39,7 @@ #include #include "uORBUtils.hpp" #include "uORBManager.hpp" +#include "px4_config.h" #include "uORBDevices.hpp" @@ -55,6 +56,7 @@ uORB::Manager* uORB::Manager::get_instance() //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- uORB::Manager::Manager() +: _comm_channel( nullptr ) { } @@ -292,3 +294,103 @@ int uORB::Manager::node_open /* everything has been OK, we can return the handle now */ return fd; } + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel) +{ + _comm_channel = channel; + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); + } +} + +uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) +{ + return _comm_channel; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_add_subscription(const char *messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName ); + int16_t rc = 0; + _remote_subscriber_topics.insert( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + // get the node name. + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + if ( node == nullptr) { + warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", + __LINE__, messageName ); + } + else{ + // node is present. + node->process_add_subscription(msgRateInHz); + } + } else { + rc = -1; + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const char * messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName ); + int16_t rc = -1; + _remote_subscriber_topics.erase( messageName ); + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName); + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const char * messageName, + int32_t length, uint8_t* data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); + + int16_t rc = -1; + char nodepath[orb_maxpath]; + int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName ); + if (ret == OK) { + uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); + // get the node name. + if ( node == nullptr) { + warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", + __LINE__, messageName, nodepath ); + + } else { + // node is present. + node->process_received_message( length, data ); + rc = 0; + } + } + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present( const char * messageName ) +{ + return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); +} diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp index 53a053dc92..e051e66efb 100644 --- a/src/modules/uORB/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -104,8 +104,7 @@ int uORBTest::UnitTest::pubsublatency_main(void) if (pubsubtest_print) { char fname[32]; - //sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup); - sprintf(fname, "/tmp/timings%u.txt", timingsgroup); + sprintf(fname, PX4_ROOTFSDIR"/fs/microsd/timings%u.txt", timingsgroup); FILE *f = fopen(fname, "w"); if (f == NULL) { warnx("Error opening file!\n"); diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index c9d49222bc..a18fa1453a 100644 --- a/src/modules/uORB/uORBUtils.cpp +++ b/src/modules/uORB/uORBUtils.cpp @@ -61,3 +61,21 @@ int uORB::Utils::node_mkpath return OK; } + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int uORB::Utils::node_mkpath(char *buf, Flavor f, + const char *orbMsgName ) +{ + unsigned len; + + unsigned index = 0; + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", + orbMsgName, index ); + + if (len >= orb_maxpath) + return -ENAMETOOLONG; + + return OK; +} diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp index dc2fb9b78e..522d255b26 100644 --- a/src/modules/uORB/uORBUtils.hpp +++ b/src/modules/uORB/uORBUtils.hpp @@ -34,6 +34,7 @@ #define _uORBUtils_hpp_ #include "uORBCommon.hpp" +#include namespace uORB { @@ -50,6 +51,12 @@ public: const struct orb_metadata *meta, int *instance = nullptr ); + + /** + * same as above except this generators the path based on the string. + */ + static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); + }; #endif // _uORBUtils_hpp_ diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index aa67972b67..dc81fb57d6 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -65,9 +65,9 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" -#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" -#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" -#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" +#define UAVCAN_NODE_DB_PATH PX4_ROOTFSDIR"/fs/microsd/uavcan.db" +#define UAVCAN_FIRMWARE_PATH PX4_ROOTFSDIR"/fs/microsd/fw" +#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" // we add two to allow for actuator_direct and busevent #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index bb9749577a..174c522383 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -54,6 +54,8 @@ #include #include +#include + #include #include @@ -86,6 +88,8 @@ static const int ERROR = -1; #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) +#define ACC_READ (0<<6) +#define MAG_READ (1<<6) extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } @@ -512,6 +516,24 @@ ACCELSIM::reset() int ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + uint8_t cmd = send[0]; + + if (cmd & DIR_READ) { + // 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 + if (cmd & ACC_READ) { + sim->getRawAccelReport(&recv[2], len-2); + } else if (cmd & MAG_READ) { + sim->getMagReport(&recv[2], len-2); + } + } + return PX4_OK; } @@ -752,7 +774,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned period = 1000000 / arg; /* check against maximum sane rate (1ms) */ - if (period < 1000) + if (period < 10000) return -EINVAL; /* update interval for next measurement */ @@ -939,11 +961,10 @@ ACCELSIM::start() //PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval); hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); - - // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but mag period is set to 0 + // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but _call_mag_interval is 0 if (_call_mag_interval == 0) { - PX4_ERR("_call_mag_interval uninitilized - would have set period delay of 0"); - _call_mag_interval = 1000; + //PX4_INFO("_call_mag_interval uninitilized - would have set period delay of 0"); + _call_mag_interval = 10000; // Max 100Hz } //PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval); @@ -986,9 +1007,10 @@ ACCELSIM::measure() struct { uint8_t cmd; uint8_t status; - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; } raw_accel_report; #pragma pack(pop) @@ -999,8 +1021,11 @@ ACCELSIM::measure() /* 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)); + raw_accel_report.cmd = DIR_READ | ACC_READ; + + if(OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { + return; + } /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1029,54 +1054,58 @@ ACCELSIM::measure() // 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; + accel_report.x_raw = (int16_t)(raw_accel_report.x/_accel_range_scale); + accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale); + accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale); - float xraw_f = raw_accel_report.x; - float yraw_f = raw_accel_report.y; - float zraw_f = raw_accel_report.z; + // float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale); + // float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale); + // float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale); - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + // // 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; + // 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; - } + // 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; + // _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.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.x = raw_accel_report.x; + accel_report.y = raw_accel_report.y; + accel_report.z = raw_accel_report.z; accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1109,11 +1138,11 @@ ACCELSIM::mag_measure() #pragma pack(push, 1) struct { uint8_t cmd; - int16_t temperature; uint8_t status; - int16_t x; - int16_t y; - int16_t z; + float temperature; + float x; + float y; + float z; } raw_mag_report; #pragma pack(pop) @@ -1125,8 +1154,11 @@ ACCELSIM::mag_measure() /* 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)); + raw_mag_report.cmd = DIR_READ | MAG_READ; + + if(OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) { + return; + } /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1146,29 +1178,33 @@ ACCELSIM::mag_measure() 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; + mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale); + mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale); + mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale); + + float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale); + float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale); + float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale); - 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); + // 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); + _last_temperature = raw_mag_report.temperature; mag_report.temperature = _last_temperature; + mag_report.x = raw_mag_report.x; + mag_report.y = raw_mag_report.y; + mag_report.z = raw_mag_report.z; _mag_reports->force(&mag_report); diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 91bd149135..a70e394213 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -275,6 +275,13 @@ BAROSIM::init() _measure_phase = 0; _reports->flush(); + _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"); + } + /* this do..while is goto without goto */ do { /* do temperature first */ @@ -312,12 +319,6 @@ BAROSIM::init() 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 == nullptr) { - PX4_ERR("failed to create sensor_baro publication"); - } //PX4_WARN("sensor_baro publication %ld", _baro_topic); } while (0); @@ -622,7 +623,14 @@ int BAROSIM::collect() { int ret; - uint32_t raw; + +#pragma pack(push, 1) + struct { + float pressure; + float altitude; + float temperature; + } baro_report; +#pragma pack(pop) perf_begin(_sample_perf); @@ -632,7 +640,7 @@ BAROSIM::collect() 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); + ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -641,84 +649,15 @@ BAROSIM::collect() /* 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; - } - + report.pressure = baro_report.pressure; + report.altitude = baro_report.altitude; + report.temperature = baro_report.temperature; + report.timestamp = hrt_absolute_time(); } 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; + report.pressure = baro_report.pressure; + report.altitude = baro_report.altitude; + report.temperature = baro_report.temperature; + report.timestamp = hrt_absolute_time(); /* publish it */ if (!(_pub_blocked)) { diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 734c2bc286..8e66dc3ac6 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -118,22 +118,26 @@ BARO_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); + int ret = transfer(&cmd, 1, static_cast(data), count); + + /* if (ret == PX4_OK) { - /* fetch the raw value */ + // 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; } @@ -204,7 +208,7 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len, if (recv_len == 0) { PX4_DEBUG("BARO_SIM measurement requested"); } - else if (send_len != 1 || send[0] != 0 || recv_len != 3) { + else if (send_len != 1 || send[0] != 0 ) { PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; } diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp new file mode 100644 index 0000000000..db948b73f6 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -0,0 +1,682 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 gps.cpp + * Driver for the GPS on a serial port + */ + +//#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +//#include +#include +#include +//#include +#include +//#include +//#include +#include +#include +#include +#include +#include + +#include + +//#include + +#define GPS_DRIVER_MODE_UBX_SIM +#define GPS_SIM_DEVICE_PATH "/dev/gps_sim" + +//#include "ubx.h" +//#include "mtk.h" +//#include "ashtech.h" + + +#define TIMEOUT_5HZ 500 +#define RATE_MEASUREMENT_PERIOD 5000000 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* class for dynamic allocation of satellite info data */ +class GPS_Sat_Info +{ +public: + struct satellite_info_s _data; +}; + + +class GPS_SIM : public device::VDev +{ +public: + GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info); + virtual ~GPS_SIM(); + + virtual int init(); + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + char _port[20]; ///< device / serial port path + volatile int _task; ///< worker task + bool _healthy; ///< flag to signal if the GPS is ok + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + //gps_driver_mode_t _mode; ///< current mode + GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object + struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position + orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position + struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info + orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info + float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output + + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ + void config(); + + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(void *arg); + + + /** + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running + */ + void task_main(void); + + /** + * Set the baudrate of the UART to the GPS + */ + int set_baudrate(unsigned baud); + + /** + * Send a reset command to the GPS + */ + void cmd_reset(); + + int receive(int timeout); + +}; + + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gps_sim_main(int argc, char *argv[]); + +namespace +{ + +GPS_SIM *g_dev = nullptr; + +} + + +GPS_SIM::GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info) : + VDev("gps", GPS_SIM_DEVICE_PATH), + _task_should_exit(false), + //_healthy(false), + //_mode_changed(false), + //_mode(GPS_DRIVER_MODE_UBX), + //_Helper(nullptr), + _Sat_Info(nullptr), + _report_gps_pos_pub(nullptr), + _p_report_sat_info(nullptr), + _report_sat_info_pub(nullptr), + _rate(0.0f), + _fake_gps(fake_gps) +{ + // /* store port name */ + // strncpy(_port, uart_path, sizeof(_port)); + // /* enforce null termination */ + // _port[sizeof(_port) - 1] = '\0'; + + /* we need this potentially before it could be set in task_main */ + g_dev = this; + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + + /* create satellite info data object if requested */ + if (enable_sat_info) { + _Sat_Info = new(GPS_Sat_Info); + _p_report_sat_info = &_Sat_Info->_data; + memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); + } + + _debug_enabled = true; +} + +GPS_SIM::~GPS_SIM() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + px4_task_delete(_task); + + g_dev = nullptr; + +} + +int +GPS_SIM::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (VDev::init() != OK) + goto out; + + /* start the GPS driver worker task */ + _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPS_SIM::task_main_trampoline, nullptr); + + if (_task < 0) { + warnx("task start failed: %d", errno); + return -errno; + } + + ret = OK; +out: + return ret; +} + +int +GPS_SIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + lock(); + + int ret = OK; + + switch (cmd) { + case SENSORIOCRESET: + cmd_reset(); + break; + + default: + /* give it to parent if no one wants it */ + ret = VDev::ioctl(filp, cmd, arg); + break; + } + + unlock(); + + return ret; +} + +void +GPS_SIM::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +int +GPS_SIM::receive(int timeout) { + Simulator *sim = Simulator::getInstance(); + simulator::RawGPSData gps; + sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); + + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = gps.lat; + _report_gps_pos.lon = gps.lon; + _report_gps_pos.alt = gps.alt; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.eph = (float)gps.eph; + _report_gps_pos.epv = (float)gps.epv; + _report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f; + _report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f; + _report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f; + _report_gps_pos.vel_d_m_s = (float)(gps.vd)/100.0f; + _report_gps_pos.cog_rad = (float)(gps.cog)*3.1415f/(100.0f * 180.0f); + _report_gps_pos.fix_type = gps.fix_type; + _report_gps_pos.satellites_used = gps.satellites_visible; + + usleep(200000); + return 1; +} + +void +GPS_SIM::task_main() +{ + + /* loop handling received serial bytes and also configuring in between */ + while (!_task_should_exit) { + + if (_fake_gps) { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph = 0.9f; + _report_gps_pos.epv = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; + + //no time and satellite information simulated + + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + + usleep(2e5); + + } else { + + // if (_Helper != nullptr) { + // delete(_Helper); + // set to zero to ensure parser is not used while not instantiated + // _Helper = nullptr; + // } + + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX_SIM: + // _Helper = new UBX_SIM(_serial_fd, &_report_gps_pos, _p_report_sat_info); + // break; + + // default: + // break; + // } + + //if (_Helper->configure(_baudrate) == 0) { + + //Publish initial report that we have access to a GPS + //Make sure to clear any stale data in case driver is reset + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.timestamp_time = hrt_absolute_time(); + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + + // GPS is obviously detected successfully, reset statistics + //_Helper->reset_update_rates(); + + while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + + if (!(_pub_blocked)) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_p_report_sat_info) { + if (_report_sat_info_pub != nullptr) { + orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); + + } else { + _report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info); + } + } + } + + //if (helper_ret & 1) { // consider only pos info updates for rate calculation */ + // last_rate_count++; + //} + + /* measure update rate every 5 seconds */ + //if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + //_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + //last_rate_measurement = hrt_absolute_time(); + //last_rate_count = 0; + //_Helper->store_update_rates(); + //_Helper->reset_update_rates(); + //} + + // if (!_healthy) { + // const char *mode_str = "unknown"; + + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX_SIM: + // mode_str = "UBX"; + // break; + + // default: + // break; + // } + + // warnx("module found: %s", mode_str); + // _healthy = true; + // } + } + + // if (_healthy) { + // warnx("module lost"); + // _healthy = false; + // _rate = 0.0f; + // } + + lock(); + //} + + // /* select next mode */ + // switch (_mode) { + // case GPS_DRIVER_MODE_UBX: + // _mode = GPS_DRIVER_MODE_MTK; + // break; + + // case GPS_DRIVER_MODE_MTK: + // _mode = GPS_DRIVER_MODE_ASHTECH; + // break; + + // case GPS_DRIVER_MODE_ASHTECH: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + + // default: + // break; + // } + } + + } + + warnx("exiting"); + + //::close(_serial_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + return; +} + + + +void +GPS_SIM::cmd_reset() +{ + +} + +void +GPS_SIM::print_info() +{ + //GPS Mode + if(_fake_gps) { + warnx("protocol: faked"); + } + + else { + warnx("protocol: SIM"); + } + + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + + if (_report_gps_pos.timestamp_position != 0) { + warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); + //warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); + //warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); + warnx("rate publication:\t%6.2f Hz", (double)_rate); + + } + + usleep(100000); +} + +/** + * Local functions in support of the shell command. + */ +namespace gps_sim +{ + +GPS_SIM *g_dev = nullptr; + +void start(const char *uart_path, bool fake_gps, bool enable_sat_info); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(const char *uart_path, bool fake_gps, bool enable_sat_info) +{ + int fd; + + if (g_dev != nullptr) + warnx("already started"); + + /* create the driver */ + g_dev = new GPS_SIM(uart_path, fake_gps, enable_sat_info); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + warnx("open: %s\n", GPS0_DEVICE_PATH); + goto fail; + } + + return; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + warnx("start failed"); +} + +/** + * Stop the driver. + */ +void +stop() +{ + delete g_dev; + g_dev = nullptr; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + + warnx("PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + warnx("failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + warnx("reset failed"); +} + +/** + * Print the status of the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "not running"); + + g_dev->print_info(); +} + +} // namespace + + +int +gps_sim_main(int argc, char *argv[]) +{ + + /* set to default */ + const char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; + bool enable_sat_info = false; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + /* work around getopt unreliability */ + if (argc > 3) { + if (!strcmp(argv[2], "-d")) { + device_name = argv[3]; + + } else { + goto out; + } + } + + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + /* Detect sat info option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-s")) + enable_sat_info = true; + } + + gps_sim::start(device_name, fake_gps, enable_sat_info); + } + + if (!strcmp(argv[1], "stop")) + gps_sim::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + gps_sim::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + gps_sim::reset(); + + /* + * Print driver status. + */ + if (!strcmp(argv[1], "status")) + gps_sim::info(); + + return 0; + +out: + warnx("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/module.mk b/src/platforms/posix/drivers/gpssim/module.mk new file mode 100644 index 0000000000..630eaae377 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/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. +# +############################################################################ + +# +# Simulated GPS driver +# + +MODULE_COMMAND = gps_sim + +SRCS = gpssim.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.cpp b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp new file mode 100644 index 0000000000..a4e9043c96 --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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 ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * + * @author Hannes Delago + * (rework, add ubx7+ compatibility) + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ubx_sim.h" +#include + +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval + + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) : + _fd(fd), + _gps_position(gps_position), + _satellite_info(satellite_info), +{ + +} + +UBX::~UBX() +{ +} + + +int UBX_SIM::configure(unsigned &baudrate) +{ + return 0; +} + +int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled +UBX_SIM::receive(const unsigned timeout) +{ + /* copy data from simulator here */ + usleep(1000000); + return 1; +} diff --git a/src/platforms/posix/drivers/gpssim/ubx_sim.h b/src/platforms/posix/drivers/gpssim/ubx_sim.h new file mode 100644 index 0000000000..5722822dad --- /dev/null +++ b/src/platforms/posix/drivers/gpssim/ubx_sim.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 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 ubx_sim.h + * + */ + +#ifndef UBX_SIM_H_ +#define UBX_SIM_H_ + + + + +class UBX_SIM +{ +public: + UBX_SIM(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); + ~UBX_SIM(); + int receive(const unsigned timeout); + int configure(unsigned &baudrate); + +private: + + + + int _fd; + struct vehicle_gps_position_s *_gps_position; + struct satellite_info_s *_satellite_info; + bool _enable_sat_info; +}; + +#endif /* UBX_SIM_H_ */ diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 1ccaecf9a6..40d8f71c1b 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -379,13 +379,13 @@ private: 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]; + float accel_x; + float accel_y; + float accel_z; + float temp; + float gyro_x; + float gyro_y; + float gyro_z; }; #pragma pack(pop) @@ -466,6 +466,9 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro // disable debug() calls _debug_enabled = false; + // Don't publish until initialized + _pub_blocked = true; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ @@ -588,6 +591,9 @@ GYROSIM::init() if (_accel_topic == nullptr) { PX4_WARN("ADVERT FAIL"); } + else { + _pub_blocked = false; + } /* advertise sensor topic, measure manually to initialize valid report */ @@ -1194,15 +1200,6 @@ void GYROSIM::measure() { 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); @@ -1214,69 +1211,10 @@ GYROSIM::measure() // 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, + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { 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. */ @@ -1286,7 +1224,11 @@ GYROSIM::measure() /* * Adjust and scale results to m/s^2. */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + grb.timestamp = hrt_absolute_time(); + arb.timestamp = grb.timestamp; + + // this sleep is needed because the timing of the drivers is not yet working + usleep(1000); // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher @@ -1312,13 +1254,13 @@ GYROSIM::measure() /* 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; + arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale); + arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale); + arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale); +/* + float xraw_f = (int16_t)(mpu_report.accel_x / _accel_range_scale); + float yraw_f = (int16_t)(mpu_report.accel_y / _accel_range_scale); + float zraw_f = (int16_t)(mpu_report.accel_z / _accel_range_scale); // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -1330,22 +1272,26 @@ GYROSIM::measure() 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; + _last_temperature = mpu_report.temp; - arb.temperature_raw = report.temp; + arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); arb.temperature = _last_temperature; - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; + arb.x = mpu_report.accel_x; + arb.y = mpu_report.accel_y; + arb.z = mpu_report.accel_z; - xraw_f = report.gyro_x; - yraw_f = report.gyro_y; - zraw_f = report.gyro_z; + grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale); + grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale); + grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale); +/* + xraw_f = (int16_t)(mpu_report.gyro_x/_gyro_range_scale); + yraw_f = (int16_t)(mpu_report.gyro_y/_gyro_range_scale); + zraw_f = (int16_t)(mpu_report.gyro_z/_gyro_range_scale); // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -1357,20 +1303,24 @@ GYROSIM::measure() 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_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); grb.temperature = _last_temperature; + grb.x = mpu_report.gyro_x; + grb.y = mpu_report.gyro_y; + grb.z = mpu_report.gyro_z; + _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); diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index 37f51ff607..b127128a66 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -350,7 +350,7 @@ ToneAlarm::start_note(unsigned note) // Silence warning of unused var do_something(period); - PX4_DEBUG("ToneAlarm::start_note %u", period); + PX4_INFO("ToneAlarm::start_note %u", period); } void @@ -361,6 +361,7 @@ ToneAlarm::stop_note() void ToneAlarm::start_tune(const char *tune) { + PX4_INFO("ToneAlarm::start_tune"); // kill any current playback hrt_cancel(&_note_call); @@ -533,7 +534,7 @@ ToneAlarm::next_note() // tune looks bad (unexpected EOF, bad character, etc.) tune_error: - printf("tune error\n"); + PX4_ERR("tune error\n"); _repeat = false; // don't loop on error // stop (and potentially restart) the tune @@ -605,7 +606,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* 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); + PX4_INFO("TONE_SET_ALARM %lu", arg); if (arg < TONE_NUMBER_OF_TUNES) { if (arg == TONE_STOP_TUNE) { diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index f756749d52..475fa3e812 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -79,9 +79,9 @@ static void run_cmd(const vector &appargs) { static void process_line(string &line) { - vector appargs(5); + vector appargs(8); - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> appargs[7]; run_cmd(appargs); } diff --git a/src/platforms/posix/px4_layer/dq_addlast.c b/src/platforms/posix/px4_layer/dq_addlast.c index 3ef08abd05..3a2ec3dbea 100644 --- a/src/platforms/posix/px4_layer/dq_addlast.c +++ b/src/platforms/posix/px4_layer/dq_addlast.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/dq_rem.c b/src/platforms/posix/px4_layer/dq_rem.c index db20762c75..21247add10 100644 --- a/src/platforms/posix/px4_layer/dq_rem.c +++ b/src/platforms/posix/px4_layer/dq_rem.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/dq_remfirst.c b/src/platforms/posix/px4_layer/dq_remfirst.c index e87acc3382..fc80923cb2 100644 --- a/src/platforms/posix/px4_layer/dq_remfirst.c +++ b/src/platforms/posix/px4_layer/dq_remfirst.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 3c1a2323fb..6ef217e218 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -301,9 +301,12 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); +#if 0 + // Use this to debug busy CPU that keeps rescheduling with 0 period time if (interval < HRT_INTERVAL_MIN) { PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); } +#endif entry->deadline = deadline; entry->period = interval; entry->callout = callout; diff --git a/src/platforms/posix/px4_layer/hrt_queue.c b/src/platforms/posix/px4_layer/hrt_queue.c index ab10714776..e45132253a 100644 --- a/src/platforms/posix/px4_layer/hrt_queue.c +++ b/src/platforms/posix/px4_layer/hrt_queue.c @@ -49,8 +49,6 @@ #include #include "hrt_work.h" -#ifdef CONFIG_SCHED_WORKQUEUE - /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ @@ -129,4 +127,3 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del return PX4_OK; } -#endif /* CONFIG_SCHED_WORKQUEUE */ diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/px4_layer/hrt_work.h index 566684eb86..d926a6d250 100644 --- a/src/platforms/posix/px4_layer/hrt_work.h +++ b/src/platforms/posix/px4_layer/hrt_work.h @@ -46,16 +46,16 @@ void hrt_work_queue_init(void); int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); void hrt_work_cancel(struct work_s *work); -inline void hrt_work_lock(void); -inline void hrt_work_unlock(void); +//inline void hrt_work_lock(void); +//inline void hrt_work_unlock(void); -inline void hrt_work_lock() +static inline void hrt_work_lock() { //PX4_INFO("hrt_work_lock"); sem_wait(&_hrt_work_lock); } -inline void hrt_work_unlock() +static inline void hrt_work_unlock() { //PX4_INFO("hrt_work_unlock"); sem_post(&_hrt_work_lock); diff --git a/src/platforms/posix/px4_layer/module.mk b/src/platforms/posix/px4_layer/module.mk index b2bd0da4bb..73b6ce9e00 100644 --- a/src/platforms/posix/px4_layer/module.mk +++ b/src/platforms/posix/px4_layer/module.mk @@ -42,6 +42,7 @@ SRCS = \ hrt_queue.c \ hrt_work_cancel.c \ work_thread.c \ + work_lock.c \ work_queue.c \ work_cancel.c \ lib_crc32.c \ diff --git a/src/platforms/posix/px4_layer/px4_posix_impl.cpp b/src/platforms/posix/px4_layer/px4_posix_impl.cpp index 0d926f0520..243716abe6 100644 --- a/src/platforms/posix/px4_layer/px4_posix_impl.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_impl.cpp @@ -48,6 +48,8 @@ #include "systemlib/param/param.h" #include "hrt_work.h" +extern pthread_t _shell_task_id; + __BEGIN_DECLS long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); @@ -63,6 +65,8 @@ void init_once(void); void init_once(void) { + _shell_task_id = pthread_self(); + PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); hrt_work_queue_init(); hrt_init(); diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index c539fa7266..ac4c6b62bf 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -57,7 +57,11 @@ #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 +#define PX4_MAX_TASKS 50 +#define SHELL_TASK_ID (PX4_MAX_TASKS+1) + +pthread_t _shell_task_id = 0; + struct task_entry { pthread_t pid; @@ -243,7 +247,7 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d", sig); - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) pid = taskmap[id].pid; else return -EINVAL; @@ -273,6 +277,24 @@ void px4_show_tasks() } __BEGIN_DECLS + +int px4_getpid() +{ + pthread_t pid = pthread_self(); + + if (pid == _shell_task_id) + return SHELL_TASK_ID; + + // Get pthread ID from the opaque ID + for (int i=0; i #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/sq_remfirst.c b/src/platforms/posix/px4_layer/sq_remfirst.c index f81c18dc2e..81751e8579 100644 --- a/src/platforms/posix/px4_layer/sq_remfirst.c +++ b/src/platforms/posix/px4_layer/sq_remfirst.c @@ -41,6 +41,7 @@ * Included Files ************************************************************/ +#include #include /************************************************************ diff --git a/src/platforms/posix/px4_layer/work_lock.c b/src/platforms/posix/px4_layer/work_lock.c new file mode 100644 index 0000000000..b2ad307d7c --- /dev/null +++ b/src/platforms/posix/px4_layer/work_lock.c @@ -0,0 +1,19 @@ +//#pragma once +#include +#include +#include "work_lock.h" + + +extern sem_t _work_lock[]; + +void work_lock(int id) +{ + //printf("work_lock %d\n", id); + sem_wait(&_work_lock[id]); +} + +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_lock.h b/src/platforms/posix/px4_layer/work_lock.h index f77f228b36..ad2e5b4a01 100644 --- a/src/platforms/posix/px4_layer/work_lock.h +++ b/src/platforms/posix/px4_layer/work_lock.h @@ -31,23 +31,13 @@ * ****************************************************************************/ -#include -#include +#ifndef _work_lock_h_ +#define _work_lock_h_ -#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]); -} +//#pragma once -inline void work_unlock(int id); -inline void work_unlock(int id) -{ - //printf("work_unlock %d\n", id); - sem_post(&_work_lock[id]); -} +void work_lock(int id); +void work_unlock(int id); +#endif // _work_lock_h_ diff --git a/src/platforms/posix/tests/muorb/module.mk b/src/platforms/posix/tests/muorb/module.mk new file mode 100644 index 0000000000..15bf4824e9 --- /dev/null +++ b/src/platforms/posix/tests/muorb/module.mk @@ -0,0 +1,47 @@ +############################################################################ +# +# 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 = muorb_test + +INCLUDE_DIRS += ${PX4_BASE}../muorb_krait \ + ${PX4_BASE}../muorb_krait/lib/include \ + ${PX4_BASE}../muorb_krait/Pal/lib + +SRCS = muorb_test_main.cpp \ + muorb_test_start_posix.cpp \ + muorb_test_example.cpp + diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.cpp b/src/platforms/posix/tests/muorb/muorb_test_example.cpp new file mode 100644 index 0000000000..37dfec5ed3 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_example.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 "muorb_test_example.h" +#include +#include +#include +#include + +px4::AppState MuorbTestExample::appState; + +int MuorbTestExample::main() +{ + appState.setRunning(true); + + int i=0; + orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status ); + if( pub_id == 0 ) + { + PX4_ERR( "error publishing esc_status" ); + return -1; + } + orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc ); + if( pub_id_vc == 0 ) + { + PX4_ERR( "error publishing vehicle_command" ); + return -1; + } + if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the vechile command message", i ); + return -1; + } + int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) ); + if ( sub_vc == PX4_ERROR ) + { + PX4_ERR( "Error subscribing to vehicle_command topic" ); + return -1; + } + + while (!appState.exitRequested() && i<100) { + + PX4_DEBUG("[%d] Doing work...", i ); + if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the esc status message for iter", i ); + break; + } + bool updated = false; + if( orb_check( sub_vc, &updated ) == 0 ) + { + if( updated ) + { + PX4_DEBUG( "[%d]Vechicle Status is updated... reading new value", i ); + if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 ) + { + PX4_ERR( "[%d]Error calling orb copy for vechivle status... ", i ); + break; + } + if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the vechile command message", i ); + break; + } + } + else + { + PX4_DEBUG( "[%d] VC topic is not updated", i ); + } + } + else + { + PX4_ERR( "[%d]Error checking the updated status for vechile command... ", i ); + break; + } + + ++i; + } + + return 0; +} diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h new file mode 100644 index 0000000000..a3625167c7 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_example.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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 +#include "uORB/topics/esc_status.h" +#include "uORB/topics/vehicle_command.h" + +class MuorbTestExample { +public: + MuorbTestExample() {}; + + ~MuorbTestExample() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +private: + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + +}; diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp new file mode 100644 index 0000000000..6ebc0ae92c --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 "muorb_test_example.h" +#include +#include "uORB/uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "muorb_test"); + + PX4_DEBUG("muorb_test"); + + // register the fast rpc channel with UORB. + uORB::Manager::get_instance()->set_uorb_communicator( uORB::KraitFastRpcChannel::GetInstance() ); + + // start the KaitFastRPC channel thread. + uORB::KraitFastRpcChannel::GetInstance()->Start(); + + MuorbTestExample hello; + hello.main(); + + uORB::KraitFastRpcChannel::GetInstance()->Stop(); + PX4_DEBUG("goodbye"); + return 0; +} diff --git a/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp new file mode 100644 index 0000000000..943605f531 --- /dev/null +++ b/src/platforms/posix/tests/muorb/muorb_test_start_posix.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 "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); + +static void usage() +{ + PX4_DEBUG("usage: muorb_test {start|stop|status}"); +} +int muorb_test_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("muorb_test", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char* const*)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + MuorbTestExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index e123a008c5..ba5f716945 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -92,6 +92,8 @@ typedef param_t px4_param_t; */ #if defined(__PX4_NUTTX) +#define PX4_ROOTFSDIR + /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) #define isspace(c) \ @@ -136,6 +138,12 @@ __END_DECLS #define px4_statfs_buf_f_bavail_t unsigned long +#if defined(__PX4_QURT) +#define PX4_ROOTFSDIR +#else +#define PX4_ROOTFSDIR "rootfs" +#endif + #endif @@ -198,6 +206,10 @@ __END_DECLS #endif #if defined(__PX4_QURT) + +#define PX4_ROOTFSDIR +#define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" + #define SIOCDEVPRIVATE 999999 // Missing math.h defines @@ -211,7 +223,6 @@ __END_DECLS #define fminf(x, y) ((x) > (y) ? y : x) #endif - #endif /* diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index a0d7f07bce..d655fa91d6 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -52,11 +52,24 @@ } #if defined(__PX4_QURT) #include +#define FARF printf -#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__); +#define __FARF_omit(level, ...) { } +#define __FARF_log(level, ...) { \ + FARF("%-5s ", level);\ + FARF(__VA_ARGS__);\ + FARF("\n");\ +} +#define __FARF_log_verbose(level, ...) { \ + FARF("%-5s ", level);\ + FARF(__VA_ARGS__);\ + FARF(" (file %s line %d)\n", __FILE__, __LINE__);\ +} + +#define PX4_DEBUG(...) __FARF_omit("DEBUG", __VA_ARGS__) +#define PX4_INFO(...) __FARF_log("INFO", __VA_ARGS__) +#define PX4_WARN(...) __FARF_log_verbose("WARN", __VA_ARGS__) +#define PX4_ERR(...) __FARF_log_verbose("ERROR", __VA_ARGS__) #elif defined(__PX4_LINUX) #include @@ -68,10 +81,10 @@ printf(" (file %s line %d)\n", __FILE__, __LINE__);\ } -#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__); +#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) diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 9c392ac42a..4e0fc8370d 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -68,6 +68,7 @@ typedef struct pollfd px4_pollfd_struct_t; #define px4_poll _GLOBAL poll #define px4_fsync _GLOBAL fsync #define px4_access _GLOBAL access +#define px4_getpid _GLOBAL getpid #elif defined(__PX4_POSIX) @@ -98,6 +99,7 @@ __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); __EXPORT int px4_access(const char *pathname, int mode); +__EXPORT int px4_getpid(void); __END_DECLS #else diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 0c9b7f24c9..a54a18b9ab 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -28,7 +28,6 @@ struct timespec 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 diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c new file mode 100644 index 0000000000..996a279d87 --- /dev/null +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 + +#define STACK_SIZE 0x8000 +static char __attribute__ ((aligned (16))) stack1[STACK_SIZE]; + +int main(int argc, char* argv[]) +{ + int ret = 0; + char *builtin[]={"libgcc.so", "libc.so", "libstdc++.so"}; + void *handle; + char *error; + void (*entry_function)() = NULL; + + printf("In DSPAL main\n"); + dlinit(3, builtin); +#if 0 + handle = dlopen ("libdspal_client.so", RTLD_LAZY); + if (!handle) { + printf("Error opening libdspal_client.so\n"); + return 1; + } + entry_function = dlsym(handle, "dspal_entry"); + if (((error = dlerror()) != NULL) || (entry_function == NULL)) { + printf("Error dlsym for dspal_entry"); + ret = 2; + } + dlclose(handle); +#endif + return ret; +} + diff --git a/src/platforms/qurt/px4_layer/commands_muorb_test.c b/src/platforms/qurt/px4_layer/commands_muorb_test.c new file mode 100644 index 0000000000..e594b9dad8 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_muorb_test.c @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + static const char *commands = + "uorb start\n" + "muorb_test start\n"; + + return commands; +} diff --git a/src/platforms/qurt/px4_layer/hrt_queue.c b/src/platforms/qurt/px4_layer/hrt_queue.c new file mode 100644 index 0000000000..04bf8bdd49 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_queue.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * 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 +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: hrt_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: + * 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 microseconds) 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 hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + struct wqueue_s *wqueue = &g_hrt_work; + + /* 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. + */ + + hrt_work_lock(); + work->qtime = hrt_absolute_time(); /* Time work queued */ + //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); + + dq_addlast((dq_entry_t *)work, &wqueue->q); + px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */ + + hrt_work_unlock(); + return PX4_OK; +} + diff --git a/src/platforms/qurt/px4_layer/hrt_thread.c b/src/platforms/qurt/px4_layer/hrt_thread.c new file mode 100644 index 0000000000..dc0f3baa97 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_thread.c @@ -0,0 +1,256 @@ +/**************************************************************************** + * libc/wqueue/work_thread.c + * + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * Modified by: Mark Charlebois to use hrt compatible times + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * 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 "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* The state of each work queue. */ +struct wqueue_s g_hrt_work; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ +sem_t _hrt_work_lock; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ +static void hrt_work_process(void); + +/**************************************************************************** + * 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 hrt_work_process() +{ + struct wqueue_s *wqueue = &g_hrt_work; + 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. + */ + + /* Default to sleeping for 1 sec */ + next = 1000000; + + hrt_work_lock(); + + 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 = hrt_absolute_time() - work->qtime; + //PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work); + if (elapsed >= work->delay) + { + /* Remove the ready-to-execute work from the list */ + + (void)dq_rem((struct dq_entry_s *)work, &wqueue->q); + //PX4_INFO("Dequeued work=%p", work); + + /* 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! + */ + + hrt_work_unlock(); + if (!worker) { + PX4_ERR("MESSED UP: worker = 0"); + } + 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. + */ + + hrt_work_lock(); + work = (FAR struct work_s *)wqueue->q.head; + } + else + { + /* This one is not ready.. will it be ready before the next + * scheduled wakeup interval? + */ + + /* Here: elapsed < work->delay */ + remaining = work->delay - elapsed; + //PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed); + if (remaining < next) + { + /* Yes.. Then schedule to wake up when the work is ready */ + + next = remaining; + } + + /* Then try the next in the list. */ + + work = (FAR struct work_s *)work->dq.flink; + //PX4_INFO("next %u work %p", next, work); + } + } + + /* Wait awhile to check the work list. We will wait here until either + * the time elapses or until we are awakened by a signal. + */ + hrt_work_unlock(); + + /* might sleep less if a signal received and new item was queued */ + //PX4_INFO("Sleeping for %u usec", next); + usleep(next); +} + +/**************************************************************************** + * Name: work_hrtthread + * + * Description: + * This is the worker threads that performs actions placed on the ISR work + * list. + * + * 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. + * + * 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 + * + ****************************************************************************/ + +static int work_hrtthread(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). + */ + + /* Then process queued work. We need to keep interrupts disabled while + * we process items in the work list. + */ + + hrt_work_process(); + } + + return PX4_OK; /* To keep some compilers happy */ +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void hrt_work_queue_init(void) +{ + sem_init(&_hrt_work_lock, 0, 1); + + // Create high priority worker thread + g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 2000, + work_hrtthread, + (char* const*)NULL); + +} + diff --git a/src/platforms/qurt/px4_layer/hrt_work.h b/src/platforms/qurt/px4_layer/hrt_work.h new file mode 100644 index 0000000000..566684eb86 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_work.h @@ -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. + * + ****************************************************************************/ + +#include +#include +#include + +#pragma once + +__BEGIN_DECLS + +extern sem_t _hrt_work_lock; +extern struct wqueue_s g_hrt_work; + +void hrt_work_queue_init(void); +int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay); +void hrt_work_cancel(struct work_s *work); + +inline void hrt_work_lock(void); +inline void hrt_work_unlock(void); + +inline void hrt_work_lock() +{ + //PX4_INFO("hrt_work_lock"); + sem_wait(&_hrt_work_lock); +} + +inline void hrt_work_unlock() +{ + //PX4_INFO("hrt_work_unlock"); + sem_post(&_hrt_work_lock); +} + +__END_DECLS + diff --git a/src/platforms/qurt/px4_layer/hrt_work_cancel.c b/src/platforms/qurt/px4_layer/hrt_work_cancel.c new file mode 100644 index 0000000000..c1c2c3bef7 --- /dev/null +++ b/src/platforms/qurt/px4_layer/hrt_work_cancel.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * libc/wqueue/work_cancel.c + * + * Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include "hrt_work.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: hrt_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 + * hrt_work_queue() again. + * + * Input parameters: + * work - The previously queue work structure to cancel + * + ****************************************************************************/ + +void hrt_work_cancel(struct work_s *work) +{ + struct wqueue_s *wqueue = &g_hrt_work; + + //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. + */ + + hrt_work_lock(); + 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; + } + + hrt_work_unlock(); +} diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 538b64229f..864f1d22c2 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -131,14 +132,18 @@ namespace px4 { extern void init_once(void); }; -int main(int argc, char **argv) +__BEGIN_DECLS +void dspal_entry() { + const char *argv[2] = { "dspal_client", NULL }; + int argc = 1; + printf("In main\n"); map apps; init_app_map(apps); px4::init_once(); - px4::init(argc, argv, "mainapp"); + px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); - for (;;) {} + for (;;) { sleep(100000); } } - +__END_DECLS diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 714263edcb..13b41db3ad 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -40,8 +40,12 @@ SRCDIR=$(dir $(MODULE_MK)) SRCS = \ px4_qurt_impl.cpp \ px4_qurt_tasks.cpp \ + hrt_thread.c \ + hrt_queue.c \ + hrt_work_cancel.c \ work_thread.c \ work_queue.c \ + work_lock.c \ work_cancel.c \ lib_crc32.c \ drv_hrt.c \ @@ -52,12 +56,16 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main.cpp + main.cpp \ + qurt_stubs.c ifeq ($(CONFIG),qurt_hello) SRCS += commands_hello.c endif ifeq ($(CONFIG),qurt_default) SRCS += commands_default.c endif +ifeq ($(CONFIG),qurt_muorb_test) +SRCS += commands_muorb_test.c +endif MAXOPTIMIZATION = -Os diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 3ff09b3d4f..a5782a6f25 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -48,22 +48,21 @@ #include #include #include -#include #include "systemlib/param/param.h" +#include "hrt_work.h" + + +extern pthread_t _shell_task_id; __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); + usleep(1000000); return 0; } @@ -79,7 +78,7 @@ void qurt_log(const char *fmt, ...) } #endif -extern int _posix_init(void); +//extern int _posix_init(void); __END_DECLS @@ -94,9 +93,12 @@ void init_once(void); void init_once(void) { // Required for QuRT - _posix_init(); + //_posix_init(); + _shell_task_id = pthread_self(); + PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); + hrt_work_queue_init(); hrt_init(); } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index d688f1104a..cf6d665724 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -57,7 +57,11 @@ #define MAX_CMD_LEN 100 -#define PX4_MAX_TASKS 100 +#define PX4_MAX_TASKS 50 +#define SHELL_TASK_ID (PX4_MAX_TASKS+1) + +pthread_t _shell_task_id = 0; + struct task_entry { pthread_t pid; @@ -262,3 +266,25 @@ void px4_show_tasks() PX4_INFO(" No running tasks"); } + +__BEGIN_DECLS + +int px4_getpid() +{ + pthread_t pid = pthread_self(); + + if (pid == _shell_task_id) + return SHELL_TASK_ID; + + // Get pthread ID from the opaque ID + for (int i=0; i +#include +#include +#include "work_lock.h" + + +extern sem_t _work_lock[]; + +void work_lock(int id) +{ + //PX4_INFO("work_lock %d", id); + sem_wait(&_work_lock[id]); +} + +void work_unlock(int id) +{ + //PX4_INFO("work_unlock %d", id); + sem_post(&_work_lock[id]); +} diff --git a/src/platforms/qurt/px4_layer/work_lock.h b/src/platforms/qurt/px4_layer/work_lock.h index f77f228b36..ad2e5b4a01 100644 --- a/src/platforms/qurt/px4_layer/work_lock.h +++ b/src/platforms/qurt/px4_layer/work_lock.h @@ -31,23 +31,13 @@ * ****************************************************************************/ -#include -#include +#ifndef _work_lock_h_ +#define _work_lock_h_ -#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]); -} +//#pragma once -inline void work_unlock(int id); -inline void work_unlock(int id) -{ - //printf("work_unlock %d\n", id); - sem_post(&_work_lock[id]); -} +void work_lock(int id); +void work_unlock(int id); +#endif // _work_lock_h_ diff --git a/src/platforms/qurt/tests/muorb/module.mk b/src/platforms/qurt/tests/muorb/module.mk new file mode 100644 index 0000000000..128d894f87 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/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 = muorb_test + +SRCS = muorb_test_main.cpp \ + muorb_test_start_qurt.cpp \ + muorb_test_example.cpp + diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp new file mode 100644 index 0000000000..fdf46c6260 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_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 "muorb_test_example.h" +#include +#include +#include + +px4::AppState MuorbTestExample::appState; + +int MuorbTestExample::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/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h new file mode 100644 index 0000000000..304b8464e0 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +class MuorbTestExample { +public: + MuorbTestExample() {}; + + ~MuorbTestExample() {}; + + int main(); + + static px4::AppState appState; /* track requests to terminate app */ +}; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp new file mode 100644 index 0000000000..2ded8976c6 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_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 muorb_test_main.cpp + * Test of Multi-uORB supoprt + * + * @author Mark Charlebois + */ +#include +#include +#include +#include "muorb_test_example.h" + +int PX4_MAIN(int argc, char **argv) +{ + px4::init(argc, argv, "muorb_test"); + + PX4_DEBUG("muorb_test"); + MuorbTestExample hello; + hello.main(); + + PX4_DEBUG("goodbye"); + return 0; +} diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp new file mode 100644 index 0000000000..f063806766 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.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 muorb_test_start_qurt.cpp + * + * @author Mark Charlebois + */ +#include "muorb_test_example.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +//using namespace px4; + +extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); + +static void usage() +{ + PX4_DEBUG("usage: muorb_test {start|stop|status}"); +} +int muorb_test_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("already running"); + /* this is not an error */ + return 0; + } + + daemon_task = px4_task_spawn_cmd("muorb_test", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char* const*)argv); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + MuorbTestExample::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (MuorbTestExample::appState.isRunning()) { + PX4_DEBUG("is running"); + + } else { + PX4_DEBUG("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 41add4973a..8889f4531f 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -100,6 +100,9 @@ usage(const char *reason) static int load(const char *devname, const char *fname) { + // sleep a while to ensure device has been set up + usleep(20000); + int dev; char buf[2048]; diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 153a16abaf..1609ddcf6a 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -213,7 +213,9 @@ do_save(const char *param_file_name) close(fd); if (result < 0) { +#ifndef __PX4_QURT (void)unlink(param_file_name); +#endif warnx("error exporting to '%s'", param_file_name); return 1; } diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index ae87cfca7a..a006284df6 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -1,18 +1,34 @@ cmake_minimum_required(VERSION 2.8) + +include( CMakeForceCompiler ) +#set( CMAKE_SYSTEM_NAME px4_posix_clang ) +CMAKE_FORCE_C_COMPILER( clang Clang ) +CMAKE_FORCE_CXX_COMPILER( clang++ Clang ) +#set( CMAKE_C_COMPILER /opt/clang-3.4.2/bin/clang ) +#set( CMAKE_CXX_COMPILER /opt/clang-3.4.2/bin/clang++ ) +#set( CMAKE_FIND_ROOT_PATH /opt/clang-3.4.2/ ) +#set( CMAKE_FIND_ROOT_PATH_MODE_PROGRAM_NEVER ) +#set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY ) +#set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY ) + project(unittests) enable_testing() + include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -fno-exceptions -fno-rtti -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ -g -Wall") else() message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=gnu99 -g") + +#set(CMAKE_INCLUDE_SYSTEM_FLAG_C "-isystem" ) +#set(CMAKE_INCLUDE_SYSTEM_FLAG_CXX "-isystem" ) set(GTEST_DIR gtest) add_subdirectory(${GTEST_DIR}) @@ -22,9 +38,14 @@ set(PX_SRC ${CMAKE_SOURCE_DIR}/../src) include_directories(${CMAKE_SOURCE_DIR}) include_directories(${PX_SRC}) include_directories(${PX_SRC}/modules) +include_directories(${PX_SRC}/modules/uORB) include_directories(${PX_SRC}/lib) include_directories(${PX_SRC}/drivers) include_directories(${PX_SRC}/platforms) +include_directories(${PX_SRC}/platforms/posix/include) +include_directories(${PX_SRC}/platforms/posix/px4_layer) +include_directories(${PX_SRC}/drivers/device ) + add_definitions(-D__EXPORT=) add_definitions(-D__PX4_TESTS) @@ -35,18 +56,51 @@ add_definitions(-DOK=0) add_definitions(-D_UNIT_TEST=) add_definitions(-D__PX4_POSIX) add_definitions(-D__PX4_LINUX) +add_definitions(-D__PX4_POSIX) # check add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) function(add_gtest) foreach(test_name ${ARGN}) - target_link_libraries(${test_name} gtest_main) + target_link_libraries(${test_name} gtest_main pthread rt ) add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) add_dependencies(unittests ${test_name}) endforeach() endfunction() +add_library( px4_platform +# ${PX_SRC}/platforms/common/px4_getopt.c + ${PX_SRC}/platforms/posix/px4_layer/px4_posix_impl.cpp + ${PX_SRC}/platforms/posix/px4_layer/px4_posix_tasks.cpp + ${PX_SRC}/platforms/posix/px4_layer/work_lock.c + ${PX_SRC}/platforms/posix/px4_layer/hrt_queue.c + ${PX_SRC}/platforms/posix/px4_layer/work_queue.c + ${PX_SRC}/platforms/posix/px4_layer/dq_rem.c + ${PX_SRC}/platforms/posix/px4_layer/sq_addlast.c + ${PX_SRC}/platforms/posix/px4_layer/lib_crc32.c + ${PX_SRC}/platforms/posix/px4_layer/sq_addafter.c + ${PX_SRC}/platforms/posix/px4_layer/queue.c + ${PX_SRC}/platforms/posix/px4_layer/work_cancel.c + ${PX_SRC}/platforms/posix/px4_layer/hrt_work_cancel.c + ${PX_SRC}/platforms/posix/px4_layer/dq_remfirst.c + ${PX_SRC}/platforms/posix/px4_layer/hrt_thread.c + ${PX_SRC}/platforms/posix/px4_layer/work_thread.c + ${PX_SRC}/platforms/posix/px4_layer/drv_hrt.c + ${PX_SRC}/platforms/posix/px4_layer/sq_remfirst.c + ${PX_SRC}/platforms/posix/px4_layer/dq_addlast.c + ${PX_SRC}/drivers/device/device_posix.cpp + ${PX_SRC}/drivers/device/vdev.cpp + ${PX_SRC}/drivers/device/vfile.cpp + ${PX_SRC}/drivers/device/vdev_posix.cpp + ${PX_SRC}/drivers/device/i2c_posix.cpp + ${PX_SRC}/drivers/device/sim.cpp + ${PX_SRC}/drivers/device/ringbuffer.cpp + ) + +#target_include_directories( px4_platform PUBLIC ${PX_SRC}/platforms ) + + # add each test add_executable(autodeclination_test autodeclination_test.cpp ${PX_SRC}/lib/geo_lookup/geo_mag_declination.c) @@ -64,6 +118,10 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c ${PX_SRC}/systemcmds/tests/test_mixer.cpp) + +target_link_libraries( mixer_test LINK_PUBLIC px4_platform ) + + add_gtest(mixer_test) # conversion_test @@ -93,4 +151,20 @@ add_executable(param_test param_test.cpp ${PX_SRC}/modules/systemlib/param/param.c ${PX_SRC}/modules/systemlib/bson/tinybson.c ) +target_link_libraries( param_test px4_platform ) + add_gtest(param_test) + +# uorb test +add_executable(uorb_tests uorb_unittests/uORBCommunicator_gtests.cpp + uorb_unittests/uORBCommunicatorMock.cpp + uorb_unittests/uORBCommunicatorMockLoopback.cpp + ${PX_SRC}/modules/uORB/uORBDevices_posix.cpp + ${PX_SRC}/modules/uORB/uORBManager_posix.cpp + ${PX_SRC}/modules/uORB/objects_common.cpp + ${PX_SRC}/modules/uORB/uORBUtils.cpp + ${PX_SRC}/modules/uORB/uORB.cpp + ) +target_link_libraries( uorb_tests px4_platform ) + +add_gtest(uorb_tests) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 41f49627d4..1c32e3bf84 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -34,10 +34,10 @@ TEST(SBUS2Test, SBUS2) uint8_t frame[30]; unsigned partial_frame_count = 0; uint16_t rc_values[18]; - uint16_t num_values; - bool sbus_failsafe; - bool sbus_frame_drop; - uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + //uint16_t num_values; + //bool sbus_failsafe; + //bool sbus_frame_drop; + //uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); float last_time = 0; @@ -59,7 +59,7 @@ TEST(SBUS2Test, SBUS2) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); //if (partial_frame_count % 25 == 0) //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.cpp b/unittests/uorb_unittests/uORBCommunicatorMock.cpp new file mode 100644 index 0000000000..d824d9da07 --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMock.cpp @@ -0,0 +1,206 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#include "uORBCommunicatorMock.hpp" +#include "uORB/uORB.h" +#include "uORBGtestTopics.hpp" +#include "px4_log.h" +#include +#include "uORBManager.hpp" + +#define LOG_TAG "uORBCommunicatorMock.cpp" + +uORB_test::uORBCommunicatorMock::uORBCommunicatorMock() +: _rx_handler( nullptr ) +{ +/* + _sub_topicA_copy_fd = orb_subscribe( ORB_ID( topicA_copy ), (void*)&_sub_semaphore ); + _sub_topicB_copy_fd = orb_subscribe( ORB_ID( topicB_copy), nullptr ); +*/ + _topic_translation_map[ "topicA" ] = "topicA_copy"; + _topic_translation_map[ "topicB" ] = "topicB_copy"; + _topic_translation_map[ "topicA_copy" ] = "topicA"; + _topic_translation_map[ "topicB_copy" ] = "topicB"; +} + +int16_t uORB_test::uORBCommunicatorMock::add_subscription +( + const char * messageName, + int32_t msgRateInHz +) +{ + + int16_t rc = 0; + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz ); + _msgCounters[messageName]._add_subscriptionCount++; + + /* + int16_t rc = -1; + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_add_subscription + ( + _topic_translation_map[messageName], + msgRateInHz + ); + } + } + */ + return rc; +} + +int16_t uORB_test::uORBCommunicatorMock::remove_subscription +( + const char * messageName +) +{ + int16_t rc = 0; + PX4_INFO( "got remove_subscription for msg[%s]", messageName ); + _msgCounters[messageName]._remove_subscriptionCount++; +/* + int16_t rc = -1; + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_remove_subscription + ( + _topic_translation_map[messageName] + ); + } + } +*/ + return rc; +} + +int16_t uORB_test::uORBCommunicatorMock::register_handler +( + uORBCommunicator::IChannelRxHandler* handler +) +{ + int16_t rc = 0; + _rx_handler = handler; + return rc; +} + + +int16_t uORB_test::uORBCommunicatorMock::send_message +( + const char * messageName, + int32_t length, + uint8_t* data +) +{ + int16_t rc = 0; + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length ); + if( uORB::Manager::get_instance()->is_remote_subscriber_present( messageName ) ) + { + _msgCounters[messageName]._send_messageCount++; + if( strcmp(messageName, "topicA") == 0 ) + { + memcpy( &_topicAData, (void*)data, length ); + } + else if( strcmp(messageName, "topicB") == 0 ) + { + memcpy( &_topicBData, (void*)data, length ); + } + else + { + //EPRINTF( "error messageName[%s] is not supported", messageName ); + } + } +/* + int16_t rc = -1; + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_received_message + ( _topic_translation_map[messageName], length, data ); + } + } +*/ + return rc; +} + + +bool uORB_test::uORBCommunicatorMock::get_remote_topicA_data +( + struct orb_topic_A* data +) +{ + bool rc = false; + memcpy( data, &_topicAData, sizeof(_topicAData) ); + rc = true; +/* + if( orb_copy(ORB_ID(topicA_copy), _sub_topicA_copy_fd, data) == OK ) + { + rc = true; + } +*/ + return rc; +} + +bool uORB_test::uORBCommunicatorMock::get_remote_topicB_data +( + struct orb_topic_B* data +) +{ + bool rc = false; + memcpy( data, &_topicBData, sizeof(_topicBData) ); + rc = true; +/* + + if( orb_copy(ORB_ID(topicB_copy), _sub_topicB_copy_fd, data) == OK ) + { + rc = true; + } +*/ + return rc; +} + +void uORB_test::uORBCommunicatorMock::reset_counters() +{ + InterfaceCounters resetCounter; + resetCounter._add_subscriptionCount = 0; + resetCounter._remove_subscriptionCount = 0; + resetCounter._send_messageCount = 0; + + std::map::iterator it; + for( it = _msgCounters.begin(); it != _msgCounters.end(); ++it ) + { + it->second = resetCounter; + } +} + +uORB_test::uORBCommunicatorMock::InterfaceCounters uORB_test::uORBCommunicatorMock::get_interface_counters(const char * messageName ) +{ + return _msgCounters[ messageName ]; +} diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp new file mode 100644 index 0000000000..e0cb3da532 --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +#ifndef _uORBCommunicatorMock_hpp_ +#define _uORBCommunicatorMock_hpp_ + +#include "uORB/uORBCommunicator.hpp" +#include "uORBGtestTopics.hpp" +#include +#include +#include + +namespace uORB_test +{ + class uORBCommunicatorMock; +} + +class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel +{ + public: + + //counters to track how many times the iterface is called from + // uorb. + typedef struct + { + int64_t _add_subscriptionCount; + int64_t _remove_subscriptionCount; + int64_t _send_messageCount; + }InterfaceCounters; + + uORBCommunicatorMock(); + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription( const char * messageName ); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ); + + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data); + + uORBCommunicator::IChannelRxHandler* get_rx_handler() + { + return _rx_handler; + } + + bool get_remote_topicA_data( struct orb_topic_A* data ); + bool get_remote_topicB_data( struct orb_topic_B* data ); + + void reset_counters(); + + InterfaceCounters get_interface_counters( const char * messageName ); + + + private: + uORBCommunicator::IChannelRxHandler* _rx_handler; + int _sub_topicA_copy_fd; + int _sub_topicB_copy_fd; + + std::map _topic_translation_map; + + struct orb_topic_A _topicAData; + struct orb_topic_B _topicBData; + + std::map _msgCounters; +}; + +#endif /* _uORBCommunicatorMock_test_hpp_ */ diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp new file mode 100644 index 0000000000..5e717bab98 --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp @@ -0,0 +1,133 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#include "uORBCommunicatorMockLoopback.hpp" +#include "uORB/uORB.h" +#include "uORBGtestTopics.hpp" +#include "uORBManager.hpp" +#include +#include "px4_log.h" + +#define LOG_TAG "uORBCommunicatorMockLoopback.cpp" + + +uORB_test::uORBCommunicatorMockLoopback::uORBCommunicatorMockLoopback() +: _rx_handler( nullptr ) +{ + //_sub_topicA_clone_fd = orb_subscribe( ORB_ID( topicA_clone ), (void*)&_sub_semaphore ); + //_sub_topicB_clone_fd = orb_subscribe( ORB_ID( topicB_clone ), nullptr ); + + _topic_translation_map[ "topicA" ] = "topicA_clone"; + _topic_translation_map[ "topicB" ] = "topicB_clone"; + _topic_translation_map[ "topicA_clone" ] = "topicA"; + _topic_translation_map[ "topicB_clone" ] = "topicB"; +} + +int16_t uORB_test::uORBCommunicatorMockLoopback::add_subscription +( + const char * messageName, + int32_t msgRateInHz +) +{ + + int16_t rc = -1; + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName, msgRateInHz ); + + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_add_subscription + ( + _topic_translation_map[messageName].c_str(), + msgRateInHz + ); + } + } + return rc; +} + +int16_t uORB_test::uORBCommunicatorMockLoopback::remove_subscription +( + const char * messageName +) +{ + int16_t rc = -1; + PX4_INFO( "got remove_subscription for msg[%s]", messageName ); + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + rc = _rx_handler->process_remove_subscription + ( + _topic_translation_map[messageName].c_str() + ); + } + } + return rc; +} + +int16_t uORB_test::uORBCommunicatorMockLoopback::register_handler +( + uORBCommunicator::IChannelRxHandler* handler +) +{ + int16_t rc = 0; + _rx_handler = handler; + return rc; +} + + +int16_t uORB_test::uORBCommunicatorMockLoopback::send_message +( + const char * messageName, + int32_t length, + uint8_t* data +) +{ + int16_t rc = -1; + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName, length ); + if( _rx_handler ) + { + if( _topic_translation_map.find( messageName ) != _topic_translation_map.end() ) + { + if( uORB::Manager::get_instance()->is_remote_subscriber_present( _topic_translation_map[messageName].c_str() ) ) + { + rc = _rx_handler->process_received_message + ( _topic_translation_map[messageName].c_str(), length, data ); + PX4_INFO( "[uORBCommunicatorMockLoopback::send_message] return from[topic(%s)] _rx_handler->process_received_message[%d]", messageName, rc ); + } + else + { + // this is eqvuilanet of not sending the message to the remote. + rc = 0; + } + } + } + return rc; +} diff --git a/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp new file mode 100644 index 0000000000..265d277d87 --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp @@ -0,0 +1,128 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#ifndef _uORBCommunicatorMockLoopback_hpp_ +#define _uORBCommunicatorMockLoopback_hpp_ + +#include "uORB/uORBCommunicator.hpp" +#include "uORBGtestTopics.hpp" +#include +#include +#include + +namespace uORB_test +{ + class uORBCommunicatorMockLoopback; +} + +class uORB_test::uORBCommunicatorMockLoopback : public uORBCommunicator::IChannel +{ + public: + + uORBCommunicatorMockLoopback(); + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription( const char * messageName, int32_t msgRateInHz ); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription( const char * messageName ); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ); + + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message( const char * messageName, int32_t length, uint8_t* data); + + uORBCommunicator::IChannelRxHandler* get_rx_handler() + { + return _rx_handler; + } + +/* + bool get_remote_topicA_data( struct orb_topic_A* data ); + bool get_remote_topicB_data( struct orb_topic_B* data ); +*/ + + + private: + uORBCommunicator::IChannelRxHandler* _rx_handler; +/* + int _sub_topicA_clone_fd; + int _sub_topicB_clone_fd; + pal::Semaphore _sub_semaphore; +*/ + + std::map _topic_translation_map; + +/* + struct orb_topic_A _topicAData; + struct orb_topic_B _topicBData; +*/ +}; + +#endif /* _uORBCommunicatorMockLoopback_hpp_ */ diff --git a/unittests/uorb_unittests/uORBCommunicator_gtests.cpp b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp new file mode 100644 index 0000000000..1ea12b7a96 --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp @@ -0,0 +1,483 @@ +/**************************************************************************** + * + * 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 "uORBCommunicatorMock.hpp" +#include "uORBCommunicatorMockLoopback.hpp" +#include "gtest/gtest.h" +#include "uORB.h" +#include "uORBManager.hpp" +#include "uORBGtestTopics.hpp" +#include "uORBDevices.hpp" +#include "px4_log.h" +#include + +#define LOG_TAG "uORBCommunicator_gtests.cpp" + +namespace px4 +{ + void init_once(); +} + +namespace uORB_test +{ + + class uORBCommunicatorTest : public ::testing::Test + { + public: + uORBCommunicatorTest() : + _masterDevice( nullptr ) + { + px4::init_once(); + + // create the Master Device and initialize it + _masterDevice = new uORB::DeviceMaster(uORB::PUBSUB); + if( _masterDevice != nullptr ) + { + _masterDevice->init(); + } + + // get the uORB::Manager and set the mock instance + // explicitly. + uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel ); + } + protected: + uORB_test::uORBCommunicatorMock _comm_channel; + struct orb_topic_A _topicA; + struct orb_topic_B _topicB; + orb_advert_t _pub_ptr; + int _sub_fd; + uORB::DeviceMaster* _masterDevice; + }; + + //================= Unit tests for add_subscription =================== + // this test will validate if the uORB calls the "add_subscription" + // method if there is atleast one subscriber in the local system. + //===================================================================== + TEST_F( uORBCommunicatorTest, add_subscription_case1 ) + { + // case 1: add subscription should not be called if there are no + // internal subscriber and only a publisher. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. check to see if add_subscription is called. + // the count should be zero. + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._add_subscriptionCount, 0 ); + + //step 1. + _topicA.val = 1; + ASSERT_NE( ( _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA) ), nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + + //step 2. + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._add_subscriptionCount, 0 ); + } + + TEST_F( uORBCommunicatorTest, add_subscription_case2 ) + { + // case 1: add subscription should not be called if there is atleast one + // internal subscriber and only a publisher. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. Add in internal subscriber. + // 3. check to see if add_subscription is called. + // the count should be 1. + // 4. unsubscribe the topic + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._add_subscriptionCount, 0 ); + + //step 1. + _topicA.val = 1; + _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.add_subscription_case2] orb_advertize(topicA) returncode:(%p)", _pub_ptr ); + ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + + // step 2 + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) ) ) , -1 ) << "Subscribe failed: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + //step 3. + c = _comm_channel.get_interface_counters( "topicA" ); + PX4_INFO( "interface counter for topicA: %d", (int)c._add_subscriptionCount ); + ASSERT_EQ( c._add_subscriptionCount, 1 ); + + //step 4. + PX4_INFO( "Before unsubscribe for Topic A" ); + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + PX4_INFO( "After unsubscribe for Topic A" ); + } + + //============ unit tests for remove_subscribtion ======= + TEST_F( uORBCommunicatorTest, remove_subscribtion ) + { + // remove subscription should be called if there after a subscription is removed. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. Add in internal subscriber. + // 3. unsubscribe the topic + // 4. check the remove_subsciption counter it should be 1. + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 0 ); + + //step 1. + // step 1. + _topicA.val = 1; + _pub_ptr = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.remove_subscribtion] orb_advertize(topicA) returncode:(%p)", _pub_ptr ); + ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 0 ); + + + // step 2 + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicA) )), -1 ); // << "Subscribe failed: " << _sub_fd << "errono: " << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 0 ); + + + //step 3. + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + + //step 4. + c = _comm_channel.get_interface_counters( "topicA" ); + ASSERT_EQ( c._remove_subscriptionCount, 1 ); + } + + //============ unit tests for send_message ======= + TEST_F(uORBCommunicatorTest, send_message_case1 ) + { + // Case1: send message should not be called when a topic is advertized + // and there are no remote remote subscribers. + // Steps: + // 0. reset the interface counters. + // 1. advertize a topic + // 2. check to see that the send message counter is 0. + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicA_sndmsg" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + //step 1. + ORB_DEFINE( topicA_sndmsg, struct orb_topic_A ); + _topicA.val = 1; + _pub_ptr = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA ); + ASSERT_TRUE( ( _pub_ptr != nullptr ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + + c = _comm_channel.get_interface_counters( "topicA_sndmsg" ); + ASSERT_EQ( c._send_messageCount, 0 ); + } + + TEST_F(uORBCommunicatorTest, send_message_case2 ) + { + // Case2: send message should be called when a topic is advertized + // and there are remote remote subscribers. + // Steps: + // 0. reset the interface counters. + // 1. Add a remote subscriber by calling the "process_add_subscription" + // 2. advertize a topic + // 3. check to see that the send message counter is 1 and check the value. + // 4. publish new data. + // 5. check to see that send_msg is incremented by 1 and check the value. + // 6. remove remote subscriber by calling the "process_remove_subscription" + // 7. publish new data. + // 8. check to see the sndmessage counter, it should not increment. + + //step 0. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + //step 1. + uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler(); + // add a local subsciber to avoid the issue of creating a node for the first time + // remote. + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB) ) ), -1 ) << "Subscribe failed for topicB: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 ); + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // step 2. + _topicB.val = 1; + _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB); + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + + //step 3. + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + struct orb_topic_B test_val; + _comm_channel.get_remote_topicB_data( &test_val ); + ASSERT_EQ( test_val.val, 1 ); + + //step 4. publish new data. + _topicB.val = 2; + ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK ); + + //step 5. + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 2 ); + + _comm_channel.get_remote_topicB_data( &test_val ); + ASSERT_EQ( test_val.val, 2 ); + + // step 6. + ASSERT_EQ( rx_handler->process_remove_subscription( "topicB" ), 0 ); + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + + //step 7. publish new data. + _topicB.val = 5; + ASSERT_EQ( orb_publish( ORB_ID(topicB), _pub_ptr, &_topicB), OK ); + + //step 8. + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 2 ); + + _comm_channel.get_remote_topicB_data( &test_val ); + ASSERT_EQ( test_val.val, 2 ); + + } + + //========== UNIT tests to verify the process_receive_message interface + //========== of rx handler. + TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case1 ) + { + // this will mimic the process of calling the process_receive_message + // from remote and verify that the local subscribers got it + // are the steps. + // 1. clear the counters. + // 2. advertize a topic + // 3. add a local subscriber. + // 4. call process_receive_message. + // 5. check to see that the subscriber got the data and the message is not sent back + // by looking at the counter for snd message. + + //step 1. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // step 2. + _topicB.val = 1; + _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + + // step 3. + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + //step 4. + uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler(); + struct orb_topic_B testVal; + testVal.val = 10; + ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 ); + + // step 5. check the values. + struct orb_topic_B receivedVal; + ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( testVal.val, receivedVal.val ) + << "copy(1) mismatch: " << testVal.val + << " expected " << receivedVal.val; + + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // cleanup. + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + } + + TEST_F( uORBCommunicatorTest, rx_handler_rcv_data_case2 ) + { + // this will mimic the process of calling the process_receive_message + // from remote and verify that the local subscribers got it + // and also the message is send back to the remote. The following + // are the steps. + // 1. clear the counters. + // 2. advertize a topic + // 3. add a local subscriber. + // 4. add remote subscriber by calling the "process_add_subscription. + // 5. call process_receive_message. + // 6. check to see that the subscriber got the data and the message is not sent back + // by looking at the counter for snd message. + + //step 1. + _comm_channel.reset_counters(); + uORB_test::uORBCommunicatorMock::InterfaceCounters c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 0 ); + + // step 2. + _topicB.val = 1; + _pub_ptr = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_ptr != nullptr ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)_pub_ptr ); + + // step 3. + ASSERT_NE( ( _sub_fd = orb_subscribe(ORB_ID(topicB)) ), -1 ) << "Subscribe failed for topicB: %d" << errno; + PX4_INFO( "subscribe fd: %d", _sub_fd ); + + //step 4. + uORBCommunicator::IChannelRxHandler* rx_handler = _comm_channel.get_rx_handler(); + ASSERT_EQ( rx_handler->process_add_subscription( "topicB", 1 ), 0 ); + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + //step 5. + struct orb_topic_B testVal; + testVal.val = 15; + ASSERT_EQ( rx_handler->process_received_message( "topicB", sizeof( testVal ), (uint8_t*)(&testVal) ), 0 ); + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + + // step 6. check the values. + struct orb_topic_B receivedVal; + ASSERT_EQ( orb_copy(ORB_ID(topicB), _sub_fd, &receivedVal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( testVal.val, receivedVal.val ) + << "copy(1) mismatch: " << testVal.val + << " expected " << receivedVal.val; + + c = _comm_channel.get_interface_counters( "topicB" ); + ASSERT_EQ( c._send_messageCount, 1 ); + + // cleanup. + ASSERT_EQ( orb_unsubscribe( _sub_fd ), OK ); + } + + TEST_F( uORBCommunicatorTest, Loopback ) + { + // create the loopback instance. + uORB_test::uORBCommunicatorMockLoopback _comm_channel_loopback; + + //intiailize the uorb to remove the node map entries; + //orb_initialize(); + + //set the communucation channel interface. + uORB::Manager::get_instance()->set_uorb_communicator( &_comm_channel_loopback ); + + // now for the actual test. + orb_advert_t pub_topicA_ptr; + int sub_topicA_fd; + int sub_topicAClone_fd; + + struct orb_topic_A topicA; + struct orb_topic_A topicAClone; + struct orb_topic_A topicALocal; + + // step 1. + topicA.val = 10; + pub_topicA_ptr = orb_advertise(ORB_ID(topicA), &topicA); + PX4_INFO( "[uORBCommunicatorTest.Loopback]orb_advertize(topicA) return code:(%p)", pub_topicA_ptr ); + ASSERT_TRUE( pub_topicA_ptr != nullptr ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08lx", (unsigned long)pub_topicA_ptr ); + + // step 2. + ASSERT_NE( ( sub_topicA_fd = orb_subscribe(ORB_ID(topicA)) ), -1 ) << "Subscribe failed: %d" << errno; + PX4_INFO( "subscribe fd: %d", sub_topicA_fd ); + + // step 3. check to see that the subscriber got a value of 10. + ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicALocal.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicALocal.val; + + // subscribe a remote subscriber. + ASSERT_NE( ( sub_topicAClone_fd = orb_subscribe(ORB_ID(topicA_clone)) ), -1 ) << "Subscribe failed: %d" << errno; + PX4_INFO( "subscribe fd: %d", sub_topicAClone_fd ); + + ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicAClone.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicAClone.val; + + // publish a new data and check to see that the data is received on the remote. + topicA.val = 15; + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK ); + + // check to see that the subscriber got a new value. + ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicALocal.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicALocal.val; + + ASSERT_EQ( orb_copy(ORB_ID(topicA_clone), sub_topicAClone_fd, &topicAClone), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicAClone.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicAClone.val; + + // remove the remote subscriber and make sure that the data is not received, + ASSERT_EQ( orb_unsubscribe( sub_topicAClone_fd ), OK ); + + // publish a new data and check to see that the data is received on local this should not crash. + topicA.val = 20; + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK ); + + // check to see that the subscriber got a new value. + ASSERT_EQ( orb_copy(ORB_ID(topicA), sub_topicA_fd, &topicALocal), OK ) << "copy(1) failed: " << errno; + ASSERT_EQ( topicA.val, topicALocal.val ) + << "copy(1) mismatch: " << topicA.val + << " expected " << topicALocal.val; + + //remove the local subscriber as well and publish new data; system should not crash. + ASSERT_EQ( orb_unsubscribe( sub_topicA_fd ), OK ); + + // publish a new data; this should not crash. + topicA.val = 25; + ASSERT_EQ( orb_publish( ORB_ID(topicA), pub_topicA_ptr, &topicA), OK ); + + } +} + + + + diff --git a/unittests/uorb_unittests/uORBGtestTopics.hpp b/unittests/uorb_unittests/uORBGtestTopics.hpp new file mode 100644 index 0000000000..94822f54d5 --- /dev/null +++ b/unittests/uorb_unittests/uORBGtestTopics.hpp @@ -0,0 +1,54 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#ifndef _uORBGtestTopics_hpp_ +#define _uORBGtestTopics_hpp_ + +#include "uORB/uORB.h" + +namespace uORB_test +{ + struct orb_topic_A + { + int16_t val; + }; + + struct orb_topic_B + { + int16_t val; + }; + + + ORB_DEFINE( topicA, struct orb_topic_A ); + ORB_DEFINE( topicB, struct orb_topic_B ); + + ORB_DEFINE( topicA_clone, struct orb_topic_A ); + ORB_DEFINE( topicB_clone, struct orb_topic_B ); +} + +#endif // _UnitTest_uORBTopics_hpp_ diff --git a/unittests/uorb_unittests/uORB_test.cpp b/unittests/uorb_unittests/uORB_test.cpp new file mode 100644 index 0000000000..43ed366df0 --- /dev/null +++ b/unittests/uorb_unittests/uORB_test.cpp @@ -0,0 +1,206 @@ +//============================================================================= +// File: uORB_test.cpp +// +// @@-COPYRIGHT-START-@@ +// +// Copyright 2014 Qualcomm Technologies, Inc. All rights reserved. +// Confidential & Proprietary - Qualcomm Technologies, Inc. ("QTI") +// +// The party receiving this software directly from QTI (the "Recipient") +// may use this software as reasonably necessary solely for the purposes +// set forth in the agreement between the Recipient and QTI (the +// "Agreement"). The software may be used in source code form solely by +// the Recipient's employees (if any) authorized by the Agreement. Unless +// expressly authorized in the Agreement, the Recipient may not sublicense, +// assign, transfer or otherwise provide the source code to any third +// party. Qualcomm Technologies, Inc. retains all ownership rights in and +// to the software +// +// This notice supersedes any other QTI notices contained within the software +// except copyright notices indicating different years of publication for +// different portions of the software. This notice does not supersede the +// application of any third party copyright notice to that third party's +// code. +// +// @@-COPYRIGHT-END-@@ +// +//============================================================================= + +#include +#include +#include +#include +#include +#include +#include +#include "uORB/uORB.h" +#include "utils/general.h" +#include "utils/logging.h" + +#include + +#define LOG_TAG "uORB_test.cpp" + +//#ifdef INCLUDE_ANDROID_UNIT_TEST_UORB +namespace uORB_test +{ + + struct orb_test + { + int val; + }; + ORB_DEFINE(orb_test, struct orb_test); + + pal::Semaphore sub_semaphore; + static int pfd, sfd; + static struct orb_test t; + bool threadTestPassed = false; + + TEST( uORBTest, pub_sub_initialization ) + { + struct orb_test u; + bool updated; + + t.val = 0; + ASSERT_NE( ( pfd = orb_advertise(ORB_ID(orb_test), &t) ), 0 ) << "Failed to advertize uORB Topic orb_test: errno: " << errno; + IPRINTF( "publist handle: 0x%08x", pfd ); + + + ASSERT_NE( ( sfd = orb_subscribe(ORB_ID(orb_test), (void *)&sub_semaphore) ), 0 ) << "Subscribe failed: %d" << errno; + IPRINTF( "subscribe fd: %d", sfd ); + + u.val = 1; + + ASSERT_EQ( orb_copy(ORB_ID(orb_test), sfd, &u), OK ) << "copy(1) failed: " << errno; + + ASSERT_EQ( u.val, t.val ) << "copy(1) mismatch: " << u.val << " expected " << t.val; + + ASSERT_EQ(orb_check(sfd, &updated), OK ) << "check(1) failed"; + + ASSERT_FALSE( updated ) << "spurious updated flag"; + + ASSERT_EQ(orb_unsubscribe(sfd), OK ); + close(pfd); + } + + + void test_subscriber_thread(void *this_ptr) + { + struct orb_test u; + bool updated; + pal::Semaphore *sub_semaphore = (pal::Semaphore *)this_ptr; + assert(sub_semaphore != nullptr); + + IPRINTF( "waiting for new subscriber data"); + sub_semaphore->wait(); + + if (OK != orb_check(sfd, &updated)) + { + EPRINTF("check(2) failed"); + return; + } + + if (!updated) + { + EPRINTF("missing updated flag"); + return; + } + + if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + { + EPRINTF("copy(2) failed: %d", errno); + return; + } + + if (u.val != t.val) + { + EPRINTF("copy(2) mismatch: %d expected %d", u.val, t.val); + return; + } + threadTestPassed = true; + } + + + TEST( uORB, pub_sub_withThread ) + { + pal::Thread sub_thread; + + threadTestPassed = false; + + // advertize the topic first if it is not created. + t.val = 1; + ASSERT_NE( (pfd = orb_advertise(ORB_ID(orb_test), &t) ), 0 ) << "Failed to advertize uORB Topic orb_test: errno: " << errno; + IPRINTF( "publist handle: 0x%08x", pfd ); + + ASSERT_NE( ( sfd = orb_subscribe(ORB_ID(orb_test), (void *)&sub_semaphore) ), 0 ) << "Subscribe failed: %d" << errno; + IPRINTF( "subscribe fd: %d", sfd ); + + sub_thread.create(test_subscriber_thread, (void *)&sub_semaphore); + + t.val = 2; + IPRINTF("try publish, creating new thread to await the results"); + + ASSERT_EQ(orb_publish(ORB_ID(orb_test), pfd, &t), OK) << "publish failed"; + + ASSERT_EQ( t.val, 2 ); + + IPRINTF("waiting for the subscriber thread to exit"); + sub_thread.wait(); + + ASSERT_TRUE( threadTestPassed ); + + ASSERT_EQ(orb_unsubscribe(sfd), OK ); + close(pfd); + } + + + +#if 0 + /* this is a hacky test that exploits the sensors app to test rate-limiting */ + + sfd = orb_subscribe(ORB_ID(sensor_combined)); + + hrt_abstime start, end; + unsigned count; + + start = hrt_absolute_time(); + count = 0; + + do + { + orb_check(sfd, &updated); + + if (updated) + { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + }while (count < 100); + + end = hrt_absolute_time(); + test_note("full-speed, 100 updates in %llu", end - start); + + orb_set_interval(sfd, 10); + + start = hrt_absolute_time(); + count = 0; + + do + { + orb_check(sfd, &updated); + + if (updated) + { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + }while (count < 100); + + end = hrt_absolute_time(); + test_note("100Hz, 100 updates in %llu", end - start); + + orb_unsubscribe(sfd); +#endif + +} +