diff --git a/.gitmodules b/.gitmodules index 3e6e75cb0e..cb9a6ccf05 100644 --- a/.gitmodules +++ b/.gitmodules @@ -19,3 +19,6 @@ [submodule "src/lib/eigen"] path = src/lib/eigen url = https://github.com/PX4/eigen.git +[submodule "src/lib/dspal"] + path = src/lib/dspal + url = https://github.com/mcharleb/dspal.git diff --git a/makefiles/posix-arm/config_eagle_adsp.mk b/makefiles/posix-arm/config_eagle_adsp.mk new file mode 100644 index 0000000000..e7e0d8cf6f --- /dev/null +++ b/makefiles/posix-arm/config_eagle_adsp.mk @@ -0,0 +1,57 @@ +# +# Makefile for the EAGLE *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device + +# +# System commands +# +MODULES += systemcmds/param +MODULES += systemcmds/ver + +# +# General system control +# +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# + +# +# Vehicle Control +# + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/uORB +MODULES += modules/dataman + +# +# 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/work_queue + +# +# Unit tests +# + +# +# muorb fastrpc changes. +# +MODULES += modules/muorb/krait diff --git a/makefiles/posix-arm/config_eagle_hil.mk b/makefiles/posix-arm/config_eagle_hil.mk index d79213dec0..522549d05b 100644 --- a/makefiles/posix-arm/config_eagle_hil.mk +++ b/makefiles/posix-arm/config_eagle_hil.mk @@ -6,6 +6,7 @@ # Board support modules # MODULES += drivers/device +MODULES += drivers/boards/sitl #MODULES += drivers/blinkm #MODULES += drivers/pwm_out_sim #MODULES += drivers/rgbled diff --git a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk index 12ea6fc29d..4eb3b0fd19 100644 --- a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk +++ b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk @@ -39,14 +39,17 @@ # 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 +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 +ifdef OECORE_NATIVE_SYSROOT +AR := $(AR) rcs +endif # Check if the right version of the toolchain is available # @@ -57,7 +60,9 @@ ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) $(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif -EXT_MUORB_LIB_ROOT = /opt/muorb_libs +ifndef POSIX_EXT_LIB_ROOT +$(error POSIX_EXT_LIB_ROOT is not set) +endif # XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup @@ -71,37 +76,6 @@ ARCHCPUFLAGS_CORTEXA8 = -mtune=cortex-a8 \ -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)) @@ -115,13 +89,13 @@ 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 + -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 # @@ -189,7 +163,8 @@ LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a) EXTRA_LIBS += -lpx4muorb -ladsprpc EXTRA_LIBS += -pthread -lm -lrt -LIB_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/libs +LIB_DIRS += $(POSIX_EXT_LIB_ROOT)/libs +INCLUDE_DIRS += $(POSIX_EXT_LIB_ROOT)/inc # Flags we pass to the C compiler # diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index 57ede0b78b..d77082a89a 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -5,6 +5,7 @@ # # Board support modules # +MODULES += drivers/boards/sitl MODULES += drivers/device MODULES += drivers/blinkm MODULES += drivers/pwm_out_sim diff --git a/makefiles/qurt/config_qurt_adsp.mk b/makefiles/qurt/config_qurt_adsp.mk index 6a1ce1d5f2..fcd63eef57 100644 --- a/makefiles/qurt/config_qurt_adsp.mk +++ b/makefiles/qurt/config_qurt_adsp.mk @@ -1,8 +1,19 @@ #Added configuration specific flags here. +ifndef HEXAGON_DRIVERS_ROOT +$(error HEXAGON_DRIVERS_ROOT is not set) +endif +ifndef EAGLE_DRIVERS_SRC +$(error EAGLE_DRIVERS_SRC is not set) +endif + +INCLUDE_DIRS += $(HEXAGON_DRIVERS_ROOT)/inc + # For Actual flight we need to link against the driver dynamic libraries -LDFLAGS += -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50 -LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc +LDFLAGS += -L${HEXAGON_DRIVERS_ROOT}/libs -lmpu9x50 +LDFLAGS += -luart_esc +LDFLAGS += -lcsr_gps +LDFLAGS += -lrc_receiver # # Makefile for the EAGLE QuRT *default* configuration @@ -13,8 +24,10 @@ LDFLAGS += -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_es # MODULES += drivers/device MODULES += modules/sensors -#MODULES += platforms/qurt/drivers/mpu9x50 -#MODULES += platforms/qurt/drivers/uart_esc +MODULES += $(EAGLE_DRIVERS_SRC)/mpu9x50 +MODULES += $(EAGLE_DRIVERS_SRC)/uart_esc +MODULES += $(EAGLE_DRIVERS_SRC)/rc_receiver +MODULES += $(EAGLE_DRIVERS_SRC)/csr_gps # # System commands @@ -47,6 +60,7 @@ MODULES += modules/systemlib/mixer MODULES += modules/uORB #MODULES += modules/dataman MODULES += modules/commander +MODULES += modules/controllib # # Libraries @@ -61,6 +75,7 @@ MODULES += lib/conversion # QuRT port # MODULES += platforms/qurt/px4_layer +MODULES += platforms/posix/work_queue # # Unit tests diff --git a/makefiles/qurt/config_qurt_hil.mk b/makefiles/qurt/config_qurt_hil.mk index 0536d7a4ad..6f14dd6a00 100644 --- a/makefiles/qurt/config_qurt_hil.mk +++ b/makefiles/qurt/config_qurt_hil.mk @@ -6,6 +6,7 @@ # Board support modules # MODULES += drivers/device +MODULES += drivers/boards/sitl #MODULES += drivers/blinkm MODULES += drivers/pwm_out_sim MODULES += drivers/led diff --git a/makefiles/qurt/firmware_qurt.mk b/makefiles/qurt/firmware_qurt.mk index 50c4b267b4..ac87683487 100644 --- a/makefiles/qurt/firmware_qurt.mk +++ b/makefiles/qurt/firmware_qurt.mk @@ -53,8 +53,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: generateuorbtopicheaders WORK_DIR=$(work_dir) \ $(FIRMWARE_GOAL) -HEXAGON_TOOLS_ROOT = /opt/6.4.05 -#V_ARCH = v4 +HEXAGON_TOOLS_ROOT ?= /opt/6.4.03 V_ARCH = v5 HEXAGON_CLANG_BIN = $(addsuffix /qc/bin,$(HEXAGON_TOOLS_ROOT)) SIM = $(HEXAGON_CLANG_BIN)/hexagon-sim diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index c54a74f203..77a9d4e838 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -35,14 +35,6 @@ #$(info TOOLCHAIN gnu-arm-eabi) -# -# Stop making if ADSP_LIB_ROOT is not set. This defines the path to -# DspAL headers and driver headers -# -ifndef DSPAL_ROOT -$(error DSPAL_ROOT is not set) -endif - # Toolchain commands. Normally only used inside this file. # HEXAGON_TOOLS_ROOT ?= /opt/6.4.03 @@ -57,7 +49,7 @@ HEXAGON_ISS_DIR = $(HEXAGON_TOOLS_ROOT)/qc/lib/iss TOOLSLIB = $(HEXAGON_TOOLS_ROOT)/dinkumware/lib/$(V_ARCH)/G0 QCTOOLSLIB = $(HEXAGON_TOOLS_ROOT)/qc/lib/$(V_ARCH)/G0 QURTLIB = $(HEXAGON_SDK_ROOT)/lib/common/qurt/ADSP$(V_ARCH)MP/lib -#DSPAL = $(PX4_BASE)/../dspal_libs/libdspal.a +DSPAL_INCS ?= $(PX4_BASE)/src/lib/dspal CC = $(HEXAGON_CLANG_BIN)/$(CROSSDEV)clang @@ -93,8 +85,7 @@ DYNAMIC_LIBS = \ # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 6.4.03 -#CROSSDEV_VER_SUPPORTED = 6.4.05 +CROSSDEV_VER_SUPPORTED = 6.4.03 6.4.05 CROSSDEV_VER_FOUND = $(shell $(CC) --version | sed -n 's/^.*version \([\. 0-9]*\),.*$$/\1/p') ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) @@ -122,18 +113,15 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) \ -Dnoreturn_function= \ -D__EXPORT= \ -Drestrict= \ - -D_DEBUG \ - -I$(DSPAL_ROOT)/ \ - -I$(DSPAL_ROOT)/dspal/include \ - -I$(DSPAL_ROOT)/dspal/sys \ - -I$(DSPAL_ROOT)/dspal/sys/sys \ - -I$(DSPAL_ROOT)/mpu_spi/inc/ \ - -I$(DSPAL_ROOT)/uart_esc/inc/ \ + -D_DEBUG \ + -I$(DSPAL_INCS)/include \ + -I$(DSPAL_INCS)/sys \ -I$(HEXAGON_TOOLS_ROOT)/gnu/hexagon/include \ -I$(PX4_BASE)/src/lib/eigen \ -I$(PX4_BASE)/src/platforms/qurt/include \ -I$(PX4_BASE)/src/platforms/posix/include \ -I$(PX4_BASE)/mavlink/include/mavlink \ + -I$(PX4_BASE)/../inc \ -I$(QURTLIB)/..//include \ -I$(HEXAGON_SDK_ROOT)/inc \ -I$(HEXAGON_SDK_ROOT)/inc/stddef \ diff --git a/msg/input_rc.msg b/msg/input_rc.msg index c07d229503..8e15a9a186 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -5,6 +5,7 @@ uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 uint8 RC_INPUT_SOURCE_MAVLINK = 6 +uint8 RC_INPUT_SOURCE_QURT = 7 uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. diff --git a/src/drivers/boards/sitl/module.mk b/src/drivers/boards/sitl/module.mk new file mode 100644 index 0000000000..c67272fc53 --- /dev/null +++ b/src/drivers/boards/sitl/module.mk @@ -0,0 +1,8 @@ +# +# Board-specific startup code for SITL +# + +SRCS = \ + sitl_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/sitl/sitl_led.c b/src/drivers/boards/sitl/sitl_led.c new file mode 100644 index 0000000000..53d033234f --- /dev/null +++ b/src/drivers/boards/sitl/sitl_led.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file sitl_led.c + * + * sitl LED backend. + */ + +#include +#include +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static bool _led_state[2] = { false , false }; + +__EXPORT void led_init() +{ + PX4_DEBUG("LED_INIT"); +} + +__EXPORT void led_on(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_ON", led); + _led_state[led] = true; + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1 || led == 0) { + PX4_DEBUG("LED%d_OFF", led); + _led_state[led] = false; + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1 || led == 0) { + _led_state[led] = !_led_state[led]; + PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); + + } +} diff --git a/src/lib/dspal b/src/lib/dspal new file mode 160000 index 0000000000..1741154446 --- /dev/null +++ b/src/lib/dspal @@ -0,0 +1 @@ +Subproject commit 1741154446c7c92c6e632d7ee1619c6d4174bf2d diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp index d729e6cd7b..a2f6850cbe 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp @@ -38,7 +38,7 @@ #include #include #include "uORB/uORBCommunicator.hpp" -#include +#include #include #include "drivers/drv_hrt.h" diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index f8c8fcec0f..5d9e29be3a 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -150,10 +150,6 @@ static void usage() __BEGIN_DECLS extern int simulator_main(int argc, char *argv[]); -extern void led_init(void); -extern void led_on(int led); -extern void led_off(int led); -extern void led_toggle(int led); __END_DECLS extern "C" { @@ -207,39 +203,3 @@ int simulator_main(int argc, char *argv[]) } } - -bool static _led_state[2] = { false , false }; - -__EXPORT void led_init() -{ - PX4_DEBUG("LED_INIT"); -} - -__EXPORT void led_on(int led) -{ - if (led == 1 || led == 0) - { - PX4_DEBUG("LED%d_ON", led); - _led_state[led] = true; - } -} - -__EXPORT void led_off(int led) -{ - if (led == 1 || led == 0) - { - PX4_DEBUG("LED%d_OFF", led); - _led_state[led] = false; - } -} - -__EXPORT void led_toggle(int led) -{ - if (led == 1 || led == 0) - { - _led_state[led] = !_led_state[led]; - PX4_DEBUG("LED%d_TOGGLE: %s", led, _led_state[led] ? "ON" : "OFF"); - - } -} - diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index c8920607ac..5a13a864c1 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -284,7 +284,7 @@ int orb_group_count(const struct orb_metadata *meta) * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ -int orb_priority(int handle, int *priority) +int orb_priority(int handle, int32_t *priority) { return uORB::Manager::get_instance()->orb_priority(handle, priority); } diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index f998c8252c..04158234bd 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -269,7 +269,7 @@ public: * priority, independent of the startup order of the associated publishers. * @return OK on success, ERROR otherwise with errno set accordingly. */ - int orb_priority(int handle, int *priority) ; + int orb_priority(int handle, int32_t *priority) ; /** * Set the minimum interval between which updates are seen for a subscription. diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index 8b2051aacf..3c4d886023 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -166,7 +166,7 @@ int uORB::Manager::orb_stat(int handle, uint64_t *time) return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } -int uORB::Manager::orb_priority(int handle, int *priority) +int uORB::Manager::orb_priority(int handle, int32_t *priority) { return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 68bfa3f7fe..7fe3f6a687 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -172,7 +172,7 @@ int uORB::Manager::orb_stat(int handle, uint64_t *time) return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } -int uORB::Manager::orb_priority(int handle, int *priority) +int uORB::Manager::orb_priority(int handle, int32_t *priority) { return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); } diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index d071893b0c..a8208313e6 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -127,6 +127,7 @@ __EXPORT extern uint64_t hrt_absolute_time(void); __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern int __px4_log_level_current; +__END_DECLS // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME #define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN @@ -162,22 +163,6 @@ __EXPORT extern int __px4_log_level_current; ****************************************************************************/ -/**************************************************************************** - * __px4_log_named_cond: - * Convert a message in the form: - * PX4_LOG_COND(__dbg_enabled, "val is %d", val); - * to - * printf("%-5s val is %d\n", "LOG", val); - * if the first arg/condition is true. - ****************************************************************************/ -#define __px4_log_named_cond(name, cond, FMT, ...) \ - __px4__log_startcond(cond)\ - "%s " \ - FMT\ - __px4__log_end_fmt \ - ,name, ##__VA_ARGS__\ - __px4__log_endline - /**************************************************************************** * __px4_log_named_cond: * Convert a message in the form: @@ -389,5 +374,4 @@ __EXPORT extern int __px4_log_level_current; #endif #define PX4_LOG_NAMED(name, FMT, ...) __px4_log_named_cond(name, true, FMT, ##__VA_ARGS__) #define PX4_LOG_NAMED_COND(name, cond, FMT, ...) __px4_log_named_cond(name, cond, FMT, ##__VA_ARGS__) -__END_DECLS #endif diff --git a/src/platforms/qurt/eagle_px4_firmware_patch.patch b/src/platforms/qurt/eagle_px4_firmware_patch.patch deleted file mode 100644 index 41ac284b51..0000000000 --- a/src/platforms/qurt/eagle_px4_firmware_patch.patch +++ /dev/null @@ -1,190 +0,0 @@ -diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h -index 4ba2f98..a7b664e 100644 ---- a/src/include/mavlink/mavlink_log.h -+++ b/src/include/mavlink/mavlink_log.h -@@ -106,10 +106,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ --#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ -- fprintf(stderr, "telem> "); \ -+#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); -+/* fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); -+*/ - - /** - * Send a mavlink critical message and print to console. -@@ -117,10 +118,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ --#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ -- fprintf(stderr, "telem> "); \ -+#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__) -+/* fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); -+*/ - - /** - * Send a mavlink emergency message and print to console. -@@ -128,11 +130,11 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ --#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ -- fprintf(stderr, "telem> "); \ -+#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); -+/* fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); -- -+*/ - struct mavlink_logmessage { - char text[MAVLINK_LOG_MAXLEN + 1]; - unsigned char severity; -diff --git a/src/lib/eigen b/src/lib/eigen ---- a/src/lib/eigen -+++ b/src/lib/eigen -@@ -1 +1 @@ --Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71 -+Subproject commit e7850ed81f9c469e02df496ef09ae32ec0379b71-dirty -diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp -index bbf5f8e..50c7d8b 100644 ---- a/src/modules/commander/PreflightCheck.cpp -+++ b/src/modules/commander/PreflightCheck.cpp -@@ -378,6 +378,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro - } - } - -+ -+#ifdef __PX4_QURT -+ // WARNING: Preflight checks are important and should be added back when -+ // all the sensors are supported -+ PX4_WARN("WARNING: Preflight checks PASS always."); -+ failed = false; -+#endif -+ - /* Report status */ - return !failed; - } -diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp -index 971e086..c165322 100644 ---- a/src/modules/commander/commander.cpp -+++ b/src/modules/commander/commander.cpp -@@ -1836,8 +1836,13 @@ int commander_thread_main(int argc, char *argv[]) - } - - /* RC input check */ -+#ifndef __PX4_QURT - if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && -- hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { -+ hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { -+#else -+ // HACK: remove old data check due to timestamp issue in QURT -+ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 ) { -+#endif - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; -@@ -2526,7 +2531,7 @@ set_control_mode() - /* set vehicle_control_mode according to set_navigation_state */ - control_mode.flag_armed = armed.armed; - control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); -- control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; -+ //control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; - control_mode.flag_control_offboard_enabled = false; - - switch (status.nav_state) { -diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c -index 892e50f..7ac6776 100644 ---- a/src/modules/position_estimator_inav/position_estimator_inav_main.c -+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c -@@ -384,7 +384,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) - }; - - /* wait for initial baro value */ -+#ifdef __PX4_QURT -+ // WARNING skipping -+ PX4_WARN("Hacked to skip baro initialization for testing."); -+ bool wait_baro = false; -+#else - bool wait_baro = true; -+#endif - - thread_running = true; - -diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp -index 3fa4458..0ef1883 100644 ---- a/src/modules/sensors/sensors.cpp -+++ b/src/modules/sensors/sensors.cpp -@@ -1099,7 +1099,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; - -+#ifdef __PX4_QURT -+// Temporary fix required due to lack of time sync between apps and dsp cores -+ raw.timestamp = hrt_absolute_time(); -+#else - raw.timestamp = gyro_report.timestamp; -+#endif - raw.gyro_errcount = gyro_report.error_count; - raw.gyro_temp = gyro_report.temperature; - } -@@ -2068,6 +2073,7 @@ Sensors::task_main() - - /* start individual sensors */ - int ret = 0; -+#if 0 - do { /* create a scope to handle exit with break */ - ret = accel_init(); - if (ret) break; -@@ -2091,7 +2097,7 @@ Sensors::task_main() - } - return; - } -- -+#endif - /* - * do subscriptions - */ -diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp -index 83a9378..ec86faf 100644 ---- a/src/systemcmds/mixer/mixer.cpp -+++ b/src/systemcmds/mixer/mixer.cpp -@@ -118,6 +118,8 @@ load(const char *devname, const char *fname) - return 1; - } - -+#ifndef __PX4_QURT -+ - if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { - warnx("can't load mixer: %s", fname); - return 1; -@@ -125,6 +127,24 @@ load(const char *devname, const char *fname) - - /* XXX pass the buffer to the device */ - int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); -+#else -+ char newbuf[] = -+ "R: 4x 10000 10000 10000 0\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 4 10000 10000 0 -10000 10000\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 5 10000 10000 0 -10000 10000\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 6 10000 10000 0 -10000 10000\n" -+ "M: 1\n" -+ "O: 10000 10000 0 -10000 10000\n" -+ "S: 0 7 10000 10000 0 -10000 10000\n"; -+ /* XXX pass the buffer to the device */ -+ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf); -+#endif - - if (ret < 0) { - warnx("error loading mixers from %s", fname); diff --git a/src/platforms/qurt/px4_layer/commands_adsp.c b/src/platforms/qurt/px4_layer/commands_adsp.c new file mode 100644 index 0000000000..0e6ee8402c --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_adsp.c @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file commands_muorb_test.c + * Commands to run for the "qurt_muorb_test" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "commander start\n" + "param set RC_RECEIVER_TYPE 1\n" + "rc_receiver start -D /dev/tty-1\n" + "attitude_estimator_q start\n" + "position_estimator_inav start\n" + //"ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "sleep 1\n" + + // Channel mapping for Spektrum DX8 + "param set RC_MAP_THROTTLE 1\n" + "param set RC_MAP_ROLL 2\n" + "param set RC_MAP_PITCH 3\n" + "param set RC_MAP_YAW 4\n" + "param set RC_MAP_MODE_SW 5\n" + "param set RC_MAP_POSCTL_SW 6\n" + + // RC calibration for Spektrum DX8 + "param set RC1_MAX 852\n" + "param set RC1_MIN 171\n" + "param set RC1_TRIM 171\n" + "param set RC1_REV 1\n" + "param set RC2_MAX 852\n" + "param set RC2_MIN 171\n" + "param set RC2_TRIM 512\n" + "param set RC2_REV -1\n" + "param set RC3_MAX 852\n" + "param set RC3_MIN 171\n" + "param set RC3_TRIM 512\n" + "param set RC3_REV 1\n" + "param set RC4_MAX 852\n" + "param set RC4_MIN 171\n" + "param set RC4_TRIM 514\n" + "param set RC4_REV -1\n" + "param set RC5_MAX 852\n" + "param set RC5_MIN 171\n" + "param set RC5_TRIM 512\n" + "param set RC5_REV 1\n" + "param set RC6_MAX 852\n" + "param set RC6_MIN 171\n" + "param set RC6_TRIM 171\n" + "param set RC6_REV 1\n" + + // // RC calibration for DX6i + // "param set RC1_MAX 2015\n" + // "param set RC1_MIN 996\n" + // "param set RC1_TRIM 1502\n" + // "param set RC1_REV -1\n" + // "param set RC2_MAX 2016 \n" + // "param set RC2_MIN 995\n" + // "param set RC2_TRIM 1500\n" + // "param set RC3_MAX 2003\n" + // "param set RC3_MIN 992\n" + // "param set RC3_TRIM 992\n" + // "param set RC4_MAX 2011\n" + // "param set RC4_MIN 997\n" + // "param set RC4_TRIM 1504\n" + // "param set RC4_REV -1\n" + // "param set RC6_MAX 2016\n" + // "param set RC6_MIN 992\n" + // "param set RC6_TRIM 1504\n" + // "param set RC_CHAN_CNT 8\n" + // "param set RC_MAP_MODE_SW 5\n" + // "param set RC_MAP_POSCTL_SW 7\n" + // "param set RC_MAP_RETURN_SW 8\n" + + "sensors start\n" + "param set MC_YAW_P 7.0\n" + "param set MC_YAWRATE_P 0.1125\n" + "param set MC_YAWRATE_I 0.0\n" + "param set MC_YAWRATE_D 0\n" + "param set MC_PITCH_P 6.0\n" + "param set MC_PITCHRATE_P 0.125\n" + "param set MC_PITCHRATE_I 0.0\n" + "param set MC_PITCHRATE_D 0.0\n" + "param set MC_ROLL_P 6.0\n" + "param set MC_ROLLRATE_P 0.1125\n" + "param set MC_ROLLRATE_I 0.0\n" + "param set MC_ROLLRATE_D 0.0\n" + "param set ATT_W_MAG 0.00\n" + "param set PE_MAG_NOISE 1.0f\n" // ekf_att_pos only + "param set SENS_BOARD_ROT 6\n" + + "param set CAL_GYRO0_XOFF 0.0\n" + "param set CAL_GYRO0_YOFF 0.0\n" + "param set CAL_GYRO0_ZOFF 0.0\n" + "param set CAL_GYRO0_XSCALE 1.000000\n" + "param set CAL_GYRO0_YSCALE 1.000000\n" + "param set CAL_GYRO0_ZSCALE 1.000000\n" + "param set CAL_ACC0_XOFF 0.0\n" + "param set CAL_ACC0_YOFF 0.0\n" + "param set CAL_ACC0_ZOFF 0.0\n" + "param set CAL_ACC0_XSCALE 1.0\n" + "param set CAL_ACC0_YSCALE 1.0\n" + "param set CAL_ACC0_ZSCALE 1.0\n" + // "param set CAL_ACC0_XOFF 0.064189\n" + // "param set CAL_ACC0_YOFF 0.153990\n" + // "param set CAL_ACC0_ZOFF -0.000567\n" + "param set MPU_GYRO_LPF_ENUM 4\n" + "param set MPU_ACC_LPF_ENUM 4\n" + "param set MPU_SAMPLE_RATE_ENUM 2\n" + "sleep 1\n" + "mpu9x50 start -D /dev/spi-1\n" + "uart_esc start -D /dev/tty-2\n" + "csr_gps start -D /dev/tty-3\n" + "param set MAV_TYPE 2\n" + "list_devices\n" + "list_files\n" + "list_tasks\n" + "list_topics\n" + + ; + + return commands; + +} + +/* +simulator numbers + "param set MC_YAW_P 1.5\n" + "param set MC_PITCH_P 3.0\n" + "param set MC_ROLL_P 3.0\n" + "param set MC_YAWRATE_P 0.2\n" + "param set MC_PITCHRATE_P 0.03\n" + "param set MC_ROLLRATE_P 0.03\n" +*/ diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index 3f6c23b732..b338aeb7ed 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -56,7 +56,7 @@ const char *get_commands() "mc_pos_control start\n" "mc_att_control start\n" "sleep 1\n" - "hil mode_pwm\n" + "pwm_out_sim mode_pwm\n" "param set RC1_MAX 2015\n" "param set RC1_MIN 996\n" "param set RC1_TRIM 1502\n" diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index f4a538a13a..45a6e4cec8 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * Basic shell to execute builtin "apps" * * @author Mark Charlebois */ @@ -56,6 +56,8 @@ static px4_task_t g_dspal_task = -1; __BEGIN_DECLS // The commands to run are specified in a target file: commands_.c extern const char *get_commands(void); +// Enable external library hook +void qurt_external_hook(void) __attribute__((weak)); __END_DECLS static void run_cmd(map &apps, const vector &appargs) { @@ -108,7 +110,7 @@ static void process_commands(map &apps, const char *cmds) // Eat leading whitespace eat_whitespace(b, i); - + for(;;) { // End of command line @@ -156,15 +158,17 @@ __END_DECLS int dspal_entry( int argc, char* argv[] ) { - //const char *argv[2] = { "dspal_client", NULL }; - //int argc = 1; - PX4_INFO("In main\n"); map apps; init_app_map(apps); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); + usleep( 1000000 ); // give time for all commands to execute before starting external function + if(qurt_external_hook) + { + qurt_external_hook(); + } for( ;; ){ usleep( 1000000 ); } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index a10df59d36..47902057cc 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -58,6 +58,9 @@ endif ifeq ($(CONFIG),qurt_hil) SRCS += commands_hil.c endif +ifeq ($(CONFIG),qurt_adsp) +SRCS += commands_adsp.c +endif MAXOPTIMIZATION = -Os