From 851a020461c99539afa0c34d6f05e9b33ee379f7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 09:48:50 -0700 Subject: [PATCH 1/2] Eagle: posix-arm and qurt changes to support Eagle HW platform The Eagle HW platform contains both a Krait (ARMv4hf compatible) cpu cluster and a Hexagon DSP running QuRT. These changes support the PX4 build for Eagle. Signed-off-by: Mark Charlebois --- Tools/qurt_apps.py | 16 ++++ makefiles/posix-arm/config_eagle_default.mk | 15 +--- makefiles/posix-arm/config_eagle_hil.mk | 86 ++++++++++++++++++ .../posix-arm/config_eagle_muorb_test.mk | 89 +++++++++++++++++++ makefiles/posix-arm/ld.script | 46 ++++++++++ .../toolchain_gnu-arm-linux-gnueabihf.mk | 13 +-- makefiles/qurt/config_qurt_hil.mk | 81 +++++++++++++++++ makefiles/qurt/config_qurt_muorb_test.mk | 79 ++++++++++++++++ makefiles/qurt/qurt_elf.mk | 6 +- makefiles/qurt/toolchain_hexagon.mk | 52 ++++++----- 10 files changed, 444 insertions(+), 39 deletions(-) create mode 100644 makefiles/posix-arm/config_eagle_hil.mk create mode 100644 makefiles/posix-arm/config_eagle_muorb_test.mk create mode 100644 makefiles/posix-arm/ld.script create mode 100644 makefiles/qurt/config_qurt_hil.mk create mode 100644 makefiles/qurt/config_qurt_muorb_test.mk diff --git a/Tools/qurt_apps.py b/Tools/qurt_apps.py index e5d75344be..b1f606c37a 100755 --- a/Tools/qurt_apps.py +++ b/Tools/qurt_apps.py @@ -48,6 +48,8 @@ print """ #include #include +#include +#include using namespace std; @@ -64,6 +66,7 @@ static int list_tasks_main(int argc, char *argv[]); static int list_files_main(int argc, char *argv[]); static int list_devices_main(int argc, char *argv[]); static int list_topics_main(int argc, char *argv[]); +static int sleep_main(int argc, char *argv[]); } @@ -78,6 +81,7 @@ print '\tapps["list_tasks"] = list_tasks_main;' print '\tapps["list_files"] = list_files_main;' print '\tapps["list_devices"] = list_devices_main;' print '\tapps["list_topics"] = list_topics_main;' +print '\tapps["sleep"] = sleep_main;' print """ } @@ -117,5 +121,17 @@ static int list_files_main(int argc, char *argv[]) px4_show_files(); return 0; } +static int sleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + PX4_WARN( "Usage: sleep " ); + return 1; + } + + unsigned long usecs = ( (unsigned long) atol( argv[1] ) ) * 1000 * 1000; + PX4_WARN("Sleeping for %s, %ld",argv[1],usecs); + usleep( usecs ); + return 0; +} """ diff --git a/makefiles/posix-arm/config_eagle_default.mk b/makefiles/posix-arm/config_eagle_default.mk index d66cc5ed73..90afa155c0 100644 --- a/makefiles/posix-arm/config_eagle_default.mk +++ b/makefiles/posix-arm/config_eagle_default.mk @@ -18,7 +18,8 @@ MODULES += modules/sensors # MODULES += systemcmds/param MODULES += systemcmds/mixer -MODULES += systemcmds/topic_listener +MODULES += systemcmds/ver +#MODULES += systemcmds/topic_listener # # General system control @@ -34,7 +35,7 @@ MODULES += modules/ekf_att_pos_estimator # # Vehicle Control # -MODULES += modules/navigator +#MODULES += modules/navigator MODULES += modules/mc_pos_control MODULES += modules/mc_att_control @@ -48,7 +49,7 @@ MODULES += modules/dataman MODULES += modules/sdlog2 MODULES += modules/simulator MODULES += modules/commander -MODULES += modules/controllib +#MODULES += modules/controllib # # Libraries @@ -58,18 +59,10 @@ MODULES += lib/mathlib/math/filter MODULES += lib/geo MODULES += lib/geo_lookup MODULES += lib/conversion - # # Linux port # MODULES += platforms/posix/px4_layer -MODULES += platforms/posix/drivers/accelsim -MODULES += platforms/posix/drivers/gyrosim -MODULES += platforms/posix/drivers/adcsim -MODULES += platforms/posix/drivers/barosim -MODULES += platforms/posix/drivers/tonealrmsim -MODULES += platforms/posix/drivers/airspeedsim -MODULES += platforms/posix/drivers/gpssim # # Unit tests diff --git a/makefiles/posix-arm/config_eagle_hil.mk b/makefiles/posix-arm/config_eagle_hil.mk new file mode 100644 index 0000000000..c70ef198a8 --- /dev/null +++ b/makefiles/posix-arm/config_eagle_hil.mk @@ -0,0 +1,86 @@ +# +# Makefile for the POSIXTEST *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/rgbled +MODULES += drivers/led +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param +#MODULES += systemcmds/mixer +#MODULES += systemcmds/topic_listener +MODULES += systemcmds/ver + +# +# General system control +# +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/navigator +#MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +MODULES += modules/dataman +MODULES += modules/sdlog2 +MODULES += modules/simulator +MODULES += modules/commander +#MODULES += modules/controllib + +# +# Libraries +# +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/geo +MODULES += lib/geo_lookup +MODULES += lib/conversion + +# +# Linux port +# +MODULES += platforms/posix/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim +#MODULES += platforms/posix/drivers/tonealrmsim +#MODULES += platforms/posix/drivers/airspeedsim +#MODULES += platforms/posix/drivers/gpssim + +# +# Unit tests +# +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + +# +# muorb fastrpc changes. +# +MODULES += modules/muorb/krait diff --git a/makefiles/posix-arm/config_eagle_muorb_test.mk b/makefiles/posix-arm/config_eagle_muorb_test.mk new file mode 100644 index 0000000000..ec080c9195 --- /dev/null +++ b/makefiles/posix-arm/config_eagle_muorb_test.mk @@ -0,0 +1,89 @@ +# +# Makefile for the POSIXTEST *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/rgbled +#MODULES += drivers/led +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +#MODULES += systemcmds/param +#MODULES += systemcmds/mixer +#MODULES += systemcmds/topic_listener + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/navigator +#MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control + +# +# Library modules +# +#MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander +#MODULES += modules/controllib + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# Linux port +# +MODULES += platforms/posix/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim +#MODULES += platforms/posix/drivers/tonealrmsim +#MODULES += platforms/posix/drivers/airspeedsim +#MODULES += platforms/posix/drivers/gpssim + +# +# muorb fastrpc changes. +# +MODULES += modules/muorb/krait + + +# +# +# Unit tests +# +#MODULES += platforms/posix/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue +MODULES += platforms/posix/tests/muorb + diff --git a/makefiles/posix-arm/ld.script b/makefiles/posix-arm/ld.script new file mode 100644 index 0000000000..32478e1e14 --- /dev/null +++ b/makefiles/posix-arm/ld.script @@ -0,0 +1,46 @@ +/**************************************************************************** + * ld.script + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Author: Mark Charlebois + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +SECTIONS +{ + /* + * Construction data for parameters. + */ + __param : ALIGN(8) { + __param_start = .; + KEEP(*(__param*)) + __param_end = .; + } +} diff --git a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk index a5964e07d9..2811512ecd 100644 --- a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk +++ b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk @@ -57,6 +57,7 @@ ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) $(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif +EXT_MUORB_LIB_ROOT = /opt/muorb_libs # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup @@ -156,12 +157,11 @@ ARCHWARNINGS = -Wall \ -Werror=reorder \ -Werror=uninitialized \ -Werror=init-self \ - -Wno-error=logical-op \ - -Wdouble-promotion \ + -Wno-error=logical-op \ -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ - -Werror=double-promotion \ + -Wno-error=double-promotion \ -fno-strength-reduce \ -Wno-error=unused-value @@ -188,9 +188,11 @@ 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 += -lpx4muorb -ladsprpc EXTRA_LIBS += -pthread -lm -lrt +LIB_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/libs + # Flags we pass to the C compiler # CFLAGS = $(ARCHCFLAGS) \ @@ -226,7 +228,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ $(EXTRADEFINES) \ $(EXTRAAFLAGS) -LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script +LDSCRIPT = $(PX4_BASE)/makefiles/posix-arm/ld.script # Flags we pass to the linker # LDFLAGS += $(EXTRALDFLAGS) \ @@ -323,6 +325,7 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) + echo "$(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC)" $(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) endef diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk new file mode 100644 index 0000000000..9d4a48eeed --- /dev/null +++ b/makefiles/qurt/config_qurt_hil.mk @@ -0,0 +1,81 @@ +# +# 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 +MODULES += systemcmds/mixer + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_q +MODULES += modules/position_estimator_inav + +# +# Vehicle Control +# +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_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 += modules/muorb/adsp/ diff --git a/makefiles/qurt/config_qurt_muorb_test.mk b/makefiles/qurt/config_qurt_muorb_test.mk new file mode 100644 index 0000000000..b503e44a69 --- /dev/null +++ b/makefiles/qurt/config_qurt_muorb_test.mk @@ -0,0 +1,79 @@ +# +# 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 +MODULES += systemcmds/mixer + +# +# 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 +MODULES += modules/mc_pos_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 += modules/muorb/adsp/ diff --git a/makefiles/qurt/qurt_elf.mk b/makefiles/qurt/qurt_elf.mk index 281a1603d2..73f1cd4ece 100644 --- a/makefiles/qurt/qurt_elf.mk +++ b/makefiles/qurt/qurt_elf.mk @@ -41,12 +41,12 @@ # What we're going to build. # -EXTRALDFLAGS = -Wl,-soname=libdspal_client.so +EXTRALDFLAGS = -Wl,-soname=libpx4.so PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a PRODUCT_SHARED_PRELINK = $(WORK_DIR)firmware.o .PHONY: firmware -firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)libdspal_client.so $(WORK_DIR)mainapp +firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)libpx4.so $(WORK_DIR)mainapp # # Built product rules @@ -65,7 +65,7 @@ $(WORK_DIR)apps.o: $(WORK_DIR)apps.cpp $(call COMPILEXX,$<, $@) mv $(WORK_DIR)apps.cpp $(WORK_DIR)apps.cpp_sav -$(WORK_DIR)libdspal_client.so: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB) +$(WORK_DIR)libpx4.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 diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 8b5bf5423e..d8e6427e4e 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -37,7 +37,8 @@ # Toolchain commands. Normally only used inside this file. # -HEXAGON_TOOLS_ROOT = /opt/6.4.05 +HEXAGON_TOOLS_ROOT = /opt/6.4.03 +#HEXAGON_TOOLS_ROOT = /opt/6.4.05 HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 V_ARCH = v5 CROSSDEV = hexagon- @@ -84,7 +85,8 @@ DYNAMIC_LIBS = \ # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 6.4.05 +CROSSDEV_VER_SUPPORTED = 6.4.03 +#CROSSDEV_VER_SUPPORTED = 6.4.05 CROSSDEV_VER_FOUND = $(shell $(CC) --version | sed -n 's/^.*version \([\. 0-9]*\),.*$$/\1/p') ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) @@ -94,7 +96,7 @@ endif # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup -MAXOPTIMIZATION ?= -O0 +MAXOPTIMIZATION := -O0 # Base CPU flags for each of the supported architectures. # @@ -108,31 +110,38 @@ $(error Board config does not define CONFIG_BOARD) endif ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -D__PX4_QURT -D__PX4_POSIX \ - -D__EXPORT= \ - -D__QDSP6_DINKUM_PTHREAD_TYPES__ \ + -D_PID_T -D_UID_T -D_TIMER_T\ -Dnoreturn_function= \ + -D__EXPORT= \ -Drestrict= \ + -D_DEBUG \ + -I$(PX4_BASE)/../dspal/include \ + -I$(PX4_BASE)/../dspal/sys \ -I$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/include \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ - -I$(PX4_BASE)/../dspal/include \ - -I$(PX4_BASE)/../dspal/sys \ -I$(PX4_BASE)/mavlink/include/mavlink \ -I$(QURTLIB)/..//include \ -I$(HEXAGON_SDK_ROOT)/inc \ -I$(HEXAGON_SDK_ROOT)/inc/stddef \ -Wno-error=shadow + + # optimisation flags # -ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g3 \ +ARCHOPTIMIZATION = \ + -O0 \ + -g \ -fno-strict-aliasing \ - -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -ffunction-sections \ -fdata-sections \ - -fpic + -fpic \ + -fno-zero-initialized-in-bss + +#-fomit-frame-pointer \ +#-funsafe-math-optimizations \ +#-ffunction-sections +#$(MAXOPTIMIZATION) # enable precise stack overflow tracking # note - requires corresponding support in NuttX @@ -140,7 +149,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # -ARCHCFLAGS = -std=gnu99 +ARCHCFLAGS = -std=gnu99 -D__CUSTOM_FILE_IO__ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__ # Generic warnings @@ -186,9 +195,9 @@ CFLAGS = $(ARCHCFLAGS) \ $(ARCHDEFINES) \ $(EXTRADEFINES) \ $(EXTRACFLAGS) \ - -fno-common \ $(addprefix -I,$(INCLUDE_DIRS)) + #-fno-common # Flags we pass to the C++ compiler # CXXFLAGS = $(ARCHCXXFLAGS) \ @@ -212,8 +221,7 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \ LDSCRIPT = $(PX4_BASE)/makefiles/posix/ld.script # Flags we pass to the linker # -LDFLAGS += -g -mv5 -nostdlib -mG0lib -G0 -fpic -shared \ - -nostartfiles \ +LDFLAGS += -g -mv5 -mG0lib -G0 -fpic -shared \ -Wl,-Bsymbolic \ -Wl,--wrap=malloc \ -Wl,--wrap=calloc \ @@ -254,7 +262,9 @@ 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 + #$(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 + #$(CCACHE) $(CC) -MD -c $(CFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" -fPIC $(abspath $1) -o $2 + $(CCACHE) $(CC) -c $(CFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" $(abspath $1) -o $2 endef # Compile C++ source $1 to $2 for use in shared library @@ -264,7 +274,9 @@ 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) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 + #$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2 + #$(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" -fPIC $(abspath $1) -o $2 + $(CCACHE) $(CXX) -c $(CXXFLAGS) -D__V_DYNAMIC__ -D__FILENAME__=\"$(notdir $1)\" $(abspath $1) -o $2 endef # Assemble $1 into $2 @@ -319,7 +331,7 @@ endef define LINK_SO @$(ECHO) "LINK_SO: $1" @$(MKDIR) -p $(dir $1) - $(HEXAGON_GCC) $(LDFLAGS) -fPIC -shared -nostartfiles -o $1 -Wl,--whole-archive $2 -Wl,--no-whole-archive $(LIBS) $(DYNAMIC_LIBS) + $(HEXAGON_GCC) $(LDFLAGS) -o $1 -Wl,--whole-archive $2 -Wl,--no-whole-archive $(LIBS) $(DYNAMIC_LIBS) endef # Link the objects in $2 into the application $1 From 60ec1c897a5aeb55b5613fe8fefb83343e769142 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 11:12:50 -0700 Subject: [PATCH 2/2] QuRT: Added muorb files muorb is used to proxy messages between the Krait and DSP. Signed-off-by: Mark Charlebois --- src/lib/version/version.h | 3 + src/modules/muorb/adsp/module.mk | 50 +++ src/modules/muorb/adsp/muorb_fastrpc.cpp | 157 +++++++++ src/modules/muorb/adsp/muorb_fastrpc.h | 55 +++ src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 318 ++++++++++++++++++ src/modules/muorb/adsp/uORBFastRpcChannel.hpp | 251 ++++++++++++++ src/modules/muorb/krait/module.mk | 49 +++ src/modules/muorb/krait/muorb_main.cpp | 84 +++++ .../muorb/krait/uORBKraitFastRpcChannel.cpp | 190 +++++++++++ .../muorb/krait/uORBKraitFastRpcChannel.hpp | 144 ++++++++ 10 files changed, 1301 insertions(+) create mode 100644 src/modules/muorb/adsp/module.mk create mode 100644 src/modules/muorb/adsp/muorb_fastrpc.cpp create mode 100644 src/modules/muorb/adsp/muorb_fastrpc.h create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.cpp create mode 100644 src/modules/muorb/adsp/uORBFastRpcChannel.hpp create mode 100644 src/modules/muorb/krait/module.mk create mode 100644 src/modules/muorb/krait/muorb_main.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp create mode 100644 src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 9d7e471adc..b71ba1d0d9 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -62,4 +62,7 @@ #ifdef CONFIG_ARCH_BOARD_SITL #define HW_ARCH "LINUXTEST" #endif +#ifdef CONFIG_ARCH_BOARD_EAGLE +#define HW_ARCH "LINUXTEST" +#endif #endif /* VERSION_H_ */ diff --git a/src/modules/muorb/adsp/module.mk b/src/modules/muorb/adsp/module.mk new file mode 100644 index 0000000000..65d0e8e555 --- /dev/null +++ b/src/modules/muorb/adsp/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build muorb +# + + +ifeq ($(PX4_TARGET_OS),qurt) + +SRCS = \ + muorb_fastrpc.cpp \ + uORBFastRpcChannel.cpp + +INCLUDE_DIRS += \ + ${PX4_BASE}/src/modules/uORB + +endif + +MAXOPTIMIZATION = -Os diff --git a/src/modules/muorb/adsp/muorb_fastrpc.cpp b/src/modules/muorb/adsp/muorb_fastrpc.cpp new file mode 100644 index 0000000000..febe578258 --- /dev/null +++ b/src/modules/muorb/adsp/muorb_fastrpc.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include "muorb_fastrpc.h" +#include "qurt.h" +#include "uORBFastRpcChannel.hpp" +#include "uORBManager.hpp" + +#include +#include +#include +#include +#include +#include "px4_log.h" +#include "uORB/topics/sensor_combined.h" +#include "uORB.h" + +#define _ENABLE_MUORB 1 + +__BEGIN_DECLS + +int dspal_main(int argc, const char *argv[]); + +__END_DECLS + + +int muorb_fastrpc_orb_initialize() +{ + int rc = 0; + PX4_WARN("Before calling dspal_entry() method..."); + // registere the fastrpc muorb with uORBManager. + uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance()); + const char *argv[2] = { "dspal", "start" }; + int argc = 2; + dspal_main(argc, argv); + PX4_WARN("After calling dspal_entry"); + return rc; +} + +int muorb_fastrpc_add_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->AddRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_add_subscription(name, 0); + + if (rc != OK) { + channel->RemoveRemoteSubscriber(name); + } + + } else { + rc = -1; + } + + return rc; +} + +int muorb_fastrpc_remove_subscriber(const char *name) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + channel->RemoveRemoteSubscriber(name); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_remove_subscription(name); + + } else { + rc = -1; + } + + return rc; + +} + +int muorb_fastrpc_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler(); + + if (rxHandler != nullptr) { + rc = rxHandler->process_received_message(name, data_len_in_bytes, (uint8_t *)data); + + } else { + rc = -1; + } + + return rc; +} + +int muorb_fastrpc_is_subscriber_present(const char *topic_name, int *status) +{ + int rc = 0; + int32_t local_status = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->is_subscriber_present(topic_name, &local_status); + + if (rc == 0) { + *status = (int)local_status; + } + + return rc; +} + +int muorb_fastrpc_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) +{ + int rc = 0; + int32_t local_msg_type = 0; + int32_t local_bytes_returned = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data, data_len_in_bytes, &local_bytes_returned); + *msg_type = (int)local_msg_type; + *bytes_returned = (int)local_bytes_returned; + return rc; +} + +int muorb_fastrpc_unblock_recieve_msg(void) +{ + int rc = 0; + uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance(); + rc = channel->unblock_get_data_method(); + return rc; +} diff --git a/src/modules/muorb/adsp/muorb_fastrpc.h b/src/modules/muorb/adsp/muorb_fastrpc.h new file mode 100644 index 0000000000..96eda20d35 --- /dev/null +++ b/src/modules/muorb/adsp/muorb_fastrpc.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + +#include +#include + +__BEGIN_DECLS + +int muorb_fastrpc_orb_initialize() __EXPORT; + +int muorb_fastrpc_add_subscriber(const char *name) __EXPORT; + +int muorb_fastrpc_remove_subscriber(const char *name) __EXPORT; + +int muorb_fastrpc_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + +int muorb_fastrpc_is_subscriber_present(const char *topic_name, int *status) __EXPORT; + +int muorb_fastrpc_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) __EXPORT; + +int muorb_fastrpc_unblock_recieve_msg(void) __EXPORT; + +__END_DECLS diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp new file mode 100644 index 0000000000..87c2bba57d --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -0,0 +1,318 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include "uORBFastRpcChannel.hpp" +#include "px4_log.h" +#include + +// static intialization. +uORB::FastRpcChannel uORB::FastRpcChannel::_Instance; + +//============================================================================== +//============================================================================== +uORB::FastRpcChannel::FastRpcChannel() + : _RxHandler(0) + , _DataQInIndex(0) + , _DataQOutIndex(0) + , _ControlQInIndex(0) + , _ControlQOutIndex(0) +{ + for (int32_t i = 0; i < _MAX_MSG_QUEUE_SIZE; ++ i) { + _DataMsgQueue[i]._MaxBufferSize = 0; + _DataMsgQueue[i]._Length = 0; + _DataMsgQueue[i]._Buffer = 0; + } + + _RemoteSubscribers.clear(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + _Subscribers.push_back(messageName); + PX4_DEBUG("Adding message[%s] to subscriber queue...", messageName); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + _Subscribers.remove(messageName); + + return rc; +} + +int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status) +{ + int16_t rc = 0; + + if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) { + *status = 1; + PX4_DEBUG("******* Found subscriber for message[%s]....", messageName); + + } else { + *status = 0; + PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size()); + int i = 0; + + for (std::list::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) { + if (*it == messageName) { + PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i); + } + + ++i; + } + } + + return rc; +} + +int16_t uORB::FastRpcChannel::unblock_get_data_method() +{ + PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()"); + _DataAvailableSemaphore.post(); + return 0; +} +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + + if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) { + //there is no-remote subscriber. So do not queue the message. + return rc; + } + + _QueueMutex.lock(); + bool overwriteData = false; + + if (IsDataQFull()) { + // queue is full. Overwrite the oldest data. + PX4_WARN("[send_message] Queue Full Overwrite the oldest data. in[%ld] out[%ld] max[%ld]", + _DataQInIndex, _DataQOutIndex, _MAX_MSG_QUEUE_SIZE); + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + overwriteData = true; + } + + // now check to see if the data queue's buffer size if large enough to memcpy the data. + // if not, delete the old buffer and re-create a new buffer of larger size. + check_and_expand_data_buffer(_DataQInIndex, length); + + // now memcpy the data to the buffer. + memcpy(_DataMsgQueue[ _DataQInIndex ]._Buffer, data, length); + _DataMsgQueue[ _DataQInIndex ]._Length = length; + _DataMsgQueue[ _DataQInIndex ]._MsgName = messageName; + + _DataQInIndex++; + + if (_DataQInIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQInIndex = 0; + } + + // the assumption here is that each caller reads only one data from either control or data queue. + if (!overwriteData) { + _DataAvailableSemaphore.post(); + } + + _QueueMutex.unlock(); + return rc; +} + +//============================================================================== +//============================================================================== +void uORB::FastRpcChannel::check_and_expand_data_buffer(int32_t index, int32_t length) +{ + if (_DataMsgQueue[ index ]._MaxBufferSize < length) { + // create a new buffer of size length and delete old buffer. + if (_DataMsgQueue[ index ]._Buffer != 0) { + delete _DataMsgQueue[ index ]._Buffer; + } + + _DataMsgQueue[ index ]._Buffer = new uint8_t[ length ]; + + if (_DataMsgQueue[ index ]._Buffer == 0) { + PX4_ERR("Error[check_and_expand_data_buffer] Failed to allocate data queue buffer of size[%ld]", length); + _DataMsgQueue[ index ]._MaxBufferSize = 0; + return; + } + + _DataMsgQueue[ index ]._MaxBufferSize = length; + } +} + +int32_t uORB::FastRpcChannel::DataQSize() +{ + int32_t rc; + rc = (_DataQInIndex - _DataQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +int32_t uORB::FastRpcChannel::ControlQSize() +{ + int32_t rc; + rc = (_ControlQInIndex - _ControlQOutIndex) + _MAX_MSG_QUEUE_SIZE; + rc %= _MAX_MSG_QUEUE_SIZE; + return rc; +} + +bool uORB::FastRpcChannel::IsControlQFull() +{ + return (ControlQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsControlQEmpty() +{ + return (ControlQSize() == 0); +} + +bool uORB::FastRpcChannel::IsDataQFull() +{ + return (DataQSize() == (_MAX_MSG_QUEUE_SIZE - 1)); +} + +bool uORB::FastRpcChannel::IsDataQEmpty() +{ + return (DataQSize() == 0); +} + +int16_t uORB::FastRpcChannel::get_data +( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned +) +{ + int16_t rc = 0; + // wait for data availability + _DataAvailableSemaphore.wait(); + _QueueMutex.lock(); + + if (DataQSize() != 0 || ControlQSize() != 0) { + if (ControlQSize() > 0) { + // read the first element of the Control Queue. + *msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type; + + if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) { + memcpy + ( + topic_name, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(), + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + + topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = 0; + + _ControlQOutIndex++; + + if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _ControlQOutIndex = 0; + } + + } else { + PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]", + topic_name_len, + _ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() + ); + rc = -1; + } + + } else { + // read the first element of the Control Queue. + *msg_type = _DATA_MSG_TYPE; + + if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) || + (_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) { + memcpy + ( + topic_name, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(), + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + + topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0; + + *bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length; + memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length); + + _DataQOutIndex++; + + if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) { + _DataQOutIndex = 0; + } + + } else { + PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ", + topic_name_len, + _DataMsgQueue[ _DataQOutIndex ]._MsgName.size() + ); + PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ", + data_len_in_bytes, + _DataMsgQueue[ _DataQOutIndex ]._Length + ); + + rc = -1; + } + } + + } else { + PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues"); + rc = -1; + } + + _QueueMutex.unlock(); + return rc; +} diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.hpp b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp new file mode 100644 index 0000000000..0446fb1772 --- /dev/null +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.hpp @@ -0,0 +1,251 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#ifndef _uORBFastRpcChannel_hpp_ +#define _uORBFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include +#include + +namespace uORB +{ +class FastRpcChannel; +} + +class uORB::FastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::FastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + //Function to return the data to krait. + int16_t get_data + ( + int32_t *msg_type, + char *topic_name, + int32_t topic_name_len, + uint8_t *data, + int32_t data_len_in_bytes, + int32_t *bytes_returned + ); + + // function to check if there are subscribers for a topic on adsp. + int16_t is_subscriber_present(const char *messageName, int32_t *status); + + // function to release the blocking semaphore for get_data method. + int16_t unblock_get_data_method(); + + uORBCommunicator::IChannelRxHandler *GetRxHandler() + { + return _RxHandler; + } + + void AddRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.insert(messageName); + } + void RemoveRemoteSubscriber(const std::string &messageName) + { + _RemoteSubscribers.erase(messageName); + } + +private: // data members + static uORB::FastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + + /// data structure to store the messages to be retrived by Krait. + static const int32_t _MAX_MSG_QUEUE_SIZE = 100; + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + struct FastRpcDataMsg { + int32_t _MaxBufferSize; + int32_t _Length; + uint8_t *_Buffer; + std::string _MsgName; + }; + + struct FastRpcControlMsg { + int32_t _Type; + std::string _MsgName; + }; + + struct FastRpcDataMsg _DataMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _DataQInIndex; + int32_t _DataQOutIndex; + + struct FastRpcControlMsg _ControlMsgQueue[ _MAX_MSG_QUEUE_SIZE ]; + int32_t _ControlQInIndex; + int32_t _ControlQOutIndex; + + std::list _Subscribers; + + //utility classes + class Mutex + { + public: + Mutex() + { + sem_init(&_Sem, 0, 1); + } + ~Mutex() + { + sem_destroy(&_Sem); + } + void lock() + { + sem_wait(&_Sem); + } + void unlock() + { + sem_post(&_Sem); + } + private: + sem_t _Sem; + + Mutex(const Mutex &); + + Mutex &operator=(const Mutex &); + }; + + class Semaphore + { + public: + Semaphore() + { + sem_init(&_Sem, 0, 0); + } + ~Semaphore() + { + sem_destroy(&_Sem); + } + void post() + { + sem_post(&_Sem); + } + void wait() + { + sem_wait(&_Sem); + } + private: + sem_t _Sem; + Semaphore(const Semaphore &); + Semaphore &operator=(const Semaphore &); + + }; + + Mutex _QueueMutex; + Semaphore _DataAvailableSemaphore; + +private://class members. + /// constructor. + FastRpcChannel(); + + void check_and_expand_data_buffer(int32_t index, int32_t length); + + bool IsControlQFull(); + bool IsControlQEmpty(); + bool IsDataQFull(); + bool IsDataQEmpty(); + int32_t DataQSize(); + int32_t ControlQSize(); + + std::set _RemoteSubscribers; +}; + +#endif /* _uORBFastRpcChannel_hpp_ */ diff --git a/src/modules/muorb/krait/module.mk b/src/modules/muorb/krait/module.mk new file mode 100644 index 0000000000..c628b53f96 --- /dev/null +++ b/src/modules/muorb/krait/module.mk @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build uORB +# + +MODULE_COMMAND = muorb + +ifeq ($(PX4_TARGET_OS),posix-arm) +SRCS = uORBKraitFastRpcChannel.cpp \ + muorb_main.cpp +INCLUDE_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/include \ + $(PX4_BASE)/src/modules/uORB \ + $(PX4_BASE)/src/modules + +EXTRA_LIBS += $(EXT_MUORB_LIB_ROOT)/krait/libs/libmuorb.so +endif + diff --git a/src/modules/muorb/krait/muorb_main.cpp b/src/modules/muorb/krait/muorb_main.cpp new file mode 100644 index 0000000000..3a320e6134 --- /dev/null +++ b/src/modules/muorb/krait/muorb_main.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "uORBManager.hpp" +#include "uORBKraitFastRpcChannel.hpp" + +extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } + +static void usage() +{ + warnx("Usage: muorb 'start', 'stop', 'status'"); +} + + +int +muorb_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -EINVAL; + } + + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + // 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(); + return OK; + + } + + if (!strcmp(argv[1], "stop")) { + + uORB::KraitFastRpcChannel::GetInstance()->Stop(); + return OK; + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + return OK; + } + + usage(); + return -EINVAL; +} diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp new file mode 100644 index 0000000000..3900a38c94 --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "uORBKraitFastRpcChannel.hpp" +#include "px4_log.h" + + +#define LOG_TAG "uORBKraitFastRpcChannel.cpp" + +// static intialization. +uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance; + +//============================================================================== +//============================================================================== +uORB::KraitFastRpcChannel::KraitFastRpcChannel() + : _RxHandler(nullptr) + , _ThreadStarted(false) + , _ShouldExit(false) +{ + _KraitWrapper.Initialize(); +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz) +{ + int16_t rc = 0; + // invoke fast_rpc call. From Idl. + PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.AddSubscriber(messageName); + PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName) +{ + int16_t rc = 0; + // invoke the fast_rpc call defined in idl. + PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName); + rc = _KraitWrapper.RemoveSubscriber(messageName); + PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc); + return rc; +} + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler) +{ + _RxHandler = handler; + return 0; +} + + +//============================================================================== +//============================================================================== +int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data) +{ + int16_t rc = 0; + // invoke the fast rpc call to send data defined in idl. + //PX4_DEBUG( "Before calling send_data for [%s] len[%d]\n", messageName.c_str(), length ); + int32_t status = 0; + + if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) { + if (status > 0) { // there are remote subscribers + rc = _KraitWrapper.SendData(messageName, length, data); + //PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() ); + + } else { + //PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() ); + } + } else { + PX4_ERR("Error returned for KraitWrapper.IsSubscriberPresent(%s)\n", messageName); + } + + //PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc ); + return rc; +} + +void uORB::KraitFastRpcChannel::Start() +{ + _ThreadStarted = true; + pthread_create(&_RecvThread, NULL, thread_start, this); +} + +void uORB::KraitFastRpcChannel::Stop() +{ + _ShouldExit = true; + _KraitWrapper.UnblockReceiveData(); + PX4_DEBUG("After calling krait_wrapper_unlock_receive_Data...\n"); + pthread_join(_RecvThread, NULL); + PX4_DEBUG("*** After calling thread wait...\n"); + _ThreadStarted = false; + _ShouldExit = false; +} + + +void uORB::KraitFastRpcChannel::thread_start(void *handler) +{ + if (handler != nullptr) { + ((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread(); + } +} + +void uORB::KraitFastRpcChannel::fastrpc_recv_thread() +{ + // sit in while loop. + int32_t rc = 0; + int32_t type = 0; + char *name = nullptr; + int32_t data_length = 0; + uint8_t *data = nullptr; + + while (!_ShouldExit) { + // call the fastrpc recv data call. + //uorb_fastrpc_recieve( &type, &name_len, name, &data_length, data ); + rc = _KraitWrapper.ReceiveData(&type, &name, &data_length, &data); + + if (rc == 0) { + switch (type) { + case _CONTROL_MSG_TYPE_ADD_SUBSCRIBER: + if (_RxHandler != nullptr) { + _RxHandler->process_add_subscription(name, 1); + PX4_DEBUG("Received add subscriber control message for: [%s]\n", name); + } + + break; + + case _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER: + if (_RxHandler != nullptr) { + _RxHandler->process_remove_subscription(name); + PX4_DEBUG("Received remove subscriber control message for: [%s]\n", name); + } + + break; + + case _DATA_MSG_TYPE: + if (_RxHandler != nullptr) { + _RxHandler->process_received_message(name, + data_length, data); + //PX4_DEBUG( "Received topic data for control message for: [%s] len[%d]\n", name, data_length ); + } + + break; + + default: + // error condition. + break; + } + + } else { + PX4_DEBUG("Error: Getting data over fastRPC channel\n"); + break; + } + } + + PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n"); +} + diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp new file mode 100644 index 0000000000..5c61f1e961 --- /dev/null +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef _uORBKraitFastRpcChannel_hpp_ +#define _uORBKraitFastRpcChannel_hpp_ + +#include +#include +#include +#include "uORB/uORBCommunicator.hpp" +#include "muorbKraitFastRpcWrapper.hpp" + +namespace uORB +{ +class KraitFastRpcChannel; +} + +class uORB::KraitFastRpcChannel : public uORBCommunicator::IChannel +{ +public: + /** + * static method to get the IChannel Implementor. + */ + static uORB::KraitFastRpcChannel *GetInstance() + { + return &(_Instance); + } + + /** + * @brief Interface to notify the remote entity of interest of a + * subscription for a message. + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param msgRate + * The max rate at which the subscriber can accept the messages. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz); + + + /** + * @brief Interface to notify the remote entity of removal of a subscription + * + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not necessarily mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t remove_subscription(const char *messageName); + + /** + * Register Message Handler. This is internal for the IChannel implementer* + */ + virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler); + + + //========================================================================= + // INTERFACES FOR Data messages + //========================================================================= + + /** + * @brief Sends the data message over the communication link. + * @param messageName + * This represents the uORB message name; This message name should be + * globally unique. + * @param length + * The length of the data buffer to be sent. + * @param data + * The actual data to be sent. + * @return + * 0 = success; This means the messages is successfully sent to the receiver + * Note: This does not mean that the receiver as received it. + * otherwise = failure. + */ + virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data); + + + void Start(); + void Stop(); + +private: // data members + static uORB::KraitFastRpcChannel _Instance; + uORBCommunicator::IChannelRxHandler *_RxHandler; + pthread_t _RecvThread; + bool _ThreadStarted; + bool _ShouldExit; + + static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1; + static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2; + static const int32_t _DATA_MSG_TYPE = 3; + + muorb::KraitFastRpcWrapper _KraitWrapper; + + + +private://class members. + /// constructor. + KraitFastRpcChannel(); + + static void thread_start(void *handler); + + void fastrpc_recv_thread(); + +}; + +#endif /* _uORBKraitFastRpcChannel_hpp_ */