yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Multi-uORB support changes - part 1

Browse Source 
This adds support for a dynamic build for QuRT and initial
Multi-uORB changes to enable communication between the DSP and
the application processor.

This part of the changes do not affect the POSIX build. This is
enablement for the QuRT build using Multi-uORB. The second part
of the changes will be added in a new module under src/modules.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-06-04 16:10:20 -07:00
parent 99c066c39c
commit 6cb26de74c
52 changed files with 3387 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
makefiles/posix/DISABLE_config_posix_muorb_test.mk Normal file
View File

@@ -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

4
makefiles/posix/config_posix_default.mk
View File

@@ -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

77
makefiles/qurt/DISABLE_config_qurt_muorb_test.mk Normal file
View File

@@ -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/

6
makefiles/qurt/config_qurt_default.mk
View File

@@ -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/

12
makefiles/qurt_elf.mk
View File

@@ -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,$@, $^)
#

9
makefiles/toolchain_gnu-arm-eabi.mk
View File

@@ -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

52
makefiles/toolchain_hexagon.mk
View File

@@ -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

1
makefiles/toolchain_native.mk
View File

@@ -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

17
src/drivers/device/device_posix.cpp
View File

@@ -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);
}
}

13
src/drivers/device/i2c_posix.cpp
View File

@@ -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

6
src/modules/uORB/module.mk
View File

@@ -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 \

167
src/modules/uORB/uORBCommunicator.hpp Normal file
View File

@@ -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 <stdint.h>
#include <string>
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_ */

116
src/modules/uORB/uORBDevices_nuttx.cpp
View File

@@ -36,10 +36,15 @@
#include <fcntl.h>
#include <errno.h>
#include <nuttx/arch.h>
#include <algorithm>
#include "uORBDevices_nuttx.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "uORBCommunicator.hpp"
#include <stdlib.h>
std::map<std::string, uORB::DeviceNode*> 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;
}

48
src/modules/uORB/uORBDevices_nuttx.hpp
View File

@@ -35,6 +35,8 @@
#define _uORBDevices_nuttx_hpp_
#include <stdint.h>
#include <string>
#include <map>
#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<std::string, uORB::DeviceNode*> _node_map;
};

116
src/modules/uORB/uORBDevices_posix.cpp
View File

@@ -35,11 +35,16 @@
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <algorithm>
#include "uORBDevices_posix.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "uORBCommunicator.hpp"
#include <stdlib.h>
std::map<std::string, uORB::DeviceNode*> 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;
}

52
src/modules/uORB/uORBDevices_posix.hpp
View File

@@ -35,9 +35,10 @@
#define _uORBDevices_posix_hpp_
#include <stdint.h>
#include <string>
#include <map>
#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<std::string, uORB::DeviceNode*> _node_map;
};
#endif /* _uORBDeviceNode_posix.hpp */

7
src/modules/uORB/uORBMain.cpp
View File

@@ -34,9 +34,12 @@
#include <string.h>
#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<struct orb_test>(ORB_ID(orb_test), true);
}
}
#endif
/*
* Print driver information.

76
src/modules/uORB/uORBManager.hpp
View File

@@ -35,7 +35,13 @@
#define _uORBManager_hpp_
#include "uORBCommon.hpp"
#include "uORBDevices.hpp"
#include <map>
#include <string>
#include <stdint.h>
#include <set>
#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<std::string> _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_ */

102
src/modules/uORB/uORBManager_nuttx.cpp
View File

@@ -39,7 +39,6 @@
#include <errno.h>
#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() );
}

102
src/modules/uORB/uORBManager_posix.cpp
View File

@@ -39,6 +39,7 @@
#include <px4_posix.h>
#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() );
}

18
src/modules/uORB/uORBUtils.cpp
View File

@@ -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;
}

7
src/modules/uORB/uORBUtils.hpp
View File

@@ -34,6 +34,7 @@
#define _uORBUtils_hpp_
#include "uORBCommon.hpp"
#include <string>
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_

1
src/platforms/posix/px4_layer/module.mk
View File

@@ -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 \

19
src/platforms/posix/px4_layer/work_lock.c Normal file
View File

@@ -0,0 +1,19 @@
//#pragma once
#include <semaphore.h>
#include <stdio.h>
#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]);
}

22
src/platforms/posix/px4_layer/work_lock.h
View File

@@ -31,23 +31,13 @@
*
****************************************************************************/
#include <semaphore.h>
#include <stdio.h>
#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_

47
src/platforms/posix/tests/muorb/module.mk Normal file
View File

@@ -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

118
src/platforms/posix/tests/muorb/muorb_test_example.cpp Normal file
View File

@@ -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 <charlebm@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
#include <px4_defines.h>
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;
}

59
src/platforms/posix/tests/muorb/muorb_test_example.h Normal file
View File

@@ -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 <charlebm@gmail.com>
*/
#pragma once
#include <px4_app.h>
#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;
};

66
src/platforms/posix/tests/muorb/muorb_test_main.cpp Normal file
View File

@@ -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 <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_log.h>
#include <px4_app.h>
#include "muorb_test_example.h"
#include <stdio.h>
#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;
}

101
src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp Normal file
View File

@@ -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 <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
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;
}

18
src/platforms/px4_log.h
View File

@@ -52,11 +52,21 @@
}
#if defined(__PX4_QURT)
#include <stdio.h>
#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 <stdio.h>

32
src/platforms/qurt/dspal/dspal_stub.c Normal file
View File

@@ -0,0 +1,32 @@
#include <stdio.h>
#include <dlfcn.h>
#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;
}

47
src/platforms/qurt/px4_layer/commands_muorb_test.c Normal file
View File

@@ -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 <charlebm@gmail.com>
*/
const char *get_commands()
{
static const char *commands =
"uorb start\n"
"muorb_test start\n";
return commands;
}

13
src/platforms/qurt/px4_layer/main.cpp
View File

@@ -39,6 +39,7 @@
#include <px4_middleware.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <px4_posix.h>
#include <vector>
#include <string>
@@ -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<string,px4_main_t> 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

7
src/platforms/qurt/px4_layer/module.mk
View File

@@ -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

5
src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
View File

@@ -51,6 +51,7 @@
#include <qurt.h>
#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();

57
src/platforms/qurt/px4_layer/qurt_stubs.c Normal file
View File

@@ -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);
}
//}

19
src/platforms/qurt/px4_layer/work_lock.c Normal file
View File

@@ -0,0 +1,19 @@
//#pragma once
#include <semaphore.h>
#include <stdio.h>
#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]);
}

22
src/platforms/qurt/px4_layer/work_lock.h
View File

@@ -31,23 +31,13 @@
*
****************************************************************************/
#include <semaphore.h>
#include <stdio.h>
#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_

43
src/platforms/qurt/tests/muorb/module.mk Normal file
View File

@@ -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

60
src/platforms/qurt/tests/muorb/muorb_test_example.cpp Normal file
View File

@@ -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 <charlebm@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
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;
}

53
src/platforms/qurt/tests/muorb/muorb_test_example.h Normal file
View File

@@ -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 <charlebm@gmail.com>
*/
#pragma once
#include <px4_app.h>
class MuorbTestExample {
public:
MuorbTestExample() {};
~MuorbTestExample() {};
int main();
static px4::AppState appState; /* track requests to terminate app */
};

56
src/platforms/qurt/tests/muorb/muorb_test_main.cpp Normal file
View File

@@ -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 <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_log.h>
#include <px4_app.h>
#include "muorb_test_example.h"
#include <stdio.h>
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "muorb_test");
PX4_DEBUG("muorb_test");
MuorbTestExample hello;
hello.main();
PX4_DEBUG("goodbye");
return 0;
}

101
src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp Normal file
View File

@@ -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 <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
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;
}

79
unittests/CMakeLists.txt
View File

@@ -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)

206
unittests/uorb_unittests/uORBCommunicatorMock.cpp Normal file
View File

@@ -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 <string.h>
#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<std::string, InterfaceCounters>::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 ];
}

135
unittests/uorb_unittests/uORBCommunicatorMock.hpp Normal file
View File

@@ -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 <map>
#include <set>
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<std::string, std::string> _topic_translation_map;
struct orb_topic_A _topicAData;
struct orb_topic_B _topicBData;
std::map<std::string, InterfaceCounters> _msgCounters;
};
#endif /* _uORBCommunicatorMock_test_hpp_ */

133
unittests/uorb_unittests/uORBCommunicatorMockLoopback.cpp Normal file
View File

@@ -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 <string.h>
#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;
}

127
unittests/uorb_unittests/uORBCommunicatorMockLoopback.hpp Normal file
View File

@@ -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 <map>
#include <set>
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<std::string, std::string> _topic_translation_map;
/*
struct orb_topic_A _topicAData;
struct orb_topic_B _topicBData;
*/
};
#endif /* _uORBCommunicatorMockLoopback_hpp_ */

481
unittests/uorb_unittests/uORBCommunicator_gtests.cpp Normal file
View File

@@ -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 <errno.h>
#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 );
}
}

54
unittests/uorb_unittests/uORBGtestTopics.hpp Normal file
View File

@@ -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_

206
unittests/uorb_unittests/uORB_test.cpp Normal file
View File

@@ -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 <unistd.h>
#include <stdarg.h>
#include <assert.h>
#include <string>
#include <palSemaphore.hpp>
#include <palThread.hpp>
#include <errno.h>
#include "uORB/uORB.h"
#include "utils/general.h"
#include "utils/logging.h"
#include <gtest/gtest.h>
#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
}