From d94aa84657cec04a53d36f6b44107e8b2082f5ce Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 28 Aug 2015 16:06:36 -0700 Subject: [PATCH 01/39] Add daemon mode to posix build In order to use upstart to run PX4 it needs to run in daemon mode. Added ability to test if a task is running in order to gracefully shut down muorb. Signed-off-by: Mark Charlebois --- makefiles/qurt/toolchain_hexagon.mk | 2 +- src/platforms/posix/main.cpp | 118 ++++++++++++++---- .../posix/px4_layer/px4_posix_tasks.cpp | 11 ++ src/platforms/px4_tasks.h | 3 + 4 files changed, 108 insertions(+), 26 deletions(-) diff --git a/makefiles/qurt/toolchain_hexagon.mk b/makefiles/qurt/toolchain_hexagon.mk index 038c299904..c54a74f203 100644 --- a/makefiles/qurt/toolchain_hexagon.mk +++ b/makefiles/qurt/toolchain_hexagon.mk @@ -47,7 +47,7 @@ endif # HEXAGON_TOOLS_ROOT ?= /opt/6.4.03 #HEXAGON_TOOLS_ROOT = /opt/6.4.05 -HEXAGON_SDK_ROOT = /opt/Hexagon_SDK/2.0 +HEXAGON_SDK_ROOT ?= /opt/Hexagon_SDK/2.0 V_ARCH = v5 CROSSDEV = hexagon- HEXAGON_BIN = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT)) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 9f914ed693..dbc4b7cc3f 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * Basic shell to execute builtin "apps" * * @author Mark Charlebois */ @@ -42,8 +42,10 @@ #include #include #include +#include -namespace px4 { +namespace px4 +{ void init_once(void); } @@ -54,21 +56,34 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" #include "px4_middleware.h" -static void run_cmd(const vector &appargs) { +static bool _ExitFlag = false; +extern "C" { + void _SigIntHandler(int sig_num); + void _SigIntHandler(int sig_num) + { + _ExitFlag = true; + } +} + +static void run_cmd(const vector &appargs) +{ // command is appargs[0] string command = appargs[0]; cout << "----------------------------------\n"; + if (apps.find(command) != apps.end()) { - const char *arg[appargs.size()+2]; + const char *arg[appargs.size() + 2]; unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { arg[i] = (char *)appargs[i].c_str(); ++i; } + arg[i] = (char *)0; cout << "Running: " << command << "\n"; - apps[command](i,(char **)arg); + apps[command](i, (char **)arg); usleep(40000); cout << "Returning: " << command << "\n"; @@ -78,40 +93,93 @@ static void run_cmd(const vector &appargs) { } } +static void usage() +{ + + cout << "./mainapp [-d] [startup_config] -h" << std::endl; + cout << " -d - Optional flag to run the app in daemon mode and does not take listen for user input." << + std::endl; + cout << " This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl; + cout << " - config file for starting/stopping px4 modules" << std::endl; + cout << " -h - help/usage information" << std::endl; +} + static void process_line(string &line) { vector appargs(8); - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> appargs[7]; + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> + appargs[7]; run_cmd(appargs); } int main(int argc, char **argv) { - px4::init_once(); + bool daemon_mode = false; + signal(SIGINT, _SigIntHandler); - px4::init(argc, argv, "mainapp"); + int index = 1; + bool error_detected = false; - // Execute a command list of provided - if (argc == 2) { - ifstream infile(argv[1]); + while (index < argc) { + if (argv[index][0] == '-') { + // the arg starts with - + if (strcmp(argv[index], "-d") == 0) { + daemon_mode = true; - if (!infile) { - cout << "failed opening script" << argv[1] << std::endl; - return 1; - } - - for (string line; getline(infile, line, '\n'); ) { - process_line(line); + } else if (strcmp(argv[index], "-h") == 0) { + usage(); + return 0; + + } else { + PX4_WARN("Unknown/unhandled parameter: %s", argv[index]); + return 1; + } + + } else { + // this is an argument that does not have '-' prefix; treat it like a file name + ifstream infile(argv[index]); + if (infile.is_open()) { + for (string line; getline(infile, line, '\n');) { + process_line(line); + } + + } else { + PX4_WARN("Error opening file: %s", argv[index]); + error_detected = true; + break; + } } + ++index; } - string mystr; - - while(1) { - cout << "Enter a command and its args:" << endl; - getline (cin,mystr); - process_line(mystr); - mystr = ""; + if (!error_detected) { + px4::init_once(); + + px4::init(argc, argv, "mainapp"); + + if (!daemon_mode) { + string mystr; + + while (!_ExitFlag) { + cout << "Enter a command and its args:" << endl; + getline(cin, mystr); + process_line(mystr); + mystr = ""; + } + + } else { + while (!_ExitFlag) { + sleep(1000000); + } + } + + if (px4_task_is_running("muorb")) { + // sending muorb stop is needed if it is running to exit cleanly + vector muorb_stop_cmd = { "muorb", "stop" }; + run_cmd(muorb_stop_cmd); + } + vector shutdown_cmd = { "shutdown" }; + run_cmd(shutdown_cmd); } } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 44f9ceff88..129709d3d7 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -280,6 +280,17 @@ void px4_show_tasks() } +bool px4_task_is_running(const char *taskname) +{ + int idx; + for (idx=0; idx < PX4_MAX_TASKS; idx++) + { + if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) { + return true; + } + } + return false; +} __BEGIN_DECLS unsigned long px4_getpid() diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 8dde7cfdd5..941c1ceb37 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -117,5 +117,8 @@ __EXPORT void px4_task_exit(int ret); /** Show a list of running tasks **/ __EXPORT void px4_show_tasks(void); +/** See if a task is running **/ +__EXPORT bool px4_task_is_running(const char *taskname); + __END_DECLS From bb696b756b4c1c2df1937e4bf8f41a0d5c9e8e4b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 29 Aug 2015 17:34:03 -0400 Subject: [PATCH 02/39] reduce make verbosity --- makefiles/firmware.mk | 4 ++-- makefiles/module.mk | 4 ++++ makefiles/nuttx/firmware_nuttx.mk | 12 ++++++------ makefiles/nuttx/toolchain_gnu-arm-eabi.mk | 1 - makefiles/posix/firmware_posix.mk | 2 +- makefiles/posix/toolchain_native.mk | 2 -- makefiles/qurt/firmware_qurt.mk | 2 +- 7 files changed, 14 insertions(+), 13 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index c59b66b21f..d5ac4d7d7a 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -232,7 +232,7 @@ $(MODULE_OBJS): workdir = $(@D) $(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) $(Q) $(MKDIR) -p $(workdir) $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \ - -C $(workdir) \ + --no-print-directory -C $(workdir) \ MODULE_WORK_DIR=$(workdir) \ MODULE_OBJ=$@ \ MODULE_MK=$(mkfile) \ @@ -292,7 +292,7 @@ $(LIBRARY_LIBS): workdir = $(@D) $(LIBRARY_LIBS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER) $(Q) $(MKDIR) -p $(workdir) $(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \ - -C $(workdir) \ + --no-print-directory -C $(workdir) \ LIBRARY_WORK_DIR=$(workdir) \ LIBRARY_LIB=$@ \ LIBRARY_MK=$(mkfile) \ diff --git a/makefiles/module.mk b/makefiles/module.mk index 3c9ca35026..c92abaec3f 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -113,7 +113,9 @@ ifeq ($(MODULE_MK),) $(error No module makefile specified) endif +ifeq ($(V),1) $(info %% MODULE_MK = $(MODULE_MK)) +endif # # Get the board/toolchain config @@ -125,10 +127,12 @@ include $(BOARD_FILE) # include $(MODULE_MK) MODULE_SRC := $(dir $(MODULE_MK)) +ifeq ($(V),1) $(info % MODULE_NAME = $(MODULE_NAME)) $(info % MODULE_SRC = $(MODULE_SRC)) $(info % MODULE_OBJ = $(MODULE_OBJ)) $(info % MODULE_WORK_DIR = $(MODULE_WORK_DIR)) +endif # # Things that, if they change, might affect everything diff --git a/makefiles/nuttx/firmware_nuttx.mk b/makefiles/nuttx/firmware_nuttx.mk index 644c3a0e72..ca4831b8bd 100644 --- a/makefiles/nuttx/firmware_nuttx.mk +++ b/makefiles/nuttx/firmware_nuttx.mk @@ -30,7 +30,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: checkgitversion generateuorbtopi @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ @@ -77,11 +77,11 @@ $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -98,12 +98,12 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) oldconfig - $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) oldconfig + $(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else diff --git a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk index 27be31da8e..0c6c5679f9 100644 --- a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk +++ b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk @@ -271,7 +271,6 @@ 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 diff --git a/makefiles/posix/firmware_posix.mk b/makefiles/posix/firmware_posix.mk index f88caf569a..f6f087d449 100644 --- a/makefiles/posix/firmware_posix.mk +++ b/makefiles/posix/firmware_posix.mk @@ -47,7 +47,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: checkgitversion generateuorbtopich @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ diff --git a/makefiles/posix/toolchain_native.mk b/makefiles/posix/toolchain_native.mk index eeba6d8f91..87e38ba349 100644 --- a/makefiles/posix/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -348,7 +348,6 @@ endef define LINK_A @$(ECHO) "LINK_A: $1" @$(MKDIR) -p $(dir $1) - echo "$(Q) $(AR) $1 $2" $(Q) $(AR) $1 $2 endef @@ -357,7 +356,6 @@ 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 endef diff --git a/makefiles/qurt/firmware_qurt.mk b/makefiles/qurt/firmware_qurt.mk index a367e4a327..50c4b267b4 100644 --- a/makefiles/qurt/firmware_qurt.mk +++ b/makefiles/qurt/firmware_qurt.mk @@ -47,7 +47,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a: generateuorbtopicheaders @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% $(Q) $(MKDIR) -p $(work_dir) - $(Q) $(MAKE) -r -C $(work_dir) \ + $(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ From aa137f4e95feb78fe2b12b3771215b24348b8821 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 29 Aug 2015 18:11:06 -0400 Subject: [PATCH 03/39] remove -Wlogical-op for Eigen --- makefiles/nuttx/toolchain_gnu-arm-eabi.mk | 1 - makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk | 2 -- makefiles/posix/toolchain_native.mk | 3 +-- 3 files changed, 1 insertion(+), 5 deletions(-) diff --git a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk index 0c6c5679f9..868c3d161e 100644 --- a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk +++ b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk @@ -147,7 +147,6 @@ ARCHWARNINGS = -Wall \ -Wshadow \ -Wfloat-equal \ -Wpointer-arith \ - -Wlogical-op \ -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter \ diff --git a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk index 2811512ecd..12ea6fc29d 100644 --- a/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk +++ b/makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk @@ -157,8 +157,6 @@ ARCHWARNINGS = -Wall \ -Werror=reorder \ -Werror=uninitialized \ -Werror=init-self \ - -Wno-error=logical-op \ - -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ -Wno-error=double-promotion \ diff --git a/makefiles/posix/toolchain_native.mk b/makefiles/posix/toolchain_native.mk index 87e38ba349..c3a766d5ef 100644 --- a/makefiles/posix/toolchain_native.mk +++ b/makefiles/posix/toolchain_native.mk @@ -183,9 +183,8 @@ ARCHWARNINGS = -Wall \ # Add compiler specific options ifeq ($(USE_GCC),1) -ARCHDEFINES += -Wno-error=logical-op +ARCHDEFINES += ARCHWARNINGS += -Wdouble-promotion \ - -Wlogical-op \ -Wformat=1 \ -Werror=unused-but-set-variable \ -Werror=double-promotion From b4e19052029d47a2790866c27ffd58d2b48a6737 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 29 Aug 2015 18:22:52 -0400 Subject: [PATCH 04/39] enable ccache for clang --- .travis.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.travis.yml b/.travis.yml index 134cdcca67..e26b0707cf 100644 --- a/.travis.yml +++ b/.travis.yml @@ -46,6 +46,12 @@ before_script: - mkdir -p ~/bin - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc + - ln -s /usr/bin/ccache ~/bin/clang++ + - ln -s /usr/bin/ccache ~/bin/clang++-3.4 + - ln -s /usr/bin/ccache ~/bin/clang++-3.5 + - ln -s /usr/bin/ccache ~/bin/clang + - ln -s /usr/bin/ccache ~/bin/clang-3.4 + - ln -s /usr/bin/ccache ~/bin/clang-3.5 - ln -s /usr/bin/ccache ~/bin/g++-4.8 - ln -s /usr/bin/ccache ~/bin/gcc-4.8 - export PATH=~/bin:$PATH From c708a6355ec8b489d04169d5f095061dd3c5b75e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 29 Aug 2015 19:50:47 -0400 Subject: [PATCH 05/39] remove unused GIT_DESC --- makefiles/firmware.mk | 2 -- 1 file changed, 2 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index d5ac4d7d7a..b8c8d4a85b 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -106,8 +106,6 @@ ifneq ($(words $(PX4_BASE)),1) $(error Cannot build when the PX4_BASE path contains one or more space characters.) endif -$(info % GIT_DESC = $(GIT_DESC)) - # # Set a default target so that included makefiles or errors here don't # cause confusion. From 5391e8a24f2c814ad5095937a9575e8663b069cb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 29 Aug 2015 20:01:07 -0400 Subject: [PATCH 06/39] unittests trivial cleanup -add -Qunused-arguments for clang -update to a recent cmake -comment unused variables --- .travis.yml | 1 + Makefile | 2 +- unittests/CMakeLists.txt | 10 ++++++++-- unittests/sbus2_test.cpp | 2 +- unittests/st24_test.cpp | 5 ++--- unittests/sumd_test.cpp | 5 ++--- unittests/uorb_unittests/uORBCommunicatorMock.hpp | 4 ++-- 7 files changed, 17 insertions(+), 12 deletions(-) diff --git a/.travis.yml b/.travis.yml index e26b0707cf..ecc69a0c16 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,6 +13,7 @@ cache: addons: apt: sources: + - kubuntu-backports - ubuntu-toolchain-r-test packages: - build-essential diff --git a/Makefile b/Makefile index a0dc23f705..bf399d19bf 100644 --- a/Makefile +++ b/Makefile @@ -220,7 +220,7 @@ qurtrun: # unit tests. .PHONY: tests tests: generateuorbtopicheaders - $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) + $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) --no-print-directory unittests) .PHONY: format check_format check_format: diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 80eb8c24e6..4285520b0c 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -11,6 +11,13 @@ CMAKE_FORCE_CXX_COMPILER( clang++ Clang ) #set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY ) #set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY ) +if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang") + add_compile_options(-Qunused-arguments) +endif() +if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + add_compile_options(-Qunused-arguments) +endif() + project(unittests) enable_testing() @@ -123,8 +130,7 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c ${PX_SRC}/systemcmds/tests/test_mixer.cpp) - -target_link_libraries( mixer_test LINK_PUBLIC px4_platform ) +target_link_libraries( mixer_test px4_platform ) add_gtest(mixer_test) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 1c32e3bf84..54c7fc412b 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -33,7 +33,7 @@ TEST(SBUS2Test, SBUS2) // Init the parser uint8_t frame[30]; unsigned partial_frame_count = 0; - uint16_t rc_values[18]; + //uint16_t rc_values[18]; //uint16_t num_values; //bool sbus_failsafe; //bool sbus_frame_drop; diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 42fa07ae74..c7fded68d4 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -41,7 +41,7 @@ TEST(ST24Test, ST24) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); uint8_t rssi; uint8_t rx_count; @@ -53,8 +53,7 @@ TEST(ST24Test, ST24) //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - - int16_t val = channels[i]; + //int16_t val = channels[i]; //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } } diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp index bf99709475..65c944676c 100644 --- a/unittests/sumd_test.cpp +++ b/unittests/sumd_test.cpp @@ -41,7 +41,7 @@ TEST(SUMDTest, SUMD) last_time = f; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); + //hrt_abstime now = hrt_absolute_time(); uint8_t rssi; uint8_t rx_count; @@ -53,8 +53,7 @@ TEST(SUMDTest, SUMD) //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - - int16_t val = channels[i]; + //int16_t val = channels[i]; //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } } diff --git a/unittests/uorb_unittests/uORBCommunicatorMock.hpp b/unittests/uorb_unittests/uORBCommunicatorMock.hpp index 8c5b861b1e..acffbdb7ab 100644 --- a/unittests/uorb_unittests/uORBCommunicatorMock.hpp +++ b/unittests/uorb_unittests/uORBCommunicatorMock.hpp @@ -126,8 +126,8 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel private: uORBCommunicator::IChannelRxHandler* _rx_handler; - int _sub_topicA_copy_fd; - int _sub_topicB_copy_fd; + //int _sub_topicA_copy_fd; + //int _sub_topicB_copy_fd; std::map _topic_translation_map; From 2579b29691fbb5d3b31ae1f84ef3329f4e95a86a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 30 Aug 2015 00:18:41 -0700 Subject: [PATCH 07/39] POSIX: Fix for daemon mode to process commands after init Commands were being processed before init was called. Signed-off-by: Mark Charlebois --- src/platforms/posix/main.cpp | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index dbc4b7cc3f..1c34b281af 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -120,6 +120,7 @@ int main(int argc, char **argv) int index = 1; bool error_detected = false; + char *commands_file = nullptr; while (index < argc) { if (argv[index][0] == '-') { @@ -139,10 +140,10 @@ int main(int argc, char **argv) } else { // this is an argument that does not have '-' prefix; treat it like a file name ifstream infile(argv[index]); - if (infile.is_open()) { - for (string line; getline(infile, line, '\n');) { - process_line(line); - } + + if (infile.good()) { + infile.close(); + commands_file = argv[index]; } else { PX4_WARN("Error opening file: %s", argv[index]); @@ -150,6 +151,7 @@ int main(int argc, char **argv) break; } } + ++index; } @@ -158,6 +160,20 @@ int main(int argc, char **argv) px4::init(argc, argv, "mainapp"); + //if commandfile is present, process the commands from the file + if (commands_file != nullptr) { + ifstream infile(commands_file); + + if (infile.is_open()) { + for (string line; getline(infile, line, '\n');) { + process_line(line); + } + + } else { + PX4_WARN("Error opening file: %s", commands_file); + } + } + if (!daemon_mode) { string mystr; @@ -179,6 +195,7 @@ int main(int argc, char **argv) vector muorb_stop_cmd = { "muorb", "stop" }; run_cmd(muorb_stop_cmd); } + vector shutdown_cmd = { "shutdown" }; run_cmd(shutdown_cmd); } From efcc4f81e251abfa18a72950fa39ae084819ebfd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 19:36:33 +0200 Subject: [PATCH 08/39] ROMFS: Do not set gains which are similar between platforms --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 -- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 -- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 1 - 3 files changed, 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 408707ec0b..c0bd5b1c85 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -16,8 +16,6 @@ then param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 - param set PE_GBIAS_PNOISE 0.000001 - param set PE_ABIAS_PNOISE 0.0002 fi # This is the gimbal pass mixer diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index eccee07e7b..1080121618 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -8,8 +8,6 @@ then param set PE_VELD_NOISE 0.35 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.25 - param set PE_GBIAS_PNOISE 0.000001 - param set PE_ABIAS_PNOISE 0.0001 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index c2340a7b16..1b7b6528cf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -37,7 +37,6 @@ then param set PE_VELD_NOISE 0.3 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.25 - param set PE_GBIAS_PNOISE 0.000001 param set PE_ABIAS_PNOISE 0.0001 fi From b5a410e9d277ebc4eebb201cfd92eac8e1dd01fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 19:36:51 +0200 Subject: [PATCH 09/39] EKF: Set better default gains --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index f496f9ed96..943365e499 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -214,23 +214,23 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. * Increasing this value will make the gyro bias converge faster but noisier. * - * @min 0.0000001 + * @min 0.00000005 * @max 0.00001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f); +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); /** * Accelerometer bias estimate process noise * - * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f. * Increasing this value makes the bias estimation faster and noisier. * * @min 0.00001 * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f); /** * Magnetometer earth frame offsets process noise From 5226ffb3a7481543751dd8fed65bd135a4f1a55d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 22:42:28 +0200 Subject: [PATCH 10/39] IRIS config: Use tighter attitude control gains --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index a2536e5418..ffaa935055 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -13,11 +13,11 @@ if [ $AUTOCNF == yes ] then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 2.5 From 76bb1eb1d8f92661df1901f76437fbda969a90d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 22:42:46 +0200 Subject: [PATCH 11/39] F330: Use tighter attitude control gains --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 4303320ffe..aea5292a24 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -16,11 +16,11 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 From d722292381a0eb4bde72615385caa8f7bf7d73f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 22:43:07 +0200 Subject: [PATCH 12/39] MC attitude controller: Use tighter attitude control gains --- src/modules/mc_att_control/mc_att_control_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 9af68a4e1e..30a50480e0 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -36,8 +36,8 @@ * Parameters for multicopter attitude controller. * * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin + * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); /** * Roll rate I gain @@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); /** * Pitch rate I gain From 4c03c5137ebb9371aaa15e1752fa53c0e0bcb6ca Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 1 Sep 2015 09:31:59 +0200 Subject: [PATCH 13/39] load correct mixer for sitl plane --- posix-configs/SITL/init/rc.fixed_wing | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index 65b69e4d0b..afd5a38117 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -37,7 +37,7 @@ navigator start ekf_att_pos_estimator start fw_att_control start fw_pos_control_l1 start -mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/IO_pass.main.mix mavlink start -u 14556 -r 60000 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 From b4839731adfdb72489ead3003e9d52251c292150 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 1 Sep 2015 09:33:15 +0200 Subject: [PATCH 14/39] use correct syntax for polling --- src/modules/fw_att_control/fw_att_control_main.cpp | 9 ++++----- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b6265ffc06..d06e9b9a74 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -648,8 +648,8 @@ FixedwingAttitudeControl::task_main() vehicle_manual_poll(); vehicle_status_poll(); - /* wakeup source(s) */ - struct pollfd fds[2]; + /* wakeup source */ + px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -660,11 +660,10 @@ FixedwingAttitudeControl::task_main() _task_running = true; while (!_task_should_exit) { - static int loop_counter = 0; /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { @@ -691,7 +690,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -703,6 +701,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 73c2bb07ac..4a02b93495 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1719,7 +1719,7 @@ FixedwingPositionControl::task_main() } /* wakeup source(s) */ - struct pollfd fds[2]; + px4_pollfd_struct_t fds[2]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -1732,7 +1732,7 @@ FixedwingPositionControl::task_main() while (!_task_should_exit) { /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { From b1850a316bc3ba7eb9c77f52cdf16035719e67c2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 1 Sep 2015 09:35:43 +0200 Subject: [PATCH 15/39] support sitl for planes --- src/modules/simulator/simulator.h | 9 ++++- src/modules/simulator/simulator_mavlink.cpp | 44 +++++++++++++-------- 2 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 0009d9e1de..dbb3d5b783 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -217,10 +218,12 @@ private: _actuator_outputs_sub(-1), _vehicle_attitude_sub(-1), _manual_sub(-1), + _vehicle_status_sub(-1), _rc_input{}, _actuators{}, _attitude{}, - _manual{} + _manual{}, + _vehicle_status{} #endif {} ~Simulator() { _instance=NULL; } @@ -255,14 +258,16 @@ private: int _actuator_outputs_sub; int _vehicle_attitude_sub; int _manual_sub; + int _vehicle_status_sub; // uORB data containers struct rc_input_values _rc_input; struct actuator_outputs_s _actuators; struct vehicle_attitude_s _attitude; struct manual_control_setpoint_s _manual; + struct vehicle_status_s _vehicle_status; - void poll_actuators(); + void poll_topics(); void handle_message(mavlink_message_t *msg, bool publish); void send_controls(); void pollForMAVLinkMessages(bool publish); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 7b6b4594f0..d6d5a22b3e 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -71,26 +71,33 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { // for now we only support quadrotors unsigned n = 4; - for (unsigned i = 0; i < 8; i++) { - if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { - if (i < n) { - // scale PWM out 900..2100 us to 0..1 for rotors */ - out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + if (_vehicle_status.is_rotary_wing) { + for (unsigned i = 0; i < 8; i++) { + if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { + if (i < n) { + // scale PWM out 900..2100 us to 0..1 for rotors */ + out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + // scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + } } else { - // scale PWM out 900..2100 us to -1..1 for other channels */ - out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + // send 0 when disarmed and for disabled channels */ + out[i] = 0.0f; } - - } else { - // send 0 when disarmed and for disabled channels */ - out[i] = 0.0f; + } + } else { + // convert back to range [-1, 1] + for (unsigned i = 0; i < 8; i++) { + out[i] = (_actuators.output[i] - 1500) / 600.0f; } } actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; - actuator_msg.pitch_elevator = out[1]; + actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1]; actuator_msg.yaw_rudder = out[2]; actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; @@ -270,14 +277,18 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 } } -void Simulator::poll_actuators() { +void Simulator::poll_topics() { // copy new actuator data if available bool updated; orb_check(_actuator_outputs_sub, &updated); - if(updated) { - //PX4_WARN("Received actuator_output0 orb_topic"); + if (updated) { orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators); } + + orb_check(_vehicle_status_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } } void *Simulator::sending_trampoline(void *) { @@ -310,7 +321,7 @@ void Simulator::send() { if (fds[0].revents & POLLIN) { // got new data to read, update all topics - poll_actuators(); + poll_topics(); send_controls(); } } @@ -419,6 +430,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) // subscribe to topics _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); // got data from simulator, now activate the sending thread pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); From 95af5fc3d0a6518a27bf50d67ea47f23b43b3eed Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 2 Sep 2015 10:24:31 +0200 Subject: [PATCH 16/39] do not run mavlink receiver before app is fully booted when using sockets --- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++++- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6fa5918fc5..f623a0d7b3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1543,11 +1543,6 @@ Mavlink::task_main(int argc, char *argv[]) /* 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; @@ -1724,6 +1719,11 @@ 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); + /* init socket if necessary */ + if (get_protocol() == UDP) { + init_udp(); + } + /* if the protocol is serial, we send the system version blindly */ if (get_protocol() == SERIAL) { send_autopilot_capabilites(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 24d0940477..08b24f339d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1761,10 +1761,17 @@ MavlinkReceiver::receive_thread(void *arg) #ifdef __PX4_POSIX struct sockaddr_in srcaddr; socklen_t addrlen = sizeof(srcaddr); + if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { + // make sure mavlink app has booted before we start using the socket + while (!_mavlink->boot_complete()) { + usleep(100000); + } + fds[0].fd = _mavlink->get_socket_fd(); fds[0].events = POLLIN; } + #endif ssize_t nread = 0; @@ -1779,7 +1786,6 @@ MavlinkReceiver::receive_thread(void *arg) } #ifdef __PX4_POSIX if (_mavlink->get_protocol() == UDP) { - if (fds[0].revents & POLLIN) { nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); } From 980217468f03bbca14bf3ed5edfed5a32a7b3de0 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 2 Sep 2015 08:08:29 +0200 Subject: [PATCH 17/39] set actuator commands to zero if vehicle status is still unknown --- src/modules/simulator/simulator_mavlink.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d6d5a22b3e..ec3a7c8a24 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -64,7 +64,7 @@ static socklen_t _addrlen = sizeof(_srcaddr); using namespace simulator; void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { - float out[8]; + float out[8] = {}; const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; @@ -95,6 +95,12 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { } } + // if vehicle status has not yet been updated, set actuator commands to zero + // this is to prevent the simulation getting into a bad state + if (_vehicle_status.timestamp == 0) { + memset(out, 0, sizeof(out)); + } + actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1]; From 50a5fb94adbc0e29dcfda261b9a89cc9a169d723 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 2 Sep 2015 18:37:57 +0200 Subject: [PATCH 18/39] fix logic in posix access function --- src/drivers/device/vdev_posix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 86c2230526..44aea614cc 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -270,7 +270,7 @@ int px4_fsync(int fd) int px4_access(const char *pathname, int mode) { - if (mode == F_OK) { + if (mode != F_OK) { errno = EINVAL; return -1; } From 4d3529164952eaddaebb18ed16f94c0b6e1b199d Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 3 Sep 2015 09:46:54 +0200 Subject: [PATCH 19/39] avoid division by zero --- src/modules/systemlib/perf_counter.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index b0a0b01d03..beea97cfb5 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -396,13 +396,12 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; float rms = sqrtf(pce->M2 / (pce->event_count-1)); - dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", handle->name, (unsigned long long)pce->event_count, (unsigned long long)pce->event_overruns, (unsigned long long)pce->time_total, - (unsigned long long)pce->time_total / pce->event_count, + pce->event_count == 0 ? 0 : (unsigned long long)pce->time_total / pce->event_count, (unsigned long long)pce->time_least, (unsigned long long)pce->time_most, (double)(1e6f * rms)); From 8c6dc8cdf50764ec9faffdb3224d214d6955a0c6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 3 Sep 2015 09:47:14 +0200 Subject: [PATCH 20/39] start logger for SITL --- posix-configs/SITL/init/rc.fixed_wing | 1 + posix-configs/SITL/init/rcS | 1 + 2 files changed, 2 insertions(+) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index afd5a38117..d521b18e11 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -44,3 +44,4 @@ mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 50 -s ATTITUDE -u 14556 mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 6a9025bc97..437874eb86 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -53,3 +53,4 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink boot_complete +sdlog2 start -r 100 -e -t -a From cb96dc107459303d36d9f761f943f4e0e7caf195 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 3 Sep 2015 09:54:10 +0200 Subject: [PATCH 21/39] do not close stdin/stdout for posix --- src/modules/sdlog2/sdlog2.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c6d13f8756..a81ab9fdb1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1272,13 +1272,16 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.sat_info_sub = -1; - /* close non-needed fd's */ +#ifdef __PX4_NUTTX + /* close non-needed fd's. We cannot do this for posix since the file + descriptors will also be closed for the parent process + */ /* close stdin */ close(0); /* close stdout */ close(1); - +#endif /* initialize thread synchronization */ pthread_mutex_init(&logbuffer_mutex, NULL); pthread_cond_init(&logbuffer_cond, NULL); From 9f300e054dd5d4812f555bb586fb1a1a7fc75adb Mon Sep 17 00:00:00 2001 From: ksschwabe Date: Thu, 3 Sep 2015 10:51:21 +0200 Subject: [PATCH 22/39] Added ability to use timer 1 and timer 8 for the tone alarm driver. --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 45 ++++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 023ce426cc..9cdcfd9189 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -118,7 +118,15 @@ #include /* Tone alarm configuration */ -#if TONE_ALARM_TIMER == 2 +#if TONE_ALARM_TIMER == 1 +# define TONE_ALARM_BASE STM32_TIM1_BASE +# define TONE_ALARM_CLOCK STM32_APB2_TIM1_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM1EN +# ifdef CONFIG_STM32_TIM1 +# error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1 +# endif +#elif TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN # define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR @@ -150,6 +158,14 @@ # ifdef CONFIG_STM32_TIM5 # error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5 # endif +#elif TONE_ALARM_TIMER == 8 +# define TONE_ALARM_BASE STM32_TIM8_BASE +# define TONE_ALARM_CLOCK STM32_APB2_TIM8_CLKIN +# define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR +# define TONE_ALARM_CLOCK_ENABLE RCC_APB2ENR_TIM8EN +# ifdef CONFIG_STM32_TIM8 +# error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8 +# endif #elif TONE_ALARM_TIMER == 9 # define TONE_ALARM_BASE STM32_TIM9_BASE # define TONE_ALARM_CLOCK STM32_APB2_TIM9_CLKIN @@ -208,6 +224,28 @@ */ #define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg)) +#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers +# define rCR1 REG(STM32_ATIM_CR1_OFFSET) +# define rCR2 REG(STM32_ATIM_CR2_OFFSET) +# define rSMCR REG(STM32_ATIM_SMCR_OFFSET) +# define rDIER REG(STM32_ATIM_DIER_OFFSET) +# define rSR REG(STM32_ATIM_SR_OFFSET) +# define rEGR REG(STM32_ATIM_EGR_OFFSET) +# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET) +# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET) +# define rCCER REG(STM32_ATIM_CCER_OFFSET) +# define rCNT REG(STM32_ATIM_CNT_OFFSET) +# define rPSC REG(STM32_ATIM_PSC_OFFSET) +# define rARR REG(STM32_ATIM_ARR_OFFSET) +# define rRCR REG(STM32_ATIM_RCR_OFFSET) +# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET) +# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET) +# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET) +# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET) +# define rBDTR REG(STM32_ATIM_BDTR_OFFSET) +# define rDCR REG(STM32_ATIM_DCR_OFFSET) +# define rDMAR REG(STM32_ATIM_DMAR_OFFSET) +#else #define rCR1 REG(STM32_GTIM_CR1_OFFSET) #define rCR2 REG(STM32_GTIM_CR2_OFFSET) #define rSMCR REG(STM32_GTIM_SMCR_OFFSET) @@ -226,6 +264,7 @@ #define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) #define rDCR REG(STM32_GTIM_DCR_OFFSET) #define rDMAR REG(STM32_GTIM_DMAR_OFFSET) +#endif class ToneAlarm : public device::CDev { @@ -396,6 +435,10 @@ ToneAlarm::init() rCCER = TONE_CCER; rDCR = 0; +#ifdef rBDTR // If using an advanced timer, you need to activate the output + rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer +#endif + /* toggle the CC output each time the count passes 1 */ TONE_rCCR = 1; From 6b7fe11c2eb6419aea257b711fecf62f33d86b83 Mon Sep 17 00:00:00 2001 From: ksschwabe Date: Thu, 3 Sep 2015 17:52:57 +0200 Subject: [PATCH 23/39] Tone_alarm: added checking to make sure HRT and Tone_alarm on different timers. --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 89 +++++++++++---------- 1 file changed, 48 insertions(+), 41 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 9cdcfd9189..4db2b06d5d 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -117,8 +117,15 @@ #include +/* Check that tone alarm and HRT timers are different */ +#if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) +# if TONE_ALARM_TIMER == HRT_TIMER +# error TONE_ALARM_TIMER and HRT_TIMER must use different timers. +# endif +#endif + /* Tone alarm configuration */ -#if TONE_ALARM_TIMER == 1 +#if TONE_ALARM_TIMER == 1 # define TONE_ALARM_BASE STM32_TIM1_BASE # define TONE_ALARM_CLOCK STM32_APB2_TIM1_CLKIN # define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB2ENR @@ -126,7 +133,7 @@ # ifdef CONFIG_STM32_TIM1 # error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1 # endif -#elif TONE_ALARM_TIMER == 2 +#elif TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE # define TONE_ALARM_CLOCK STM32_APB1_TIM2_CLKIN # define TONE_ALARM_CLOCK_POWER_REG STM32_RCC_APB1ENR @@ -191,7 +198,7 @@ # error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11 # endif #else -# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver. +# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver. #endif #if TONE_ALARM_CHANNEL == 1 @@ -225,45 +232,45 @@ #define REG(_reg) (*(volatile uint32_t *)(TONE_ALARM_BASE + _reg)) #if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers -# define rCR1 REG(STM32_ATIM_CR1_OFFSET) -# define rCR2 REG(STM32_ATIM_CR2_OFFSET) -# define rSMCR REG(STM32_ATIM_SMCR_OFFSET) -# define rDIER REG(STM32_ATIM_DIER_OFFSET) -# define rSR REG(STM32_ATIM_SR_OFFSET) -# define rEGR REG(STM32_ATIM_EGR_OFFSET) -# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET) -# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET) -# define rCCER REG(STM32_ATIM_CCER_OFFSET) -# define rCNT REG(STM32_ATIM_CNT_OFFSET) -# define rPSC REG(STM32_ATIM_PSC_OFFSET) -# define rARR REG(STM32_ATIM_ARR_OFFSET) -# define rRCR REG(STM32_ATIM_RCR_OFFSET) -# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET) -# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET) -# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET) -# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET) -# define rBDTR REG(STM32_ATIM_BDTR_OFFSET) -# define rDCR REG(STM32_ATIM_DCR_OFFSET) -# define rDMAR REG(STM32_ATIM_DMAR_OFFSET) +# define rCR1 REG(STM32_ATIM_CR1_OFFSET) +# define rCR2 REG(STM32_ATIM_CR2_OFFSET) +# define rSMCR REG(STM32_ATIM_SMCR_OFFSET) +# define rDIER REG(STM32_ATIM_DIER_OFFSET) +# define rSR REG(STM32_ATIM_SR_OFFSET) +# define rEGR REG(STM32_ATIM_EGR_OFFSET) +# define rCCMR1 REG(STM32_ATIM_CCMR1_OFFSET) +# define rCCMR2 REG(STM32_ATIM_CCMR2_OFFSET) +# define rCCER REG(STM32_ATIM_CCER_OFFSET) +# define rCNT REG(STM32_ATIM_CNT_OFFSET) +# define rPSC REG(STM32_ATIM_PSC_OFFSET) +# define rARR REG(STM32_ATIM_ARR_OFFSET) +# define rRCR REG(STM32_ATIM_RCR_OFFSET) +# define rCCR1 REG(STM32_ATIM_CCR1_OFFSET) +# define rCCR2 REG(STM32_ATIM_CCR2_OFFSET) +# define rCCR3 REG(STM32_ATIM_CCR3_OFFSET) +# define rCCR4 REG(STM32_ATIM_CCR4_OFFSET) +# define rBDTR REG(STM32_ATIM_BDTR_OFFSET) +# define rDCR REG(STM32_ATIM_DCR_OFFSET) +# define rDMAR REG(STM32_ATIM_DMAR_OFFSET) #else -#define rCR1 REG(STM32_GTIM_CR1_OFFSET) -#define rCR2 REG(STM32_GTIM_CR2_OFFSET) -#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) -#define rDIER REG(STM32_GTIM_DIER_OFFSET) -#define rSR REG(STM32_GTIM_SR_OFFSET) -#define rEGR REG(STM32_GTIM_EGR_OFFSET) -#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) -#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) -#define rCCER REG(STM32_GTIM_CCER_OFFSET) -#define rCNT REG(STM32_GTIM_CNT_OFFSET) -#define rPSC REG(STM32_GTIM_PSC_OFFSET) -#define rARR REG(STM32_GTIM_ARR_OFFSET) -#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) -#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) -#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) -#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) -#define rDCR REG(STM32_GTIM_DCR_OFFSET) -#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) +# define rCR1 REG(STM32_GTIM_CR1_OFFSET) +# define rCR2 REG(STM32_GTIM_CR2_OFFSET) +# define rSMCR REG(STM32_GTIM_SMCR_OFFSET) +# define rDIER REG(STM32_GTIM_DIER_OFFSET) +# define rSR REG(STM32_GTIM_SR_OFFSET) +# define rEGR REG(STM32_GTIM_EGR_OFFSET) +# define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) +# define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) +# define rCCER REG(STM32_GTIM_CCER_OFFSET) +# define rCNT REG(STM32_GTIM_CNT_OFFSET) +# define rPSC REG(STM32_GTIM_PSC_OFFSET) +# define rARR REG(STM32_GTIM_ARR_OFFSET) +# define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) +# define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) +# define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) +# define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) +# define rDCR REG(STM32_GTIM_DCR_OFFSET) +# define rDMAR REG(STM32_GTIM_DMAR_OFFSET) #endif class ToneAlarm : public device::CDev From e3ea42a0c4d8ef0fdfe7f5b89150c8aa6dc6ffca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 14:25:04 +0200 Subject: [PATCH 24/39] Remove sim configs default to joystick in --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 3 --- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 3 --- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 3 --- ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 3 --- ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 3 --- 5 files changed, 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 4100cdb4a3..7938c47bae 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -39,8 +39,5 @@ then param set FW_RR_P 0.3 fi -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set HIL yes set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 52c512ed8c..e1f2cdfe8d 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_x -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index c590f9ac86..7aa888c763 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_+ -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 0369da5980..57bcd24d02 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -11,7 +11,4 @@ sh /etc/init.d/rc.fw_defaults set HIL yes -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 278df193aa..5e0b6cd746 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -36,8 +36,5 @@ fi set HIL yes -# Enable gamepad / joystick support -param set COM_RC_IN_MODE 2 - # Set the AERT mixer for HIL (even if the malolo is a flying wing) set MIXER AERT From 3a2e2ef3047b49678e3b72f0132d1e94c396a9a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 14:25:31 +0200 Subject: [PATCH 25/39] Do not default to Joystick input for SITL --- posix-configs/SITL/init/rc.fixed_wing | 1 - posix-configs/SITL/init/rcS | 1 - posix-configs/SITL/init/rc_iris_ros | 1 - 3 files changed, 3 deletions(-) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index d521b18e11..3d65b7a4c7 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -4,7 +4,6 @@ param load param set MAV_TYPE 1 param set SYS_AUTOSTART 3033 param set SYS_RESTART_TYPE 2 -param set COM_RC_IN_MODE 2 dataman start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 437874eb86..26bf98cb8b 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -8,7 +8,6 @@ param set MC_YAW_P 2.0 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 -param set COM_RC_IN_MODE 2 dataman start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 558fa6464b..a46208e5fa 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -8,7 +8,6 @@ param set MC_YAW_P 2.0 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 -param set COM_RC_IN_MODE 2 dataman start param set CAL_GYRO0_ID 2293760 param set CAL_ACC0_ID 1376256 From 6e9a460c17b69548bcb5e13ab46c8e8279115689 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 14:56:18 +0200 Subject: [PATCH 26/39] MAVLink receiver: Clean up joystick interface --- src/modules/mavlink/mavlink_receiver.cpp | 26 +++++------------------- 1 file changed, 5 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 08b24f339d..c333ba7cc4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -990,8 +990,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) switch_pos_t MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) { - // XXX non-standard 3 pos switch decoding - return (buttons >> (sw * 2)) & 3; + // This 2-bit method should be used in the future: (buttons >> (sw * 2)) & 3; + return (buttons & (1 << sw)); } int @@ -1101,26 +1101,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.values[0] = man.x / 2 + 1500; /* pitch */ rc.values[1] = man.y / 2 + 1500; - - /* - * yaw needs special handling as some joysticks have a circular mechanical mask, - * which makes the corner positions unreachable. - * scale yaw up and clip it to overcome this. - */ - rc.values[2] = man.r / 1.1f + 1500; - if (rc.values[2] > 2000) { - rc.values[2] = 2000; - } else if (rc.values[2] < 1000) { - rc.values[2] = 1000; - } - + /* yaw */ + rc.values[2] = man.r / 2 + 1500; /* throttle */ - rc.values[3] = man.z / 0.9f + 1000; - if (rc.values[3] > 2000) { - rc.values[3] = 2000; - } else if (rc.values[3] < 1000) { - rc.values[3] = 1000; - } + rc.values[3] = man.z / 2 + 1000; /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); From f2a780dffdbb1c338e2f6c137941b9afbe7fcd0a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 14:56:42 +0200 Subject: [PATCH 27/39] Manual control: Add switches --- src/modules/mavlink/mavlink_messages.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 67809ed7be..2829b97cf3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2057,7 +2057,14 @@ protected: msg.y = manual.y * 1000; msg.z = manual.z * 1000; msg.r = manual.r * 1000; + unsigned shift = 2; msg.buttons = 0; + msg.buttons |= (manual.mode_switch << (shift * 0)); + msg.buttons |= (manual.return_switch << (shift * 1)); + msg.buttons |= (manual.posctl_switch << (shift * 2)); + msg.buttons |= (manual.loiter_switch << (shift * 3)); + msg.buttons |= (manual.acro_switch << (shift * 4)); + msg.buttons |= (manual.offboard_switch << (shift * 5)); _mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg); } From a058ce4b8ee298ec3cb7edcd5a585dd899dac120 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 14:57:05 +0200 Subject: [PATCH 28/39] Mission: Do not emit status messages if nothing changes --- src/modules/mavlink/mavlink_mission.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 2c1a3d88f6..df8f9a1bf3 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -479,8 +479,6 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } - - _mavlink->send_statustext_info("WPM: mission is empty"); } send_mission_count(msg->sysid, msg->compid, _count); From d93337017de5860732c0a5ae4e8f5f0c3e6ba525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 14:57:23 +0200 Subject: [PATCH 29/39] Commander: Do not emit status message if RC becomes available first time --- src/modules/commander/commander.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 397083a3b5..5f901c663f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1226,7 +1226,8 @@ int commander_thread_main(int argc, char *argv[]) status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1863,17 +1864,16 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - 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)) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_info(mavlink_fd, "Detected radio control"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_info(mavlink_fd, "RC SIGNAL REGAINED after %llums", + mavlink_log_info(mavlink_fd, "MANUAL CONTROL REGAINED after %llums", (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } @@ -1979,7 +1979,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000); + mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; From 4fae86b5ac758f66e34139f5a015debe476578ea Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 3 Sep 2015 17:49:54 +0200 Subject: [PATCH 30/39] mavlink socket: stream to localhost before actually receiving packets --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- src/modules/mavlink/mavlink_main.h | 5 ++++- src/modules/mavlink/mavlink_receiver.cpp | 7 +++++++ 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f623a0d7b3..4928010830 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -974,11 +974,11 @@ Mavlink::init_udp() return; } - unsigned char inbuf[256]; - socklen_t addrlen = sizeof(_src_addr); + // set default target address + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + _src_addr.sin_port = htons(_network_port); - // wait for client to connect to socket - recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen); #endif } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 00716480cf..5789b11124 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -47,6 +47,7 @@ #else #include #include +#include #include #endif #include @@ -333,7 +334,9 @@ public: unsigned short get_network_port() { return _network_port; } int get_socket_fd () { return _socket_fd; }; - +#ifdef __PX4_POSIX + struct sockaddr_in * get_client_source_address() {return &_src_addr;}; +#endif static bool boot_complete() { return _boot_complete; } protected: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c333ba7cc4..e879c78b86 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1776,6 +1776,13 @@ MavlinkReceiver::receive_thread(void *arg) } else { // could be TCP or other protocol } + + struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); + int localhost = (127 << 24) + 1; + if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { + // if we were sending to localhost before but have a new host then accept him + memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); + } #endif /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { From ae2dfe002642bd0e445255fecc26280468bcda85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 19:11:17 +0200 Subject: [PATCH 31/39] Fix MAVLink network init --- src/modules/mavlink/mavlink_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4928010830..1dfc4cbe17 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -94,6 +94,7 @@ #endif static const int ERROR = -1; +#define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec #define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate @@ -885,6 +886,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID 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 + warnx("TCP transport pending implementation"); } #endif @@ -975,9 +977,10 @@ Mavlink::init_udp() } // set default target address + memset((char *)&_src_addr, 0, sizeof(_src_addr)); _src_addr.sin_family = AF_INET; inet_aton("127.0.0.1", &_src_addr.sin_addr); - _src_addr.sin_port = htons(_network_port); + _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); #endif } From f63dd12952d6f13f84a87729ed963b85939ef631 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 19:11:40 +0200 Subject: [PATCH 32/39] Fix MAVLink MANUAL_CONTROL simulation mode handling --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e879c78b86..b92d77b49c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1104,7 +1104,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) /* yaw */ rc.values[2] = man.r / 2 + 1500; /* throttle */ - rc.values[3] = man.z / 2 + 1000; + rc.values[3] = man.z / 1 + 1000; /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); From be92c1189b6c0dbeb73cfadaf8fe601e380e84d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 19:57:44 +0200 Subject: [PATCH 33/39] Fix handling of RC mode selection --- src/modules/commander/commander.cpp | 15 +++++++++------ src/modules/commander/state_machine_helper.cpp | 4 +--- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5f901c663f..f9477d6f7f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1220,14 +1220,17 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check + int32_t rc_in_off = 0; param_get(_param_autostart_id, &autostart_id); if (is_hil_setup(autostart_id)) { // HIL configuration selected: real sensors will be disabled status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { + param_get(_param_rc_in_off, &rc_in_off); + status.rc_input_mode = rc_in_off; status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, - checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1243,7 +1246,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; - int32_t rc_in_off = 0; int32_t datalink_regain_timeout = 0; /* Thresholds for engine failure detection */ @@ -1913,8 +1915,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY && - sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { + if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* we check outside of the transition function here because the requirement @@ -1925,13 +1926,15 @@ int commander_thread_main(int argc, char *argv[]) (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - } else { + } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } + } else { + print_reject_arm("NOT ARMING: Configuration error"); } stick_on_counter = 0; @@ -1978,7 +1981,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (!status.rc_signal_lost) { + if (!status.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp = sp_man.timestamp; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 67864ebf38..9227a141de 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -245,9 +245,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { - if (!fRunPreArmChecks) { - mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - } + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); feedback_provided = true; valid_transition = false; status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; From 86d1e38f7aa33c6fb960dd92eb66551f5d5bf585 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 19:58:04 +0200 Subject: [PATCH 34/39] MAVLink: Improve switch handling --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b92d77b49c..58124f2133 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -991,7 +991,7 @@ switch_pos_t MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) { // This 2-bit method should be used in the future: (buttons >> (sw * 2)) & 3; - return (buttons & (1 << sw)); + return (buttons & (1 << sw)) ? manual_control_setpoint_s::SWITCH_POS_ON : manual_control_setpoint_s::SWITCH_POS_OFF; } int From 348484fac342495ee33086542d10b2883827dd73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Sep 2015 19:58:23 +0200 Subject: [PATCH 35/39] POSIX: Start commander after sensors --- posix-configs/SITL/init/rc.fixed_wing | 2 +- posix-configs/SITL/init/rcS | 2 +- posix-configs/SITL/init/rc_iris_ros | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index 3d65b7a4c7..096ad53302 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -29,8 +29,8 @@ barosim start adcsim start gpssim start pwm_out_sim mode_pwm -commander start sensors start +commander start land_detector start fixedwing navigator start ekf_att_pos_estimator start diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 26bf98cb8b..8e35bed56f 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -34,8 +34,8 @@ barosim start adcsim start gpssim start pwm_out_sim mode_pwm -commander start sensors start +commander start land_detector start multicopter navigator start attitude_estimator_q start diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index a46208e5fa..21257c6633 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -48,8 +48,8 @@ barosim start adcsim start gpssim start pwm_out_sim mode_pwm -commander start sensors start +commander start land_detector start multicopter navigator start attitude_estimator_q start From b2bef75bb4e6ef7ac04540883a9c973874f1a0b2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 4 Sep 2015 17:00:57 +0200 Subject: [PATCH 36/39] start simulated airspeed sensor driver for fixed wing SITL --- posix-configs/SITL/init/rc.fixed_wing | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index 096ad53302..e1a5883d0a 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -28,6 +28,7 @@ accelsim start barosim start adcsim start gpssim start +measairspeedsim start pwm_out_sim mode_pwm sensors start commander start From 0fe272c9b3fcac06a9f30f9ab19fd94ad9d969c8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 4 Sep 2015 17:01:33 +0200 Subject: [PATCH 37/39] support simulated airspeed --- src/modules/simulator/simulator.cpp | 9 +++++++++ src/modules/simulator/simulator.h | 11 +++++++++++ src/modules/simulator/simulator_mavlink.cpp | 8 +++++++- 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index f0345ba036..f8c8fcec0f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -86,6 +86,11 @@ bool Simulator::getGPSSample(uint8_t *buf, int len) return _gps.copyData(buf, len); } +bool Simulator::getAirspeedSample(uint8_t *buf, int len) +{ + return _airspeed.copyData(buf, len); +} + void Simulator::write_MPU_data(void *buf) { _mpu.writeData(buf); } @@ -106,6 +111,10 @@ void Simulator::write_gps_data(void *buf) { _gps.writeData(buf); } +void Simulator::write_airspeed_data(void *buf) { + _airspeed.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index dbb3d5b783..af4b8a746c 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -94,6 +94,13 @@ struct RawBaroData { }; #pragma pack(pop) +#pragma pack(push, 1) +struct RawAirspeedData { + float temperature; + float diff_pressure; +}; +#pragma pack(pop) + #pragma pack(push, 1) struct RawGPSData { int32_t lat; @@ -191,12 +198,14 @@ public: bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); bool getGPSSample(uint8_t *buf, int len); + bool getAirspeedSample(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); + void write_airspeed_data(void *buf); bool isInitialized() { return _initialized; } @@ -207,6 +216,7 @@ private: _baro(1), _mag(1), _gps(1), + _airspeed(1), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -238,6 +248,7 @@ private: simulator::Report _baro; simulator::Report _mag; simulator::Report _gps; + simulator::Report _airspeed; // uORB publisher handlers orb_advert_t _accel_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ec3a7c8a24..ddbc79db76 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -187,11 +187,17 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) { RawBaroData baro; // calculate air pressure from altitude (valid for low altitude) - baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt; + baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar baro.altitude = imu->pressure_alt; baro.temperature = imu->temperature; write_baro_data((void *)&baro); + + RawAirspeedData airspeed; + airspeed.temperature = imu->temperature; + airspeed.diff_pressure = imu->diff_pressure; + + write_airspeed_data((void *)&airspeed); } void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { From 9f90b877456648f0f6f742d50c1b730d8ec2e152 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 4 Sep 2015 17:06:22 +0200 Subject: [PATCH 38/39] implemented driver for simulated airspeed --- .../posix/drivers/airspeedsim/airspeedsim.cpp | 73 +- .../posix/drivers/airspeedsim/airspeedsim.h | 27 +- .../drivers/airspeedsim/meas_airspeed_sim.cpp | 631 ++++++++++++++++++ .../posix/drivers/airspeedsim/module.mk | 3 +- 4 files changed, 697 insertions(+), 37 deletions(-) create mode 100644 src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index 43cd080628..9cb0ac73d2 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -35,14 +35,12 @@ * @file ets_airspeed.cpp * @author Simon Wilks * @author Mark Charlebois (simulator) - * - * Driver for the Simulated Airspeed Sensor based on Eagle Tree Airspeed V3. + * @author Roman Bapst (simulator) + * Driver for the Simulated AirspeedSim Sensor based on Eagle Tree AirspeedSim V3. */ #include -#include - #include #include #include @@ -56,8 +54,6 @@ #include #include -#include - #include #include #include @@ -71,12 +67,15 @@ #include #include -#include +#include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : - I2C("Airspeed", path, bus, address), +#include "airspeedsim.h" + +AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) : + VDev("AIRSPEEDSIM", path), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), + _retries(0), _max_differential_pressure_pa(0), _sensor_ok(false), _last_published_sensor_ok(true), /* initialize differently to force publication */ @@ -97,7 +96,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha memset(&_work, 0, sizeof(_work)); } -Airspeed::~Airspeed() +AirspeedSim::~AirspeedSim() { /* make sure we are truly inactive */ stop(); @@ -116,18 +115,21 @@ Airspeed::~Airspeed() } int -Airspeed::init() +AirspeedSim::init() { int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + /* init base class */ + if (VDev::init() != OK) { + DEVICE_DEBUG("VDev init failed"); goto out; + } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); @@ -154,7 +156,7 @@ out: } int -Airspeed::probe() +AirspeedSim::probe() { /* on initial power up the device may need more than one retry for detection. Once it is running the number of retries can @@ -169,7 +171,7 @@ Airspeed::probe() } int -Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) +AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -272,12 +274,13 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + //return I2C::ioctl(filp, cmd, arg); XXX SIM should be super class + return 0; } } ssize_t -Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) +AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(differential_pressure_s); differential_pressure_s *abuf = reinterpret_cast(buffer); @@ -336,24 +339,24 @@ Airspeed::read(device::file_t *filp, char *buffer, size_t buflen) } void -Airspeed::start() +AirspeedSim::start() { /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1); } void -Airspeed::stop() +AirspeedSim::stop() { work_cancel(HPWORK, &_work); } void -Airspeed::update_status() +AirspeedSim::update_status() { if (_sensor_ok != _last_published_sensor_ok) { /* notify about state change */ @@ -375,9 +378,9 @@ Airspeed::update_status() } void -Airspeed::cycle_trampoline(void *arg) +AirspeedSim::cycle_trampoline(void *arg) { - Airspeed *dev = (Airspeed *)arg; + AirspeedSim *dev = (AirspeedSim *)arg; dev->cycle(); // XXX we do not know if this is @@ -387,7 +390,7 @@ Airspeed::cycle_trampoline(void *arg) } void -Airspeed::print_info() +AirspeedSim::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -397,8 +400,26 @@ Airspeed::print_info() } void -Airspeed::new_report(const differential_pressure_s &report) +AirspeedSim::new_report(const differential_pressure_s &report) { if (!_reports->force(&report)) perf_count(_buffer_overflows); } + +int +AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { + if (recv_len > 0) { + // this is equivalent to the collect phase + Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { + PX4_ERR("Error BARO_SIM::transfer no simulator"); + return -ENODEV; + } + PX4_DEBUG("BARO_SIM::transfer getting sample"); + sim->getAirspeedSample(recv, recv_len); + } else { + // we don't need measure phase + } + + return 0; +} \ No newline at end of file diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h index 66f4c58627..2c668fc97f 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h @@ -40,7 +40,7 @@ #include -#include +//#include #include #include @@ -55,10 +55,11 @@ #include #include -#include -#include -#include +//#include +//#include +//#include +#include #include #include @@ -69,6 +70,7 @@ #include #include #include +#include #include #include @@ -87,7 +89,7 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class __EXPORT AirspeedSim : public device::I2C +class __EXPORT AirspeedSim : public device::VDev { public: AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path); @@ -95,8 +97,8 @@ public: virtual int init(); - virtual ssize_t read(px4_dev_handle_t *handlep, char *buffer, size_t buflen); - virtual int ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. @@ -104,9 +106,11 @@ public: virtual void print_info(); private: - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; perf_counter_t _buffer_overflows; + unsigned _retries; // XXX this should come from the SIM class + /* this class has pointer data members and should not be copied */ AirspeedSim(const AirspeedSim &); AirspeedSim &operator=(const AirspeedSim &); @@ -122,16 +126,19 @@ protected: virtual int measure() = 0; virtual int collect() = 0; + virtual int transfer(const uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + /** * Update the subsystem status */ void update_status(); - work_s _work; + struct work_s _work; float _max_differential_pressure_pa; bool _sensor_ok; bool _last_published_sensor_ok; - int _measure_ticks; + unsigned _measure_ticks; bool _collect_phase; float _diff_pres_offset; diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp new file mode 100644 index 0000000000..5aca386acb --- /dev/null +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -0,0 +1,631 @@ +/**************************************************************************** + * + * Copyright (c) 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 meas_airspeed_sim.cpp + * @author Lorenz Meier + * @author Sarthak Kaingade + * @author Simon Wilks + * @author Thomas Gubler + * @author Roman Bapst + * + * Driver for a simulated airspeed sensor. + * + */ + + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include "airspeedsim.h" + +/* I2C bus address is 1010001x */ +#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ +#define PATH_MS4525 "/dev/ms4525" +/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ +#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ +#define PATH_MS5525 "/dev/ms5525" + +/* Register address */ +#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ + +/* Measurement rate is 100Hz */ +#define MEAS_RATE 100 +#define MEAS_DRIVER_FILTER_FREQ 1.2f +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ + +class MEASAirspeedSim : public AirspeedSim +{ +public: + MEASAirspeedSim(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); + +protected: + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + virtual void cycle(); + virtual int measure(); + virtual int collect(); + + math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa, float &temperature); + + int _t_system_power; + struct system_power_s system_power; +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int measairspeedsim_main(int argc, char *argv[]); + +MEASAirspeedSim::MEASAirspeedSim(int bus, int address, const char *path) : AirspeedSim(bus, address, + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1), + system_power{} +{} + +int +MEASAirspeedSim::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = 0; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int +MEASAirspeedSim::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + #pragma pack(push, 1) + struct { + float temperature; + float diff_pressure; + } airspeed_report; + #pragma pack(pop) + + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report)); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint8_t status = 0; + + switch (status) { + case 0: + break; + + case 1: + + /* fallthrough */ + case 2: + + /* fallthrough */ + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + } + + float temperature = airspeed_report.temperature; + float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar + + struct differential_pressure_s report; + + /* track maximum differential pressure measured (so we can work out top speed). */ + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; + } + + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.temperature = temperature; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); + + report.differential_pressure_raw_pa = diff_press_pa_raw; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + + if (_airspeed_pub != nullptr && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } + + new_report(report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + + return ret; +} + +void +MEASAirspeedSim::cycle() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + + if (OK != ret) { + /* restart the measurement state machine */ + start(); + _sensor_ok = false; + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&AirspeedSim::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&AirspeedSim::cycle_trampoline, + this, + USEC2TICK(CONVERSION_INTERVAL)); +} + +/** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + + if (_t_system_power == -1) { + // not available + return; + } + + bool updated = false; + orb_check(_t_system_power, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 65.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + + diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + + temperature -= voltage_diff * temp_slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + +/** + * Local functions in support of the shell command. + */ +namespace meas_airspeed_sim +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MEASAirspeedSim *g_dev = nullptr; + +int start(int i2c_bus); +int stop(); +int test(); +int reset(); +int info(); + +/** + * Start the driver. + * + * This function call only returns once the driver is up and running + * or failed to detect the sensor. + */ +int +start(int i2c_bus) +{ + int fd; + + if (g_dev != nullptr) { + PX4_WARN("already started"); + } + + /* create the driver, try the MS4525DO first */ + g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); + + /* check if the MS4525DO was instantiated */ + if (g_dev == nullptr) { + goto fail; + } + + /* try the MS5525DSO next if init fails */ + if (OK != g_dev->AirspeedSim::init()) { + delete g_dev; + g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); + + /* check if the MS5525DSO was instantiated */ + if (g_dev == nullptr) { + goto fail; + } + + /* both versions failed if the init for the MS5525DSO fails, give up */ + if (OK != g_dev->AirspeedSim::init()) { + goto fail; + } + } + + /* set the poll rate to default, starts automatic data collection */ + fd = px4_open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + return 0; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_ERR("no MS4525 airspeedSim sensor connected"); + return 1; +} + +/** + * Stop the driver + */ +int +stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + PX4_ERR("driver not running"); + } + + return 0; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + struct differential_pressure_s report; + ssize_t sz; + int ret; + + int fd = px4_open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525); + return 1; + } + + /* do a simple demand read */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("immediate read failed"); + return 1; + } + + PX4_WARN("single read"); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + + /* start the sensor polling at 2Hz */ + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_WARN("failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + warnx("timed out"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_WARN("periodic read failed"); + } + + PX4_WARN("periodic read %u", i); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); + } + + /* reset the sensor polling to its default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_ERR("failed to set default rate"); + return 1; + } + + PX4_WARN("PASS"); + + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + int fd = open(PATH_MS4525, O_RDONLY); + + if (fd < 0) { + PX4_ERR("failed "); + return 1; + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return 1; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + return 1; + } + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +} // namespace + + +static void +meas_airspeed_usage() +{ + PX4_WARN("usage: meas_airspeed_sim command [options]"); + PX4_WARN("options:"); + PX4_WARN("\t-b --bus i2cbus (%d)", 1); + PX4_WARN("command:"); + PX4_WARN("\tstart|stop|reset|test|info"); +} + +int +measairspeedsim_main(int argc, char *argv[]) +{ + int i2c_bus = 1;//PX4_I2C_BUS_DEFAULT; + + int i; + + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + i2c_bus = atoi(argv[i + 1]); + } + } + } + + int ret; + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + ret = meas_airspeed_sim::start(i2c_bus); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + ret = meas_airspeed_sim::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + ret = meas_airspeed_sim::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + ret = meas_airspeed_sim::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + ret = meas_airspeed_sim::info(); + } + + meas_airspeed_usage(); + return ret; +} diff --git a/src/platforms/posix/drivers/airspeedsim/module.mk b/src/platforms/posix/drivers/airspeedsim/module.mk index 2d971e4d28..fb12fa04a2 100644 --- a/src/platforms/posix/drivers/airspeedsim/module.mk +++ b/src/platforms/posix/drivers/airspeedsim/module.mk @@ -34,7 +34,8 @@ # # Makefile to build the generic airspeed driver. # +MODULE_COMMAND = measairspeedsim -SRCS = airspeedsim.cpp +SRCS = airspeedsim.cpp meas_airspeed_sim.cpp MAXOPTIMIZATION = -Os From 329f7c013066cf2a2a5252784e4784db74e542f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Sep 2015 10:28:36 +0200 Subject: [PATCH 39/39] MAVLink: Consolidate message forwarding flag --- src/modules/mavlink/mavlink_main.cpp | 13 ++++--------- src/modules/mavlink/mavlink_main.h | 1 - 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1dfc4cbe17..dee3fc7e35 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -154,7 +154,6 @@ Mavlink::Mavlink() : _receive_thread {}, _verbose(false), _forwarding_on(false), - _passing_on(false), _ftp_on(false), #ifndef __PX4_POSIX _uart_fd(-1), @@ -1322,7 +1321,7 @@ Mavlink::message_buffer_mark_read(int n) void Mavlink::pass_message(const mavlink_message_t *msg) { - if (_passing_on) { + if (_forwarding_on) { /* size is 8 bytes plus variable payload */ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; pthread_mutex_lock(&_message_buffer_mutex); @@ -1497,10 +1496,6 @@ Mavlink::task_main(int argc, char *argv[]) _forwarding_on = true; break; - case 'p': - _passing_on = true; - break; - case 'v': _verbose = true; break; @@ -1566,7 +1561,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { /* initialize message buffer if multiplexing is on or its needed for FTP. * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. @@ -1838,7 +1833,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* pass messages from other UARTs or FTP worker */ - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { bool is_part; uint8_t *read_ptr; @@ -1947,7 +1942,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ px4_close(_mavlink_fd); - if (_passing_on || _ftp_on) { + if (_forwarding_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5789b11124..4d28b1c6e5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -379,7 +379,6 @@ private: bool _verbose; bool _forwarding_on; - bool _passing_on; bool _ftp_on; #ifndef __PX4_QURT int _uart_fd;