From 6cb26de74c5a08d9a6112121f88e343ab6311d5d Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 4 Jun 2015 16:10:20 -0700 Subject: [PATCH] Multi-uORB support changes - part 1 This adds support for a dynamic build for QuRT and initial Multi-uORB changes to enable communication between the DSP and the application processor. This part of the changes do not affect the POSIX build. This is enablement for the QuRT build using Multi-uORB. The second part of the changes will be added in a new module under src/modules. Signed-off-by: Mark Charlebois --- .../posix/DISABLE_config_posix_muorb_test.mk | 78 +++ makefiles/posix/config_posix_default.mk | 4 + .../qurt/DISABLE_config_qurt_muorb_test.mk | 77 +++ makefiles/qurt/config_qurt_default.mk | 6 +- makefiles/qurt_elf.mk | 12 +- makefiles/toolchain_gnu-arm-eabi.mk | 9 +- makefiles/toolchain_hexagon.mk | 52 +- makefiles/toolchain_native.mk | 1 + src/drivers/device/device_posix.cpp | 17 +- src/drivers/device/i2c_posix.cpp | 13 +- src/modules/uORB/module.mk | 6 +- src/modules/uORB/uORBCommunicator.hpp | 167 ++++++ src/modules/uORB/uORBDevices_nuttx.cpp | 116 ++++- src/modules/uORB/uORBDevices_nuttx.hpp | 48 ++ src/modules/uORB/uORBDevices_posix.cpp | 116 ++++- src/modules/uORB/uORBDevices_posix.hpp | 52 +- src/modules/uORB/uORBMain.cpp | 7 +- src/modules/uORB/uORBManager.hpp | 76 ++- src/modules/uORB/uORBManager_nuttx.cpp | 102 +++- src/modules/uORB/uORBManager_posix.cpp | 102 ++++ src/modules/uORB/uORBUtils.cpp | 18 + src/modules/uORB/uORBUtils.hpp | 7 + src/platforms/posix/px4_layer/module.mk | 1 + src/platforms/posix/px4_layer/work_lock.c | 19 + src/platforms/posix/px4_layer/work_lock.h | 22 +- src/platforms/posix/tests/muorb/module.mk | 47 ++ .../posix/tests/muorb/muorb_test_example.cpp | 118 +++++ .../posix/tests/muorb/muorb_test_example.h | 59 +++ .../posix/tests/muorb/muorb_test_main.cpp | 66 +++ .../tests/muorb/muorb_test_start_posix.cpp | 101 ++++ src/platforms/px4_log.h | 18 +- src/platforms/qurt/dspal/dspal_stub.c | 32 ++ .../qurt/px4_layer/commands_muorb_test.c | 47 ++ src/platforms/qurt/px4_layer/main.cpp | 13 +- src/platforms/qurt/px4_layer/module.mk | 7 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 5 +- src/platforms/qurt/px4_layer/qurt_stubs.c | 57 +++ src/platforms/qurt/px4_layer/work_lock.c | 19 + src/platforms/qurt/px4_layer/work_lock.h | 22 +- src/platforms/qurt/tests/muorb/module.mk | 43 ++ .../qurt/tests/muorb/muorb_test_example.cpp | 60 +++ .../qurt/tests/muorb/muorb_test_example.h | 53 ++ .../qurt/tests/muorb/muorb_test_main.cpp | 56 ++ .../tests/muorb/muorb_test_start_qurt.cpp | 101 ++++ unittests/CMakeLists.txt | 79 ++- .../uorb_unittests/uORBCommunicatorMock.cpp | 206 ++++++++ .../uorb_unittests/uORBCommunicatorMock.hpp | 135 +++++ .../uORBCommunicatorMockLoopback.cpp | 133 +++++ .../uORBCommunicatorMockLoopback.hpp | 127 +++++ .../uORBCommunicator_gtests.cpp | 481 ++++++++++++++++++ unittests/uorb_unittests/uORBGtestTopics.hpp | 54 ++ unittests/uorb_unittests/uORB_test.cpp | 206 ++++++++ 52 files changed, 3387 insertions(+), 86 deletions(-) create mode 100644 makefiles/posix/DISABLE_config_posix_muorb_test.mk create mode 100644 makefiles/qurt/DISABLE_config_qurt_muorb_test.mk create mode 100644 src/modules/uORB/uORBCommunicator.hpp create mode 100644 src/platforms/posix/px4_layer/work_lock.c create mode 100644 src/platforms/posix/tests/muorb/module.mk create mode 100644 src/platforms/posix/tests/muorb/muorb_test_example.cpp create mode 100644 src/platforms/posix/tests/muorb/muorb_test_example.h create mode 100644 src/platforms/posix/tests/muorb/muorb_test_main.cpp create mode 100644 src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp create mode 100644 src/platforms/qurt/dspal/dspal_stub.c create mode 100644 src/platforms/qurt/px4_layer/commands_muorb_test.c create mode 100644 src/platforms/qurt/px4_layer/qurt_stubs.c create mode 100644 src/platforms/qurt/px4_layer/work_lock.c create mode 100644 src/platforms/qurt/tests/muorb/module.mk create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_example.cpp create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_example.h create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_main.cpp create mode 100644 src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.cpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMock.hpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp create mode 100644 unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp create mode 100644 unittests/uorb_unittests/uORBCommunicator_gtests.cpp create mode 100644 unittests/uorb_unittests/uORBGtestTopics.hpp create mode 100644 unittests/uorb_unittests/uORB_test.cpp 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/config_posix_default.mk b/makefiles/posix/config_posix_default.mk index a675e0644a..30dada7bbd 100644 --- a/makefiles/posix/config_posix_default.mk +++ b/makefiles/posix/config_posix_default.mk @@ -79,3 +79,7 @@ MODULES += platforms/posix/drivers/gpssim #MODULES += platforms/posix/tests/hrt_test #MODULES += platforms/posix/tests/wqueue +# +# muorb fastrpc changes. +# +#MODULES += $(PX4_BASE)../muorb_krait 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/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/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/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 6c4e64f54c..143b15536c 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -125,6 +125,9 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # note - requires corresponding support in NuttX INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +LIBSTDCXX := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libstdc++.a) +LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a) + # Language-specific flags # ARCHCFLAGS = -std=gnu99 @@ -262,7 +265,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 @@ -278,7 +282,8 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group + $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group + #$(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group endef # Convert $1 from a linked object to a raw binary in $2 diff --git a/makefiles/toolchain_hexagon.mk b/makefiles/toolchain_hexagon.mk index 7135158aa8..4574574999 100644 --- a/makefiles/toolchain_hexagon.mk +++ b/makefiles/toolchain_hexagon.mk @@ -38,7 +38,8 @@ # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT = /opt/6.4.05 -HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 +#HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 +HEXAGON_SDK_ROOT = /prj/atlanticus/users/rkintada/Hexagon_SDK/2.0 #V_ARCH = v4 V_ARCH = v5 CROSSDEV = hexagon- @@ -60,8 +61,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 +80,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 +100,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 +121,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 +133,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 +205,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 +214,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 +242,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 +321,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 +329,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/toolchain_native.mk b/makefiles/toolchain_native.mk index 0b8223bfd0..fb730cd9fa 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/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/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..ddf7a0db4c 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -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/modules/uORB/module.mk b/src/modules/uORB/module.mk index 31a2b24e4a..bb6d4ea98b 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -46,9 +46,13 @@ 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 + 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..ad407bd7d8 --- /dev/null +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * 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 +#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 std::string& 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 std::string& 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 std::string& 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 std::string& 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 std::string& 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 std::string& 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..319f724a67 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 +std::map 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[std::string(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 std::string& nodepath ) +{ + uORB::DeviceNode* rc = nullptr; + if( _node_map.find( nodepath ) != _node_map.end() ) + { + rc = _node_map[nodepath]; + } + return rc; +} + diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index 3d8c0f90ad..e26598531a 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -35,6 +35,8 @@ #define _uORBDevices_nuttx_hpp_ #include +#include +#include #include "uORBCommon.hpp" @@ -119,6 +121,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 +186,9 @@ private: // private class methods. return sd; } + bool _IsRemoteSubscriberPresent; + int32_t _subscriber_count; + /** * Perform a deferred update for a rate-limited subscriber. */ @@ -166,6 +208,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 +226,11 @@ public: DeviceMaster(Flavor f); virtual ~DeviceMaster(); + static uORB::DeviceNode* GetDeviceNode( const std::string& node_name ); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: Flavor _flavor; + static std::map _node_map; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index 9035a32bdc..037ff395b3 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; @@ -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; @@ -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,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath ) +{ + uORB::DeviceNode* rc = nullptr; + if( _node_map.find( nodepath ) != _node_map.end() ) + { + rc = _node_map[nodepath]; + } + return rc; +} diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index ce22b84321..11e333d83f 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 std::string& 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..89b9d7c13f 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -35,7 +35,13 @@ #define _uORBManager_hpp_ #include "uORBCommon.hpp" +#include "uORBDevices.hpp" +#include +#include #include +#include + +#include "uORBCommunicator.hpp" namespace uORB { @@ -47,7 +53,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 +287,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 std::string& messageName ); + private: // class methods /** * Advertise a node; don't consider it an error if the node has @@ -316,10 +343,57 @@ class uORB::Manager private: // data members static Manager _Instance; + // the communicator channel instance. + uORBCommunicator::IChannel* _comm_channel; + std::set _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 std::string& 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 std::string& 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 std::string& 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..3a188c1646 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 std::string& messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName.c_str() ); + 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.c_str() ); + } + else{ + // node is present. + node->process_add_subscription(msgRateInHz); + } + } else { + rc = -1; + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const std::string& messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName.c_str() ); + 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.c_str()); + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const std::string& messageName, + int32_t length, uint8_t* data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + + 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.c_str(), nodepath ); + + } else { + // node is present. + node->process_received_message( length, data ); + rc = 0; + } + } + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +{ + return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); +} diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 21187bf359..1954de2784 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 std::string& messageName, + int32_t msgRateInHz) +{ + warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", + __LINE__, messageName.c_str() ); + 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.c_str() ); + } + else{ + // node is present. + node->process_add_subscription(msgRateInHz); + } + } else { + rc = -1; + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_remove_subscription( + const std::string& messageName) +{ + warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", + __LINE__, messageName.c_str() ); + 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.c_str()); + } else { + // node is present. + node->process_remove_subscription(); + rc = 0; + } + } + return rc; +} + +//----------------------------------------------------------------------------- +//----------------------------------------------------------------------------- +int16_t uORB::Manager::process_received_message(const std::string& messageName, + int32_t length, uint8_t* data) +{ + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + + 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.c_str(), nodepath ); + + } else { + // node is present. + node->process_received_message( length, data ); + rc = 0; + } + } + return rc; +} + +bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +{ + return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); +} diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index c9d49222bc..3bedc5ecd9 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 std::string& orbMsgName ) +{ + unsigned len; + + unsigned index = 0; + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", + orbMsgName.c_str(), 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..f9df35eee8 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 std::string& orbMsgName); + }; #endif // _uORBUtils_hpp_ 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/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_log.h b/src/platforms/px4_log.h index a0d7f07bce..2adfd40f9f 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -52,11 +52,21 @@ } #if defined(__PX4_QURT) #include +#include "HAP_farf.h" -#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_DEBUG(...) __px4_log("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(...) FARF(HIGH, __VA_ARGS__); +#define PX4_INFO(...) FARF(HIGH, __VA_ARGS__); +#define PX4_WARN(...) FARF(HIGH, __VA_ARGS__); +#define PX4_ERR(...) FARF(HIGH, __VA_ARGS__); + +//#define PX4_INFO(...) __px4_log_omit("INFO", __VA_ARGS__); +//#define PX4_WARN(...) __px4_log_omit("WARN", __VA_ARGS__); +//#define PX4_ERR(...) __px4_log_omit("ERROR", __VA_ARGS__); #elif defined(__PX4_LINUX) #include diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c new file mode 100644 index 0000000000..311a352909 --- /dev/null +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -0,0 +1,32 @@ +#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/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..aa1a3b7d61 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -42,6 +42,7 @@ SRCS = \ px4_qurt_tasks.cpp \ work_thread.c \ work_queue.c \ + work_lock.c \ work_cancel.c \ lib_crc32.c \ drv_hrt.c \ @@ -52,12 +53,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..0987c6feb1 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -51,6 +51,7 @@ #include #include "systemlib/param/param.h" + __BEGIN_DECLS // FIXME - sysconf(_SC_CLK_TCK) not supported @@ -79,7 +80,7 @@ void qurt_log(const char *fmt, ...) } #endif -extern int _posix_init(void); +//extern int _posix_init(void); __END_DECLS @@ -94,7 +95,7 @@ void init_once(void); void init_once(void) { // Required for QuRT - _posix_init(); + //_posix_init(); work_queues_init(); hrt_init(); diff --git a/src/platforms/qurt/px4_layer/qurt_stubs.c b/src/platforms/qurt/px4_layer/qurt_stubs.c new file mode 100644 index 0000000000..67955e5121 --- /dev/null +++ b/src/platforms/qurt/px4_layer/qurt_stubs.c @@ -0,0 +1,57 @@ + +//extern "C" { + +void _Read_uleb( void ) +{ +} + +void _Parse_fde_instr( void ) +{ +} + +void _Parse_csd( void ) +{ +} + +void _Locksyslock( void ) +{ +} + +void _Unlocksyslock( void ) +{ +} + +void _Valbytes( void ) +{ +} + +void _Get_eh_data( void ) +{ +} + +void _Parse_lsda( void ) +{ +} + +void __cxa_guard_release( void ) +{ +} + +void _Read_enc_ptr( void ) +{ +} + +void _Read_sleb( void ) +{ +} + +void __cxa_guard_acquire( void ) +{ +} + +void __cxa_pure_virtual() +{ + while (1); +} + +//} diff --git a/src/platforms/qurt/px4_layer/work_lock.c b/src/platforms/qurt/px4_layer/work_lock.c new file mode 100644 index 0000000000..b2ad307d7c --- /dev/null +++ b/src/platforms/qurt/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/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..d330233b37 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_example.h + * Example app for Linux + * + * @author Mark Charlebois + */ +#pragma once + +#include + +class 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..5e3ad9bb22 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_main.cpp + * Example for Linux + * + * @author Mark Charlebois + */ +#include +#include +#include +#include "muorb_test_example.h" +#include + +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..943605f531 --- /dev/null +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hello_start_linux.cpp + * + * @author Thomas Gubler + * @author Mark Charlebois + */ +#include "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/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index ae87cfca7a..810e0a2600 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,48 @@ 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/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/dq_remfirst.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 +115,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 +148,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/uorb_unittests/uORBCommunicatorMock.cpp b/unittests/uorb_unittests/uORBCommunicatorMock.cpp new file mode 100644 index 0000000000..118bd95202 --- /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 std::string& messageName, + int32_t msgRateInHz +) +{ + + int16_t rc = 0; + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName.c_str(), 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 std::string& messageName +) +{ + int16_t rc = 0; + PX4_INFO( "got remove_subscription for msg[%s]", messageName.c_str() ); + _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 std::string& messageName, + int32_t length, + uint8_t* data +) +{ + int16_t rc = 0; + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName.c_str(), length ); + if( uORB::Manager::get_instance()->is_remote_subscriber_present( messageName ) ) + { + _msgCounters[messageName]._send_messageCount++; + if( messageName == "topicA" ) + { + memcpy( &_topicAData, (void*)data, length ); + } + else if( messageName == "topicB" ) + { + memcpy( &_topicBData, (void*)data, length ); + } + else + { + //EPRINTF( "error messageName[%s] is not supported", messageName.c_str() ); + } + } +/* + 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 std::string& messageName ) +{ + return _msgCounters[ messageName ]; +} diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp new file mode 100644 index 0000000000..9279d6446d --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -0,0 +1,135 @@ +//============================================================================= +// 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 _uORBCommunicatorMock_hpp_ +#define _uORBCommunicatorMock_hpp_ + +#include "uORB/uORBCommunicator.hpp" +#include "uORBGtestTopics.hpp" +#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 std::string& 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 std::string& 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 std::string& 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 std::string& 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..e52d1d58f8 --- /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 std::string& messageName, + int32_t msgRateInHz +) +{ + + int16_t rc = -1; + PX4_INFO( "got add_subscription for msg[%s] rate[%d]", messageName.c_str(), msgRateInHz ); + + 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::uORBCommunicatorMockLoopback::remove_subscription +( + const std::string& messageName +) +{ + int16_t rc = -1; + PX4_INFO( "got remove_subscription for msg[%s]", messageName.c_str() ); + 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::uORBCommunicatorMockLoopback::register_handler +( + uORBCommunicator::IChannelRxHandler* handler +) +{ + int16_t rc = 0; + _rx_handler = handler; + return rc; +} + + +int16_t uORB_test::uORBCommunicatorMockLoopback::send_message +( + const std::string& messageName, + int32_t length, + uint8_t* data +) +{ + int16_t rc = -1; + PX4_INFO( "send_message for msg[%s] datalen[%d]", messageName.c_str(), 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] ) ) + { + rc = _rx_handler->process_received_message + ( _topic_translation_map[messageName], length, data ); + PX4_INFO( "[uORBCommunicatorMockLoopback::send_message] return from[topic(%s)] _rx_handler->process_received_message[%d]", messageName.c_str(), 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..2caef3977e --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp @@ -0,0 +1,127 @@ +//============================================================================= +// 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 + +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 std::string& 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 std::string& 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 std::string& 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..86c5cdc90b --- /dev/null +++ b/unittests/uorb_unittests/uORBCommunicator_gtests.cpp @@ -0,0 +1,481 @@ +//============================================================================= +// 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 "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; + int _pub_fd; + 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. + // step 1. + _topicA.val = 1; + ASSERT_NE( ( _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA) ), 0 ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + //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. + // step 1. + _topicA.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.add_subscription_case2] orb_advertize(topicA) returncode:(%d)", _pub_fd ); + ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + // 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_fd = orb_advertise(ORB_ID(topicA), &_topicA); + PX4_INFO( "[uORBCommunicatorTest.remove_subscribtion] orb_advertize(topicA) returncode:(%d)", _pub_fd ); + ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + 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. + // step 1. + ORB_DEFINE( topicA_sndmsg, struct orb_topic_A ); + _topicA.val = 1; + _pub_fd = orb_advertise(ORB_ID(topicA_sndmsg ), &_topicA ); + ASSERT_TRUE( ( _pub_fd != -1 && _pub_fd != 0 ) ) << "Failed to advertize uORB Topic topicA_sndmsg: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + 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_fd = orb_advertise(ORB_ID(topicB), &_topicB); + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "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_fd, &_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_fd, &_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_fd = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + // 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_fd = orb_advertise(ORB_ID(topicB), &_topicB); + ASSERT_TRUE( _pub_fd != -1 && _pub_fd != 0 ) << "Failed to advertize uORB Topic topicB: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", _pub_fd ); + + // 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. + int pub_topicA_fd; + 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_fd = orb_advertise(ORB_ID(topicA), &topicA); + PX4_INFO( "[uORBCommunicatorTest.Loopback]orb_advertize(topicA) return code:(%d)", pub_topicA_fd ); + ASSERT_TRUE( pub_topicA_fd != -1 && pub_topicA_fd != 0 ) << "Failed to advertize uORB Topic orb_topic_A: errno: " << errno; + PX4_INFO( "publist handle: 0x%08x", pub_topicA_fd ); + + // 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_fd, &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_fd, &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_fd, &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 + +} +