dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'bringup-m1' of https://github.com/mcharleb/Firmware into bringup-m2 

Conflicts:
	Tools/posix_apps.py

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-06-11 20:38:47 -07:00
parent 6ba0f246f0 fb402bc096
commit aaac4708a5
110 changed files with 6502 additions and 581 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -49,4 +49,5 @@ unittests/build
*.pretty
xcode
src/platforms/posix/px4_messages/
src/platforms/posix-arm/px4_messages/
src/platforms/qurt/px4_messages/

7
Makefile
View File

@ -33,7 +33,7 @@
# Top-level Makefile for building PX4 firmware images.
#
TARGETS := nuttx posix qurt
TARGETS := nuttx posix posix-arm qurt
EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS))
ifneq ($(EXPLICIT_TARGET),)
export PX4_TARGET_OS=$(EXPLICIT_TARGET)
@ -241,6 +241,9 @@ endif
ifeq ($(PX4_TARGET_OS),posix)
include $(PX4_BASE)makefiles/firmware_posix.mk
endif
ifeq ($(PX4_TARGET_OS),posix-arm)
include $(PX4_BASE)makefiles/firmware_posix.mk
endif
ifeq ($(PX4_TARGET_OS),qurt)
include $(PX4_BASE)makefiles/firmware_qurt.mk
endif
@ -285,7 +288,7 @@ testbuild:
$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
$(Q) (zip -r Firmware.zip $(PX4_BASE)/Images)
nuttx posix qurt:
nuttx posix posix-arm qurt:
ifeq ($(GOALS),)
make PX4_TARGET_OS=$@ $(GOALS)
else

12
Tools/posix_apps.py
View File

@ -47,6 +47,7 @@ print("""
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdlib.h>
using namespace std;
@ -63,6 +64,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[]);
}
@ -80,6 +82,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("""
return apps;
}
@ -121,5 +124,14 @@ 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) {
cout << "Usage: sleep <seconds>" << endl;
return 1;
}
sleep(atoi(argv[1]));
return 0;
}
""")

3
makefiles/firmware.mk
View File

@ -548,6 +548,9 @@ endif
ifeq ($(PX4_TARGET_OS),posix)
include $(MK_DIR)/posix_elf.mk
endif
ifeq ($(PX4_TARGET_OS),posix-arm)
include $(MK_DIR)/posix_elf.mk
endif
ifeq ($(PX4_TARGET_OS),qurt)
include $(MK_DIR)/qurt_elf.mk
endif

2
makefiles/nuttx/board_aerocore.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = CORTEXM4F
CONFIG_BOARD = AEROCORE
include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk

2
makefiles/nuttx/board_px4-stm32f4discovery.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = CORTEXM4F
CONFIG_BOARD = PX4_STM32F4DISCOVERY
include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk

2
makefiles/nuttx/board_px4fmu-v1.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = CORTEXM4F
CONFIG_BOARD = PX4FMU_V1
include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk

2
makefiles/nuttx/board_px4fmu-v2.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = CORTEXM4F
CONFIG_BOARD = PX4FMU_V2
include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk

2
makefiles/nuttx/board_px4io-v1.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = CORTEXM3
CONFIG_BOARD = PX4IO_V1
include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk

2
makefiles/nuttx/board_px4io-v2.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = CORTEXM3
CONFIG_BOARD = PX4IO_V2
include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
include $(PX4_MK_DIR)/nuttx/toolchain_gnu-arm-eabi.mk

3
makefiles/toolchain_gnu-arm-eabi.mk → makefiles/nuttx/toolchain_gnu-arm-eabi.mk
View File

@ -125,6 +125,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
# note - requires corresponding support in NuttX
INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a)
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
@ -263,6 +265,7 @@ define PRELINK
@$(ECHO) "PRELINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1
#$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1
endef
# Update the archive $1 with the files in $2

40
makefiles/posix-arm.mk Normal file
View File

@ -0,0 +1,40 @@
#
# Copyright (C) 2012 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.
#
#
# Rules and definitions related to handling the Linux specific impl when
# building firmware.
#
MODULES += \
platforms/common \
platforms/posix/px4_layer

11
makefiles/posix-arm/board_eagle.mk Normal file
View File

@ -0,0 +1,11 @@
#
# Board-specific definitions for the POSIX port of PX4
#
#
# Configure the toolchain
#
CONFIG_ARCH = CORTEXA8
CONFIG_BOARD = EAGLE
include $(PX4_MK_DIR)/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk

85
makefiles/posix-arm/config_eagle_default.mk Normal file
View File

@ -0,0 +1,85 @@
#
# 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
#
# 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 += $(PX4_BASE)../muorb_krait

329
makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk Normal file
View File

@ -0,0 +1,329 @@
#
# Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#
# Definitions for a generic GNU ARM-EABI toolchain
#
#$(info TOOLCHAIN arm-linux-gnueabihf)
# Toolchain commands. Normally only used inside this file.
#
CROSSDEV = arm-linux-gnueabihf-
CC = $(CROSSDEV)gcc
CXX = $(CROSSDEV)g++
CPP = $(CROSSDEV)gcc -E
LD = $(CROSSDEV)ld
AR = $(CROSSDEV)ar rcs
NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
# Check if the right version of the toolchain is available
#
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.2 4.8.4 4.9.3
CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion)
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
# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
MAXOPTIMIZATION ?= -O3
# Base CPU flags for each of the supported architectures.
#
ARCHCPUFLAGS_CORTEXA8 = -mtune=cortex-a8 \
-mthumb-interwork \
-march=armv7-a \
-mfloat-abi=hard \
-mfpu=neon
ARCHCPUFLAGS_CORTEXM4F = -mcpu=cortex-m4 \
-mthumb \
-march=armv7e-m \
-mfpu=fpv4-sp-d16 \
-mfloat-abi=hard
ARCHCPUFLAGS_CORTEXM4 = -mcpu=cortex-m4 \
-mthumb \
-march=armv7e-m \
-mfloat-abi=soft
ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \
-mthumb \
-march=armv7-m \
-mfloat-abi=soft
# Enabling stack checks if OS was build with them
#
TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h
TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1
ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;)
ifeq ("$(ENABLE_STACK_CHECKS)","0")
ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
else
ARCHINSTRUMENTATIONDEFINES_CORTEXM4F =
ARCHINSTRUMENTATIONDEFINES_CORTEXM4 =
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
endif
# Pick the right set of flags for the architecture.
#
ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
ifeq ($(ARCHCPUFLAGS),)
$(error Must set CONFIG_ARCH to one of CORTEXA8 CORTEXM4F, CORTEXM4 or CORTEXM3)
endif
# Set the board flags
#
ifeq ($(CONFIG_BOARD),)
$(error Board config does not define CONFIG_BOARD)
endif
ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \
-D__PX4_LINUX -D__PX4_POSIX \
-Dnoreturn_function= \
-I$(PX4_BASE)/src/modules/systemlib \
-I$(PX4_BASE)/src/lib/eigen \
-I$(PX4_BASE)/src/platforms/posix/include \
-I$(PX4_BASE)/mavlink/include/mavlink \
-Wno-error=shadow
# optimisation flags
#
ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
-g3 \
-fno-strict-aliasing \
-fomit-frame-pointer \
-funsafe-math-optimizations \
-fno-builtin-printf \
-ffunction-sections \
-fdata-sections
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__
# Generic warnings
#
ARCHWARNINGS = -Wall \
-Wextra \
-Werror \
-Wfloat-equal \
-Wpointer-arith \
-Wmissing-declarations \
-Wpacked \
-Wno-unused-parameter \
-Wno-packed \
-Werror=format-security \
-Werror=array-bounds \
-Wfatal-errors \
-Werror=unused-variable \
-Werror=reorder \
-Werror=uninitialized \
-Werror=init-self \
-Wno-error=logical-op \
-Wdouble-promotion \
-Wlogical-op \
-Wformat=1 \
-Werror=unused-but-set-variable \
-Werror=double-promotion \
-fno-strength-reduce \
-Wno-error=unused-value
ARCHOPTIMIZATION += -fno-strength-reduce
# -Werror=float-conversion - works, just needs to be phased in with some effort and needs GCC 4.9+
# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
# C-specific warnings
#
ARCHCWARNINGS = $(ARCHWARNINGS) \
-Wbad-function-cast \
-Wstrict-prototypes \
-Wmissing-prototypes \
-Wnested-externs
# C++-specific warnings
#
ARCHWARNINGSXX = $(ARCHWARNINGS) \
-Wno-missing-field-initializers
# pull in *just* libm from the toolchain ... this is grody
LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
#EXTRA_LIBS += $(LIBM)
#EXTRA_LIBS += ${PX4_BASE}../muorb_krait/lib/libmuorb.so
EXTRA_LIBS += -pthread -lm -lrt
# Flags we pass to the C compiler
#
CFLAGS = $(ARCHCFLAGS) \
$(ARCHCWARNINGS) \
$(ARCHOPTIMIZATION) \
$(ARCHCPUFLAGS) \
$(ARCHINCLUDES) \
$(INSTRUMENTATIONDEFINES) \
$(ARCHDEFINES) \
$(EXTRADEFINES) \
$(EXTRACFLAGS) \
-fno-common \
$(addprefix -I,$(INCLUDE_DIRS))
# Flags we pass to the C++ compiler
#
CXXFLAGS = $(ARCHCXXFLAGS) \
$(ARCHWARNINGSXX) \
$(ARCHOPTIMIZATION) \
$(ARCHCPUFLAGS) \
$(ARCHXXINCLUDES) \
$(INSTRUMENTATIONDEFINES) \
$(ARCHDEFINES) \
-DCONFIG_WCHAR_BUILTIN \
$(EXTRADEFINES) \
$(EXTRACXXFLAGS) \
-Wno-effc++ \
$(addprefix -I,$(INCLUDE_DIRS))
# Flags we pass to the assembler
#
AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \
$(EXTRADEFINES) \
$(EXTRAAFLAGS)
LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script
# Flags we pass to the linker
#
LDFLAGS += $(EXTRALDFLAGS) \
$(addprefix -L,$(LIB_DIRS))
# Compiler support library
#
LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name)
# Files that the final link depends on
#
#LINK_DEPS += $(LDSCRIPT)
LINK_DEPS +=
# Files to include to get automated dependencies
#
DEP_INCLUDES = $(subst .o,.d,$(OBJS))
# Compile C source $1 to object $2
# as a side-effect, generate a dependency file
#
define COMPILE
@$(ECHO) "CC: $1"
@$(MKDIR) -p $(dir $2)
$(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2
endef
# Compile C++ source $1 to $2
# as a side-effect, generate a dependency file
#
define COMPILEXX
@$(ECHO) "CXX: $1"
@$(MKDIR) -p $(dir $2)
@echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
endef
# Assemble $1 into $2
#
define ASSEMBLE
@$(ECHO) "AS: $1"
@$(MKDIR) -p $(dir $2)
$(Q) $(CC) -c $(AFLAGS) $(abspath $1) -o $2
endef
# Produce partially-linked $1 from files in $2
#
#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold
define PRELINK
@$(ECHO) "PRELINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) -Ur -o $1 $2
endef
# Produce partially-linked $1 from files in $2
#
#$(Q) $(LD) -Ur -o $1 $2 # -Ur not supported in ld.gold
define PRELINKF
@$(ECHO) "PRELINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) -Ur -T$(LDSCRIPT) -o $1 $2
endef
# $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
# Update the archive $1 with the files in $2
#
define ARCHIVE
@$(ECHO) "AR: $2"
@$(MKDIR) -p $(dir $1)
$(Q) $(AR) $1 $2
endef
# Link the objects in $2 into the shared library $1
#
define LINK_A
@$(ECHO) "LINK_A: $1"
@$(MKDIR) -p $(dir $1)
echo "$(Q) $(AR) $1 $2"
$(Q) $(AR) $1 $2
endef
# Link the objects in $2 into the shared library $1
#
define LINK_SO
@$(ECHO) "LINK_SO: $1"
@$(MKDIR) -p $(dir $1)
echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)"
$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc
endef
# Link the objects in $2 into the application $1
#
define LINK
@$(ECHO) "LINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $1 $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC)
endef

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

@ -0,0 +1,78 @@
#
# Makefile for the Foo *default* configuration
#
#
# Board support modules
#
MODULES += drivers/device
#MODULES += drivers/blinkm
#MODULES += drivers/hil
#MODULES += drivers/led
#MODULES += drivers/rgbled
#MODULES += modules/sensors
#MODULES += drivers/ms5611
#
# System commands
#
MODULES += systemcmds/param
#
# General system control
#
#MODULES += modules/mavlink
#
# Estimation modules (EKF/ SO3 / other filters)
#
#MODULES += modules/attitude_estimator_ekf
#MODULES += modules/ekf_att_pos_estimator
#
# Vehicle Control
#
#MODULES += modules/mc_att_control
#
# Library modules
#
MODULES += modules/systemlib
#MODULES += modules/systemlib/mixer
MODULES += modules/uORB
#MODULES += modules/dataman
#MODULES += modules/sdlog2
#MODULES += modules/simulator
#MODULES += modules/commander
#
# Libraries
#
#MODULES += lib/mathlib
#MODULES += lib/mathlib/math/filter
#MODULES += lib/geo
#MODULES += lib/geo_lookup
#MODULES += lib/conversion
#
# posix port
#
MODULES += platforms/posix/px4_layer
#MODULES += platforms/posix/drivers/accelsim
#MODULES += platforms/posix/drivers/gyrosim
#MODULES += platforms/posix/drivers/adcsim
#MODULES += platforms/posix/drivers/barosim
#
# muorb fastrpc changes.
#
#MODULES += $(PX4_BASE)../muorb_krait
#
# Unit tests
#
MODULES += platforms/posix/tests/muorb
#MODULES += platforms/posix/tests/vcdev_test
#MODULES += platforms/posix/tests/hrt_test
#MODULES += platforms/posix/tests/wqueue

2
makefiles/posix/board_posix.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = NATIVE
CONFIG_BOARD = POSIXTEST
include $(PX4_MK_DIR)/toolchain_native.mk
include $(PX4_MK_DIR)/posix/toolchain_native.mk

5
makefiles/posix/config_posix_default.mk
View File

@ -69,6 +69,7 @@ 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
@ -78,3 +79,7 @@ MODULES += platforms/posix/drivers/airspeedsim
#MODULES += platforms/posix/tests/hrt_test
#MODULES += platforms/posix/tests/wqueue
#
# muorb fastrpc changes.
#
#MODULES += $(PX4_BASE)../muorb_krait

1
makefiles/toolchain_native.mk → makefiles/posix/toolchain_native.mk
View File

@ -210,6 +210,7 @@ ARCHWARNINGSXX = $(ARCHWARNINGS) \
# pull in *just* libm from the toolchain ... this is grody
LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
#EXTRA_LIBS += $(LIBM)
#EXTRA_LIBS += ${PX4_BASE}../muorb_krait/lib/libmuorb.so
EXTRA_LIBS += -pthread -lm -lrt
# Flags we pass to the C compiler

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

@ -0,0 +1,77 @@
#
# Makefile for the Foo *default* configuration
#
#
# Board support modules
#
MODULES += drivers/device
#MODULES += drivers/blinkm
#MODULES += drivers/hil
#MODULES += drivers/led
#MODULES += drivers/rgbled
#MODULES += modules/sensors
#MODULES += drivers/ms5611
#
# System commands
#
#MODULES += systemcmds/param
#
# General system control
#
#MODULES += modules/mavlink
#
# Estimation modules (EKF/ SO3 / other filters)
#
#MODULES += modules/attitude_estimator_ekf
#MODULES += modules/ekf_att_pos_estimator
#
# Vehicle Control
#
#MODULES += modules/mc_att_control
#
# Library modules
#
#MODULES += modules/systemlib
#MODULES += modules/systemlib/mixer
MODULES += modules/uORB
#MODULES += modules/dataman
#MODULES += modules/sdlog2
#MODULES += modules/simulator
#MODULES += modules/commander
#
# Libraries
#
#MODULES += lib/mathlib
#MODULES += lib/mathlib/math/filter
#MODULES += lib/geo
#MODULES += lib/geo_lookup
#MODULES += lib/conversion
#
# QuRT port
#
MODULES += platforms/qurt/px4_layer
#MODULES += platforms/posix/drivers/accelsim
#MODULES += platforms/posix/drivers/gyrosim
#MODULES += platforms/posix/drivers/adcsim
#MODULES += platforms/posix/drivers/barosim
#
# Unit tests
#
MODULES += platforms/qurt/tests/muorb
#MODULES += platforms/posix/tests/vcdev_test
#MODULES += platforms/posix/tests/hrt_test
#MODULES += platforms/posix/tests/wqueue
#
# sources for muorb over fastrpc
#
MODULES += $(PX4_BASE)/../muorb_qurt/

2
makefiles/qurt/board_qurt.mk
View File

@ -8,4 +8,4 @@
CONFIG_ARCH = HEXAGON
CONFIG_BOARD = QURTTEST
include $(PX4_MK_DIR)/toolchain_hexagon.mk
include $(PX4_MK_DIR)/qurt/toolchain_hexagon.mk

6
makefiles/qurt/config_qurt_default.mk
View File

@ -15,7 +15,7 @@ MODULES += drivers/blinkm
MODULES += drivers/hil
MODULES += drivers/led
MODULES += drivers/rgbled
#MODULES += modules/sensors
MODULES += modules/sensors
#MODULES += drivers/ms5611
#
@ -76,3 +76,7 @@ MODULES += platforms/posix/tests/vcdev_test
MODULES += platforms/posix/tests/hrt_test
MODULES += platforms/posix/tests/wqueue
#
# sources for muorb over fastrpc
#
#MODULES += $(PX4_BASE)/../muorb_qurt/

50
makefiles/toolchain_hexagon.mk → makefiles/qurt/toolchain_hexagon.mk
View File

@ -39,7 +39,6 @@
#
HEXAGON_TOOLS_ROOT = /opt/6.4.05
HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0
#V_ARCH = v4
V_ARCH = v5
CROSSDEV = hexagon-
HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT))
@ -60,8 +59,10 @@ AR = $(HEXAGON_BIN)/$(CROSSDEV)ar rcs
NM = $(HEXAGON_BIN)/$(CROSSDEV)nm
OBJCOPY = $(HEXAGON_BIN)/$(CROSSDEV)objcopy
OBJDUMP = $(HEXAGON_BIN)/$(CROSSDEV)objdump
HEXAGON_GCC = $(HEXAGON_BIN)/$(CROSSDEV)gcc
QURTLIBS = \
$(QCTOOLSLIB)/libdl.a \
$(TOOLSLIB)/init.o \
$(TOOLSLIB)/libc.a \
$(TOOLSLIB)/libqcc.a \
@ -77,7 +78,8 @@ QURTLIBS = \
$(QCTOOLSLIB)/libhexagon.a \
$(TOOLSLIB)/fini.o
DYNAMIC_LIBS = \
-Wl,$(TOOLSLIB)/pic/libstdc++.a
# Check if the right version of the toolchain is available
@ -96,7 +98,7 @@ MAXOPTIMIZATION ?= -O0
# Base CPU flags for each of the supported architectures.
#
ARCHCPUFLAGS = -m$(V_ARCH)
ARCHCPUFLAGS = -m$(V_ARCH) -G0
# Set the board flags
@ -117,6 +119,8 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \
-I$(PX4_BASE)/../dspal/sys \
-I$(PX4_BASE)/mavlink/include/mavlink \
-I$(QURTLIB)/..//include \
-I$(HEXAGON_SDK_ROOT)/inc \
-I$(HEXAGON_SDK_ROOT)/inc/stddef \
-Wno-error=shadow
# optimisation flags
@ -127,7 +131,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
-fomit-frame-pointer \
-funsafe-math-optimizations \
-ffunction-sections \
-fdata-sections
-fdata-sections \
-fpic
# enable precise stack overflow tracking
# note - requires corresponding support in NuttX
@ -198,10 +203,6 @@ CXXFLAGS = $(ARCHCXXFLAGS) \
$(EXTRACXXFLAGS) \
$(addprefix -I,$(INCLUDE_DIRS))
ifeq (1,$(V_dynamic))
CXX_FLAGS += -fpic -D__V_DYNAMIC__
endif
# Flags we pass to the assembler
#
AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \
@ -211,7 +212,16 @@ AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \
LDSCRIPT = $(PX4_BASE)/posix-configs/posixtest/scripts/ld.script
# Flags we pass to the linker
#
LDFLAGS += -g -nostdlib --section-start .start=0x1d000000\
LDFLAGS += -g -mv5 -nostdlib -mG0lib -G0 -fpic -shared \
-nostartfiles \
-Wl,-Bsymbolic \
-Wl,--wrap=malloc \
-Wl,--wrap=calloc \
-Wl,--wrap=free \
-Wl,--wrap=realloc \
-Wl,--wrap=memalign \
-Wl,--wrap=__stack_chk_fail \
-lc \
$(EXTRALDFLAGS) \
$(addprefix -L,$(LIB_DIRS))
@ -230,21 +240,31 @@ DEP_INCLUDES = $(subst .o,.d,$(OBJS))
# Compile C source $1 to object $2
# as a side-effect, generate a dependency file
#
define COMPILE
define COMPILENOSHARED
@$(ECHO) "CC: $1"
@$(MKDIR) -p $(dir $2)
@echo $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2
$(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2
endef
# Compile C++ source $1 to $2
# Compile C source $1 to object $2 for use in shared library
# as a side-effect, generate a dependency file
#
define COMPILE
@$(ECHO) "CC: $1"
@$(MKDIR) -p $(dir $2)
@echo $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2
$(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2
endef
# Compile C++ source $1 to $2 for use in shared library
# as a side-effect, generate a dependency file
#
define COMPILEXX
@$(ECHO) "CXX: $1"
@$(MKDIR) -p $(dir $2)
@echo $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
$(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) -D__V_DYNAMIC__ -fPIC $(abspath $1) -o $2
endef
# Assemble $1 into $2
@ -299,8 +319,7 @@ endef
define LINK_SO
@$(ECHO) "LINK_SO: $1"
@$(MKDIR) -p $(dir $1)
echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)"
$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc
$(HEXAGON_GCC) $(LDFLAGS) -fPIC -shared -nostartfiles -o $1 -Wl,--whole-archive $2 -Wl,--no-whole-archive $(LIBS) $(DYNAMIC_LIBS)
endef
# Link the objects in $2 into the application $1
@ -308,7 +327,6 @@ endef
define LINK
@$(ECHO) "LINK: $1"
@$(MKDIR) -p $(dir $1)
@echo $(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group
$(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group
$(LD) --section-start .start=0x1d000000 -o $1 --start-group $2 $(EXTRA_LIBS) $(QURTLIBS) --end-group --dynamic-linker= -E --force-dynamic
endef

12
makefiles/qurt_elf.mk
View File

@ -40,11 +40,13 @@
#
# What we're going to build.
#
EXTRALDFLAGS = -Wl,-soname=libdspal_client.so
PRODUCT_SHARED_LIB = $(WORK_DIR)firmware.a
PRODUCT_SHARED_PRELINK = $(WORK_DIR)firmware.o
.PHONY: firmware
firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)mainapp
firmware: $(PRODUCT_SHARED_LIB) $(WORK_DIR)libdspal_client.so $(WORK_DIR)mainapp
#
# Built product rules
@ -63,7 +65,13 @@ $(WORK_DIR)apps.o: $(WORK_DIR)apps.cpp
$(call COMPILEXX,$<, $@)
mv $(WORK_DIR)apps.cpp $(WORK_DIR)apps.cpp_sav
$(WORK_DIR)mainapp: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB)
$(WORK_DIR)libdspal_client.so: $(WORK_DIR)apps.o $(PRODUCT_SHARED_LIB)
$(call LINK_SO,$@, $^)
$(WORK_DIR)dspal_stub.o: $(PX4_BASE)/src/platforms/qurt/dspal/dspal_stub.c
$(call COMPILENOSHARED,$^, $@)
$(WORK_DIR)mainapp: $(WORK_DIR)dspal_stub.o
$(call LINK,$@, $^)
#

2
posix-configs/posixtest/init/rc.S
View File

@ -1,9 +1,11 @@
uorb start
simulator start -s
sleep 2
barosim start
adcsim start
accelsim start
gyrosim start
tone_alarm start
param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1310720
param set CAL_ACC1_ID 1376256

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

@ -85,10 +85,11 @@ Device::log(const char *fmt, ...)
PX4_INFO("[%s] ", _name);
va_start(ap, fmt);
vprintf(fmt, ap);
PX4_INFO( fmt, ap );
//vprintf(fmt, ap);
va_end(ap);
printf("\n");
fflush(stdout);
//printf("\n");
//fflush(stdout);
}
void
@ -97,12 +98,14 @@ Device::debug(const char *fmt, ...)
va_list ap;
if (_debug_enabled) {
printf("<%s> ", _name);
PX4_INFO("<%s> ", _name);
//printf("<%s> ", _name);
va_start(ap, fmt);
vprintf(fmt, ap);
//vprintf(fmt, ap);
PX4_INFO(fmt, ap);
va_end(ap);
printf("\n");
fflush(stdout);
//printf("\n");
//fflush(stdout);
}
}

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

@ -69,7 +69,7 @@ I2C::I2C(const char *name,
_address(address),
_fd(-1)
{
warnx("I2C::I2C name = %s devname = %s", name, devname);
//warnx("I2C::I2C name = %s devname = %s", name, devname);
// fill in _device_id fields for a I2C device
_device_id.devid_s.bus_type = DeviceBusType_I2C;
_device_id.devid_s.bus = bus;
@ -81,7 +81,9 @@ I2C::I2C(const char *name,
I2C::~I2C()
{
if (_fd >= 0) {
#ifndef __PX4_QURT
::close(_fd);
#endif
_fd = -1;
}
}
@ -116,6 +118,7 @@ I2C::init()
_fd = 10000;
}
else {
#ifndef __PX4_QURT
// Open the actual I2C device and map to the virtual dev name
_fd = ::open(get_devname(), O_RDWR);
if (_fd < 0) {
@ -123,6 +126,7 @@ I2C::init()
px4_errno = errno;
return PX4_ERROR;
}
#endif
}
return ret;
@ -246,8 +250,11 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen)
warnx ("2C SIM I2C::read");
return 0;
}
#ifndef __PX4_QURT
return ::read(_fd, buffer, buflen);
#else
return 0;
#endif
}
ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen)
@ -257,7 +264,11 @@ ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen)
return buflen;
}
#ifndef __PX4_QURT
return ::write(_fd, buffer, buflen);
#else
return buflen;
#endif
}
} // namespace device

2
src/drivers/device/vdev.cpp
View File

@ -64,7 +64,7 @@ private:
px4_dev_t() {}
};
#define PX4_MAX_DEV 50
#define PX4_MAX_DEV 100
static px4_dev_t *devmap[PX4_MAX_DEV];
/*

69
src/drivers/hil/hil.cpp
View File

@ -108,6 +108,7 @@ public:
int set_mode(Mode mode);
int set_pwm_rate(unsigned rate);
int _task;
private:
static const unsigned _max_actuators = 4;
@ -115,7 +116,6 @@ private:
Mode _mode;
int _update_rate;
int _current_update_rate;
int _task;
int _t_actuators;
int _t_armed;
orb_advert_t _t_outputs;
@ -165,15 +165,15 @@ HIL *g_hil;
HIL::HIL() :
#ifdef __PX4_NUTTX
CDev(
CDev
#else
VDev(
VDev
#endif
"hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/),
("hilservo", PWM_OUTPUT0_DEVICE_PATH/*"/dev/hil" XXXL*/),
_task(-1),
_mode(MODE_NONE),
_update_rate(50),
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
_t_outputs(0),
@ -228,8 +228,9 @@ HIL::init()
ret = VDev::init();
#endif
if (ret != OK)
if (ret != OK) {
return ret;
}
// XXX already claimed with CDEV
///* try to claim the generic PWM output device node as well - it's OK if we fail at this */
@ -408,21 +409,18 @@ HIL::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
PX4_ISFINITE(outputs.output[i]) &&
@ -675,7 +673,7 @@ enum PortMode {
PORT2_16PWM,
};
PortMode g_port_mode;
static PortMode g_port_mode = PORT_MODE_UNDEFINED;
int
hil_new_mode(PortMode new_mode)
@ -740,31 +738,6 @@ hil_new_mode(PortMode new_mode)
return OK;
}
int
hil_start(void)
{
int ret = OK;
if (g_hil == nullptr) {
g_hil = new HIL;
if (g_hil == nullptr) {
ret = -ENOMEM;
} else {
ret = g_hil->init();
if (ret != OK) {
delete g_hil;
g_hil = nullptr;
}
}
}
return ret;
}
int
test(void)
{
@ -830,17 +803,19 @@ hil_main(int argc, char *argv[])
const char *verb;
int ret = OK;
if (hil_start() != OK) {
warnx("failed to start the HIL driver");
return 1;
}
if (argc < 2) {
usage();
return -EINVAL;
}
verb = argv[1];
if (g_hil == nullptr) {
g_hil = new HIL;
if (g_hil == nullptr) {
return -ENOMEM;
}
}
/*
* Mode switches.
*/
@ -873,10 +848,9 @@ hil_main(int argc, char *argv[])
return OK;
/* switch modes */
return hil_new_mode(new_mode);
ret = hil_new_mode(new_mode);
}
if (!strcmp(verb, "test")) {
else if (!strcmp(verb, "test")) {
ret = test();
}
@ -888,5 +862,14 @@ hil_main(int argc, char *argv[])
usage();
ret = -EINVAL;
}
if ( ret == OK && g_hil->_task == -1 ) {
ret = g_hil->init();
if (ret != OK) {
warnx("failed to start the HIL driver");
delete g_hil;
g_hil = nullptr;
}
}
return ret;
}

138
src/modules/mavlink/mavlink_main.cpp
View File

@ -163,6 +163,7 @@ Mavlink::Mavlink() :
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_udp_initialised(false),
_flow_control_enabled(true),
_last_write_success_time(0),
_last_write_try_time(0),
@ -173,6 +174,9 @@ Mavlink::Mavlink() :
_rate_tx(0.0f),
_rate_txerr(0.0f),
_rate_rx(0.0f),
_socket_fd(-1),
_protocol(SERIAL),
_network_port(14556),
_rstatus {},
_message_buffer {},
_message_buffer_mutex {},
@ -325,6 +329,20 @@ Mavlink::get_instance_for_device(const char *device_name)
return nullptr;
}
Mavlink *
Mavlink::get_instance_for_network_port(unsigned long port)
{
Mavlink *inst;
LL_FOREACH(::_mavlink_instances, inst) {
if (inst->_network_port == port) {
return inst;
}
}
return nullptr;
}
int
Mavlink::destroy_all_instances()
{
@ -672,6 +690,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
_is_usb_uart = true;
}
#ifdef __PX4_LINUX
/* Put in raw mode */
cfmakeraw(&uart_config);
#endif
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name);
::close(_uart_fd);
@ -800,14 +823,14 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
pthread_mutex_lock(&_send_mutex);
unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
_last_write_try_time = hrt_absolute_time();
if (get_protocol() == SERIAL) {
/* check if there is space in the buffer, let it overflow else */
unsigned buf_free = get_free_tx_buf();
if (buf_free < packet_len) {
/* no enough space in buffer to send */
count_txerr();
@ -815,6 +838,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
pthread_mutex_unlock(&_send_mutex);
return;
}
}
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
@ -839,11 +863,21 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
size_t ret = -1;
#ifndef __PX4_POSIX
/* send message to UART */
ssize_t ret = ::write(_uart_fd, buf, packet_len);
if (get_protocol() == SERIAL) {
ret = ::write(_uart_fd, buf, packet_len);
}
#else
if (get_protocol() == UDP) {
ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
} else if (get_protocol() == TCP) {
// not implemented, but possible to do so
}
#endif
if (ret != (int) packet_len) {
if (ret != (size_t) packet_len) {
count_txerr();
count_txerrbytes(packet_len);
@ -851,7 +885,6 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
_last_write_success_time = _last_write_try_time;
count_txbytes(packet_len);
}
#endif
pthread_mutex_unlock(&_send_mutex);
}
@ -908,6 +941,35 @@ Mavlink::resend_message(mavlink_message_t *msg)
pthread_mutex_unlock(&_send_mutex);
}
void
Mavlink::init_udp()
{
#ifdef __PX4_LINUX
PX4_INFO("Setting up UDP w/port %d\n",_network_port);
memset((char *)&_myaddr, 0, sizeof(_myaddr));
_myaddr.sin_family = AF_INET;
_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
_myaddr.sin_port = htons(_network_port);
if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
PX4_WARN("create socket failed\n");
return;
}
if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
PX4_WARN("bind failed\n");
return;
}
unsigned char inbuf[256];
socklen_t addrlen = sizeof(_src_addr);
// wait for client to connect to socket
recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen);
#endif
}
void
Mavlink::handle_message(const mavlink_message_t *msg)
{
@ -1298,8 +1360,10 @@ Mavlink::task_main(int argc, char *argv[])
bool err_flag = false;
int myoptind=1;
const char *myoptarg = NULL;
char* eptr;
int temp_int_arg;
while ((ch = px4_getopt(argc, argv, "b:r:d:m:fpvwx", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "b:r:d:u:m:fpvwx", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'b':
_baudrate = strtoul(myoptarg, NULL, 10);
@ -1323,6 +1387,18 @@ Mavlink::task_main(int argc, char *argv[])
case 'd':
_device_name = myoptarg;
set_protocol(SERIAL);
break;
case 'u':
temp_int_arg = strtoul(myoptarg, &eptr, 10);
if ( *eptr == '\0' ) {
_network_port = temp_int_arg;
set_protocol(UDP);
} else {
warnx("invalid data udp_port '%s'", myoptarg);
err_flag = true;
}
break;
// case 'e':
@ -1387,11 +1463,19 @@ Mavlink::task_main(int argc, char *argv[])
return ERROR;
}
if (get_protocol() == SERIAL) {
warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
} else if (get_protocol() == UDP) {
warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port);
}
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* init socket if necessary */
if (get_protocol() == UDP) {
init_udp();
}
#ifndef __PX4_POSIX
struct termios uart_config_original;
@ -1528,7 +1612,7 @@ Mavlink::task_main(int argc, char *argv[])
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
send_autopilot_capabilites();
//send_autopilot_capabilites();
while (!_task_should_exit) {
/* main loop */
@ -1815,6 +1899,11 @@ Mavlink::stream_command(int argc, char *argv[])
const char *device_name = DEFAULT_DEVICE_NAME;
float rate = -1.0f;
const char *stream_name = nullptr;
unsigned short network_port = 0;
char* eptr;
int temp_int_arg;
bool provided_device = false;
bool provided_network_port = false;
argc -= 2;
argv += 2;
@ -1837,13 +1926,22 @@ Mavlink::stream_command(int argc, char *argv[])
i++;
} else if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
provided_device = true;
device_name = argv[i + 1];
i++;
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
stream_name = argv[i + 1];
i++;
} else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
provided_network_port = true;
temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
if ( *eptr == '\0' ) {
network_port = temp_int_arg;
} else {
err_flag = true;
}
i++;
} else {
err_flag = true;
}
@ -1852,7 +1950,16 @@ Mavlink::stream_command(int argc, char *argv[])
}
if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
Mavlink *inst = get_instance_for_device(device_name);
Mavlink *inst = nullptr;
if (provided_device && !provided_network_port) {
inst = get_instance_for_device(device_name);
} else if (provided_network_port && !provided_device) {
inst = get_instance_for_network_port(network_port);
} else if (provided_device && provided_network_port) {
warnx("please provide either a device name or a network port");
return 1;
}
if (inst != nullptr) {
inst->configure_stream_threadsafe(stream_name, rate);
@ -1861,12 +1968,17 @@ Mavlink::stream_command(int argc, char *argv[])
// If the link is not running we should complain, but not fall over
// because this is so easy to get wrong and not fatal. Warning is sufficient.
if (provided_device) {
warnx("mavlink for device %s is not running", device_name);
return 0;
} else {
warnx("mavlink for network on port %hu is not running", network_port);
}
return 1;
}
} else {
warnx("usage: mavlink stream [-d device] -s stream -r rate");
warnx("usage: mavlink stream [-d device] [-u network_port] -s stream -r rate");
return 1;
}
@ -1875,7 +1987,7 @@ Mavlink::stream_command(int argc, char *argv[])
static void usage()
{
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
warnx("usage: mavlink {start|stop-all|stream} [-d device] [-u udp_port] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
}
int mavlink_main(int argc, char *argv[])

31
src/modules/mavlink/mavlink_main.h
View File

@ -46,6 +46,8 @@
#include <nuttx/fs/fs.h>
#else
#include <drivers/device/device.h>
#include <sys/socket.h>
#include <netinet/in.h>
#endif
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
@ -65,6 +67,12 @@
#include "mavlink_parameters.h"
#include "mavlink_ftp.h"
enum Protocol {
SERIAL = 0,
UDP,
TCP,
};
#ifdef __PX4_NUTTX
class Mavlink
#else
@ -105,6 +113,8 @@ public:
static Mavlink *get_instance_for_device(const char *device_name);
static Mavlink *get_instance_for_network_port(unsigned long port);
static int destroy_all_instances();
static int get_status_all_instances();
@ -190,6 +200,11 @@ public:
*/
void set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; }
/**
* Set communication protocol for this mavlink instance
*/
void set_protocol(Protocol p) {_protocol = p;};
/**
* Get the manual input generation mode
*
@ -305,6 +320,10 @@ public:
unsigned get_system_type() { return _system_type; }
Protocol get_protocol() { return _protocol; };
unsigned short get_network_port() { return _network_port; }
protected:
Mavlink *next;
@ -364,6 +383,7 @@ private:
char *_subscribe_to_stream;
float _subscribe_to_stream_rate;
bool _udp_initialised;
bool _flow_control_enabled;
uint64_t _last_write_success_time;
@ -377,6 +397,15 @@ private:
float _rate_txerr;
float _rate_rx;
#ifdef __PX4_POSIX
struct sockaddr_in _myaddr;
struct sockaddr_in _src_addr;
#endif
int _socket_fd;
Protocol _protocol;
unsigned short _network_port;
struct telemetry_status_s _rstatus; ///< receive status
struct mavlink_message_buffer {
@ -439,6 +468,8 @@ private:
*/
void update_rate_mult();
void init_udp();
#ifdef __PX4_NUTTX
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
#else

4
src/modules/navigator/navigator_main.cpp
View File

@ -257,7 +257,7 @@ Navigator::task_main_trampoline(int argc, char *argv[])
void
Navigator::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
_geofence.setMavlinkFd(_mavlink_fd);
/* Try to load the geofence:
@ -346,7 +346,7 @@ Navigator::task_main()
if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
/* try to reopen the mavlink log device with specified interval */
mavlink_open_time = hrt_abstime() + mavlink_open_interval;
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
}
static bool have_geofence_position_data = false;

30
src/modules/simulator/simulator.cpp
View File

@ -71,11 +71,41 @@ bool Simulator::getRawAccelReport(uint8_t *buf, int len)
return _accel.copyData(buf, len);
}
bool Simulator::getMagReport(uint8_t *buf, int len)
{
return _mag.copyData(buf, len);
}
bool Simulator::getBaroSample(uint8_t *buf, int len)
{
return _baro.copyData(buf, len);
}
bool Simulator::getGPSSample(uint8_t *buf, int len)
{
return _gps.copyData(buf, len);
}
void Simulator::write_MPU_data(void *buf) {
_mpu.writeData(buf);
}
void Simulator::write_accel_data(void *buf) {
_accel.writeData(buf);
}
void Simulator::write_mag_data(void *buf) {
_mag.writeData(buf);
}
void Simulator::write_baro_data(void *buf) {
_baro.writeData(buf);
}
void Simulator::write_gps_data(void *buf) {
_gps.writeData(buf);
}
int Simulator::start(int argc, char *argv[])
{
int ret = 0;

89
src/modules/simulator/simulator.h
View File

@ -48,6 +48,7 @@
#include <drivers/drv_baro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <uORB/uORB.h>
#include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h>
@ -61,31 +62,63 @@ namespace simulator {
// FIXME - what is the endianness of these on actual device?
#pragma pack(push, 1)
struct RawAccelData {
int16_t x;
int16_t y;
int16_t z;
float temperature;
float x;
float y;
float z;
};
#pragma pack(pop)
#pragma pack(push, 1)
struct RawMagData {
float temperature;
float x;
float y;
float z;
};
#pragma pack(pop)
#pragma pack(push, 1)
struct RawMPUData {
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
float accel_x;
float accel_y;
float accel_z;
float temp;
float gyro_x;
float gyro_y;
float gyro_z;
};
#pragma pack(pop)
#pragma pack(push, 1)
struct RawBaroData {
uint8_t d[3];
float pressure;
float altitude;
float temperature;
};
#pragma pack(pop)
#pragma pack(push, 1)
struct RawGPSData {
int32_t lat;
int32_t lon;
int32_t alt;
uint16_t eph;
uint16_t epv;
uint16_t vel;
int16_t vn;
int16_t ve;
int16_t vd;
uint16_t cog;
uint8_t fix_type;
uint8_t satellites_visible;
};
#pragma pack(pop)
template <typename RType> class Report {
public:
Report(int readers) :
_readidx(0),
_max_readers(readers),
_report_len(sizeof(RType))
{
@ -158,23 +191,36 @@ public:
static int start(int argc, char *argv[]);
bool getRawAccelReport(uint8_t *buf, int len);
bool getMagReport(uint8_t *buf, int len);
bool getMPUReport(uint8_t *buf, int len);
bool getBaroSample(uint8_t *buf, int len);
bool getGPSSample(uint8_t *buf, int len);
void write_MPU_data(void *buf);
void write_accel_data(void *buf);
void write_mag_data(void *buf);
void write_baro_data(void *buf);
void write_gps_data(void *buf);
private:
Simulator() :
_accel(1),
_mpu(1),
_baro(1),
_mag(1),
_gps(1),
_sensor_combined_pub(nullptr)
#ifndef __PX4_QURT
,
_manual_control_sp_pub(nullptr),
_rc_channels_pub(nullptr),
_actuator_outputs_sub(-1),
_vehicle_attitude_sub(-1),
_manual_sub(-1),
_sensor{},
_manual_control_sp{},
_rc_input{},
_actuators{},
_attitude{}
_attitude{},
_manual{}
#endif
{}
~Simulator() { _instance=NULL; }
@ -189,6 +235,8 @@ private:
simulator::Report<simulator::RawAccelData> _accel;
simulator::Report<simulator::RawMPUData> _mpu;
simulator::Report<simulator::RawBaroData> _baro;
simulator::Report<simulator::RawMagData> _mag;
simulator::Report<simulator::RawGPSData> _gps;
// uORB publisher handlers
orb_advert_t _accel_pub;
@ -202,29 +250,32 @@ private:
#ifndef __PX4_QURT
// uORB publisher handlers
orb_advert_t _manual_control_sp_pub;
orb_advert_t _rc_channels_pub;
// uORB subscription handlers
int _actuator_outputs_sub;
int _vehicle_attitude_sub;
int _manual_sub;
// uORB data containers
struct sensor_combined_s _sensor;
struct manual_control_setpoint_s _manual_control_sp;
struct rc_input_values _rc_input;
struct actuator_outputs_s _actuators;
struct vehicle_attitude_s _attitude;
struct manual_control_setpoint_s _manual;
int _fd;
unsigned char _buf[200];
hrt_abstime _time_last;
struct sockaddr_in _srcaddr;
socklen_t _addrlen = sizeof(_srcaddr);
void poll_topics();
void poll_actuators();
void handle_message(mavlink_message_t *msg);
void send_data();
void send_controls();
void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu);
void update_gps(mavlink_hil_gps_t *gps_sim);
static void *sending_trampoline(void *);
void send();
#endif

326
src/modules/simulator/simulator_mavlink.cpp
View File

@ -30,7 +30,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <termios.h>
#include <px4_log.h>
#include <px4_time.h>
#include "simulator.h"
@ -43,9 +43,15 @@ using namespace simulator;
#define UDP_PORT 14550
#define PIXHAWK_DEVICE "/dev/ttyACM0"
#define PRESS_GROUND 101325.0f
#define DENSITY 1.2041f
#define GRAVITY 9.81f
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
static int openUart(const char *uart_name, int baud);
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
float out[8];
@ -84,69 +90,91 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
actuator_msg.nav_mode = 0;
}
void Simulator::send_data() {
// check if it's time to send new data
hrt_abstime time_now = hrt_absolute_time();
if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) {
_time_last = time_now;
void Simulator::send_controls() {
mavlink_hil_controls_t msg;
pack_actuator_message(msg);
send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
// can add more messages here, can also setup different timings
}
}
static void fill_manual_control_sp_msg(struct manual_control_setpoint_s *manual, mavlink_manual_control_t *man_msg) {
manual->timestamp = hrt_absolute_time();
manual->x = man_msg->x / 1000.0f;
manual->y = man_msg->y / 1000.0f;
manual->r = man_msg->r / 1000.0f;
manual->z = man_msg->z / 1000.0f;
static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) {
rc->timestamp_publication = hrt_absolute_time();
rc->timestamp_last_signal = hrt_absolute_time();
rc->channel_count = rc_channels->chancount;
rc->rssi = rc_channels->rssi;
rc->values[0] = rc_channels->chan1_raw;
rc->values[1] = rc_channels->chan2_raw;
rc->values[2] = rc_channels->chan3_raw;
rc->values[3] = rc_channels->chan4_raw;
rc->values[4] = rc_channels->chan5_raw;
rc->values[5] = rc_channels->chan6_raw;
rc->values[6] = rc_channels->chan7_raw;
rc->values[7] = rc_channels->chan8_raw;
rc->values[8] = rc_channels->chan9_raw;
rc->values[9] = rc_channels->chan10_raw;
rc->values[10] = rc_channels->chan11_raw;
rc->values[11] = rc_channels->chan12_raw;
rc->values[12] = rc_channels->chan13_raw;
rc->values[13] = rc_channels->chan14_raw;
rc->values[14] = rc_channels->chan15_raw;
rc->values[15] = rc_channels->chan16_raw;
rc->values[16] = rc_channels->chan17_raw;
rc->values[17] = rc_channels->chan18_raw;
}
static void fill_sensors_from_imu_msg(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) {
hrt_abstime timestamp = hrt_absolute_time();
sensor->timestamp = timestamp;
sensor->gyro_raw[0] = imu->xgyro * 1000.0f;
sensor->gyro_raw[1] = imu->ygyro * 1000.0f;
sensor->gyro_raw[2] = imu->zgyro * 1000.0f;
sensor->gyro_rad_s[0] = imu->xgyro;
sensor->gyro_rad_s[1] = imu->ygyro;
sensor->gyro_rad_s[2] = imu->zgyro;
void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) {
// write sensor data to memory so that drivers can copy data from there
RawMPUData mpu;
mpu.accel_x = imu->xacc;
mpu.accel_y = imu->yacc;
mpu.accel_z = imu->zacc;
mpu.temp = imu->temperature;
mpu.gyro_x = imu->xgyro;
mpu.gyro_y = imu->ygyro;
mpu.gyro_z = imu->zgyro;
sensor->accelerometer_raw[0] = imu->xacc; // mg2ms2;
sensor->accelerometer_raw[1] = imu->yacc; // mg2ms2;
sensor->accelerometer_raw[2] = imu->zacc; // mg2ms2;
sensor->accelerometer_m_s2[0] = imu->xacc;
sensor->accelerometer_m_s2[1] = imu->yacc;
sensor->accelerometer_m_s2[2] = imu->zacc;
sensor->accelerometer_mode = 0; // TODO what is this?
sensor->accelerometer_range_m_s2 = 32.7f; // int16
sensor->accelerometer_timestamp = timestamp;
sensor->timestamp = timestamp;
write_MPU_data((void *)&mpu);
sensor->adc_voltage_v[0] = 0.0f;
sensor->adc_voltage_v[1] = 0.0f;
sensor->adc_voltage_v[2] = 0.0f;
RawAccelData accel;
accel.x = imu->xacc;
accel.y = imu->yacc;
accel.z = imu->zacc;
sensor->magnetometer_raw[0] = imu->xmag * 1000.0f;
sensor->magnetometer_raw[1] = imu->ymag * 1000.0f;
sensor->magnetometer_raw[2] = imu->zmag * 1000.0f;
sensor->magnetometer_ga[0] = imu->xmag;
sensor->magnetometer_ga[1] = imu->ymag;
sensor->magnetometer_ga[2] = imu->zmag;
sensor->magnetometer_range_ga = 32.7f; // int16
sensor->magnetometer_mode = 0; // TODO what is this
sensor->magnetometer_cuttoff_freq_hz = 50.0f;
sensor->magnetometer_timestamp = timestamp;
write_accel_data((void *)&accel);
sensor->baro_pres_mbar = imu->abs_pressure;
sensor->baro_alt_meter = imu->pressure_alt;
sensor->baro_temp_celcius = imu->temperature;
sensor->baro_timestamp = timestamp;
RawMagData mag;
mag.x = imu->xmag;
mag.y = imu->ymag;
mag.z = imu->zmag;
write_mag_data((void *)&mag);
RawBaroData baro;
// calculate air pressure from altitude (valid for low altitude)
baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt;
baro.altitude = imu->pressure_alt;
baro.temperature = imu->temperature;
write_baro_data((void *)&baro);
}
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
RawGPSData gps;
gps.lat = gps_sim->lat;
gps.lon = gps_sim->lon;
gps.alt = gps_sim->alt;
gps.eph = gps_sim->eph;
gps.epv = gps_sim->epv;
gps.vel = gps_sim->vel;
gps.vn = gps_sim->vn;
gps.ve = gps_sim->ve;
gps.vd = gps_sim->vd;
gps.cog = gps_sim->cog;
gps.fix_type = gps_sim->fix_type;
gps.satellites_visible = gps_sim->satellites_visible;
write_gps_data((void *)&gps);
sensor->differential_pressure_pa = imu->diff_pressure * 1e2f; //from hPa to Pa
sensor->differential_pressure_timestamp = timestamp;
}
void Simulator::handle_message(mavlink_message_t *msg) {
@ -154,27 +182,26 @@ void Simulator::handle_message(mavlink_message_t *msg) {
case MAVLINK_MSG_ID_HIL_SENSOR:
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
fill_sensors_from_imu_msg(&_sensor, &imu);
// publish message
if(_sensor_combined_pub == nullptr) {
_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &_sensor);
} else {
orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &_sensor);
}
update_sensors(&_sensor, &imu);
break;
case MAVLINK_MSG_ID_MANUAL_CONTROL:
case MAVLINK_MSG_ID_HIL_GPS:
mavlink_hil_gps_t gps_sim;
mavlink_msg_hil_gps_decode(msg, &gps_sim);
update_gps(&gps_sim);
break;
mavlink_manual_control_t man_ctrl_sp;
mavlink_msg_manual_control_decode(msg, &man_ctrl_sp);
fill_manual_control_sp_msg(&_manual_control_sp, &man_ctrl_sp);
case MAVLINK_MSG_ID_RC_CHANNELS:
mavlink_rc_channels_t rc_channels;
mavlink_msg_rc_channels_decode(msg, &rc_channels);
fill_rc_input_msg(&_rc_input, &rc_channels);
// publish message
if(_manual_control_sp_pub == nullptr) {
_manual_control_sp_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual_control_sp);
if(_rc_channels_pub == nullptr) {
_rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_sp_pub, &_manual_control_sp);
orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input);
}
break;
}
@ -214,18 +241,13 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
}
}
void Simulator::poll_topics() {
// copy new data if available
void Simulator::poll_actuators() {
// copy new actuator data if available
bool updated;
orb_check(_actuator_outputs_sub, &updated);
if(updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
}
orb_check(_vehicle_attitude_sub, &updated);
if(updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude);
}
}
void *Simulator::sending_trampoline(void *) {
@ -238,11 +260,11 @@ void Simulator::send() {
fds[0].fd = _actuator_outputs_sub;
fds[0].events = POLLIN;
_time_last = hrt_absolute_time();
int pret;
while(true) {
// wait for up to 100ms for data
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
//timed out
if (pret == 0)
@ -258,8 +280,8 @@ void Simulator::send() {
if (fds[0].revents & POLLIN) {
// got new data to read, update all topics
poll_topics();
send_data();
poll_actuators();
send_controls();
}
}
}
@ -288,16 +310,6 @@ void Simulator::updateSamples()
struct mag_report mag;
memset(&mag, 0 ,sizeof(mag));
// init publishers
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
// subscribe to topics
_actuator_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
// try to setup udp socket for communcation with simulator
memset((char *)&_myaddr, 0, sizeof(_myaddr));
_myaddr.sin_family = AF_INET;
@ -328,11 +340,9 @@ void Simulator::updateSamples()
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT;
(void)pthread_attr_setschedparam(&sender_thread_attr, &param);
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
pthread_attr_destroy(&sender_thread_attr);
// setup serial connection to autopilot (used to get manual controls)
int serial_fd = open(PIXHAWK_DEVICE, O_RDWR);
int serial_fd = openUart(PIXHAWK_DEVICE, 115200);
if (serial_fd < 0) {
PX4_WARN("failed to open %s", PIXHAWK_DEVICE);
@ -355,10 +365,30 @@ void Simulator::updateSamples()
fds[1].events = POLLIN;
int len = 0;
// wait for first data from simulator and respond with first controls
// this is important for the UDP communication to work
int pret = -1;
while (pret <= 0) {
pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100);
}
if (fds[0].revents & POLLIN) {
len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
send_controls();
}
// subscribe to topics
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
// got data from simulator, now activate the sending thread
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
pthread_attr_destroy(&sender_thread_attr);
// wait for new mavlink messages to arrive
while (true) {
int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
//timed out
if (pret == 0)
@ -405,17 +435,107 @@ void Simulator::updateSamples()
}
}
}
}
}
// publish these messages so that attitude estimator does not complain
hrt_abstime time_last = hrt_absolute_time();
baro.timestamp = time_last;
accel.timestamp = time_last;
gyro.timestamp = time_last;
mag.timestamp = time_last;
// publish the sensor values
orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro);
orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro);
orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
int openUart(const char *uart_name, int baud)
{
/* process baud rate */
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
baud);
return -EINVAL;
}
/* open uart */
int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
if (uart_fd < 0) {
return uart_fd;
}
/* Try to set baud rate */
struct termios uart_config;
memset(&uart_config, 0, sizeof(uart_config));
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
::close(uart_fd);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart_fd, &uart_config);
/* USB serial is indicated by /dev/ttyACM0*/
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
::close(uart_fd);
return -1;
}
}
// Make raw
cfmakeraw(&uart_config);
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name);
::close(uart_fd);
return -1;
}
return uart_fd;
}

129
src/modules/uORB/ORBMap.hpp Normal file
View File

@ -0,0 +1,129 @@
/****************************************************************************
*
* 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 <string.h>
#include <stdlib.h>
namespace uORB
{
class DeviceNode;
class ORBMap;
}
class uORB::ORBMap
{
public:
struct Node {
struct Node *next;
const char * node_name;
uORB::DeviceNode *node;
};
ORBMap() :
_top(nullptr),
_end(nullptr)
{ }
~ORBMap() {
while (_top != nullptr) {
unlinkNext(_top);
if (_top->next == nullptr) {
free((void *)_top->node_name);
free(_top);
_top = nullptr;
_end = nullptr;
}
}
}
void insert(const char *node_name, uORB::DeviceNode*node)
{
Node **p;
if (_top == nullptr)
p = &_top;
else
p = &_end->next;
*p = (Node *)malloc(sizeof(Node));
if (_end)
_end = _end->next;
else {
_end = _top;
}
_end->next = nullptr;
_end->node_name = strdup(node_name);
_end->node = node;
}
bool find(const char *node_name)
{
Node *p = _top;
while (p) {
if (strcmp(p->node_name, node_name) == 0) {
return true;
}
p = p->next;
}
return false;
}
uORB::DeviceNode* get(const char *node_name)
{
Node *p = _top;
while (p) {
if (strcmp(p->node_name, node_name) == 0) {
return p->node;
}
}
return nullptr;
}
void unlinkNext(Node *a)
{
Node *b = a->next;
if (b != nullptr) {
if (_end == b) {
_end = a;
}
a->next = b->next;
free((void *)b->node_name);
free(b);
}
}
private:
Node *_top;
Node *_end;
};

128
src/modules/uORB/ORBSet.hpp Normal file
View File

@ -0,0 +1,128 @@
/****************************************************************************
*
* 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
class ORBSet
{
public:
struct Node {
struct Node *next;
const char * node_name;
};
ORBSet() :
_top(nullptr),
_end(nullptr)
{ }
~ORBSet() {
while (_top != nullptr) {
unlinkNext(_top);
if (_top->next == nullptr) {
free((void *)_top->node_name);
free(_top);
_top = nullptr;
}
}
}
void insert(const char *node_name)
{
Node **p;
if (_top == nullptr)
p = &_top;
else
p = &_end->next;
*p = (Node *)malloc(sizeof(Node));
if (_end)
_end = _end->next;
else {
_end = _top;
}
_end->next = nullptr;
_end->node_name = strdup(node_name);
}
bool find(const char *node_name)
{
Node *p = _top;
while (p) {
if (strcmp(p->node_name, node_name) == 0) {
return true;
}
p = p->next;
}
return false;
}
bool erase(const char *node_name)
{
Node *p = _top;
if (_top && (strcmp(_top->node_name, node_name) == 0)) {
p = _top->next;
free((void *)_top->node_name);
free(_top);
_top = p;
if (_top == nullptr) {
_end = nullptr;
}
return true;
}
while (p->next) {
if (strcmp(p->next->node_name, node_name) == 0) {
unlinkNext(p);
return true;
}
}
return nullptr;
}
private:
void unlinkNext(Node *a)
{
Node *b = a->next;
if (b != nullptr) {
if (_end == b) {
_end = a;
}
a->next = b->next;
free((void *)b->node_name);
free(b);
}
}
Node *_top;
Node *_end;
};

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

@ -46,9 +46,16 @@ SRCS = uORBDevices_nuttx.cpp \
else
SRCS = uORBDevices_posix.cpp \
uORBTest_UnitTest.cpp \
uORBManager_posix.cpp
endif
ifeq ($(PX4_TARGET_OS),posix)
SRCS += uORBTest_UnitTest.cpp
endif
ifeq ($(PX4_TARGET_OS),posix-arm)
SRCS += uORBTest_UnitTest.cpp
endif
SRCS += objects_common.cpp \
Publication.cpp \
Subscription.cpp \

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

@ -0,0 +1,166 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef _uORBCommunicator_hpp_
#define _uORBCommunicator_hpp_
#include <stdint.h>
namespace uORBCommunicator
{
class IChannel;
class IChannelRxHandler;
}
/**
* Interface to enable remote subscriptions. The implementor of this interface
* shall manage the communication channel. It can be fastRPC or tcp or ip.
*/
class uORBCommunicator::IChannel
{
public:
//=========================================================================
// INTERFACES FOR Control messages over a channel.
//=========================================================================
/**
* @brief Interface to notify the remote entity of interest of a
* subscription for a message.
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ) = 0;
/**
* @brief Interface to notify the remote entity of removal of a subscription
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not necessarily mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t remove_subscription( const char *messageName ) = 0;
/**
* Register Message Handler. This is internal for the IChannel implementer*
*/
virtual int16_t register_handler( uORBCommunicator::IChannelRxHandler* handler ) = 0;
//=========================================================================
// INTERFACES FOR Data messages
//=========================================================================
/**
* @brief Sends the data message over the communication link.
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t send_message( const char *messageName, int32_t length, uint8_t* data) = 0;
};
/**
* Class passed to the communication link implement to provide callback for received
* messages over a channel.
*/
class uORBCommunicator::IChannelRxHandler
{
public:
/**
* Interface to process a received AddSubscription from remote.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_add_subscription( const char *messageName, int32_t msgRateInHz ) = 0;
/**
* Interface to process a received control msg to remove subscription
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_remove_subscription( const char *messageName ) = 0;
/**
* Interface to process the received data message.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_received_message( const char *messageName, int32_t length, uint8_t* data ) = 0;
};
#endif /* _uORBCommunicator_hpp_ */

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

@ -36,10 +36,15 @@
#include <fcntl.h>
#include <errno.h>
#include <nuttx/arch.h>
#include <algorithm>
#include "uORBDevices_nuttx.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "uORBCommunicator.hpp"
#include <stdlib.h>
uORB::ORBMap uORB::DeviceMaster::_node_map;
uORB::DeviceNode::DeviceNode
(
const struct orb_metadata *meta,
@ -53,7 +58,9 @@ uORB::DeviceNode::DeviceNode
_last_update(0),
_generation(0),
_publisher(0),
_priority(priority)
_priority(priority),
_IsRemoteSubscriberPresent( false ),
_subscriber_count(0)
{
// enable debug() calls
_debug_enabled = true;
@ -120,6 +127,8 @@ uORB::DeviceNode::open(struct file *filp)
ret = CDev::open(filp);
add_internal_subscriber();
if (ret != OK)
delete sd;
@ -142,7 +151,9 @@ uORB::DeviceNode::close(struct file *filp)
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
remove_internal_subscriber();
delete sd;
sd = nullptr;
}
}
@ -295,7 +306,19 @@ uORB::DeviceNode::publish
errno = EIO;
return ERROR;
}
/*
* if the write is successful, send the data over the Multi-ORB link
*/
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( ch != nullptr )
{
if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 )
{
warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel",
__LINE__, meta->o_name );
return ERROR;
}
}
return OK;
}
@ -421,6 +444,79 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg)
node->update_deferred();
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void uORB::DeviceNode::add_internal_subscriber()
{
_subscriber_count++;
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( ch != nullptr && _subscriber_count > 0 )
{
ch->add_subscription( _meta->o_name, 1 );
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void uORB::DeviceNode::remove_internal_subscriber()
{
_subscriber_count--;
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( ch != nullptr && _subscriber_count == 0 )
{
ch->remove_subscription( _meta->o_name );
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz )
{
// if there is already data in the node, send this out to
// the remote entity.
// send the data to the remote entity.
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher.
{
ch->send_message( _meta->o_name, _meta->o_size, _data);
}
return OK;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::DeviceNode::process_remove_subscription()
{
return OK;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data)
{
int16_t ret = -1;
if( length != (int32_t)(_meta->o_size) )
{
warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]",
__LINE__, _meta->o_name, (int)length, (int)_meta->o_size );
return ERROR;
}
/* call the devnode write method with no file pointer */
ret = write(nullptr, (const char *)data, _meta->o_size);
if (ret < 0)
return ERROR;
if (ret != (int)_meta->o_size) {
errno = EIO;
return ERROR;
}
return OK;
}
uORB::DeviceMaster::DeviceMaster(Flavor f) :
CDev((f == PUBSUB) ? "obj_master" : "param_master",
(f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH),
@ -509,6 +605,11 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
free((void *)objname);
free((void *)devpath);
}
else
{
// add to the node map;.
_node_map.insert(nodepath, node);
}
group_tries++;
@ -529,3 +630,14 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
return CDev::ioctl(filp, cmd, arg);
}
}
uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath )
{
uORB::DeviceNode* rc = nullptr;
if( _node_map.find( nodepath ) )
{
rc = _node_map.get(nodepath);
}
return rc;
}

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

@ -35,6 +35,9 @@
#define _uORBDevices_nuttx_hpp_
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include "ORBMap.hpp"
#include "uORBCommon.hpp"
@ -119,6 +122,43 @@ public:
const void *data
);
/**
* processes a request for add subscription from remote
* @param rateInHz
* Specifies the desired rate for the message.
* @return
* 0 = success
* otherwise failure.
*/
int16_t process_add_subscription( int32_t rateInHz );
/**
* processes a request to remove a subscription from remote.
*/
int16_t process_remove_subscription();
/**
* processed the received data message from remote.
*/
int16_t process_received_message( int32_t length, uint8_t* data );
/**
* Add the subscriber to the node's list of subscriber. If there is
* remote proxy to which this subscription needs to be sent, it will
* done via uORBCommunicator::IChannel interface.
* @param sd
* the subscriber to be added.
*/
void add_internal_subscriber();
/**
* Removes the subscriber from the list. Also notifies the remote
* if there a uORBCommunicator::IChannel instance.
* @param sd
* the Subscriber to be removed.
*/
void remove_internal_subscriber();
protected:
virtual pollevent_t poll_state(struct file *filp);
virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
@ -147,6 +187,9 @@ private: // private class methods.
return sd;
}
bool _IsRemoteSubscriberPresent;
int32_t _subscriber_count;
/**
* Perform a deferred update for a rate-limited subscriber.
*/
@ -166,6 +209,10 @@ private: // private class methods.
* @return True if the topic should appear updated to the subscriber
*/
bool appears_updated(SubscriberData *sd);
// disable copy and assignment operators
DeviceNode( const DeviceNode& );
DeviceNode& operator=( const DeviceNode& );
};
/**
@ -180,9 +227,11 @@ public:
DeviceMaster(Flavor f);
virtual ~DeviceMaster();
static uORB::DeviceNode* GetDeviceNode( const char * node_name );
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
private:
Flavor _flavor;
static ORBMap _node_map;
};

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

@ -35,11 +35,16 @@
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <algorithm>
#include "uORBDevices_posix.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "uORBCommunicator.hpp"
#include <stdlib.h>
std::map<std::string, uORB::DeviceNode*> uORB::DeviceMaster::_node_map;
uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp)
{
@ -60,7 +65,8 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name,
_last_update(0),
_generation(0),
_publisher(0),
_priority(priority)
_priority(priority),
_subscriber_count(0)
{
// enable debug() calls
//_debug_enabled = true;
@ -85,7 +91,7 @@ uORB::DeviceNode::open(device::file_t *filp)
lock();
if (_publisher == 0) {
_publisher = getpid();
_publisher = px4_getpid();
ret = PX4_OK;
} else {
@ -127,6 +133,8 @@ uORB::DeviceNode::open(device::file_t *filp)
ret = VDev::open(filp);
add_internal_subscriber();
if (ret != PX4_OK) {
warnx("ERROR: VDev::open failed\n");
delete sd;
@ -145,7 +153,7 @@ uORB::DeviceNode::close(device::file_t *filp)
{
//warnx("uORB::DeviceNode::close fd = %d", filp->fd);
/* is this the publisher closing? */
if (getpid() == _publisher) {
if (px4_getpid() == _publisher) {
_publisher = 0;
} else {
@ -153,7 +161,9 @@ uORB::DeviceNode::close(device::file_t *filp)
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
remove_internal_subscriber();
delete sd;
sd = nullptr;
}
}
@ -310,6 +320,19 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
return ERROR;
}
/*
* if the write is successful, send the data over the Multi-ORB link
*/
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( ch != nullptr )
{
if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 )
{
warnx( "[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel",
__LINE__, meta->o_name );
return ERROR;
}
}
return PX4_OK;
}
@ -437,6 +460,80 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg)
node->update_deferred();
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void uORB::DeviceNode::add_internal_subscriber()
{
_subscriber_count++;
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( ch != nullptr && _subscriber_count > 0 )
{
ch->add_subscription( _meta->o_name, 1 );
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void uORB::DeviceNode::remove_internal_subscriber()
{
_subscriber_count--;
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( ch != nullptr && _subscriber_count == 0 )
{
ch->remove_subscription( _meta->o_name );
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz )
{
// if there is already data in the node, send this out to
// the remote entity.
// send the data to the remote entity.
uORBCommunicator::IChannel* ch = uORB::Manager::get_instance()->get_uorb_communicator();
if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher.
{
ch->send_message( _meta->o_name, _meta->o_size, _data);
}
return 0;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::DeviceNode::process_remove_subscription()
{
return 0;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data)
{
int16_t ret = -1;
if( length != (int32_t)(_meta->o_size) )
{
warnx( "[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]",
__LINE__, _meta->o_name, (int)length, (int)_meta->o_size );
return ERROR;
}
/* call the devnode write method with no file pointer */
ret = write(nullptr, (const char *)data, _meta->o_size);
if (ret < 0)
return ERROR;
if (ret != (int)_meta->o_size) {
errno = EIO;
return ERROR;
}
return PX4_OK;
}
uORB::DeviceMaster::DeviceMaster(Flavor f) :
VDev((f == PUBSUB) ? "obj_master" : "param_master",
(f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH),
@ -528,6 +625,12 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
free((void *)objname);
free((void *)devpath);
}
else
{
// add to the node map;.
_node_map[std::string(nodepath)] = node;
}
group_tries++;
@ -549,3 +652,13 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
}
uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath )
{
uORB::DeviceNode* rc = nullptr;
std::string np(nodepath);
if( _node_map.find( np ) != _node_map.end() )
{
rc = _node_map[np];
}
return rc;
}

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

@ -35,9 +35,10 @@
#define _uORBDevices_posix_hpp_
#include <stdint.h>
#include <string>
#include <map>
#include "uORBCommon.hpp"
namespace uORB
{
class DeviceNode;
@ -58,6 +59,43 @@ public:
static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data );
/**
* processes a request for add subscription from remote
* @param rateInHz
* Specifies the desired rate for the message.
* @return
* 0 = success
* otherwise failure.
*/
int16_t process_add_subscription( int32_t rateInHz );
/**
* processes a request to remove a subscription from remote.
*/
int16_t process_remove_subscription();
/**
* processed the received data message from remote.
*/
int16_t process_received_message( int32_t length, uint8_t* data );
/**
* Add the subscriber to the node's list of subscriber. If there is
* remote proxy to which this subscription needs to be sent, it will
* done via uORBCommunicator::IChannel interface.
* @param sd
* the subscriber to be added.
*/
void add_internal_subscriber();
/**
* Removes the subscriber from the list. Also notifies the remote
* if there a uORBCommunicator::IChannel instance.
* @param sd
* the Subscriber to be removed.
*/
void remove_internal_subscriber();
protected:
virtual pollevent_t poll_state(device::file_t *filp);
virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events);
@ -81,6 +119,8 @@ private:
SubscriberData *filp_to_sd(device::file_t *filp);
int32_t _subscriber_count;
/**
* Perform a deferred update for a rate-limited subscriber.
*/
@ -100,6 +140,11 @@ private:
* @return True if the topic should appear updated to the subscriber
*/
bool appears_updated(SubscriberData *sd);
// disable copy and assignment operators
DeviceNode( const DeviceNode& );
DeviceNode& operator=( const DeviceNode& );
};
/**
@ -114,9 +159,12 @@ public:
DeviceMaster(Flavor f);
~DeviceMaster();
static uORB::DeviceNode* GetDeviceNode( const char *node_name );
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
private:
Flavor _flavor;
static std::map<std::string, uORB::DeviceNode*> _node_map;
};
#endif /* _uORBDeviceNode_posix.hpp */

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

@ -34,9 +34,12 @@
#include <string.h>
#include "uORBDevices.hpp"
#include "uORB.h"
#include "uORBTest_UnitTest.hpp"
#include "uORBCommon.hpp"
#ifndef __PX4_QURT
#include "uORBTest_UnitTest.hpp"
#endif
extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
static uORB::DeviceMaster *g_dev = nullptr;
@ -85,6 +88,7 @@ uorb_main(int argc, char *argv[])
return OK;
}
#ifndef __PX4_QURT
/*
* Test the driver/device.
*/
@ -108,6 +112,7 @@ uorb_main(int argc, char *argv[])
return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
}
}
#endif
/*
* Print driver information.

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

@ -35,7 +35,17 @@
#define _uORBManager_hpp_
#include "uORBCommon.hpp"
#include "uORBDevices.hpp"
#include <stdint.h>
#ifdef __PX4_NUTTX
#include "ORBSet.hpp"
#else
#include <string>
#include <set>
#define ORBSet std::set<std::string>
#endif
#include "uORBCommunicator.hpp"
namespace uORB
{
@ -47,7 +57,7 @@ namespace uORB
* uORB nodes for each uORB topics and also implements the behavor of the
* uORB Api's.
*/
class uORB::Manager
class uORB::Manager : public uORBCommunicator::IChannelRxHandler
{
public:
// public interfaces for this class.
@ -281,6 +291,27 @@ class uORB::Manager
*/
int orb_set_interval(int handle, unsigned interval) ;
/**
* Method to set the uORBCommunicator::IChannel instance.
* @param comm_channel
* The IChannel instance to talk to remote proxies.
* @note:
* Currently this call only supports the use of one IChannel
* Future extensions may include more than one IChannel's.
*/
void set_uorb_communicator(uORBCommunicator::IChannel* comm_channel);
/**
* Gets the uORB Communicator instance.
*/
uORBCommunicator::IChannel* get_uorb_communicator( void );
/**
* Utility method to check if there is a remote subscriber present
* for a given topic
*/
bool is_remote_subscriber_present( const char * messageName );
private: // class methods
/**
* Advertise a node; don't consider it an error if the node has
@ -316,10 +347,57 @@ class uORB::Manager
private: // data members
static Manager _Instance;
// the communicator channel instance.
uORBCommunicator::IChannel* _comm_channel;
ORBSet _remote_subscriber_topics;
private: //class methods
Manager();
/**
* Interface to process a received AddSubscription from remote.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_add_subscription(const char * messageName,
int32_t msgRateInHz);
/**
* Interface to process a received control msg to remove subscription
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_remove_subscription(const char * messageName);
/**
* Interface to process the received data message.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_received_message(const char * messageName,
int32_t length, uint8_t* data);
};
#endif /* _uORBManager_hpp_ */

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

@ -39,7 +39,6 @@
#include <errno.h>
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "uORBDevices.hpp"
//========================= Static initializations =================
@ -55,6 +54,7 @@ uORB::Manager* uORB::Manager::get_instance()
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
uORB::Manager::Manager()
: _comm_channel( nullptr )
{
}
@ -280,3 +280,103 @@ int uORB::Manager::node_open
/* everything has been OK, we can return the handle now */
return fd;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel)
{
_comm_channel = channel;
if (_comm_channel != nullptr) {
_comm_channel->register_handler(this);
}
}
uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void )
{
return _comm_channel;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::Manager::process_add_subscription(const char * messageName,
int32_t msgRateInHz)
{
warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
__LINE__, messageName );
int16_t rc = 0;
_remote_subscriber_topics.insert( messageName );
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
if (ret == OK) {
// get the node name.
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
if ( node == nullptr) {
warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
__LINE__, messageName );
}
else{
// node is present.
node->process_add_subscription(msgRateInHz);
}
} else {
rc = -1;
}
return rc;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::Manager::process_remove_subscription(
const char * messageName)
{
warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
__LINE__, messageName );
int16_t rc = -1;
_remote_subscriber_topics.erase( messageName );
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
if (ret == OK) {
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
// get the node name.
if ( node == nullptr) {
warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
__LINE__, messageName);
} else {
// node is present.
node->process_remove_subscription();
rc = 0;
}
}
return rc;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::Manager::process_received_message(const char * messageName,
int32_t length, uint8_t* data)
{
//warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
int16_t rc = -1;
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
if (ret == OK) {
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
// get the node name.
if ( node == nullptr) {
warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
__LINE__, messageName, nodepath );
} else {
// node is present.
node->process_received_message( length, data );
rc = 0;
}
}
return rc;
}
bool uORB::Manager::is_remote_subscriber_present( const char * messageName )
{
return _remote_subscriber_topics.find( messageName );
}

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

@ -39,6 +39,7 @@
#include <px4_posix.h>
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "px4_config.h"
#include "uORBDevices.hpp"
@ -55,6 +56,7 @@ uORB::Manager* uORB::Manager::get_instance()
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
uORB::Manager::Manager()
: _comm_channel( nullptr )
{
}
@ -292,3 +294,103 @@ int uORB::Manager::node_open
/* everything has been OK, we can return the handle now */
return fd;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void uORB::Manager::set_uorb_communicator( uORBCommunicator::IChannel* channel)
{
_comm_channel = channel;
if (_comm_channel != nullptr) {
_comm_channel->register_handler(this);
}
}
uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void )
{
return _comm_channel;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::Manager::process_add_subscription(const char *messageName,
int32_t msgRateInHz)
{
warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
__LINE__, messageName );
int16_t rc = 0;
_remote_subscriber_topics.insert( messageName );
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
if (ret == OK) {
// get the node name.
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
if ( node == nullptr) {
warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
__LINE__, messageName );
}
else{
// node is present.
node->process_add_subscription(msgRateInHz);
}
} else {
rc = -1;
}
return rc;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::Manager::process_remove_subscription(
const char * messageName)
{
warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
__LINE__, messageName );
int16_t rc = -1;
_remote_subscriber_topics.erase( messageName );
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
if (ret == OK) {
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
// get the node name.
if ( node == nullptr) {
warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
__LINE__, messageName);
} else {
// node is present.
node->process_remove_subscription();
rc = 0;
}
}
return rc;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int16_t uORB::Manager::process_received_message(const char * messageName,
int32_t length, uint8_t* data)
{
//warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
int16_t rc = -1;
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, PUBSUB, messageName );
if (ret == OK) {
uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
// get the node name.
if ( node == nullptr) {
warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
__LINE__, messageName, nodepath );
} else {
// node is present.
node->process_received_message( length, data );
rc = 0;
}
}
return rc;
}
bool uORB::Manager::is_remote_subscriber_present( const char * messageName )
{
return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() );
}

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

@ -61,3 +61,21 @@ int uORB::Utils::node_mkpath
return OK;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int uORB::Utils::node_mkpath(char *buf, Flavor f,
const char *orbMsgName )
{
unsigned len;
unsigned index = 0;
len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param",
orbMsgName, index );
if (len >= orb_maxpath)
return -ENAMETOOLONG;
return OK;
}

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

@ -34,6 +34,7 @@
#define _uORBUtils_hpp_
#include "uORBCommon.hpp"
#include <string>
namespace uORB
{
@ -50,6 +51,12 @@ public:
const struct orb_metadata *meta,
int *instance = nullptr
);
/**
* same as above except this generators the path based on the string.
*/
static int node_mkpath(char *buf, Flavor f, const char *orbMsgName);
};
#endif // _uORBUtils_hpp_

168
src/platforms/posix/drivers/accelsim/accelsim.cpp
View File

@ -54,6 +54,8 @@
#include <unistd.h>
#include <px4_getopt.h>
#include <simulator/simulator.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@ -86,6 +88,8 @@ static const int ERROR = -1;
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ACC_READ (0<<6)
#define MAG_READ (1<<6)
extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); }
@ -512,6 +516,24 @@ ACCELSIM::reset()
int
ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
uint8_t cmd = send[0];
if (cmd & DIR_READ) {
// Get data from the simulator
Simulator *sim = Simulator::getInstance();
if (sim == NULL)
return ENODEV;
// FIXME - not sure what interrupt status should be
recv[1] = 0;
// skip cmd and status bytes
if (cmd & ACC_READ) {
sim->getRawAccelReport(&recv[2], len-2);
} else if (cmd & MAG_READ) {
sim->getMagReport(&recv[2], len-2);
}
}
return PX4_OK;
}
@ -752,7 +774,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg)
unsigned period = 1000000 / arg;
/* check against maximum sane rate (1ms) */
if (period < 1000)
if (period < 10000)
return -EINVAL;
/* update interval for next measurement */
@ -939,11 +961,10 @@ ACCELSIM::start()
//PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval);
hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this);
// There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but mag period is set to 0
// There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but _call_mag_interval is 0
if (_call_mag_interval == 0) {
PX4_ERR("_call_mag_interval uninitilized - would have set period delay of 0");
_call_mag_interval = 1000;
//PX4_INFO("_call_mag_interval uninitilized - would have set period delay of 0");
_call_mag_interval = 10000; // Max 100Hz
}
//PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval);
@ -986,9 +1007,10 @@ ACCELSIM::measure()
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
float temperature;
float x;
float y;
float z;
} raw_accel_report;
#pragma pack(pop)
@ -999,8 +1021,11 @@ ACCELSIM::measure()
/* fetch data from the sensor */
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
raw_accel_report.cmd = DIR_READ;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
raw_accel_report.cmd = DIR_READ | ACC_READ;
if(OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) {
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
@ -1029,54 +1054,58 @@ ACCELSIM::measure()
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
accel_report.z_raw = raw_accel_report.z;
accel_report.x_raw = (int16_t)(raw_accel_report.x/_accel_range_scale);
accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale);
accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale);
float xraw_f = raw_accel_report.x;
float yraw_f = raw_accel_report.y;
float zraw_f = raw_accel_report.z;
// float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale);
// float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale);
// float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale);
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// // apply user specified rotation
// rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
// float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
// float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
// float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
/*
we have logs where the accelerometers get stuck at a fixed
large value. We want to detect this and mark the sensor as
being faulty
*/
if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
fabsf(_last_accel[1] - y_in_new) < 0.001f &&
fabsf(_last_accel[2] - z_in_new) < 0.001f &&
fabsf(x_in_new) > 20 &&
fabsf(y_in_new) > 20 &&
fabsf(z_in_new) > 20) {
_constant_accel_count += 1;
} else {
_constant_accel_count = 0;
}
if (_constant_accel_count > 100) {
// we've had 100 constant accel readings with large
// values. The sensor is almost certainly dead. We
// will raise the error_count so that the top level
// flight code will know to avoid this sensor, but
// we'll still give the data so that it can be logged
// and viewed
perf_count(_bad_values);
_constant_accel_count = 0;
}
// if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
// fabsf(_last_accel[1] - y_in_new) < 0.001f &&
// fabsf(_last_accel[2] - z_in_new) < 0.001f &&
// fabsf(x_in_new) > 20 &&
// fabsf(y_in_new) > 20 &&
// fabsf(z_in_new) > 20) {
// _constant_accel_count += 1;
// } else {
// _constant_accel_count = 0;
// }
// if (_constant_accel_count > 100) {
// // we've had 100 constant accel readings with large
// // values. The sensor is almost certainly dead. We
// // will raise the error_count so that the top level
// // flight code will know to avoid this sensor, but
// // we'll still give the data so that it can be logged
// // and viewed
// perf_count(_bad_values);
// _constant_accel_count = 0;
// }
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
// _last_accel[0] = x_in_new;
// _last_accel[1] = y_in_new;
// _last_accel[2] = z_in_new;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
// accel_report.x = _accel_filter_x.apply(x_in_new);
// accel_report.y = _accel_filter_y.apply(y_in_new);
// accel_report.z = _accel_filter_z.apply(z_in_new);
accel_report.x = raw_accel_report.x;
accel_report.y = raw_accel_report.y;
accel_report.z = raw_accel_report.z;
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
@ -1109,11 +1138,11 @@ ACCELSIM::mag_measure()
#pragma pack(push, 1)
struct {
uint8_t cmd;
int16_t temperature;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
float temperature;
float x;
float y;
float z;
} raw_mag_report;
#pragma pack(pop)
@ -1125,8 +1154,11 @@ ACCELSIM::mag_measure()
/* fetch data from the sensor */
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
raw_mag_report.cmd = DIR_READ;
transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report));
raw_mag_report.cmd = DIR_READ | MAG_READ;
if(OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) {
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
@ -1146,29 +1178,33 @@ ACCELSIM::mag_measure()
mag_report.timestamp = hrt_absolute_time();
mag_report.x_raw = raw_mag_report.x;
mag_report.y_raw = raw_mag_report.y;
mag_report.z_raw = raw_mag_report.z;
mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale);
mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale);
mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale);
float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale);
float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale);
float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale);
float xraw_f = mag_report.x_raw;
float yraw_f = mag_report.y_raw;
float zraw_f = mag_report.z_raw;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report.scaling = _mag_range_scale;
mag_report.range_ga = (float)_mag_range_ga;
mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
// mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
// mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
// mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
// mag_report.scaling = _mag_range_scale;
// mag_report.range_ga = (float)_mag_range_ga;
// mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
/* remember the temperature. The datasheet isn't clear, but it
* seems to be a signed offset from 25 degrees C in units of 0.125C
*/
_last_temperature = 25 + (raw_mag_report.temperature * 0.125f);
_last_temperature = raw_mag_report.temperature;
mag_report.temperature = _last_temperature;
mag_report.x = raw_mag_report.x;
mag_report.y = raw_mag_report.y;
mag_report.z = raw_mag_report.z;
_mag_reports->force(&mag_report);

109
src/platforms/posix/drivers/barosim/baro.cpp
View File

@ -275,6 +275,13 @@ BAROSIM::init()
_measure_phase = 0;
_reports->flush();
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic == (orb_advert_t)(-1)) {
PX4_ERR("failed to create sensor_baro publication");
}
/* this do..while is goto without goto */
do {
/* do temperature first */
@ -312,12 +319,6 @@ BAROSIM::init()
ret = OK;
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic == nullptr) {
PX4_ERR("failed to create sensor_baro publication");
}
//PX4_WARN("sensor_baro publication %ld", _baro_topic);
} while (0);
@ -622,7 +623,14 @@ int
BAROSIM::collect()
{
int ret;
uint32_t raw;
#pragma pack(push, 1)
struct {
float pressure;
float altitude;
float temperature;
} baro_report;
#pragma pack(pop)
perf_begin(_sample_perf);
@ -632,7 +640,7 @@ BAROSIM::collect()
report.error_count = perf_event_count(_comms_errors);
/* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->dev_read(0, (void *)&raw, 0);
ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report));
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
@ -641,84 +649,15 @@ BAROSIM::collect()
/* handle a measurement */
if (_measure_phase == 0) {
/* temperature offset (in ADC units) */
int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8);
/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23);
/* base sensor scale/offset values */
_SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8);
_OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7);
/* temperature compensation */
if (_TEMP < 2000) {
int32_t T2 = POW2(dT) >> 31;
int64_t f = POW2((int64_t)_TEMP - 2000);
int64_t OFF2 = 5 * f >> 1;
int64_t SENS2 = 5 * f >> 2;
if (_TEMP < -1500) {
int64_t f2 = POW2(_TEMP + 1500);
OFF2 += 7 * f2;
SENS2 += 11 * f2 >> 1;
}
_TEMP -= T2;
_OFF -= OFF2;
_SENS -= SENS2;
}
report.pressure = baro_report.pressure;
report.altitude = baro_report.altitude;
report.temperature = baro_report.temperature;
report.timestamp = hrt_absolute_time();
} else {
/* pressure calculation, result in Pa */
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
_P = P * 0.01f;
_T = _TEMP * 0.01f;
/* generate a new report */
report.temperature = _TEMP / 100.0f;
report.pressure = P / 100.0f; /* convert to millibar */
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
/*
* PERFORMANCE HINT:
*
* The single precision calculation is 50 microseconds faster than the double
* precision variant. It is however not obvious if double precision is required.
* Pending more inspection and tests, we'll leave the double precision variant active.
*
* Measurements:
* double precision: barosim_read: 992 events, 258641us elapsed, min 202us max 305us
* single precision: barosim_read: 963 events, 208066us elapsed, min 202us max 241us
*/
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
double p1 = _msl_pressure / 1000.0;
/* measured pressure in kPa */
double p = P / 1000.0;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
report.pressure = baro_report.pressure;
report.altitude = baro_report.altitude;
report.temperature = baro_report.temperature;
report.timestamp = hrt_absolute_time();
/* publish it */
if (!(_pub_blocked)) {

12
src/platforms/posix/drivers/barosim/baro_sim.cpp
View File

@ -118,22 +118,26 @@ BARO_SIM::init()
int
BARO_SIM::dev_read(unsigned offset, void *data, unsigned count)
{
/*
union _cvt {
uint8_t b[4];
uint32_t w;
} *cvt = (_cvt *)data;
uint8_t buf[3];
*/
/* read the most recent measurement */
uint8_t cmd = 0;
int ret = transfer(&cmd, 1, &buf[0], 3);
int ret = transfer(&cmd, 1, static_cast<uint8_t *>(data), count);
/*
if (ret == PX4_OK) {
/* fetch the raw value */
// fetch the raw value
cvt->b[0] = buf[2];
cvt->b[1] = buf[1];
cvt->b[2] = buf[0];
cvt->b[3] = 0;
}
*/
return ret;
}
@ -204,7 +208,7 @@ BARO_SIM::transfer(const uint8_t *send, unsigned send_len,
if (recv_len == 0) {
PX4_DEBUG("BARO_SIM measurement requested");
}
else if (send_len != 1 || send[0] != 0 || recv_len != 3) {
else if (send_len != 1 || send[0] != 0 ) {
PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len);
return 1;
}

682
src/platforms/posix/drivers/gpssim/gpssim.cpp Normal file
View File

@ -0,0 +1,682 @@
/****************************************************************************
*
* Copyright (c) 2015 Roman Bapst. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gps.cpp
* Driver for the GPS on a serial port
*/
//#include <nuttx/clock.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <px4_config.h>
//#include <nuttx/arch.h>
//#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/device.h>
//#include <drivers/device/i2c.h>
#include <systemlib/systemlib.h>
//#include <systemlib/perf_counter.h>
//#include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h>
#include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <simulator/simulator.h>
//#include <board_config.h>
#define GPS_DRIVER_MODE_UBX_SIM
#define GPS_SIM_DEVICE_PATH "/dev/gps_sim"
//#include "ubx.h"
//#include "mtk.h"
//#include "ashtech.h"
#define TIMEOUT_5HZ 500
#define RATE_MEASUREMENT_PERIOD 5000000
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/* class for dynamic allocation of satellite info data */
class GPS_Sat_Info
{
public:
struct satellite_info_s _data;
};
class GPS_SIM : public device::VDev
{
public:
GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info);
virtual ~GPS_SIM();
virtual int init();
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
private:
bool _task_should_exit; ///< flag to make the main worker task exit
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate; ///< current baudrate
char _port[20]; ///< device / serial port path
volatile int _task; ///< worker task
bool _healthy; ///< flag to signal if the GPS is ok
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
bool _mode_changed; ///< flag that the GPS mode has changed
//gps_driver_mode_t _mode; ///< current mode
GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object
struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output
/**
* Try to configure the GPS, handle outgoing communication to the GPS
*/
void config();
/**
* Trampoline to the worker task
*/
static void task_main_trampoline(void *arg);
/**
* Worker task: main GPS thread that configures the GPS and parses incoming data, always running
*/
void task_main(void);
/**
* Set the baudrate of the UART to the GPS
*/
int set_baudrate(unsigned baud);
/**
* Send a reset command to the GPS
*/
void cmd_reset();
int receive(int timeout);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int gps_sim_main(int argc, char *argv[]);
namespace
{
GPS_SIM *g_dev = nullptr;
}
GPS_SIM::GPS_SIM(const char *uart_path, bool fake_gps, bool enable_sat_info) :
VDev("gps", GPS_SIM_DEVICE_PATH),
_task_should_exit(false),
//_healthy(false),
//_mode_changed(false),
//_mode(GPS_DRIVER_MODE_UBX),
//_Helper(nullptr),
_Sat_Info(nullptr),
_report_gps_pos_pub(nullptr),
_p_report_sat_info(nullptr),
_report_sat_info_pub(nullptr),
_rate(0.0f),
_fake_gps(fake_gps)
{
// /* store port name */
// strncpy(_port, uart_path, sizeof(_port));
// /* enforce null termination */
// _port[sizeof(_port) - 1] = '\0';
/* we need this potentially before it could be set in task_main */
g_dev = this;
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
/* create satellite info data object if requested */
if (enable_sat_info) {
_Sat_Info = new(GPS_Sat_Info);
_p_report_sat_info = &_Sat_Info->_data;
memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
}
_debug_enabled = true;
}
GPS_SIM::~GPS_SIM()
{
/* tell the task we want it to go away */
_task_should_exit = true;
/* spin waiting for the task to stop */
for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
/* give it another 100ms */
usleep(100000);
}
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
px4_task_delete(_task);
g_dev = nullptr;
}
int
GPS_SIM::init()
{
int ret = ERROR;
/* do regular cdev init */
if (VDev::init() != OK)
goto out;
/* start the GPS driver worker task */
_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPS_SIM::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
return -errno;
}
ret = OK;
out:
return ret;
}
int
GPS_SIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
lock();
int ret = OK;
switch (cmd) {
case SENSORIOCRESET:
cmd_reset();
break;
default:
/* give it to parent if no one wants it */
ret = VDev::ioctl(filp, cmd, arg);
break;
}
unlock();
return ret;
}
void
GPS_SIM::task_main_trampoline(void *arg)
{
g_dev->task_main();
}
int
GPS_SIM::receive(int timeout) {
Simulator *sim = Simulator::getInstance();
simulator::RawGPSData gps;
sim->getGPSSample((uint8_t *)&gps, sizeof(gps));
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.lat = gps.lat;
_report_gps_pos.lon = gps.lon;
_report_gps_pos.alt = gps.alt;
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.eph = (float)gps.eph;
_report_gps_pos.epv = (float)gps.epv;
_report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f;
_report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f;
_report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f;
_report_gps_pos.vel_d_m_s = (float)(gps.vd)/100.0f;
_report_gps_pos.cog_rad = (float)(gps.cog)*3.1415f/(100.0f * 180.0f);
_report_gps_pos.fix_type = gps.fix_type;
_report_gps_pos.satellites_used = gps.satellites_visible;
usleep(200000);
return 1;
}
void
GPS_SIM::task_main()
{
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {
if (_fake_gps) {
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
_report_gps_pos.alt = (int32_t)1200e3f;
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.s_variance_m_s = 10.0f;
_report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 3;
_report_gps_pos.eph = 0.9f;
_report_gps_pos.epv = 1.8f;
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated
if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
usleep(2e5);
} else {
// if (_Helper != nullptr) {
// delete(_Helper);
// set to zero to ensure parser is not used while not instantiated
// _Helper = nullptr;
// }
// switch (_mode) {
// case GPS_DRIVER_MODE_UBX_SIM:
// _Helper = new UBX_SIM(_serial_fd, &_report_gps_pos, _p_report_sat_info);
// break;
// default:
// break;
// }
//if (_Helper->configure(_baudrate) == 0) {
//Publish initial report that we have access to a GPS
//Make sure to clear any stale data in case driver is reset
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.timestamp_time = hrt_absolute_time();
if (!(_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
// GPS is obviously detected successfully, reset statistics
//_Helper->reset_update_rates();
while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (_p_report_sat_info) {
if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
}
//if (helper_ret & 1) { // consider only pos info updates for rate calculation */
// last_rate_count++;
//}
/* measure update rate every 5 seconds */
//if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
//_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
//last_rate_measurement = hrt_absolute_time();
//last_rate_count = 0;
//_Helper->store_update_rates();
//_Helper->reset_update_rates();
//}
// if (!_healthy) {
// const char *mode_str = "unknown";
// switch (_mode) {
// case GPS_DRIVER_MODE_UBX_SIM:
// mode_str = "UBX";
// break;
// default:
// break;
// }
// warnx("module found: %s", mode_str);
// _healthy = true;
// }
}
// if (_healthy) {
// warnx("module lost");
// _healthy = false;
// _rate = 0.0f;
// }
lock();
//}
// /* select next mode */
// switch (_mode) {
// case GPS_DRIVER_MODE_UBX:
// _mode = GPS_DRIVER_MODE_MTK;
// break;
// case GPS_DRIVER_MODE_MTK:
// _mode = GPS_DRIVER_MODE_ASHTECH;
// break;
// case GPS_DRIVER_MODE_ASHTECH:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
// default:
// break;
// }
}
}
warnx("exiting");
//::close(_serial_fd);
/* tell the dtor that we are exiting */
_task = -1;
return;
}
void
GPS_SIM::cmd_reset()
{
}
void
GPS_SIM::print_info()
{
//GPS Mode
if(_fake_gps) {
warnx("protocol: faked");
}
else {
warnx("protocol: SIM");
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
warnx("sat info: %s, noise: %d, jamming detected: %s",
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
_report_gps_pos.noise_per_ms,
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
//warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
//warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
}
usleep(100000);
}
/**
* Local functions in support of the shell command.
*/
namespace gps_sim
{
GPS_SIM *g_dev = nullptr;
void start(const char *uart_path, bool fake_gps, bool enable_sat_info);
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{
int fd;
if (g_dev != nullptr)
warnx("already started");
/* create the driver */
g_dev = new GPS_SIM(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
warnx("open: %s\n", GPS0_DEVICE_PATH);
goto fail;
}
return;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
warnx("start failed");
}
/**
* Stop the driver.
*/
void
stop()
{
delete g_dev;
g_dev = nullptr;
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
warnx("PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = px4_open(GPS_SIM_DEVICE_PATH, O_RDONLY);
if (fd < 0)
warnx("failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
warnx("reset failed");
}
/**
* Print the status of the driver.
*/
void
info()
{
if (g_dev == nullptr)
errx(1, "not running");
g_dev->print_info();
}
} // namespace
int
gps_sim_main(int argc, char *argv[])
{
/* set to default */
const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
bool enable_sat_info = false;
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
/* work around getopt unreliability */
if (argc > 3) {
if (!strcmp(argv[2], "-d")) {
device_name = argv[3];
} else {
goto out;
}
}
/* Detect fake gps option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-f"))
fake_gps = true;
}
/* Detect sat info option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-s"))
enable_sat_info = true;
}
gps_sim::start(device_name, fake_gps, enable_sat_info);
}
if (!strcmp(argv[1], "stop"))
gps_sim::stop();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
gps_sim::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
gps_sim::reset();
/*
* Print driver status.
*/
if (!strcmp(argv[1], "status"))
gps_sim::info();
return 0;
out:
warnx("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
return 1;
}

42
src/platforms/posix/drivers/gpssim/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2012, 2013 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.
#
############################################################################
#
# Simulated GPS driver
#
MODULE_COMMAND = gps_sim
SRCS = gpssim.cpp
MAXOPTIMIZATION = -Os

98
src/platforms/posix/drivers/gpssim/ubx_sim.cpp Normal file
View File

@ -0,0 +1,98 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ubx.cpp
*
* U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* @author Hannes Delago
* (rework, add ubx7+ compatibility)
*
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
*/
#include <assert.h>
#include <math.h>
#include <poll.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <drivers/drv_hrt.h>
#include "ubx_sim.h"
#include <simulator/simulator.h>
#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
_fd(fd),
_gps_position(gps_position),
_satellite_info(satellite_info),
{
}
UBX::~UBX()
{
}
int UBX_SIM::configure(unsigned &baudrate)
{
return 0;
}
int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
UBX_SIM::receive(const unsigned timeout)
{
/* copy data from simulator here */
usleep(1000000);
return 1;
}

63
src/platforms/posix/drivers/gpssim/ubx_sim.h Normal file
View File

@ -0,0 +1,63 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ubx_sim.h
*
*/
#ifndef UBX_SIM_H_
#define UBX_SIM_H_
class UBX_SIM
{
public:
UBX_SIM(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
~UBX_SIM();
int receive(const unsigned timeout);
int configure(unsigned &baudrate);
private:
int _fd;
struct vehicle_gps_position_s *_gps_position;
struct satellite_info_s *_satellite_info;
bool _enable_sat_info;
};
#endif /* UBX_SIM_H_ */

142
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
View File

@ -379,13 +379,13 @@ private:
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
float accel_x;
float accel_y;
float accel_z;
float temp;
float gyro_x;
float gyro_y;
float gyro_z;
};
#pragma pack(pop)
@ -466,6 +466,9 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro
// disable debug() calls
_debug_enabled = false;
// Don't publish until initialized
_pub_blocked = true;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM;
/* Prime _gyro with parents devid. */
@ -588,6 +591,9 @@ GYROSIM::init()
if (_accel_topic == nullptr) {
PX4_WARN("ADVERT FAIL");
}
else {
_pub_blocked = false;
}
/* advertise sensor topic, measure manually to initialize valid report */
@ -1194,15 +1200,6 @@ void
GYROSIM::measure()
{
struct MPUReport mpu_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
@ -1214,69 +1211,10 @@ GYROSIM::measure()
// sensor transfer at high clock speed
//set_frequency(GYROSIM_HIGH_BUS_SPEED);
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
return;
/*
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a VDev bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) {
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
/*
* Report buffers.
*/
@ -1286,7 +1224,11 @@ GYROSIM::measure()
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
grb.timestamp = hrt_absolute_time();
arb.timestamp = grb.timestamp;
// this sleep is needed because the timing of the drivers is not yet working
usleep(1000);
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
@ -1312,13 +1254,13 @@ GYROSIM::measure()
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
arb.x_raw = (int16_t)(mpu_report.accel_x / _accel_range_scale);
arb.y_raw = (int16_t)(mpu_report.accel_y / _accel_range_scale);
arb.z_raw = (int16_t)(mpu_report.accel_z / _accel_range_scale);
/*
float xraw_f = (int16_t)(mpu_report.accel_x / _accel_range_scale);
float yraw_f = (int16_t)(mpu_report.accel_y / _accel_range_scale);
float zraw_f = (int16_t)(mpu_report.accel_z / _accel_range_scale);
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
@ -1330,22 +1272,26 @@ GYROSIM::measure()
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
*/
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = (report.temp) / 361.0f + 35.0f;
_last_temperature = mpu_report.temp;
arb.temperature_raw = report.temp;
arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f);
arb.temperature = _last_temperature;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
arb.x = mpu_report.accel_x;
arb.y = mpu_report.accel_y;
arb.z = mpu_report.accel_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
/*
xraw_f = (int16_t)(mpu_report.gyro_x/_gyro_range_scale);
yraw_f = (int16_t)(mpu_report.gyro_y/_gyro_range_scale);
zraw_f = (int16_t)(mpu_report.gyro_z/_gyro_range_scale);
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
@ -1357,20 +1303,24 @@ GYROSIM::measure()
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
*/
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f);
grb.temperature = _last_temperature;
grb.x = mpu_report.gyro_x;
grb.y = mpu_report.gyro_y;
grb.z = mpu_report.gyro_z;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (!(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);

7
src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
View File

@ -350,7 +350,7 @@ ToneAlarm::start_note(unsigned note)
// Silence warning of unused var
do_something(period);
PX4_DEBUG("ToneAlarm::start_note %u", period);
PX4_INFO("ToneAlarm::start_note %u", period);
}
void
@ -361,6 +361,7 @@ ToneAlarm::stop_note()
void
ToneAlarm::start_tune(const char *tune)
{
PX4_INFO("ToneAlarm::start_tune");
// kill any current playback
hrt_cancel(&_note_call);
@ -533,7 +534,7 @@ ToneAlarm::next_note()
// tune looks bad (unexpected EOF, bad character, etc.)
tune_error:
printf("tune error\n");
PX4_ERR("tune error\n");
_repeat = false; // don't loop on error
// stop (and potentially restart) the tune
@ -605,7 +606,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg)
/* decide whether to increase the alarm level to cmd or leave it alone */
switch (cmd) {
case TONE_SET_ALARM:
debug("TONE_SET_ALARM %u", arg);
PX4_INFO("TONE_SET_ALARM %lu", arg);
if (arg < TONE_NUMBER_OF_TUNES) {
if (arg == TONE_STOP_TUNE) {

4
src/platforms/posix/main.cpp
View File

@ -79,9 +79,9 @@ static void run_cmd(const vector<string> &appargs) {
static void process_line(string &line)
{
vector<string> appargs(5);
vector<string> appargs(8);
stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4];
stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> appargs[7];
run_cmd(appargs);
}

1
src/platforms/posix/px4_layer/dq_addlast.c
View File

@ -41,6 +41,7 @@
* Included Files
************************************************************/
#include <stddef.h>
#include <queue.h>
/************************************************************

1
src/platforms/posix/px4_layer/dq_rem.c
View File

@ -41,6 +41,7 @@
* Included Files
************************************************************/
#include <stddef.h>
#include <queue.h>
/************************************************************

1
src/platforms/posix/px4_layer/dq_remfirst.c
View File

@ -41,6 +41,7 @@
* Included Files
************************************************************/
#include <stddef.h>
#include <queue.h>
/************************************************************

3
src/platforms/posix/px4_layer/drv_hrt.c
View File

@ -301,9 +301,12 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
if (entry->deadline != 0)
sq_rem(&entry->link, &callout_queue);
#if 0
// Use this to debug busy CPU that keeps rescheduling with 0 period time
if (interval < HRT_INTERVAL_MIN) {
PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);
}
#endif
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;

3
src/platforms/posix/px4_layer/hrt_queue.c
View File

@ -49,8 +49,6 @@
#include <px4_workqueue.h>
#include "hrt_work.h"
#ifdef CONFIG_SCHED_WORKQUEUE
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
@ -129,4 +127,3 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
return PX4_OK;
}
#endif /* CONFIG_SCHED_WORKQUEUE */

8
src/platforms/posix/px4_layer/hrt_work.h
View File

@ -46,16 +46,16 @@ void hrt_work_queue_init(void);
int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay);
void hrt_work_cancel(struct work_s *work);
inline void hrt_work_lock(void);
inline void hrt_work_unlock(void);
//inline void hrt_work_lock(void);
//inline void hrt_work_unlock(void);
inline void hrt_work_lock()
static inline void hrt_work_lock()
{
//PX4_INFO("hrt_work_lock");
sem_wait(&_hrt_work_lock);
}
inline void hrt_work_unlock()
static inline void hrt_work_unlock()
{
//PX4_INFO("hrt_work_unlock");
sem_post(&_hrt_work_lock);

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

@ -42,6 +42,7 @@ SRCS = \
hrt_queue.c \
hrt_work_cancel.c \
work_thread.c \
work_lock.c \
work_queue.c \
work_cancel.c \
lib_crc32.c \

4
src/platforms/posix/px4_layer/px4_posix_impl.cpp
View File

@ -48,6 +48,8 @@
#include "systemlib/param/param.h"
#include "hrt_work.h"
extern pthread_t _shell_task_id;
__BEGIN_DECLS
long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
@ -63,6 +65,8 @@ void init_once(void);
void init_once(void)
{
_shell_task_id = pthread_self();
PX4_INFO("Shell id is %lu", _shell_task_id);
work_queues_init();
hrt_work_queue_init();
hrt_init();

26
src/platforms/posix/px4_layer/px4_posix_tasks.cpp
View File

@ -57,7 +57,11 @@
#define MAX_CMD_LEN 100
#define PX4_MAX_TASKS 100
#define PX4_MAX_TASKS 50
#define SHELL_TASK_ID (PX4_MAX_TASKS+1)
pthread_t _shell_task_id = 0;
struct task_entry
{
pthread_t pid;
@ -243,7 +247,7 @@ int px4_task_kill(px4_task_t id, int sig)
pthread_t pid;
PX4_DEBUG("Called px4_task_kill %d", sig);
if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0)
pid = taskmap[id].pid;
else
return -EINVAL;
@ -273,6 +277,24 @@ void px4_show_tasks()
}
__BEGIN_DECLS
int px4_getpid()
{
pthread_t pid = pthread_self();
if (pid == _shell_task_id)
return SHELL_TASK_ID;
// Get pthread ID from the opaque ID
for (int i=0; i<PX4_MAX_TASKS; ++i) {
if (taskmap[i].isused && taskmap[i].pid == pid) {
return i;
}
}
PX4_ERR("px4_getpid() called from unknown thread context!");
return -EINVAL;
}
const char *getprogname();
const char *getprogname()
{

1
src/platforms/posix/px4_layer/sq_addlast.c
View File

@ -41,6 +41,7 @@
* Included Files
************************************************************/
#include <stddef.h>
#include <queue.h>
/************************************************************

1
src/platforms/posix/px4_layer/sq_remfirst.c
View File

@ -41,6 +41,7 @@
* Included Files
************************************************************/
#include <stddef.h>
#include <queue.h>
/************************************************************

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

@ -0,0 +1,19 @@
//#pragma once
#include <semaphore.h>
#include <stdio.h>
#include "work_lock.h"
extern sem_t _work_lock[];
void work_lock(int id)
{
//printf("work_lock %d\n", id);
sem_wait(&_work_lock[id]);
}
void work_unlock(int id)
{
//printf("work_unlock %d\n", id);
sem_post(&_work_lock[id]);
}

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

@ -31,23 +31,13 @@
*
****************************************************************************/
#include <semaphore.h>
#include <stdio.h>
#ifndef _work_lock_h_
#define _work_lock_h_
#pragma once
extern sem_t _work_lock[];
inline void work_lock(int id);
inline void work_lock(int id)
{
//printf("work_lock %d\n", id);
sem_wait(&_work_lock[id]);
}
//#pragma once
inline void work_unlock(int id);
inline void work_unlock(int id)
{
//printf("work_unlock %d\n", id);
sem_post(&_work_lock[id]);
}
void work_lock(int id);
void work_unlock(int id);
#endif // _work_lock_h_

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

@ -0,0 +1,47 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Publisher Example Application
#
MODULE_COMMAND = muorb_test
INCLUDE_DIRS += ${PX4_BASE}../muorb_krait \
${PX4_BASE}../muorb_krait/lib/include \
${PX4_BASE}../muorb_krait/Pal/lib
SRCS = muorb_test_main.cpp \
muorb_test_start_posix.cpp \
muorb_test_example.cpp

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

@ -0,0 +1,118 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_example.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
#include <px4_defines.h>
px4::AppState MuorbTestExample::appState;
int MuorbTestExample::main()
{
appState.setRunning(true);
int i=0;
orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status );
if( pub_id == 0 )
{
PX4_ERR( "error publishing esc_status" );
return -1;
}
orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc );
if( pub_id_vc == 0 )
{
PX4_ERR( "error publishing vehicle_command" );
return -1;
}
if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
{
PX4_ERR( "[%d]Error publishing the vechile command message", i );
return -1;
}
int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) );
if ( sub_vc == PX4_ERROR )
{
PX4_ERR( "Error subscribing to vehicle_command topic" );
return -1;
}
while (!appState.exitRequested() && i<100) {
PX4_DEBUG("[%d] Doing work...", i );
if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR )
{
PX4_ERR( "[%d]Error publishing the esc status message for iter", i );
break;
}
bool updated = false;
if( orb_check( sub_vc, &updated ) == 0 )
{
if( updated )
{
PX4_DEBUG( "[%d]Vechicle Status is updated... reading new value", i );
if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 )
{
PX4_ERR( "[%d]Error calling orb copy for vechivle status... ", i );
break;
}
if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR )
{
PX4_ERR( "[%d]Error publishing the vechile command message", i );
break;
}
}
else
{
PX4_DEBUG( "[%d] VC topic is not updated", i );
}
}
else
{
PX4_ERR( "[%d]Error checking the updated status for vechile command... ", i );
break;
}
++i;
}
return 0;
}

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

@ -0,0 +1,59 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_example.h
* Example app for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#pragma once
#include <px4_app.h>
#include "uORB/topics/esc_status.h"
#include "uORB/topics/vehicle_command.h"
class MuorbTestExample {
public:
MuorbTestExample() {};
~MuorbTestExample() {};
int main();
static px4::AppState appState; /* track requests to terminate app */
private:
struct esc_status_s m_esc_status;
struct vehicle_command_s m_vc;
};

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

@ -0,0 +1,66 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_main.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_log.h>
#include <px4_app.h>
#include "muorb_test_example.h"
#include <stdio.h>
#include "uORB/uORBManager.hpp"
#include "uORBKraitFastRpcChannel.hpp"
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "muorb_test");
PX4_DEBUG("muorb_test");
// register the fast rpc channel with UORB.
uORB::Manager::get_instance()->set_uorb_communicator( uORB::KraitFastRpcChannel::GetInstance() );
// start the KaitFastRPC channel thread.
uORB::KraitFastRpcChannel::GetInstance()->Start();
MuorbTestExample hello;
hello.main();
uORB::KraitFastRpcChannel::GetInstance()->Stop();
PX4_DEBUG("goodbye");
return 0;
}

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

@ -0,0 +1,101 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_start_linux.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]);
static void usage()
{
PX4_DEBUG("usage: muorb_test {start|stop|status}");
}
int muorb_test_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("already running");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("muorb_test",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
16000,
PX4_MAIN,
(char* const*)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
MuorbTestExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("is running");
} else {
PX4_DEBUG("not started");
}
return 0;
}
usage();
return 1;
}

29
src/platforms/px4_log.h
View File

@ -52,11 +52,24 @@
}
#if defined(__PX4_QURT)
#include <stdio.h>
#define FARF printf
#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__);
#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__);
#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__);
#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__);
#define __FARF_omit(level, ...) { }
#define __FARF_log(level, ...) { \
FARF("%-5s ", level);\
FARF(__VA_ARGS__);\
FARF("\n");\
}
#define __FARF_log_verbose(level, ...) { \
FARF("%-5s ", level);\
FARF(__VA_ARGS__);\
FARF(" (file %s line %d)\n", __FILE__, __LINE__);\
}
#define PX4_DEBUG(...) __FARF_omit("DEBUG", __VA_ARGS__)
#define PX4_INFO(...) __FARF_log("INFO", __VA_ARGS__)
#define PX4_WARN(...) __FARF_log_verbose("WARN", __VA_ARGS__)
#define PX4_ERR(...) __FARF_log_verbose("ERROR", __VA_ARGS__)
#elif defined(__PX4_LINUX)
#include <stdio.h>
@ -68,10 +81,10 @@
printf(" (file %s line %d)\n", __FILE__, __LINE__);\
}
#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__);
#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__);
#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__);
#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__);
#define PX4_DEBUG(...) __px4_log_omit("DEBUG", __VA_ARGS__)
#define PX4_INFO(...) __px4_log("INFO", __VA_ARGS__)
#define PX4_WARN(...) __px4_log_verbose("WARN", __VA_ARGS__)
#define PX4_ERR(...) __px4_log_verbose("ERROR", __VA_ARGS__)
#elif defined(__PX4_ROS)

2
src/platforms/px4_posix.h
View File

@ -68,6 +68,7 @@ typedef struct pollfd px4_pollfd_struct_t;
#define px4_poll _GLOBAL poll
#define px4_fsync _GLOBAL fsync
#define px4_access _GLOBAL access
#define px4_getpid _GLOBAL getpid
#elif defined(__PX4_POSIX)
@ -98,6 +99,7 @@ __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg);
__EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout);
__EXPORT int px4_fsync(int fd);
__EXPORT int px4_access(const char *pathname, int mode);
__EXPORT int px4_getpid(void);
__END_DECLS
#else

1
src/platforms/px4_time.h
View File

@ -28,7 +28,6 @@ struct timespec
int px4_clock_gettime(clockid_t clk_id, struct timespec *tp);
int px4_clock_settime(clockid_t clk_id, struct timespec *tp);
__EXPORT int usleep(useconds_t usec);
__EXPORT unsigned int sleep(unsigned int sec);
__END_DECLS

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

@ -0,0 +1,64 @@
/****************************************************************************
*
* 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 <stdio.h>
#include <dlfcn.h>
#define STACK_SIZE 0x8000
static char __attribute__ ((aligned (16))) stack1[STACK_SIZE];
int main(int argc, char* argv[])
{
int ret = 0;
char *builtin[]={"libgcc.so", "libc.so", "libstdc++.so"};
void *handle;
char *error;
void (*entry_function)() = NULL;
printf("In DSPAL main\n");
dlinit(3, builtin);
#if 0
handle = dlopen ("libdspal_client.so", RTLD_LAZY);
if (!handle) {
printf("Error opening libdspal_client.so\n");
return 1;
}
entry_function = dlsym(handle, "dspal_entry");
if (((error = dlerror()) != NULL) || (entry_function == NULL)) {
printf("Error dlsym for dspal_entry");
ret = 2;
}
dlclose(handle);
#endif
return ret;
}

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

@ -0,0 +1,47 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file commands_muorb_test.c
* Commands to run for the "qurt_muorb_test" config
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
const char *get_commands()
{
static const char *commands =
"uorb start\n"
"muorb_test start\n";
return commands;
}

129
src/platforms/qurt/px4_layer/hrt_queue.c Normal file
View File

@ -0,0 +1,129 @@
/****************************************************************************
* libc/wqueue/work_queue.c
*
* Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <px4_config.h>
#include <px4_defines.h>
#include <signal.h>
#include <stdint.h>
#include <queue.h>
#include <stdio.h>
#include <semaphore.h>
#include <drivers/drv_hrt.h>
#include <px4_workqueue.h>
#include "hrt_work.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Private Variables
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: hrt_work_queue
*
* Description:
* Queue work to be performed at a later time. All queued work will be
* performed on the worker thread of of execution (not the caller's).
*
* The work structure is allocated by caller, but completely managed by
* the work queue logic. The caller should never modify the contents of
* the work queue structure; the caller should not call work_queue()
* again until either (1) the previous work has been performed and removed
* from the queue, or (2) work_cancel() has been called to cancel the work
* and remove it from the work queue.
*
* Input parameters:
* work - The work structure to queue
* worker - The worker callback to be invoked. The callback will invoked
* on the worker thread of execution.
* arg - The argument that will be passed to the workder callback when
* int is invoked.
* delay - Delay (in microseconds) from the time queue until the worker
* is invoked. Zero means to perform the work immediately.
*
* Returned Value:
* Zero on success, a negated errno on failure
*
****************************************************************************/
int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
struct wqueue_s *wqueue = &g_hrt_work;
/* First, initialize the work structure */
work->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */
/* Now, time-tag that entry and put it in the work queue. This must be
* done with interrupts disabled. This permits this function to be called
* from with task logic or interrupt handlers.
*/
hrt_work_lock();
work->qtime = hrt_absolute_time(); /* Time work queued */
//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
dq_addlast((dq_entry_t *)work, &wqueue->q);
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
hrt_work_unlock();
return PX4_OK;
}

256
src/platforms/qurt/px4_layer/hrt_thread.c Normal file
View File

@ -0,0 +1,256 @@
/****************************************************************************
* libc/wqueue/work_thread.c
*
* Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
* Modified by: Mark Charlebois to use hrt compatible times
*
* 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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <px4_config.h>
#include <px4_defines.h>
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <queue.h>
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include "hrt_work.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/* The state of each work queue. */
struct wqueue_s g_hrt_work;
/****************************************************************************
* Private Variables
****************************************************************************/
sem_t _hrt_work_lock;
/****************************************************************************
* Private Functions
****************************************************************************/
static void hrt_work_process(void);
/****************************************************************************
* Name: work_process
*
* Description:
* This is the logic that performs actions placed on any work list.
*
* Input parameters:
* wqueue - Describes the work queue to be processed
*
* Returned Value:
* None
*
****************************************************************************/
static void hrt_work_process()
{
struct wqueue_s *wqueue = &g_hrt_work;
volatile FAR struct work_s *work;
worker_t worker;
FAR void *arg;
uint64_t elapsed;
uint32_t remaining;
uint32_t next;
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
/* Default to sleeping for 1 sec */
next = 1000000;
hrt_work_lock();
work = (FAR struct work_s *)wqueue->q.head;
while (work)
{
/* Is this work ready? It is ready if there is no delay or if
* the delay has elapsed. qtime is the time that the work was added
* to the work queue. It will always be greater than or equal to
* zero. Therefore a delay of zero will always execute immediately.
*/
elapsed = hrt_absolute_time() - work->qtime;
//PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work);
if (elapsed >= work->delay)
{
/* Remove the ready-to-execute work from the list */
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
//PX4_INFO("Dequeued work=%p", work);
/* Extract the work description from the entry (in case the work
* instance by the re-used after it has been de-queued).
*/
worker = work->worker;
arg = work->arg;
/* Mark the work as no longer being queued */
work->worker = NULL;
/* Do the work. Re-enable interrupts while the work is being
* performed... we don't have any idea how long that will take!
*/
hrt_work_unlock();
if (!worker) {
PX4_ERR("MESSED UP: worker = 0");
}
else {
worker(arg);
}
/* Now, unfortunately, since we re-enabled interrupts we don't
* know the state of the work list and we will have to start
* back at the head of the list.
*/
hrt_work_lock();
work = (FAR struct work_s *)wqueue->q.head;
}
else
{
/* This one is not ready.. will it be ready before the next
* scheduled wakeup interval?
*/
/* Here: elapsed < work->delay */
remaining = work->delay - elapsed;
//PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed);
if (remaining < next)
{
/* Yes.. Then schedule to wake up when the work is ready */
next = remaining;
}
/* Then try the next in the list. */
work = (FAR struct work_s *)work->dq.flink;
//PX4_INFO("next %u work %p", next, work);
}
}
/* Wait awhile to check the work list. We will wait here until either
* the time elapses or until we are awakened by a signal.
*/
hrt_work_unlock();
/* might sleep less if a signal received and new item was queued */
//PX4_INFO("Sleeping for %u usec", next);
usleep(next);
}
/****************************************************************************
* Name: work_hrtthread
*
* Description:
* This is the worker threads that performs actions placed on the ISR work
* list.
*
* work_hpthread and work_lpthread: These are the kernel mode work queues
* (also build in the flat build). One of these threads also performs
* periodic garbage collection (that is otherwise performed by the idle
* thread if CONFIG_SCHED_WORKQUEUE is not defined).
*
* These worker threads are started by the OS during normal bringup.
*
* All of these entrypoints are referenced by OS internally and should not
* not be accessed by application logic.
*
* Input parameters:
* argc, argv (not used)
*
* Returned Value:
* Does not return
*
****************************************************************************/
static int work_hrtthread(int argc, char *argv[])
{
/* Loop forever */
for (;;)
{
/* First, perform garbage collection. This cleans-up memory de-allocations
* that were queued because they could not be freed in that execution
* context (for example, if the memory was freed from an interrupt handler).
* NOTE: If the work thread is disabled, this clean-up is performed by
* the IDLE thread (at a very, very low priority).
*/
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
hrt_work_process();
}
return PX4_OK; /* To keep some compilers happy */
}
/****************************************************************************
* Public Functions
****************************************************************************/
void hrt_work_queue_init(void)
{
sem_init(&_hrt_work_lock, 0, 1);
// Create high priority worker thread
g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
2000,
work_hrtthread,
(char* const*)NULL);
}

65
src/platforms/qurt/px4_layer/hrt_work.h Normal file
View File

@ -0,0 +1,65 @@
/****************************************************************************
*
* 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 <px4_log.h>
#include <semaphore.h>
#include <px4_workqueue.h>
#pragma once
__BEGIN_DECLS
extern sem_t _hrt_work_lock;
extern struct wqueue_s g_hrt_work;
void hrt_work_queue_init(void);
int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay);
void hrt_work_cancel(struct work_s *work);
inline void hrt_work_lock(void);
inline void hrt_work_unlock(void);
inline void hrt_work_lock()
{
//PX4_INFO("hrt_work_lock");
sem_wait(&_hrt_work_lock);
}
inline void hrt_work_unlock()
{
//PX4_INFO("hrt_work_unlock");
sem_post(&_hrt_work_lock);
}
__END_DECLS

111
src/platforms/qurt/px4_layer/hrt_work_cancel.c Normal file
View File

@ -0,0 +1,111 @@
/****************************************************************************
* libc/wqueue/work_cancel.c
*
* Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 NuttX 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <px4_config.h>
#include <px4_defines.h>
#include <queue.h>
#include <px4_workqueue.h>
#include "hrt_work.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Private Variables
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: hrt_work_cancel
*
* Description:
* Cancel previously queued work. This removes work from the work queue.
* After work has been canceled, it may be re-queue by calling
* hrt_work_queue() again.
*
* Input parameters:
* work - The previously queue work structure to cancel
*
****************************************************************************/
void hrt_work_cancel(struct work_s *work)
{
struct wqueue_s *wqueue = &g_hrt_work;
//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
/* Cancelling the work is simply a matter of removing the work structure
* from the work queue. This must be done with interrupts disabled because
* new work is typically added to the work queue from interrupt handlers.
*/
hrt_work_lock();
if (work->worker != NULL)
{
/* A little test of the integrity of the work queue */
//DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail);
//DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head);
/* Remove the entry from the work queue and make sure that it is
* mark as availalbe (i.e., the worker field is nullified).
*/
dq_rem((FAR dq_entry_t *)work, &wqueue->q);
work->worker = NULL;
}
hrt_work_unlock();
}

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

@ -39,6 +39,7 @@
#include <px4_middleware.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <px4_posix.h>
#include <vector>
#include <string>
@ -131,14 +132,18 @@ namespace px4 {
extern void init_once(void);
};
int main(int argc, char **argv)
__BEGIN_DECLS
void dspal_entry()
{
const char *argv[2] = { "dspal_client", NULL };
int argc = 1;
printf("In main\n");
map<string,px4_main_t> apps;
init_app_map(apps);
px4::init_once();
px4::init(argc, argv, "mainapp");
px4::init(argc, (char **)argv, "mainapp");
process_commands(apps, get_commands());
for (;;) {}
for (;;) { sleep(100000); }
}
__END_DECLS

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

@ -40,8 +40,12 @@ SRCDIR=$(dir $(MODULE_MK))
SRCS = \
px4_qurt_impl.cpp \
px4_qurt_tasks.cpp \
hrt_thread.c \
hrt_queue.c \
hrt_work_cancel.c \
work_thread.c \
work_queue.c \
work_lock.c \
work_cancel.c \
lib_crc32.c \
drv_hrt.c \
@ -52,12 +56,16 @@ SRCS = \
sq_remfirst.c \
sq_addafter.c \
dq_rem.c \
main.cpp
main.cpp \
qurt_stubs.c
ifeq ($(CONFIG),qurt_hello)
SRCS += commands_hello.c
endif
ifeq ($(CONFIG),qurt_default)
SRCS += commands_default.c
endif
ifeq ($(CONFIG),qurt_muorb_test)
SRCS += commands_muorb_test.c
endif
MAXOPTIMIZATION = -Os

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

@ -48,22 +48,21 @@
#include <errno.h>
#include <unistd.h>
#include <semaphore.h>
#include <qurt.h>
#include "systemlib/param/param.h"
#include "hrt_work.h"
extern pthread_t _shell_task_id;
__BEGIN_DECLS
// FIXME - sysconf(_SC_CLK_TCK) not supported
long PX4_TICKS_PER_SEC = 1000;
void usleep(useconds_t usec) {
qurt_timer_sleep(usec);
}
unsigned int sleep(unsigned int sec)
{
for (unsigned int i=0; i< sec; i++)
qurt_timer_sleep(1000000);
usleep(1000000);
return 0;
}
@ -79,7 +78,7 @@ void qurt_log(const char *fmt, ...)
}
#endif
extern int _posix_init(void);
//extern int _posix_init(void);
__END_DECLS
@ -94,9 +93,12 @@ void init_once(void);
void init_once(void)
{
// Required for QuRT
_posix_init();
//_posix_init();
_shell_task_id = pthread_self();
PX4_INFO("Shell id is %lu", _shell_task_id);
work_queues_init();
hrt_work_queue_init();
hrt_init();
}

28
src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
View File

@ -57,7 +57,11 @@
#define MAX_CMD_LEN 100
#define PX4_MAX_TASKS 100
#define PX4_MAX_TASKS 50
#define SHELL_TASK_ID (PX4_MAX_TASKS+1)
pthread_t _shell_task_id = 0;
struct task_entry
{
pthread_t pid;
@ -262,3 +266,25 @@ void px4_show_tasks()
PX4_INFO(" No running tasks");
}
__BEGIN_DECLS
int px4_getpid()
{
pthread_t pid = pthread_self();
if (pid == _shell_task_id)
return SHELL_TASK_ID;
// Get pthread ID from the opaque ID
for (int i=0; i<PX4_MAX_TASKS; ++i) {
if (taskmap[i].isused && taskmap[i].pid == pid) {
return i;
}
}
PX4_ERR("px4_getpid() called from unknown thread context!");
return -EINVAL;
}
__END_DECLS

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

@ -0,0 +1,89 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
//extern "C" {
void _Read_uleb( void )
{
}
void _Parse_fde_instr( void )
{
}
void _Parse_csd( void )
{
}
void _Locksyslock( void )
{
}
void _Unlocksyslock( void )
{
}
void _Valbytes( void )
{
}
void _Get_eh_data( void )
{
}
void _Parse_lsda( void )
{
}
void __cxa_guard_release( void )
{
}
void _Read_enc_ptr( void )
{
}
void _Read_sleb( void )
{
}
void __cxa_guard_acquire( void )
{
}
void __cxa_pure_virtual()
{
while (1);
}
//}

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

@ -0,0 +1,51 @@
/****************************************************************************
*
* 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 <px4_log.h>
#include <semaphore.h>
#include <stdio.h>
#include "work_lock.h"
extern sem_t _work_lock[];
void work_lock(int id)
{
//PX4_INFO("work_lock %d", id);
sem_wait(&_work_lock[id]);
}
void work_unlock(int id)
{
//PX4_INFO("work_unlock %d", id);
sem_post(&_work_lock[id]);
}

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

@ -31,23 +31,13 @@
*
****************************************************************************/
#include <semaphore.h>
#include <stdio.h>
#ifndef _work_lock_h_
#define _work_lock_h_
#pragma once
extern sem_t _work_lock[];
inline void work_lock(int id);
inline void work_lock(int id)
{
//printf("work_lock %d\n", id);
sem_wait(&_work_lock[id]);
}
//#pragma once
inline void work_unlock(int id);
inline void work_unlock(int id)
{
//printf("work_unlock %d\n", id);
sem_post(&_work_lock[id]);
}
void work_lock(int id);
void work_unlock(int id);
#endif // _work_lock_h_

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

@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Publisher Example Application
#
MODULE_COMMAND = muorb_test
SRCS = muorb_test_main.cpp \
muorb_test_start_qurt.cpp \
muorb_test_example.cpp

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

@ -0,0 +1,60 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_example.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
px4::AppState MuorbTestExample::appState;
int MuorbTestExample::main()
{
appState.setRunning(true);
int i=0;
while (!appState.exitRequested() && i<5) {
PX4_DEBUG(" Doing work...");
++i;
}
return 0;
}

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

@ -0,0 +1,47 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_app.h>
class MuorbTestExample {
public:
MuorbTestExample() {};
~MuorbTestExample() {};
int main();
static px4::AppState appState; /* track requests to terminate app */
};

55
src/platforms/qurt/tests/muorb/muorb_test_main.cpp Normal file
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.
*
****************************************************************************/
/**
* @file muorb_test_main.cpp
* Test of Multi-uORB supoprt
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_log.h>
#include <px4_app.h>
#include "muorb_test_example.h"
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "muorb_test");
PX4_DEBUG("muorb_test");
MuorbTestExample hello;
hello.main();
PX4_DEBUG("goodbye");
return 0;
}

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

@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file muorb_test_start_qurt.cpp
*
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]);
static void usage()
{
PX4_DEBUG("usage: muorb_test {start|stop|status}");
}
int muorb_test_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("already running");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("muorb_test",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
16000,
PX4_MAIN,
(char* const*)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
MuorbTestExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("is running");
} else {
PX4_DEBUG("not started");
}
return 0;
}
usage();
return 1;
}

3
src/systemcmds/mixer/mixer.cpp
View File

@ -100,6 +100,9 @@ usage(const char *reason)
static int
load(const char *devname, const char *fname)
{
// sleep a while to ensure device has been set up
usleep(20000);
int dev;
char buf[2048];

2
src/systemcmds/param/param.c
View File

@ -213,7 +213,9 @@ do_save(const char *param_file_name)
close(fd);
if (result < 0) {
#ifndef __PX4_QURT
(void)unlink(param_file_name);
#endif
warnx("error exporting to '%s'", param_file_name);
return 1;
}

Some files were not shown because too many files have changed in this diff Show More