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 + +} +