Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge pull request #2503 from mcharleb/posix-arm-updates

Browse Source 
Eagle: posix-arm and qurt changes to support Eagle HW platform
This commit is contained in:
Lorenz Meier
2015-07-01 21:09:49 +02:00
parent 9c7450248f 60ec1c897a
commit c63f5f0486
20 changed files with 1745 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/qurt_apps.py
+16
View File
@@ -48,6 +48,8 @@ print """
#include <px4_tasks.h>
#include <px4_posix.h>
#include <px4_log.h>
#include <stdlib.h>
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 <seconds>" );
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;
}
"""
makefiles/posix-arm/config_eagle_default.mk
+4 -11
View File
@@ -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
makefiles/posix-arm/config_eagle_hil.mk
+86
View File
@@ -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
makefiles/posix-arm/config_eagle_muorb_test.mk
+89
View File
@@ -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
makefiles/posix-arm/ld.script
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
* ld.script
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
* Author: Mark Charlebois <charlebm@gmail.com>
*
* 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 = .;
}
}
makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk
+8 -5
View File
@@ -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
makefiles/qurt/config_qurt_hil.mk
+81
View File
@@ -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/
makefiles/qurt/config_qurt_muorb_test.mk
+79
View File
@@ -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/
makefiles/qurt/qurt_elf.mk
+3 -3
View File
@@ -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
makefiles/qurt/toolchain_hexagon.mk
+32 -20
View File
@@ -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
src/lib/version/version.h
+3
View File
@@ -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_ */
src/modules/muorb/adsp/module.mk
+50
View File
@@ -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
src/modules/muorb/adsp/muorb_fastrpc.cpp
+157
View File
@@ -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 <px4_middleware.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <map>
#include <string>
#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;
}
src/modules/muorb/adsp/muorb_fastrpc.h
+55
View File
@@ -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 <systemlib/visibility.h>
#include <stdint.h>
__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
src/modules/muorb/adsp/uORBFastRpcChannel.cpp
+318
View File
@@ -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 <algorithm>
// 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<std::string>::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;
}
src/modules/muorb/adsp/uORBFastRpcChannel.hpp
+251
View File
@@ -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 <stdint.h>
#include <string>
#include <list>
#include "uORB/uORBCommunicator.hpp"
#include <semaphore.h>
#include <set>
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<std::string> _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<std::string> _RemoteSubscribers;
};
#endif /* _uORBFastRpcChannel_hpp_ */
src/modules/muorb/krait/module.mk
+49
View File
@@ -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
src/modules/muorb/krait/muorb_main.cpp
+84
View File
@@ -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 <string.h>
#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;
}
src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp
+190
View File
@@ -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");
}
src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp
+144
View File
@@ -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 <stdint.h>
#include <string>
#include <pthread.h>
#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_ */