diff --git a/ROMFS/px4fmu_test/mixers/IO_pass.mix b/ROMFS/px4fmu_test/mixers/IO_pass.mix index 42321f0ad9..39f875ddb9 100644 --- a/ROMFS/px4fmu_test/mixers/IO_pass.mix +++ b/ROMFS/px4fmu_test/mixers/IO_pass.mix @@ -1,24 +1,38 @@ +Passthrough mixer for PX4IO +============================ + +This file defines passthrough mixers suitable for testing. + +Channel group 0, channels 0-7 are passed directly through to the outputs. + M: 1 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 0 1 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 0 2 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 0 4 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 0 5 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 0 6 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_test/mixers/quad_test.mix b/ROMFS/px4fmu_test/mixers/quad_test.mix deleted file mode 100644 index a4876bea21..0000000000 --- a/ROMFS/px4fmu_test/mixers/quad_test.mix +++ /dev/null @@ -1,25 +0,0 @@ -Multirotor mixer for TEST -=========================== - -This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%. - -R: 4w 10000 10000 10000 0 - -Gimbal / payload mixer for last four channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_test/mixers/quad_x.main.mix b/ROMFS/px4fmu_test/mixers/quad_x.main.mix new file mode 100644 index 0000000000..bf034e7c1f --- /dev/null +++ b/ROMFS/px4fmu_test/mixers/quad_x.main.mix @@ -0,0 +1,7 @@ +R: 4x 10000 10000 10000 0 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 3 5 10000 10000 0 -10000 10000 +M: 1 +O: 10000 10000 0 -10000 10000 +S: 3 6 10000 10000 0 -10000 10000 diff --git a/Tools/adb_upload.sh b/Tools/adb_upload.sh index b4e4f649cb..5b9475abcc 100755 --- a/Tools/adb_upload.sh +++ b/Tools/adb_upload.sh @@ -5,13 +5,16 @@ if [[ "$#" < 2 ]]; then exit fi -echo "Wait for device..." -adb wait-for-device -echo "Uploading..." - # Get last argument for last; do true; done +echo "Wait for device..." +adb wait-for-device + +echo "Creating folder structure..." +adb shell mkdir -p $last + +echo "Uploading..." # Go through source files and push them one by one. i=0 for arg diff --git a/Tools/adb_upload_to_bebop.sh b/Tools/adb_upload_to_bebop.sh index 48edf0007e..b54f938a01 100755 --- a/Tools/adb_upload_to_bebop.sh +++ b/Tools/adb_upload_to_bebop.sh @@ -1,9 +1,9 @@ #!/bin/bash -if [ -z ${BEBOP_IP+x} ]; then +if [ -z ${BEBOP_IP+x} ]; then ip=192.168.42.1 echo "\$BEBOP_IP is not set (use default: $ip)" -else +else ip=$BEBOP_IP echo "\$BEBOP_IP is set to $ip" fi diff --git a/Tools/decode_backtrace.py b/Tools/decode_backtrace.py index 64f8a8ea30..a9eedb5c29 100755 --- a/Tools/decode_backtrace.py +++ b/Tools/decode_backtrace.py @@ -63,7 +63,7 @@ def usage(): msg = """ Usage: Tools/decode_backtrace.py -This will load the symbols for /src/firmware/posix/px4 +This will load the symbols for /bin/px4 The user just needs to copy and paste the backtrace into the terminal where decode_backtrace.py is running. @@ -75,7 +75,7 @@ func = [] # Load the symbols from the binary def load_symbol_map(): - output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/src/firmware/posix/px4"]) + output = subprocess.check_output(["nm", "-p", "-C", os.sys.argv[1]+"/bin/px4"]) data = output.split("\n") data.sort() diff --git a/Tools/sitl_multiple_run.sh b/Tools/sitl_multiple_run.sh index 7f590d87cb..a15ab1d3ec 100755 --- a/Tools/sitl_multiple_run.sh +++ b/Tools/sitl_multiple_run.sh @@ -43,7 +43,7 @@ while [ $n -le $sitl_num ]; do pushd "$working_dir" &>/dev/null echo "starting instance $n in $(pwd)" - sudo -b -u $user ../src/firmware/posix/px4 -d "$src_path" rcS >out.log 2>err.log + sudo -b -u $user ../bin/px4 -d "$src_path" rcS >out.log 2>err.log popd &>/dev/null n=$(($n + 1)) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 4a516b8814..53c9e3c6ae 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -5,7 +5,7 @@ set -e echo args: $@ sitl_bin=$1 -rcS_dir=$2 +rcS_path=$2 debugger=$3 program=$4 model=$5 @@ -15,7 +15,7 @@ build_path=$7 echo SITL ARGS echo sitl_bin: $sitl_bin -echo rcS_dir: $rcS_dir +echo rcS_path: $rcS_path echo debugger: $debugger echo program: $program echo model: $model @@ -63,7 +63,7 @@ fi if [ "$#" -lt 7 ] then - echo usage: sitl_run.sh rc_script rcS_dir debugger program model src_path build_path + echo usage: sitl_run.sh sitl_bin rcS_path debugger program model src_path build_path echo "" exit 1 fi @@ -123,10 +123,18 @@ cd $working_dir # Do not exit on failure now from here on because we want the complete cleanup set +e -sitl_command="$sudo_enabled $sitl_bin $no_pxh $chroot_enabled $src_path $src_path/${rcS_dir}/${model}" +sitl_command="$sitl_bin $no_pxh $rootfs $rootfs/${rcS_path}" echo SITL COMMAND: $sitl_command +# Prepend to path to prioritize PX4 commands over potentially already +# installed PX4 commands. +export PATH="$build_path/bin":$PATH + +export PX4_SIM_MODEL=${model} + +pushd $rootfs + if [[ -n "$DONT_RUN" ]] then echo "Not running simulation (\$DONT_RUN is set)." @@ -156,9 +164,11 @@ then echo "######################################################################" read else - $sitl_command + eval $sitl_command fi +popd + if [[ -z "$DONT_RUN" ]] then if [ "$program" == "jmavsim" ] diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index c04108b261..0bba4a1cfe 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -39,6 +39,7 @@ set(config_module_list systemcmds/ver systemcmds/esc_calib systemcmds/reboot + systemcmds/shutdown systemcmds/topic_listener systemcmds/tune_control systemcmds/perf diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake index 70e1377317..5aee64186c 100644 --- a/cmake/configs/posix_sdflight_default.cmake +++ b/cmake/configs/posix_sdflight_default.cmake @@ -41,6 +41,7 @@ set(config_module_list systemcmds/led_control systemcmds/mixer systemcmds/ver + systemcmds/shutdown systemcmds/topic_listener systemcmds/tune_control diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 544d43f2b2..b27477d63b 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -34,6 +34,7 @@ set(config_module_list systemcmds/perf systemcmds/pwm systemcmds/reboot + systemcmds/shutdown systemcmds/sd_bench systemcmds/top systemcmds/topic_listener @@ -147,7 +148,7 @@ set(config_module_list # Default config_sitl_rcS_dir (posix_sitl_default), this is overwritten later # for the config posix_sitl_efk2 and set again, explicitly, for posix_sitl_lpe, # which are based on posix_sitl_default. -set(config_sitl_rcS_dir posix-configs/SITL/init/ekf2 CACHE INTERNAL "init script dir for sitl") +set(config_sitl_rcS_dir etc/init/rcS CACHE INTERNAL "init script dir for sitl") set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") @@ -162,3 +163,4 @@ if(REPLAY_FILE) message("Building with uorb publisher rules support") add_definitions(-DORB_USE_PUBLISHER_RULES) endif() + diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake index ae6191e508..d8ee1acc8a 100644 --- a/cmake/configs/posix_sitl_ekf2.cmake +++ b/cmake/configs/posix_sitl_ekf2.cmake @@ -1,5 +1 @@ include(cmake/configs/posix_sitl_default.cmake) - -set(config_sitl_rcS_dir - posix-configs/SITL/init/ekf2 - ) diff --git a/cmake/posix/apps.h_in b/cmake/posix/apps.h_in new file mode 100644 index 0000000000..bf76b9725d --- /dev/null +++ b/cmake/posix/apps.h_in @@ -0,0 +1,80 @@ +/* builtin command list - automatically generated, do not edit */ +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; + +extern void px4_show_devices(void); + +extern "C" { +${builtin_apps_decl_string} +static int list_tasks_main(int argc, char *argv[]); +static int list_files_main(int argc, char *argv[]); +static int list_devices_main(int argc, char *argv[]); +static int list_topics_main(int argc, char *argv[]); +static int sleep_main(int argc, char *argv[]); +} + +static map app_map(void) +{ + static map apps; + +${builtin_apps_string} + apps["list_tasks"] = list_tasks_main; + apps["list_files"] = list_files_main; + apps["list_devices"] = list_devices_main; + apps["list_topics"] = list_topics_main; + apps["sleep"] = sleep_main; + + return apps; +} + +map apps = app_map(); + +static void list_builtins(void) +{ + printf("Builtin Commands:\n"); + for (map::iterator it=apps.begin(); it!=apps.end(); ++it) + printf("\t%s", it->first.c_str()); +} + +static int list_tasks_main(int argc, char *argv[]) +{ + px4_show_tasks(); + return 0; +} + +static int list_devices_main(int argc, char *argv[]) +{ + px4_show_devices(); + return 0; +} + +static int list_topics_main(int argc, char *argv[]) +{ + px4_show_topics(); + return 0; +} + +static int list_files_main(int argc, char *argv[]) +{ + px4_show_files(); + return 0; +} + +static int sleep_main(int argc, char *argv[]) +{ + if (argc != 2) { + printf("Usage: sleep \n"); + return 1; + } + sleep(atoi(argv[1])); + return 0; +} + diff --git a/cmake/posix/px4-alias.sh_in b/cmake/posix/px4-alias.sh_in new file mode 100644 index 0000000000..e6ebb88681 --- /dev/null +++ b/cmake/posix/px4-alias.sh_in @@ -0,0 +1,13 @@ +#!/usr/bin/bash + +# File is auto-generated by cmake compilation, do not edit. + +# Ignore the expand_aliases command in zshell. +if [ ! -n "$ZSH_VERSION" ]; then + shopt -s expand_aliases +fi + +# Don't stop on errors. +#set -e + +${alias_string} diff --git a/launch/iris_env.sh b/launch/iris_env.sh new file mode 100644 index 0000000000..bfaeeb39cc --- /dev/null +++ b/launch/iris_env.sh @@ -0,0 +1,2 @@ +#!/bin/bash +export PX4_SIM_MODEL="iris" diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch index 70bb338181..408122c9c8 100644 --- a/launch/mavros_posix_sitl.launch +++ b/launch/mavros_posix_sitl.launch @@ -14,7 +14,8 @@ - + + diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch index 64df6d349d..4a8d00ceca 100644 --- a/launch/posix_sitl.launch +++ b/launch/posix_sitl.launch @@ -14,7 +14,8 @@ - + + @@ -26,7 +27,9 @@ - + + @@ -39,3 +42,8 @@ + + + + + diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 45997df367..4adc266dd3 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -85,6 +85,7 @@ set(msg_files power_button_state.msg pwm_input.msg qshell_req.msg + qshell_retval.msg rate_ctrl_status.msg rc_channels.msg rc_parameter_map.msg diff --git a/msg/qshell_req.msg b/msg/qshell_req.msg index fa75390edd..478d1e7028 100644 --- a/msg/qshell_req.msg +++ b/msg/qshell_req.msg @@ -1,3 +1,4 @@ -int32[100] string -uint64 MAX_STRLEN = 100 -uint64 strlen +uint8[100] cmd +uint32 MAX_STRLEN = 100 +uint32 strlen +uint32 sequence diff --git a/msg/qshell_retval.msg b/msg/qshell_retval.msg new file mode 100644 index 0000000000..f51d16ea7f --- /dev/null +++ b/msg/qshell_retval.msg @@ -0,0 +1,2 @@ +int32 return_value +uint32 sequence diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 228b1b2b1f..a93613ce82 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -5,10 +5,22 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}) get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) + +set(PX4_BASH_PREFIX "px4-") + +add_definitions("-DPX4_BASH_PREFIX=\"${PX4_BASH_PREFIX}\"") + px4_posix_generate_builtin_commands( OUT apps MODULE_LIST ${module_libraries}) +px4_posix_generate_alias( + OUT ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/px4-alias.sh + MODULE_LIST ${module_libraries} + PREFIX ${PX4_BASH_PREFIX} +) + if (("${BOARD}" STREQUAL "eagle") OR ("${BOARD}" STREQUAL "excelsior")) include(fastrpc) include(linux_app) @@ -185,4 +197,37 @@ install( ${PROJECT_SOURCE_DIR}/Tools/sitl_gazebo/package.xml DESTINATION ${PROJECT_NAME}/Tools/sitl_gazebo - ) \ No newline at end of file + ) + + +# symlinks +px4_posix_generate_symlinks( + MODULE_LIST ${module_libraries} + PREFIX ${PX4_BASH_PREFIX} + TARGET px4 +) + +add_custom_command(TARGET px4 + POST_BUILD + COMMAND mkdir -p ${CMAKE_BINARY_DIR}/rootfs/etc/ + ) + +add_custom_command(TARGET px4 + POST_BUILD + COMMAND mkdir -p ${CMAKE_BINARY_DIR}/rootfs/etc/ + ) + +add_custom_command(TARGET px4 + POST_BUILD + COMMAND cp -R ${PROJECT_SOURCE_DIR}/posix-configs/SITL/init ${CMAKE_BINARY_DIR}/rootfs/etc/ +) + +add_custom_command(TARGET px4 + POST_BUILD + COMMAND cp -R ${PROJECT_SOURCE_DIR}/ROMFS/sitl/mixers ${CMAKE_BINARY_DIR}/rootfs/etc/ +) + +add_custom_command(TARGET px4 + POST_BUILD + COMMAND cp -R ${PROJECT_SOURCE_DIR}/ROMFS/px4fmu_common/mixers ${CMAKE_BINARY_DIR}/rootfs/etc/ +) diff --git a/platforms/posix/cmake/px4_impl_os.cmake b/platforms/posix/cmake/px4_impl_os.cmake index c7ed95297e..217d1662a5 100644 --- a/platforms/posix/cmake/px4_impl_os.cmake +++ b/platforms/posix/cmake/px4_impl_os.cmake @@ -103,6 +103,65 @@ function(px4_posix_generate_builtin_commands) configure_file(${PX4_SOURCE_DIR}/src/platforms/apps.h.in ${OUT}.h) endfunction() + +# TODO: document API +function(px4_posix_generate_alias) + px4_parse_function_args( + NAME px4_posix_generate_alias + ONE_VALUE OUT PREFIX + MULTI_VALUE MODULE_LIST + REQUIRED OUT PREFIX MODULE_LIST + ARGN ${ARGN}) + + set(alias_string) + foreach(module ${MODULE_LIST}) + foreach(property MAIN STACK PRIORITY) + get_target_property(${property} ${module} ${property}) + if(NOT ${property}) + set(${property} ${${property}_DEFAULT}) + endif() + endforeach() + if (MAIN) + set(alias_string + "${alias_string}alias ${MAIN}='${PREFIX}${MAIN}'\n" + ) + endif() + endforeach() + configure_file(${PX4_SOURCE_DIR}/cmake/posix/px4-alias.sh_in + ${OUT} + ) +endfunction() + + +# TODO: document API +function(px4_posix_generate_symlinks) + px4_parse_function_args( + NAME px4_posix_generate_symlinks + ONE_VALUE TARGET PREFIX + MULTI_VALUE MODULE_LIST + REQUIRED TARGET PREFIX MODULE_LIST + ARGN ${ARGN}) + + foreach(module ${MODULE_LIST}) + + foreach(property MAIN STACK PRIORITY) + get_target_property(${property} ${module} ${property}) + if(NOT ${property}) + set(${property} ${${property}_DEFAULT}) + endif() + endforeach() + if (MAIN) + set(ln_name "${PREFIX}${MAIN}") + add_custom_command(TARGET ${TARGET} + POST_BUILD + COMMAND ${CMAKE_COMMAND} -E create_symlink ${TARGET} ${ln_name} + WORKING_DIRECTORY "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}" + ) + endif() + endforeach() +endfunction() + + #============================================================================= # # px4_os_add_flags diff --git a/platforms/posix/include/px4_daemon/server_io.h b/platforms/posix/include/px4_daemon/server_io.h new file mode 100644 index 0000000000..88c417501d --- /dev/null +++ b/platforms/posix/include/px4_daemon/server_io.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 server_io.h + * + * These are helper functions to send the stdout over a pipe + * back to the client. + * + * @author Julian Oes + * @author Beat Küng + */ +#pragma once + +#include + +__BEGIN_DECLS + +/** + * Get the stdout pipe buffer in order to write to fill it. + * + * @param buffer: pointer to buffer that will be set in function. + * @param max_length: length of the assigned buffer. + * @param is_atty: true if we are writing to a terminal (and can e.g. use colors). + * @return 0 on success + */ +__EXPORT int get_stdout_pipe_buffer(char **buffer, unsigned *max_length, bool *is_atty); + +/** + * Write the filled bytes to the pipe. + * + * @param buffer_length: the number of bytes that should be written. + * @return 0 on success + */ +__EXPORT int send_stdout_pipe_buffer(unsigned buffer_length); +__END_DECLS diff --git a/platforms/posix/src/CMakeLists.txt b/platforms/posix/src/CMakeLists.txt index 602b1b7107..00bab5f2de 100644 --- a/platforms/posix/src/CMakeLists.txt +++ b/platforms/posix/src/CMakeLists.txt @@ -31,4 +31,5 @@ # ############################################################################ +add_subdirectory(px4_daemon) add_subdirectory(px4_layer) diff --git a/platforms/posix/src/main.cpp b/platforms/posix/src/main.cpp index 54478e838d..42bb1be99a 100644 --- a/platforms/posix/src/main.cpp +++ b/platforms/posix/src/main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2015-2018 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 @@ -32,84 +32,473 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * + * This is the main() of PX4 for POSIX. + * + * The application is designed as a daemon/server app with multiple clients. + * Both, the server and the client is started using this main() function. + * + * If the executable is called with its usual name 'px4', it will start the + * server. However, if it is started with an executable name (symlink) starting + * with 'px4-' such as 'px4-navigator', it will start as a client and try to + * connect to the server. + * + * The symlinks for all modules are created using the build system. * * @author Mark Charlebois * @author Roman Bapst + * @author Julian Oes + * @author Beat Küng */ -#include -#include #include -#include -#include +#include #include -#include #include +#include +#include +#include + +#include +#include +#include +#include + #include "apps.h" #include "px4_middleware.h" -#include "px4_posix.h" -#include "px4_log.h" #include "DriverFramework.hpp" -#include -#include -#include +#include "px4_middleware.h" +#include "px4_daemon/client.h" +#include "px4_daemon/server.h" +#include "px4_daemon/pxh.h" + + +static const char *LOCK_FILE_PATH = "/tmp/px4_lock"; + +#ifndef PATH_MAX +#define PATH_MAX 1024 +#endif + + +static volatile bool _exit_requested = false; + namespace px4 { -void init_once(); +void init_once(void); } -using namespace std; +static void sig_int_handler(int sig_num); +static void sig_fpe_handler(int sig_num); +static void sig_segv_handler(int sig_num); -#define CMD_BUFF_SIZE 100 +static void register_sig_handler(); +static void set_cpu_scaling(); +static int create_symlinks_if_needed(std::string &data_path); +static int create_dirs(); +static int run_startup_bash_script(const char *commands_file); +static void wait_to_exit(); +static bool is_already_running(); +static void print_usage(); +static bool dir_exists(const std::string &path); +static bool file_exists(const std::string &name); +static std::string pwd(); -#ifdef PATH_MAX -const unsigned path_max_len = PATH_MAX; + +#ifdef __PX4_SITL_MAIN_OVERRIDE +int SITL_MAIN(int argc, char **argv); + +int SITL_MAIN(int argc, char **argv) #else -const unsigned path_max_len = 1024; +int main(int argc, char **argv) +#endif +{ + bool is_client = false; + bool pxh_off = false; + + /* Symlinks point to all commands that can be used as a client with a prefix. */ + const char prefix[] = PX4_BASH_PREFIX; + + if (strstr(argv[0], prefix)) { + is_client = true; + } + + if (is_client) { + + if (!is_already_running()) { + PX4_ERR("PX4 daemon not running yet"); + return -1; + } + + /* Remove the prefix. */ + argv[0] += strlen(prefix); + + px4_daemon::Client client; + client.generate_uuid(); + client.register_sig_handler(); + return client.process_args(argc, (const char **)argv); + + } else { + if (is_already_running()) { + PX4_ERR("PX4 daemon already running"); + return -1; + } + + /* Server/daemon apps need to parse the command line arguments. */ + + int positional_arg_count = 0; + std::string data_path = ""; + std::string commands_file = ""; + std::string node_name = ""; + + // parse arguments + for (int i = 1; i < argc; ++i) { + if (argv[i][0] == '-') { + + if (strcmp(argv[i], "-h") == 0) { + print_usage(); + return 0; + + } else if (strcmp(argv[i], "-d") == 0) { + pxh_off = true; + + } else { + printf("Unknown/unhandled parameter: %s\n", argv[i]); + print_usage(); + return 1; + } + + } else if (!strncmp(argv[i], "__", 2)) { + + // FIXME: what is this? + // ros arguments + if (!strncmp(argv[i], "__name:=", 8)) { + std::string name_arg = argv[i]; + node_name = name_arg.substr(8); + PX4_INFO("node name: %s", node_name.c_str()); + } + + } else { + //printf("positional argument\n"); + + positional_arg_count += 1; + + if (positional_arg_count == 1) { + commands_file = argv[i]; + + } else if (positional_arg_count == 2) { + data_path = commands_file; + commands_file = argv[i]; + } + } + + if (positional_arg_count != 2 && positional_arg_count != 1) { + PX4_ERR("Error expected 1 or 2 position arguments, got %d", positional_arg_count); + print_usage(); + return -1; + } + } + + if (!file_exists(commands_file)) { + PX4_ERR("Error opening startup file, does not exist: %s", commands_file.c_str()); + return -1; + } + + PX4_INFO("startup file: %s", commands_file.c_str()); + + register_sig_handler(); + set_cpu_scaling(); + + px4_daemon::Server server; + server.start(); + + int ret = create_symlinks_if_needed(data_path); + + if (ret != PX4_OK) { + return ret; + } + + ret = create_dirs(); + + if (ret != PX4_OK) { + return ret; + } + + DriverFramework::Framework::initialize(); + + px4::init_once(); + px4::init(argc, argv, "px4"); + + ret = run_startup_bash_script(commands_file.c_str()); + + // We now block here until we need to exit. + if (pxh_off) { + wait_to_exit(); + + } else { + px4_daemon::Pxh pxh; + pxh.run_pxh(); + } + + // When we exit, we need to stop muorb on Snapdragon. + +#ifdef __PX4_POSIX_EAGLE + // Sending muorb stop is needed if it is running to exit cleanly. + // TODO: we should check with px4_task_is_running("muorb") before stopping it. + std::string muorb_stop_cmd("muorb stop"); + px4_daemon::Pxh::process_line(muorb_stop_cmd, true); #endif -static volatile bool _ExitFlag = false; + std::string cmd("shutdown"); + px4_daemon::Pxh::process_line(cmd, true); -static struct termios orig_term; - -extern "C" { - void _SigIntHandler(int sig_num); - void _SigIntHandler(int sig_num) - { - cout.flush(); - cout << endl << "Exiting..." << endl; - cout.flush(); - _ExitFlag = true; } - void _SigFpeHandler(int sig_num); - void _SigFpeHandler(int sig_num) - { - cout.flush(); - cout << endl << "floating point exception" << endl; - PX4_BACKTRACE(); - cout.flush(); + return PX4_OK; +} + +int create_symlinks_if_needed(std::string &data_path) +{ + std::string current_path = pwd(); + + if (data_path.size() == 0) { + // No data path given, we'll just try to use the current working dir. + data_path = current_path; + PX4_INFO("assuming working directory is rootfs, no symlinks needed."); + return PX4_OK; } - void _SigSegvHandler(int sig_num); - void _SigSegvHandler(int sig_num) - { - cout.flush(); - cout << endl << "segmentation fault" << endl; - PX4_BACKTRACE(); - cout.flush(); + if (data_path.compare(current_path) == 0) { + // We are already running in the data path, so no need to symlink + PX4_INFO("working directory seems to be rootfs, no symlinks needed"); + return PX4_OK; + } + + std::vector path_sym_links; + path_sym_links.push_back("etc"); + path_sym_links.push_back("test_data"); + + for (const auto &it = path_sym_links.begin(); it != path_sym_links.end(); ++it) { + + PX4_DEBUG("path sym link: %s", it->c_str());; + + std::string src_path = data_path + "/" + *it; + std::string dest_path = current_path + "/" + *it; + + if (dir_exists(dest_path.c_str())) { + continue; + } + + PX4_INFO("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str()); + + // create sym-links + int ret = symlink(src_path.c_str(), dest_path.c_str()); + + if (ret != 0) { + PX4_ERR("Error creating symlink %s -> %s", src_path.c_str(), dest_path.c_str()); + return ret; + + } else { + PX4_DEBUG("Successfully created symlink %s -> %s", src_path.c_str(), dest_path.c_str()); + } + } + + return PX4_OK; +} + +int create_dirs() +{ + std::string current_path = pwd(); + + std::vector dirs; + dirs.push_back("log"); + + for (int i = 0; i < dirs.size(); i++) { + std::string dir = dirs[i]; + PX4_DEBUG("mkdir: %s", dir.c_str());; + std::string dir_path = current_path + "/" + dir; + + if (dir_exists(dir_path)) { + continue; + } + + // create dirs + int ret = mkdir(dir_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + + if (ret != OK) { + PX4_WARN("failed creating new dir: %s", dir_path.c_str()); + return ret; + + } else { + PX4_DEBUG("Successfully created dir %s", dir_path.c_str()); + } + } + + return PX4_OK; +} + + +void register_sig_handler() +{ + // SIGINT + struct sigaction sig_int {}; + sig_int.sa_handler = sig_int_handler; + sig_int.sa_flags = 0;// not SA_RESTART!; + + // SIGFPE + struct sigaction sig_fpe {}; + sig_int.sa_handler = sig_fpe_handler; + sig_int.sa_flags = 0;// not SA_RESTART!; + + // SIGPIPE + // We want to ignore if a PIPE has been closed. + struct sigaction sig_pipe {}; + sig_pipe.sa_handler = SIG_IGN; + + // SIGSEGV + struct sigaction sig_segv {}; + sig_segv.sa_handler = sig_segv_handler; + sig_segv.sa_flags = SA_RESTART | SA_SIGINFO; + + sigaction(SIGINT, &sig_int, NULL); + //sigaction(SIGTERM, &sig_int, NULL); + sigaction(SIGFPE, &sig_fpe, NULL); + sigaction(SIGPIPE, &sig_pipe, NULL); + sigaction(SIGSEGV, &sig_segv, nullptr); +} + +void sig_int_handler(int sig_num) +{ + fflush(stdout); + printf("\nExiting...\n"); + fflush(stdout); + px4_daemon::Pxh::stop(); + _exit_requested = true; +} + +void sig_fpe_handler(int sig_num) +{ + fflush(stdout); + printf("\nfloating point exception\n"); + PX4_BACKTRACE(); + fflush(stdout); + px4_daemon::Pxh::stop(); + _exit_requested = true; +} + +void sig_segv_handler(int sig_num) +{ + fflush(stdout); + printf("\nSegmentation Fault\n"); + PX4_BACKTRACE(); + fflush(stdout); +} + +void set_cpu_scaling() +{ +#ifdef __PX4_POSIX_EAGLE + // On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run + // at the maximum frequency all the time. + // Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon. + system("echo performance > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor"); + system("echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor"); + + // Alternatively we could also raise the minimum frequency to save some power, + // unfortunately this still lead to some drops. + //system("echo 1190400 > /sys/devices/system/cpu/cpu0/cpufreq/scaling_min_freq"); +#endif +} + +int run_startup_bash_script(const char *commands_file) +{ + std::string bash_command("bash "); + + bash_command += commands_file; + + PX4_INFO("Calling bash script: %s", bash_command.c_str()); + + int ret = 0; + + if (!bash_command.empty()) { + ret = system(bash_command.c_str()); + + if (ret == 0) { + PX4_INFO("Startup script returned successfully"); + + } else { + PX4_WARN("Startup script returned with return value: %d", ret); + } + + } else { + PX4_INFO("Startup script empty"); + } + + return ret; +} + +void wait_to_exit() +{ + while (!_exit_requested) { + usleep(100000); } } -static inline bool fileExists(const string &name) +void print_usage() +{ + printf("Usage for Server/daemon process: \n"); + printf("\n"); + printf(" px4 [-h|-d] [rootfs_directory] startup_file\n"); + printf("\n"); + printf(" directory where startup files and mixers are located,\n"); + printf(" (if not given, CWD is used)\n"); + printf(" bash start script to be used as startup\n"); + printf(" -h help/usage information\n"); + printf(" -d daemon mode, don't start pxh shell\n"); + printf("\n"); + printf("Usage for client: \n"); + printf("\n"); + printf(" px4-MODULE command using symlink.\n"); + printf(" e.g.: px4-commander status\n"); +} + +bool is_already_running() +{ + struct flock fl; + int fd = open(LOCK_FILE_PATH, O_RDWR | O_CREAT, 0666); + + if (fd < 0) { + return false; + } + + fl.l_type = F_WRLCK; + fl.l_whence = SEEK_SET; + fl.l_start = 0; + fl.l_len = 0; + fl.l_pid = getpid(); + + if (fcntl(fd, F_SETLK, &fl) == -1) { + // We failed to create a file lock, must be already locked. + + if (errno == EACCES || errno == EAGAIN) { + return true; + } + } + + return false; +} + +bool px4_exit_requested(void) +{ + return _exit_requested; +} + +bool file_exists(const std::string &name) { struct stat buffer; return (stat(name.c_str(), &buffer) == 0); } -static inline bool dirExists(const string &path) +bool dir_exists(const std::string &path) { struct stat info; @@ -124,564 +513,8 @@ static inline bool dirExists(const string &path) } } -static inline void touch(const string &name) +std::string pwd() { - fstream fs; - fs.open(name, ios::out); - fs.close(); + char temp[PATH_MAX]; + return (getcwd(temp, PATH_MAX) ? std::string(temp) : std::string("")); } - -static int mkpath(const char *path, mode_t mode); - -static int do_mkdir(const char *path, mode_t mode) -{ - struct stat st; - int status = 0; - - if (stat(path, &st) != 0) { - /* Directory does not exist. EEXIST for race condition */ - if (mkdir(path, mode) != 0 && errno != EEXIST) { - status = -1; - } - - } else if (!S_ISDIR(st.st_mode)) { - errno = ENOTDIR; - status = -1; - } - - return (status); -} - -/** -** mkpath - ensure all directories in path exist -** Algorithm takes the pessimistic view and works top-down to ensure -** each directory in path exists, rather than optimistically creating -** the last element and working backwards. -*/ -static int mkpath(const char *path, mode_t mode) -{ - char *pp; - char *sp; - int status; - char *copypath = strdup(path); - - status = 0; - pp = copypath; - - while (status == 0 && (sp = strchr(pp, '/')) != nullptr) { - if (sp != pp) { - /* Neither root nor double slash in path */ - *sp = '\0'; - status = do_mkdir(copypath, mode); - *sp = '/'; - } - - pp = sp + 1; - } - - if (status == 0) { - status = do_mkdir(path, mode); - } - - free(copypath); - return (status); -} - -static string pwd() -{ - char temp[path_max_len]; - return (getcwd(temp, path_max_len) ? string(temp) : string("")); -} - -static void print_prompt() -{ - cout.flush(); - cout << "pxh> "; - cout.flush(); -} - -static void run_cmd(const vector &appargs, bool exit_on_fail, bool silently_fail = false) -{ - static apps_map_type apps; - static bool initialized = false; - - if (!initialized) { - init_app_map(apps); - initialized = true; - } - - // command is appargs[0] - string command = appargs[0]; - - if (apps.find(command) != apps.end()) { - 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 *)nullptr; - - int retval = apps[command](i, (char **)arg); - - if (retval) { - cout << "Command '" << command << "' failed, returned " << retval << endl; - - if (exit_on_fail && retval) { - exit(retval); - } - } - - } else if (command == "help") { - list_builtins(apps); - - } else if (command.length() == 0 || command[0] == '#') { - // Do nothing - - } else if (!silently_fail) { - cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; - - } -} - -static void usage() -{ - - cout << "./px4 [-d] [data_directory] startup_config [-h]" << endl; - cout << " -d - Optional flag to run the app in daemon mode and does not listen for user input." << - endl; - cout << " This is needed if px4 is intended to be run as a upstart job on linux" << endl; - cout << " - directory where ROMFS and posix-configs are located (if not given, CWD is used)" << endl; - cout << " - config file for starting/stopping px4 modules" << endl; - cout << " -h - help/usage information" << endl; -} - -static void process_line(string &line, bool exit_on_fail) -{ - vector appargs(20); - - stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> - appargs[7] >> appargs[8] >> appargs[9] >> appargs[10] >> appargs[11] >> appargs[12] >> appargs[13] >> - appargs[14] >> appargs[15] >> appargs[16] >> appargs[17] >> appargs[18] >> appargs[19]; - run_cmd(appargs, exit_on_fail); -} - -static void restore_term() -{ - cout << "Restoring terminal\n"; - tcsetattr(0, TCSANOW, &orig_term); -} - -bool px4_exit_requested(void) -{ - return _ExitFlag; -} - -static void set_cpu_scaling() -{ -#if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) - // On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run - // at the maximum frequency all the time. - // Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon. - system("echo performance > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor"); - system("echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor"); - - // Alternatively we could also raise the minimum frequency to save some power, - // unfortunately this still lead to some drops. - //system("echo 1190400 > /sys/devices/system/cpu/cpu0/cpufreq/scaling_min_freq"); -#endif -} - -#ifdef __PX4_SITL_MAIN_OVERRIDE -int SITL_MAIN(int argc, char **argv); - -int SITL_MAIN(int argc, char **argv) -#else -int main(int argc, char **argv) -#endif -{ - bool daemon_mode = false; - bool chroot_on = false; - - tcgetattr(0, &orig_term); - atexit(restore_term); - - // SIGINT - struct sigaction sig_int {}; - sig_int.sa_handler = _SigIntHandler; - sig_int.sa_flags = 0;// not SA_RESTART!; - sigaction(SIGINT, &sig_int, nullptr); - - // SIGFPE - struct sigaction sig_fpe {}; - sig_fpe.sa_handler = _SigFpeHandler; - sig_fpe.sa_flags = 0;// not SA_RESTART!; - sigaction(SIGFPE, &sig_fpe, nullptr); - - // SIGSEGV - struct sigaction sig_segv {}; - sig_segv.sa_handler = _SigSegvHandler; - sig_segv.sa_flags = SA_RESTART | SA_SIGINFO; - sigaction(SIGSEGV, &sig_segv, nullptr); - - set_cpu_scaling(); - - int index = 1; - string commands_file; - int positional_arg_count = 0; - string data_path; - string node_name; - - // parse arguments - while (index < argc) { - //cout << "arg: " << index << " : " << argv[index] << endl; - - if (argv[index][0] == '-') { - // the arg starts with - - if (strncmp(argv[index], "-d", 2) == 0) { - daemon_mode = true; - - } else if (strncmp(argv[index], "-h", 2) == 0) { - usage(); - return 0; - - } else if (strncmp(argv[index], "-c", 2) == 0) { - chroot_on = true; - - } else { - PX4_ERR("Unknown/unhandled parameter: %s", argv[index]); - return 1; - } - - } else if (!strncmp(argv[index], "__", 2)) { - //cout << "ros argument" << endl; - - // ros arguments - if (!strncmp(argv[index], "__name:=", 8)) { - string name_arg = argv[index]; - node_name = name_arg.substr(8); - cout << "node name: " << node_name << endl; - } - - } else { - //cout << "positional argument" << endl; - - positional_arg_count += 1; - - if (positional_arg_count == 1) { - data_path = argv[index]; - - } else if (positional_arg_count == 2) { - commands_file = argv[index]; - } - } - - ++index; - } - - if (positional_arg_count != 2 && positional_arg_count != 1) { - PX4_ERR("Error expected 1 or 2 position arguments, got %d", positional_arg_count); - usage(); - return -1; - } - - bool symlinks_needed = true; - - if (positional_arg_count == 1) { //data path is optional - commands_file = data_path; - symlinks_needed = false; - - } else { - cout << "data path: " << data_path << endl; - } - - cout << "commands file: " << commands_file << endl; - - if (commands_file.empty()) { - PX4_ERR("Error commands file not specified"); - return -1; - } - - if (!fileExists(commands_file)) { - PX4_ERR("Error opening commands file, does not exist: %s", commands_file.c_str()); - return -1; - } - - // create sym-links - if (symlinks_needed) { - vector path_sym_links; - path_sym_links.push_back("ROMFS"); - path_sym_links.push_back("posix-configs"); - path_sym_links.push_back("test_data"); - - for (unsigned i = 0; i < path_sym_links.size(); i++) { - string path_sym_link = path_sym_links[i]; - //cout << "path sym link: " << path_sym_link << endl; - string src_path = data_path + "/" + path_sym_link; - string dest_path = pwd() + "/" + path_sym_link; - - PX4_DEBUG("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str()); - - if (dirExists(path_sym_link) || src_path == dest_path) { continue; } - - // create sym-links - int ret = symlink(src_path.c_str(), dest_path.c_str()); - - if (ret != 0) { - PX4_ERR("Error creating symlink %s -> %s", - src_path.c_str(), dest_path.c_str()); - return ret; - - } else { - PX4_DEBUG("Successfully created symlink %s -> %s", - src_path.c_str(), dest_path.c_str()); - } - } - } - - // setup rootfs - const string eeprom_path = "./rootfs/eeprom/"; - const string microsd_path = "./rootfs/fs/microsd/"; - - if (!fileExists(eeprom_path + "parameters")) { - cout << "creating new parameters file" << endl; - mkpath(eeprom_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR); - touch(eeprom_path + "parameters"); - } - - if (!fileExists(microsd_path + "dataman")) { - cout << "creating new dataman file" << endl; - mkpath(microsd_path.c_str(), S_IRUSR | S_IWUSR | S_IXUSR); - touch(microsd_path + "dataman"); - } - - // initialize - DriverFramework::Framework::initialize(); - px4::init_once(); - - px4::init(argc, argv, "px4"); - - // if commandfile is present, process the commands from the file - if (!commands_file.empty()) { - ifstream infile(commands_file.c_str()); - - if (infile.is_open()) { - for (string line; getline(infile, line, '\n');) { - - if (px4_exit_requested()) { - break; - } - - // TODO: this should be true but for that we have to check all startup files - process_line(line, false); - } - - } else { - PX4_ERR("Error opening commands file: %s", commands_file.c_str()); - } - } - - if (chroot_on) { - // Lock this application in the current working dir - // this is not an attempt to secure the environment, - // rather, to replicate a deployed file system. - - char pwd_path[path_max_len]; - const char *folderpath = "/rootfs/"; - - if (nullptr == getcwd(pwd_path, sizeof(pwd_path))) { - PX4_ERR("Failed acquiring working dir, abort."); - exit(1); - } - - if (nullptr == strcat(pwd_path, folderpath)) { - PX4_ERR("Failed completing path, abort."); - exit(1); - } - - if (chroot(pwd_path)) { - PX4_ERR("Failed chrooting application, path: %s, error: %s.", pwd_path, strerror(errno)); - exit(1); - } - - if (chdir("/")) { - PX4_ERR("Failed changing to root dir, path: %s, error: %s.", pwd_path, strerror(errno)); - exit(1); - } - } - - if (!daemon_mode) { - string mystr; - string string_buffer[CMD_BUFF_SIZE]; - int buf_ptr_write = 0; - int buf_ptr_read = 0; - uint8_t cursor_position = - 0; // position of the cursor from right to left (0: all the way to the right, mystr.length: all the way to the left) - - print_prompt(); - - // change input mode so that we can manage shell - struct termios term; - tcgetattr(0, &term); - term.c_lflag &= ~ICANON; - term.c_lflag &= ~ECHO; - tcsetattr(0, TCSANOW, &term); - setbuf(stdin, nullptr); - - while (!_ExitFlag) { - - int c = getchar(); - string add_string; // string to add at current cursor position - bool update_prompt = true; - - switch (c) { - case EOF: - _ExitFlag = true; - break; - - case 127: // backslash - if (mystr.length() - cursor_position > 0) { - mystr.erase(mystr.length() - cursor_position - 1, 1); - - } - - break; - - case '\n': // user hit enter - if (buf_ptr_write == CMD_BUFF_SIZE) { - buf_ptr_write = 0; - } - - if (buf_ptr_write > 0) { - if (!mystr.empty() && mystr != string_buffer[buf_ptr_write - 1]) { - string_buffer[buf_ptr_write] = mystr; - buf_ptr_write++; - } - - } else { - if (!mystr.empty() && mystr != string_buffer[CMD_BUFF_SIZE - 1]) { - string_buffer[buf_ptr_write] = mystr; - buf_ptr_write++; - } - } - - cout << endl; - process_line(mystr, false); - // reset string and cursor position - mystr = ""; - cursor_position = 0; - buf_ptr_read = buf_ptr_write; - - print_prompt(); - break; - - case '\033': { // arrow keys - c = getchar(); // skip first one, does not have the info - c = getchar(); - - if (c == 'A') { // arrow up - buf_ptr_read--; - - } else if (c == 'B') { // arrow down - if (buf_ptr_read < buf_ptr_write) { - buf_ptr_read++; - } - - } else if (c == 'C') { // arrow right - if (cursor_position > 0) { - printf("\033[1C"); - cursor_position--; - } - - break; - - } else if (c == 'D') { // arrow left - if (cursor_position < mystr.length()) { - printf("\033[1D"); - cursor_position++; - } - - break; - - } else if (c == 'H') { // Home (go to the beginning of the command) - cursor_position = mystr.length(); - break; - - } else if (c == '1') { // Home (go to the beginning of the command, Editing key) - (void)getchar(); // swallow '~' - cursor_position = mystr.length(); - break; - - } else if (c == 'F') { // End (go to the end of the command) - cursor_position = 0; - break; - - } else if (c == '4') { // End (go to the end of the command, Editing key) - (void)getchar(); // swallow '~' - cursor_position = 0; - break; - } - - if (buf_ptr_read < 0) { - buf_ptr_read = 0; - } - - string saved_cmd = string_buffer[buf_ptr_read]; - mystr = saved_cmd; - cursor_position = 0; // move cursor to end of line - - break; - } - - default: // any other input - if (c > 3) { - add_string += (char)c; - - } else { - update_prompt = false; - } - - break; - } - - if (update_prompt) { - // reprint prompt with mystr - mystr.insert(mystr.length() - cursor_position, add_string); - printf("%c[2K", 27); - cout << (char)13; - print_prompt(); - cout << mystr; - - // Move the cursor to its position - if (cursor_position > 0) { - printf("\033[%dD", cursor_position); - } - } - - } - - } else { - while (!_ExitFlag) { - usleep(100000); - } - } - - // TODO: Always try to stop muorb for QURT because px4_task_is_running doesn't seem to work. - if (true) { - //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, !daemon_mode, true); - } - - vector shutdown_cmd = { "shutdown" }; - run_cmd(shutdown_cmd, true); - DriverFramework::Framework::shutdown(); - - return OK; -} - -/* vim: set noet fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/platforms/posix/src/px4_daemon/CMakeLists.txt b/platforms/posix/src/px4_daemon/CMakeLists.txt new file mode 100644 index 0000000000..9704af02c1 --- /dev/null +++ b/platforms/posix/src/px4_daemon/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ + +px4_add_library(px4_daemon + pxh.cpp + history.cpp + client.cpp + server.cpp + server_io.cpp + pipe_protocol.cpp + ) + diff --git a/platforms/posix/src/px4_daemon/client.cpp b/platforms/posix/src/px4_daemon/client.cpp new file mode 100644 index 0000000000..bd064fe34e --- /dev/null +++ b/platforms/posix/src/px4_daemon/client.cpp @@ -0,0 +1,345 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 client.cpp + * + * @author Julian Oes + * @author Beat Küng + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "client.h" + +namespace px4_daemon +{ + + +namespace client +{ +static Client *_instance; +} + +Client::Client() : + _uuid(0), + _client_send_pipe_fd(-1) +{ + client::_instance = this; +} + +Client::~Client() +{ + client::_instance = nullptr; +} + +int +Client::generate_uuid() +{ + int rand_fd = open("/dev/urandom", O_RDONLY); + + if (rand_fd < 0) { + PX4_ERR("open urandom"); + return rand_fd; + } + + int ret = 0; + + int rand_read = read(rand_fd, &_uuid, sizeof(_uuid)); + + if (rand_read != sizeof(_uuid)) { + PX4_ERR("rand read fail"); + ret = -errno; + } + + close(rand_fd); + return ret; +} + +int +Client::process_args(const int argc, const char **argv) +{ + // Prepare return pipe first to avoid a race. + int ret = _prepare_recv_pipe(); + + if (ret != 0) { + PX4_ERR("Could not prepare recv pipe"); + return -2; + } + + ret = _send_cmds(argc, argv); + + if (ret != 0) { + PX4_ERR("Could not send commands"); + return -3; + } + + return _listen(); +} + + +int +Client::_prepare_recv_pipe() +{ + int ret = get_client_recv_pipe_path(_uuid, _recv_pipe_path, sizeof(_recv_pipe_path)); + + if (ret < 0) { + PX4_ERR("failed to assemble path"); + return ret; + } + + ret = mkfifo(_recv_pipe_path, 0666); + + if (ret < 0) { + PX4_ERR("pipe %s already exists, errno: %d, %s", _recv_pipe_path, errno, strerror(errno)); + return ret; + } + + return 0; +} + +int +Client::_send_cmds(const int argc, const char **argv) +{ + // Send the command to server. + client_send_packet_s packet; + packet.header.msg_id = client_send_packet_s::message_header_s::e_msg_id::EXECUTE; + packet.header.client_uuid = _uuid; + packet.payload.execute_msg.is_atty = isatty(STDOUT_FILENO); + + // Concat arguments to send them. + std::string cmd_buf; + + for (int i = 0; i < argc; ++i) { + cmd_buf += argv[i]; + + if (i + 1 != argc) { + cmd_buf += " "; + } + } + + if (cmd_buf.size() >= sizeof(packet.payload.execute_msg.cmd)) { + PX4_ERR("commmand too long"); + return -1; + } + + strcpy((char *)packet.payload.execute_msg.cmd, cmd_buf.c_str()); + + // The size is +1 because we want to include the null termination. + packet.header.payload_length = cmd_buf.size() + 1; + + _client_send_pipe_fd = open(CLIENT_SEND_PIPE_PATH, O_WRONLY); + + if (_client_send_pipe_fd < 0) { + PX4_ERR("pipe open fail"); + return _client_send_pipe_fd; + } + + int bytes_to_send = get_client_send_packet_length(&packet); + int bytes_sent = write(_client_send_pipe_fd, &packet, bytes_to_send); + + if (bytes_sent != bytes_to_send) { + PX4_ERR("write fail"); + return bytes_sent; + } + + return 0; +} + +int +Client::_listen() +{ + int client_recv_pipe_fd = open(_recv_pipe_path, O_RDONLY); + + if (client_recv_pipe_fd < 0) { + PX4_ERR("open failed, errno: %d, %s", errno, strerror(errno)); + } + + bool exit_loop = false; + int exit_arg = 0; + + while (!exit_loop) { + + // We only read as much as we need, otherwise we might get out of + // sync with packets. + client_recv_packet_s packet_recv; + int bytes_read = read(client_recv_pipe_fd, &packet_recv, sizeof(client_recv_packet_s::header)); + + if (bytes_read > 0) { + + // Using the header we can determine how big the payload is. + int payload_to_read = sizeof(packet_recv) + - sizeof(packet_recv.header) + - sizeof(packet_recv.payload) + + packet_recv.header.payload_length; + + // Again, we only read as much as we need because otherwise we need + // hold a buffer and parse it. + bytes_read = read(client_recv_pipe_fd, (char *)&packet_recv + bytes_read, payload_to_read); + + if (bytes_read > 0) { + + int retval = 0; + bool should_exit = false; + + int parse_ret = _parse_client_recv_packet(packet_recv, retval, should_exit); + + if (parse_ret != 0) { + PX4_ERR("retval could not be parsed"); + exit_arg = -1; + + } else { + exit_arg = retval; + } + + exit_loop = should_exit; + + } else if (bytes_read == 0) { + exit_arg = 0; + exit_loop = true; + } + + } else if (bytes_read == 0) { + // 0 means the pipe has been closed by all clients. + exit_arg = 0; + exit_loop = true; + } + } + + close(_client_send_pipe_fd); + return exit_arg; +} + +int +Client::_parse_client_recv_packet(const client_recv_packet_s &packet, int &retval, bool &should_exit) +{ + switch (packet.header.msg_id) { + case client_recv_packet_s::message_header_s::e_msg_id::RETVAL: + + should_exit = true; + return _retval_cmd_packet(packet, retval); + + case client_recv_packet_s::message_header_s::e_msg_id::STDOUT: + + should_exit = false; + return _stdout_msg_packet(packet); + + default: + should_exit = true; + PX4_ERR("recv msg_id not handled: %d", (int)packet.header.msg_id); + return -1; + } +} + +int +Client::_retval_cmd_packet(const client_recv_packet_s &packet, int &retval) +{ + if (packet.header.payload_length == sizeof(packet.payload.retval_msg.retval)) { + retval = packet.payload.retval_msg.retval; + return 0; + + } else { + PX4_ERR("payload size wrong"); + return -1; + } +} + +int +Client::_stdout_msg_packet(const client_recv_packet_s &packet) +{ + if (packet.header.payload_length <= sizeof(packet.payload.stdout_msg.text)) { + + printf("%s", packet.payload.stdout_msg.text); + + return 0; + + } else { + PX4_ERR("payload size wrong"); + return -1; + } +} + +void +Client::register_sig_handler() +{ + // Register handlers for Ctrl+C to kill the thread if something hangs. + struct sigaction sig_int {}; + sig_int.sa_handler = Client::_static_sig_handler; + + // Without the flag SA_RESTART, we can't use open() after Ctrl+C has + // been pressed, and we can't wait for the return value from the + // cancelled command. + sig_int.sa_flags = SA_RESTART; + sigaction(SIGINT, &sig_int, NULL); + sigaction(SIGTERM, &sig_int, NULL); +} + +void +Client::_static_sig_handler(int sig_num) +{ + client::_instance->_sig_handler(sig_num); +} + +void +Client::_sig_handler(int sig_num) +{ + client_send_packet_s packet; + packet.header.msg_id = client_send_packet_s::message_header_s::e_msg_id::KILL; + packet.header.client_uuid = _uuid; + packet.payload.kill_msg.cmd_id = sig_num; + packet.header.payload_length = sizeof(packet.payload.kill_msg.cmd_id); + + if (_client_send_pipe_fd < 0) { + PX4_ERR("pipe open fail"); + exit(-1); + } + + int bytes_to_send = get_client_send_packet_length(&packet); + int bytes_sent = write(_client_send_pipe_fd, &packet, bytes_to_send); + + if (bytes_sent != bytes_to_send) { + PX4_ERR("write fail"); + exit(-1); + } +} + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_daemon/client.h b/platforms/posix/src/px4_daemon/client.h new file mode 100644 index 0000000000..b0a606cfcb --- /dev/null +++ b/platforms/posix/src/px4_daemon/client.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 client.h + * + * The client can write a command to the pipe that is supplied by the server. + * It will then open another pipe with its own UUID encoded and listen for + * stdout of the process that it started and the return value. + * + * It the client receives a signal (e.g. Ctrl+C) it will catch it and send it + * as a message to the server in order to terminate the thread. + * + * @author Julian Oes + * @author Beat Küng + */ +#pragma once + +#include + +#include "pipe_protocol.h" + +namespace px4_daemon +{ + + +class Client +{ +public: + Client(); + ~Client(); + + /** + * Initialize the unique ID of the client. + * + * @return 0 on success. + */ + int generate_uuid(); + + /** + * Make sure to catch signals in order to forward them to the server. + */ + void register_sig_handler(); + + /** + * Process the supplied command line arguments and send them to server. + * + * @param argc: number of arguments + * @param argv: argument values + * @return 0 on success + */ + int process_args(const int argc, const char **argv); + +private: + int _prepare_recv_pipe(); + int _send_cmds(const int argc, const char **argv); + int _listen(); + + int _parse_client_recv_packet(const client_recv_packet_s &packet, int &retval, bool &should_exit); + + int _retval_cmd_packet(const client_recv_packet_s &packet, int &retval); + int _stdout_msg_packet(const client_recv_packet_s &packet); + + + static void _static_sig_handler(int sig_num); + void _sig_handler(int sig_num); + + uint64_t _uuid; + int _client_send_pipe_fd; + char _recv_pipe_path[RECV_PIPE_PATH_LEN]; +}; + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_daemon/history.cpp b/platforms/posix/src/px4_daemon/history.cpp new file mode 100644 index 0000000000..0d601bae7b --- /dev/null +++ b/platforms/posix/src/px4_daemon/history.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2015-2016 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 history.cpp + * + * @author Julian Oes + */ + +#include +#include + +#include "history.h" + +namespace px4_daemon +{ + +void History::try_to_add(const std::string &line) +{ + // Don't save an empty line. + if (line.empty()) { + return; + } + + // Don't add duplicate entries. + if (!_history.empty() && line.compare(_history.back()) == 0) { + return; + } + + if (_history.size() == MAX_HISTORY_SIZE) { + _history.erase(_history.begin()); + } + + _history.push_back(line); +} + +void History::reset_to_end() +{ + _current_history_entry = _history.end(); +} + +void History::try_to_save_current_line(const std::string &line) +{ + // Don't save what's currently entered line if there is no history + // entry to switch to. + if (_history.empty()) { + return; + } + + // Don't save the current line if we are already jumping around in + // the history, and we must have already saved it. + if (_current_history_entry != _history.end()) { + return; + } + + _current_line = line; +} + +void History::get_previous(std::string &line) +{ + if (_history.empty()) { + return; + } + + if (_current_history_entry == _history.begin()) { + return; + } + + _current_history_entry = std::prev(_current_history_entry); + line = *_current_history_entry; +} + +void History::get_next(std::string &line) +{ + if (_history.empty()) { + return; + } + + // Already at the end, don't even try to get the next. + if (_current_history_entry == _history.end()) { + line = _current_line; + return; + } + + _current_history_entry = std::next(_current_history_entry); + + // We might have reached next now, ignore it and use what we saved + // in the beginning. + if (_current_history_entry == _history.end()) { + line = _current_line; + return; + } + + line = *_current_history_entry; +} + +} // namespace px4_daemon diff --git a/platforms/posix/src/px4_daemon/history.h b/platforms/posix/src/px4_daemon/history.h new file mode 100644 index 0000000000..b29ca8c531 --- /dev/null +++ b/platforms/posix/src/px4_daemon/history.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 history.h + * + * Simple command history for the PX4 shell (pxh). + * + * This allows to go back in the history of entered commands. + * Additionally, before going back in the history, the current prompt can get saved. + * + * @author Julian Oes + */ +#pragma once + +#include +#include + +namespace px4_daemon +{ + +class History +{ +public: + /** + * Try to append the current line to the history. + * Ignore the line if it is empty or duplicate of the + * last added one. + * + * Drop the first entry of the history if we reach the + * MAX_HISTORY_SIZE. + * + * @param line: command line to be added. + */ + void try_to_add(const std::string &line); + + /** + * After executing a command in the shell, we want to be at + * the end of the history again. + */ + void reset_to_end(); + + /** + * If we start scrolling up in the history, we can try to save + * the current command line. When we scroll back down, we can + * get it out again. + * + * @param line: line to be saved + */ + void try_to_save_current_line(const std::string &line); + + + /** + * Set the previous (earlier) command from the history. + * + * @param line: swap to previous line if available. + */ + void get_previous(std::string &line); + + /** + * Set the next (more recent) command from the history. + * + * @param line: swap to next line if available, otherwise saved current. + */ + void get_next(std::string &line); + + static const unsigned MAX_HISTORY_SIZE = 100; +private: + std::vector _history; + std::vector::iterator _current_history_entry; + std::string _current_line; +}; + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_daemon/pipe_protocol.cpp b/platforms/posix/src/px4_daemon/pipe_protocol.cpp new file mode 100644 index 0000000000..0cdf49c94b --- /dev/null +++ b/platforms/posix/src/px4_daemon/pipe_protocol.cpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 pipe_protocol.cpp + * + * @author Julian Oes + * @author Beat Küng + */ + +#include +#include +#include + +#include "pipe_protocol.h" + +namespace px4_daemon +{ + + +unsigned get_client_send_packet_length(const client_send_packet_s *packet) +{ + return sizeof(client_send_packet_s) - sizeof(packet->payload) + packet->header.payload_length + sizeof(uint8_t); +} + +unsigned get_client_recv_packet_length(const client_recv_packet_s *packet) +{ + return sizeof(client_recv_packet_s) - sizeof(packet->payload) + packet->header.payload_length; +} + +int get_client_recv_pipe_path(const uint64_t uuid, char *path, const size_t path_len) +{ + return snprintf(path, path_len, "%s_%016" PRIx64, CLIENT_RECV_PIPE_PATH, uuid); +} + + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_daemon/pipe_protocol.h b/platforms/posix/src/px4_daemon/pipe_protocol.h new file mode 100644 index 0000000000..b0bcd0ba37 --- /dev/null +++ b/platforms/posix/src/px4_daemon/pipe_protocol.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 pipe_protocol.h + * + * @author Julian Oes + * @author Beat Küng + */ +#pragma once + +#include + +namespace px4_daemon +{ + + +static const char CLIENT_SEND_PIPE_PATH[] = "/tmp/px4_client_send_pipe"; +static const char CLIENT_RECV_PIPE_PATH[] = "/tmp/px4_client_recv_pipe"; + +static const unsigned RECV_PIPE_PATH_LEN = 64; + +struct client_send_packet_s { + struct message_header_s { + enum class e_msg_id : uint8_t { + EXECUTE, + KILL + } msg_id; + uint64_t client_uuid; + unsigned payload_length; + } header; + + union { + struct execute_msg_s { + uint8_t is_atty; + uint8_t cmd[512 - sizeof(message_header_s) - sizeof(uint8_t)]; + } execute_msg; + struct kill_msg_s { + int cmd_id; + } kill_msg; + } payload; +}; + +// We have per client receiver a pipe with the uuid in its file path. +struct client_recv_packet_s { + struct message_header_s { + enum class e_msg_id : uint8_t { + RETVAL, + STDOUT + } msg_id; + unsigned payload_length; + } header; + + union { + struct retval_msg_s { + int retval; + } retval_msg; + struct stdout_msg_s { + uint8_t text[512 - sizeof(message_header_s)]; + } stdout_msg; + } payload; +}; + +unsigned get_client_send_packet_length(const client_send_packet_s *packet); +unsigned get_client_recv_packet_length(const client_recv_packet_s *packet); +int get_client_recv_pipe_path(const uint64_t uuid, char *path, const size_t path_len); + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_daemon/pxh.cpp b/platforms/posix/src/px4_daemon/pxh.cpp new file mode 100644 index 0000000000..60f840ee37 --- /dev/null +++ b/platforms/posix/src/px4_daemon/pxh.cpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (C) 2015-2016 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 pxh.cpp + * + * This is a simple PX4 shell implementation used to start modules. + * + * @author Mark Charlebois + * @author Roman Bapst + * @author Julian Oes + */ + +#include +#include +#include +#include + +#include "pxh.h" + +namespace px4_daemon +{ + +Pxh *Pxh::_instance = nullptr; + +apps_map_type Pxh::_apps = {}; + + +Pxh::Pxh() +{ + _instance = this; +} + +Pxh::~Pxh() +{ + _instance = nullptr; +} + +int Pxh::process_line(const std::string &line, bool silently_fail) +{ + if (line.empty()) { + return 0; + } + + if (_apps.size() == 0) { + init_app_map(_apps); + } + + std::stringstream line_stream(line); + std::string word; + std::vector words; + + // First arg should be the command. + while (line_stream >> word) { + words.push_back(word); + } + + const std::string &command(words.front()); + + if (_apps.find(command) != _apps.end()) { + + // Note that argv[argc] always needs to be a nullptr. + // Therefore add one more entry. + const char *arg[words.size() + 1]; + + for (unsigned i = 0; i < words.size(); ++i) { + arg[i] = (char *)words[i].c_str(); + } + + // Explicitly set this nullptr. + arg[words.size()] = nullptr; + + int retval = _apps[command](words.size(), (char **)arg); + + if (retval) { + if (!silently_fail) { + printf("Command '%s' failed, returned %d.\n", command.c_str(), retval); + } + } + + return retval; + + } else if (command.compare("help") == 0) { + list_builtins(_apps); + return 0; + + } else if (command.length() == 0 || command[0] == '#') { + // Do nothing + return 0; + + } else if (!silently_fail) { + //std::cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; + printf("Invalid command: %s\ntype 'help' for a list of commands\n", command.c_str()); + return -1; + + } else { + return -1; + } +} + + +void Pxh::run_pxh() +{ + _should_exit = false; + + _setup_term(); + + std::string mystr = ""; + int cursor_position = 0; // position of the cursor from right to left + // (0: all the way to the right, mystr.length: all the way to the left) + + _print_prompt(); + + while (!_should_exit) { + + int c = getchar(); + std::string add_string; // string to add at current cursor position + bool update_prompt = true; + + switch (c) { + case EOF: + _should_exit = true; + break; + + case 127: // backslash + if ((int)mystr.length() - cursor_position > 0) { + mystr.erase(mystr.length() - cursor_position - 1, 1); + } + + break; + + case '\n': // user hit enter + _history.try_to_add(mystr); + _history.reset_to_end(); + + printf("\n"); + process_line(mystr, false); + // reset string and cursor position + mystr = ""; + cursor_position = 0; + + update_prompt = false; + _print_prompt(); + break; + + case '\033': { // arrow keys + c = getchar(); // skip first one, does not have the info + c = getchar(); + + if (c == 'A') { // arrow up + _history.try_to_save_current_line(mystr); + _history.get_previous(mystr); + cursor_position = 0; // move cursor to end of line + + } else if (c == 'B') { // arrow down + _history.get_next(mystr); + cursor_position = 0; // move cursor to end of line + + } else if (c == 'C') { // arrow right + if (cursor_position > 0) { + cursor_position--; + } + + } else if (c == 'D') { // arrow left + if (cursor_position < (int)mystr.length()) { + cursor_position++; + } + + } else if (c == 'H') { // Home (go to the beginning of the command) + cursor_position = mystr.length(); + + } else if (c == '1') { // Home (go to the beginning of the command, Editing key) + (void)getchar(); // swallow '~' + cursor_position = mystr.length(); + + } else if (c == 'F') { // End (go to the end of the command) + cursor_position = 0; + + } else if (c == '4') { // End (go to the end of the command, Editing key) + (void)getchar(); // swallow '~' + cursor_position = 0; + } + + break; + } + + default: // any other input + if (c > 3) { + add_string += (char)c; + + } else { + update_prompt = false; + } + + break; + } + + + if (update_prompt) { + // reprint prompt with mystr + mystr.insert(mystr.length() - cursor_position, add_string); + _clear_line(); + _print_prompt(); + printf("%s", mystr.c_str()); + + // Move the cursor to its position + if (cursor_position > 0) { + _move_cursor(cursor_position); + } + } + + } + + return; +} + +void Pxh::stop() +{ + _restore_term(); + + if (_instance) { + _instance->_should_exit = true; + } +} + +void Pxh::_setup_term() +{ + // Make sure we restore terminal at exit. + tcgetattr(0, &_orig_term); + atexit(Pxh::_restore_term); + + // change input mode so that we can manage shell + struct termios term; + tcgetattr(0, &term); + term.c_lflag &= ~ICANON; + term.c_lflag &= ~ECHO; + tcsetattr(0, TCSANOW, &term); + setbuf(stdin, NULL); +} + +void Pxh::_restore_term(void) +{ + if (_instance) { + tcsetattr(0, TCSANOW, &_instance->_orig_term); + } +} + +void Pxh::_print_prompt() +{ + fflush(stdout); + printf("pxh> "); + fflush(stdout); +} + +void Pxh::_clear_line() +{ + printf("%c[2K%c", (char)27, (char)13); +} +void Pxh::_move_cursor(int position) +{ + printf("\033[%dD", position); +} + +} // namespace px4_daemon diff --git a/platforms/posix/src/px4_daemon/pxh.h b/platforms/posix/src/px4_daemon/pxh.h new file mode 100644 index 0000000000..0fd6018c62 --- /dev/null +++ b/platforms/posix/src/px4_daemon/pxh.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 pxh.h + * + * The POSIX PX4 implementation features a simple shell to start modules + * or use system commands. + * + * @author Mark Charlebois + * @author Roman Bapst + * @author Julian Oes + */ +#pragma once + +#include +#include +#include + +#include +#include "history.h" + +namespace px4_daemon +{ + + +class Pxh +{ +public: + Pxh(); + ~Pxh(); + + /** + * Process and run one command line. + * + * @param silently_fail: don't make a fuss on failure + * @return 0 if successful. */ + static int process_line(const std::string &line, bool silently_fail); + + /** + * Run the pxh shell. This will only return if stop() is called. + */ + void run_pxh(); + + /** + * Can be called to stop pxh. + */ + static void stop(); + +private: + void _print_prompt(); + void _move_cursor(int position); + void _clear_line(); + + void _setup_term(); + static void _restore_term(); + + bool _should_exit; + History _history; + struct termios _orig_term; + + static apps_map_type _apps; + static Pxh *_instance; +}; + + + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_daemon/server.cpp b/platforms/posix/src/px4_daemon/server.cpp new file mode 100644 index 0000000000..cb6e2f1908 --- /dev/null +++ b/platforms/posix/src/px4_daemon/server.cpp @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 server.cpp + * + * @author Julian Oes + * @author Beat Küng + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "pxh.h" +#include "server.h" + + + +namespace px4_daemon +{ + +Server *Server::_instance = nullptr; + +Server::Server() +{ + _instance = this; +} + +Server::~Server() +{ + _instance = nullptr; +} + +int +Server::start() +{ + if (0 != pthread_create(&_server_main_pthread, + NULL, + _server_main_trampoline, + NULL)) { + + PX4_ERR("error creating client handler thread"); + + return -1; + } + + return 0; +} + +void * +Server::_server_main_trampoline(void *arg) +{ + if (_instance) { + _instance->_server_main(arg); + } + + return NULL; +} + +void Server::_pthread_key_destructor(void *arg) +{ + delete ((CmdThreadSpecificData *)arg); +} + +void +Server::_server_main(void *arg) +{ + // Set thread specific pipe to supplied file descriptor. + int ret = pthread_key_create(&_key, _pthread_key_destructor); + + if (ret != 0) { + PX4_ERR("failed to create pthread key"); + return; + } + + // Delete pipe in case it exists already. + unlink(CLIENT_SEND_PIPE_PATH); + + // Create new pipe to listen to clients. + mkfifo(CLIENT_SEND_PIPE_PATH, 0666); + int client_send_pipe_fd = open(CLIENT_SEND_PIPE_PATH, O_RDONLY); + + while (true) { + + client_send_packet_s packet; + + int bytes_read = read(client_send_pipe_fd, &packet, sizeof(packet)); + + if (bytes_read > 0) { + _parse_client_send_packet(packet); + + } else if (bytes_read == 0) { + // 0 means the pipe has been closed by all clients + // and we need to re-open it. + close(client_send_pipe_fd); + client_send_pipe_fd = open(CLIENT_SEND_PIPE_PATH, O_RDONLY); + } + } + + close(client_send_pipe_fd); + return; +} + +void +Server::_parse_client_send_packet(const client_send_packet_s &packet) +{ + switch (packet.header.msg_id) { + case client_send_packet_s::message_header_s::e_msg_id::EXECUTE: + _execute_cmd_packet(packet); + break; + + case client_send_packet_s::message_header_s::e_msg_id::KILL: + _kill_cmd_packet(packet); + break; + + default: + PX4_ERR("send msg_id not handled"); + break; + } +} + +void +Server::_execute_cmd_packet(const client_send_packet_s &packet) +{ + if (packet.header.payload_length == 0) { + PX4_ERR("command length 0"); + return; + } + + // We open the client's specific pipe to write the return value and stdout back to. + // The pipe's path is created knowing the UUID of the client. + char path[RECV_PIPE_PATH_LEN] = {}; + int ret = get_client_recv_pipe_path(packet.header.client_uuid, path, RECV_PIPE_PATH_LEN); + + if (ret < 0) { + PX4_ERR("failed to assemble path"); + return; + } + + int pipe_fd = open(path, O_WRONLY); + + if (pipe_fd < 0) { + PX4_ERR("pipe open fail"); + return; + } + + // To execute a command we start a new thread. + pthread_t new_pthread; + + // We need to copy everything that the new thread needs because we will go + // out of scope. + RunCmdArgs *args = new RunCmdArgs; + memcpy(args->cmd, packet.payload.execute_msg.cmd, sizeof(args->cmd)); + args->client_uuid = packet.header.client_uuid; + args->pipe_fd = pipe_fd; + args->is_atty = packet.payload.execute_msg.is_atty; + + if (0 != pthread_create(&new_pthread, NULL, Server::_run_cmd, (void *)args)) { + PX4_ERR("could not start pthread"); + delete args; + return; + } + + // We keep two maps for cleanup if a thread is finished or killed. + _client_uuid_to_pthread.insert(std::pair + (packet.header.client_uuid, new_pthread)); + _pthread_to_pipe_fd.insert(std::pair + (new_pthread, pipe_fd)); +} + + +void +Server::_kill_cmd_packet(const client_send_packet_s &packet) +{ + // TODO: we currently ignore the signal type. + + pthread_t pthread_to_kill = _client_uuid_to_pthread.get(packet.header.client_uuid); + + // TODO: use a more graceful exit method to avoid resource leaks + int ret = pthread_cancel(pthread_to_kill); + + if (ret != 0) { + PX4_ERR("failed to cancel thread"); + } + + _cleanup_thread(packet.header.client_uuid); + + // We don't send retval when we get killed. + // The client knows this and just exits without confirmation. +} + + + +void +*Server::_run_cmd(void *arg) +{ + RunCmdArgs *args = (RunCmdArgs *)arg; + + // Copy arguments so that we can cleanup the arg structure. + uint64_t client_uuid = args->client_uuid; + int pipe_fd = args->pipe_fd; + bool is_atty = args->is_atty; + std::string message_str(args->cmd); + + // Clean up the args from the heap in case the thread gets canceled + // from outside. + delete args; + + // We register thread specific data. This is used for PX4_INFO (etc.) log calls. + CmdThreadSpecificData *thread_data_ptr; + + if ((thread_data_ptr = (CmdThreadSpecificData *)pthread_getspecific(_instance->_key)) == NULL) { + thread_data_ptr = new CmdThreadSpecificData; + thread_data_ptr->pipe_fd = pipe_fd; + thread_data_ptr->is_atty = is_atty; + thread_data_ptr->packet.header.msg_id = client_recv_packet_s::message_header_s::e_msg_id::STDOUT; + + (void)pthread_setspecific(_instance->_key, (void *)thread_data_ptr); + } + + // Run the actual command. + int retval = Pxh::process_line(message_str, true); + + // Report return value. + _send_retval(pipe_fd, retval, client_uuid); + + // Clean up before returning. + _instance->_cleanup_thread(client_uuid); + + return NULL; +} + + +void +Server::_send_retval(const int pipe_fd, const int retval, const uint64_t client_uuid) +{ + client_recv_packet_s packet; + + packet.header.msg_id = client_recv_packet_s::message_header_s::e_msg_id::RETVAL; + packet.header.payload_length = sizeof(packet.payload.retval_msg); + packet.payload.retval_msg.retval = retval; + + int bytes_to_send = get_client_recv_packet_length(&packet); + int bytes_sent = write(pipe_fd, &packet, bytes_to_send); + + if (bytes_sent != bytes_to_send) { + printf("write fail\n"); + return; + } +} + +void +Server::_cleanup_thread(const uint64_t client_uuid) +{ + pthread_t pthread_killed = _client_uuid_to_pthread.get(client_uuid); + int pipe_fd = _pthread_to_pipe_fd.get(pthread_killed); + + close(pipe_fd); + + char path[RECV_PIPE_PATH_LEN] = {}; + get_client_recv_pipe_path(client_uuid, path, RECV_PIPE_PATH_LEN); + unlink(path); + + _client_uuid_to_pthread.erase(client_uuid); + _pthread_to_pipe_fd.erase(pthread_killed); +} + +} //namespace px4_daemon diff --git a/platforms/posix/src/px4_daemon/server.h b/platforms/posix/src/px4_daemon/server.h new file mode 100644 index 0000000000..44bcf0918a --- /dev/null +++ b/platforms/posix/src/px4_daemon/server.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 server.h + * + * The server (also called daemon) opens a pipe for clients to write to. + * + * Once a client connects it will send a command as well as a unique ID. + * The server will return the stdout of the executing command, as well as the return + * value to the client on a client specific pipe. + * The client specific pipe are idenified by the unique ID of the client. + * + * There should only every be one server running, therefore the static instance. + * The Singleton implementation is not complete, but it should be obvious not + * to instantiate multiple servers. + * + * @author Julian Oes + * @author Beat Küng + */ +#pragma once + +#include +#include +#include +#include + +#include "pipe_protocol.h" +#include "threadsafe_map.h" + + +namespace px4_daemon +{ + +class Server +{ +public: + Server(); + ~Server(); + + /** + * Start the server. This will spawn a thread with a + * while loop waiting for clients sending commands. + * + * @return 0 if started successfully + */ + int start(); + + + struct CmdThreadSpecificData { + int pipe_fd; // pipe fd to send data to descriptor + bool is_atty; // whether file descriptor refers to a terminal + client_recv_packet_s packet; + }; + + static bool is_running() + { + return _instance != nullptr; + } + + static pthread_key_t get_pthread_key() + { + return _instance->_key; + } +private: + static void *_server_main_trampoline(void *arg); + void _server_main(void *arg); + + void _parse_client_send_packet(const client_send_packet_s &packet); + void _execute_cmd_packet(const client_send_packet_s &packet); + void _kill_cmd_packet(const client_send_packet_s &packet); + void _cleanup_thread(const uint64_t client_uuid); + + static void _send_retval(const int pipe_fd, const int retval, const uint64_t client_uuid); + + struct RunCmdArgs { + char cmd[sizeof(client_send_packet_s::payload.execute_msg.cmd)]; + uint64_t client_uuid; + bool is_atty; + int pipe_fd; + }; + + static void *_run_cmd(void *arg); + + pthread_t _server_main_pthread; + + ThreadsafeMap _pthread_to_pipe_fd; + ThreadsafeMap _client_uuid_to_pthread; + + pthread_key_t _key; + + static void _pthread_key_destructor(void *arg); + + static Server *_instance; +}; + + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_daemon/server_io.cpp b/platforms/posix/src/px4_daemon/server_io.cpp new file mode 100644 index 0000000000..67e9e138be --- /dev/null +++ b/platforms/posix/src/px4_daemon/server_io.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 server_io.cpp + * + * @author Julian Oes + * @author Beat Küng + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "server.h" +#include +#include "pipe_protocol.h" + + +using namespace px4_daemon; + + +int get_stdout_pipe_buffer(char **buffer, unsigned *max_length, bool *is_atty) +{ + // The thread specific data won't be initialized if the server is not running. + Server::CmdThreadSpecificData *thread_data_ptr; + + if (!Server::is_running()) { + return -1; + } + + // If we are not in a thread that has been started by a client, we don't + // have any thread specific data set and we won't have a pipe to write + // stdout to. + if ((thread_data_ptr = (Server::CmdThreadSpecificData *)pthread_getspecific( + Server::get_pthread_key())) == NULL) { + return -1; + } + +#ifdef __PX4_POSIX_EAGLE + + // XXX FIXME: thread_data_ptr is set to 0x1 in the main thread on Snapdragon + // even though the pthread_key has been created. + // We can catch this using the check below but we have no clue why this happens. + if (thread_data_ptr == (void *)0x1) { + return -1; + } + +#endif + + client_recv_packet_s *packet = &thread_data_ptr->packet; + + *buffer = (char *)packet->payload.stdout_msg.text; + *max_length = sizeof(packet->payload.stdout_msg.text); + *is_atty = thread_data_ptr->is_atty; + + return 0; +} + +int send_stdout_pipe_buffer(unsigned buffer_length) +{ + assert(buffer_length <= sizeof(client_recv_packet_s::payload.stdout_msg.text)); + + Server::CmdThreadSpecificData *thread_data_ptr; + + if (!Server::is_running()) { + return -1; + } + + if ((thread_data_ptr = (Server::CmdThreadSpecificData *)pthread_getspecific( + Server::get_pthread_key())) == NULL) { + return -1; + } + + client_recv_packet_s *packet = &thread_data_ptr->packet; + packet->header.payload_length = buffer_length; + + int pipe_fd = thread_data_ptr->pipe_fd; + int bytes_to_send = get_client_recv_packet_length(packet); + + // Check if we can write first by writing 0 bytes. + // If we don't do this, we'll get SIGPIPE and be very unhappy + // because the whole process will go down. + int ret = write(pipe_fd, NULL, 0); + + if (ret == 0 && errno == EPIPE) { + printf("Error: can't write to closed pipe, giving up.\n"); + pthread_exit(NULL); + } + + int bytes_sent = write(pipe_fd, packet, bytes_to_send); + + if (bytes_sent != bytes_to_send) { + printf("write fail\n"); + return -1; + } + + return 0; +} diff --git a/platforms/posix/src/px4_daemon/threadsafe_map.h b/platforms/posix/src/px4_daemon/threadsafe_map.h new file mode 100644 index 0000000000..c3a9955a7d --- /dev/null +++ b/platforms/posix/src/px4_daemon/threadsafe_map.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 threadsafe_map.h + * + * This is a small wrapper for std::map to make it thread-safe. + * + * @author Julian Oes + */ +#pragma once + +#include +#include + + +namespace px4_daemon +{ + +template +class ThreadsafeMap +{ +public: + ThreadsafeMap() : + _mutex(PTHREAD_MUTEX_INITIALIZER) {}; + ~ThreadsafeMap() {}; + + /** + * Insert an entry into the map. + */ + void + insert(std::pair pair) + { + _lock(); + _map.insert(pair); + _unlock(); + } + + /** + * Get an entry into from the map. + */ + Type get(Key key) + { + // Supposedly locking is not needed to read but since we're + // not sure, let's lock anyway. + _lock(); + const Type temp = _map[key]; + _unlock(); + return temp; + } + + void erase(Key key) + { + _lock(); + _map.erase(key); + _unlock(); + } +private: + void _lock() + { + pthread_mutex_lock(&_mutex); + } + + void _unlock() + { + pthread_mutex_unlock(&_mutex); + } + + std::map _map; + pthread_mutex_t _mutex; + +}; + +} // namespace px4_daemon + diff --git a/platforms/posix/src/px4_layer/CMakeLists.txt b/platforms/posix/src/px4_layer/CMakeLists.txt index cf566b52b0..dc752e595b 100644 --- a/platforms/posix/src/px4_layer/CMakeLists.txt +++ b/platforms/posix/src/px4_layer/CMakeLists.txt @@ -54,6 +54,7 @@ add_library(px4_layer ${SHMEM_SRCS} ) target_link_libraries(px4_layer PRIVATE work_queue) +target_link_libraries(px4_layer PRIVATE px4_daemon) if (EXTRA_DEPENDS) add_dependencies(px4_layer ${EXTRA_DEPENDS}) diff --git a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp b/platforms/posix/src/px4_layer/px4_posix_tasks.cpp index 3e69b542a3..f8570fcc91 100644 --- a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp +++ b/platforms/posix/src/px4_layer/px4_posix_tasks.cpp @@ -122,16 +122,11 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int char *const argv[]) { - int rv; - int argc = 0; int i; + int argc = 0; unsigned int len = 0; - unsigned long offset; - unsigned long structsize; - char *p = (char *)argv; - - pthread_attr_t attr; struct sched_param param = {}; + char *p = (char *)argv; // Calculate argc while (p != (char *)nullptr) { @@ -145,12 +140,12 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int len += strlen(p) + 1; } - structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); + unsigned long structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); // not safe to pass stack data to the thread creation pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len); memset(taskdata, 0, structsize + len); - offset = ((unsigned long)taskdata) + structsize; + unsigned long offset = ((unsigned long)taskdata) + structsize; strncpy(taskdata->name, name, 16); taskdata->name[15] = 0; @@ -169,7 +164,8 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int PX4_DEBUG("starting task %s", name); - rv = pthread_attr_init(&attr); + pthread_attr_t attr; + int rv = pthread_attr_init(&attr); if (rv != 0) { PX4_ERR("px4_task_spawn_cmd: failed to init thread attrs"); @@ -230,10 +226,9 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int pthread_mutex_lock(&task_mutex); - int taskid = 0; - + px4_task_t taskid = 0; for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].isused == false) { + if (!taskmap[i].isused) { taskmap[i].name = name; taskmap[i].isused = true; taskid = i; @@ -276,7 +271,7 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int pthread_attr_destroy(&attr); pthread_mutex_unlock(&task_mutex); - return i; + return taskid; } int px4_task_delete(px4_task_t id) diff --git a/platforms/qurt/src/px4_layer/main.cpp b/platforms/qurt/src/px4_layer/main.cpp index dffc84e9a8..2fe6b57aac 100644 --- a/platforms/qurt/src/px4_layer/main.cpp +++ b/platforms/qurt/src/px4_layer/main.cpp @@ -1,5 +1,5 @@ /**************************************************************************** - * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2015-2016 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 @@ -31,9 +31,11 @@ ****************************************************************************/ /** * @file main.cpp - * Basic shell to execute builtin "apps" + * + * Basic shell to execute builtin "apps" on QURT. * * @author Mark Charlebois + * @author Julian Oes */ #include @@ -47,19 +49,20 @@ #include #include #include + #include "get_commands.h" #include "apps.h" #include "DriverFramework.hpp" #define MAX_ARGS 8 // max number of whitespace separated args after app name -using namespace std; static px4_task_t g_dspal_task = -1; __BEGIN_DECLS // The commands to run are specified in a target file: commands_.c extern const char *get_commands(void); + // Enable external library hook void qurt_external_hook(void) __attribute__((weak)); __END_DECLS @@ -68,10 +71,10 @@ void qurt_external_hook(void) { } -static void run_cmd(apps_map_type &apps, const vector &appargs) +static void run_cmd(apps_map_type &apps, const std::vector &appargs) { // command is appargs[0] - string command = appargs[0]; + std::string command = appargs[0]; //replaces app.find with iterator code to avoid null pointer exception for (apps_map_type::iterator it = apps.begin(); it != apps.end(); ++it) @@ -117,7 +120,7 @@ void eat_whitespace(const char *&b, int &i) static void process_commands(apps_map_type &apps, const char *cmds) { - vector appargs; + std::vector appargs; int i = 0; const char *b = cmds; char arg[256]; @@ -177,39 +180,12 @@ int dspal_main(int argc, char *argv[]); __END_DECLS -#define COMMANDS_ADSP_FILE "/dev/fs/px4.config" - const char *get_commands() { - PX4_INFO("attempting to open the ADSP command file: %s", COMMANDS_ADSP_FILE); - int fd = open(COMMANDS_ADSP_FILE, O_RDONLY); - - if (fd > 0) { - static char *commands; - char buf[4096]; - int bytes_read, total_bytes = 0; - PX4_INFO("reading commands from %s\n", COMMANDS_ADSP_FILE); - - do { - bytes_read = read(fd, (void *)buf, sizeof(buf)); - - if (bytes_read > 0) { - commands = (char *)realloc(commands, total_bytes + bytes_read); - memcpy(commands + total_bytes, buf, bytes_read); - total_bytes += bytes_read; - } - } while ((unsigned int)bytes_read > 0); - - close(fd); - - return (const char *)commands; - } - - PX4_ERR("Could not open %s\n", COMMANDS_ADSP_FILE); - - static const char *commands = - "uorb start\nqshell start\n" - ; + // All that needs to be started automatically on the DSP side + // are uorb and qshell. After that, everything else can get + // started from the main startup script on the Linux side. + static const char *commands = "uorb start\nqshell start\n"; return commands; } diff --git a/posix-configs/SITL/init/10016_iris b/posix-configs/SITL/init/10016_iris new file mode 100644 index 0000000000..5ad6462f84 --- /dev/null +++ b/posix-configs/SITL/init/10016_iris @@ -0,0 +1,11 @@ +#!nsh +# +# @name 3DR Iris Quadrotor SITL +# +# @type Quadrotor Wide +# +# @maintainer Anton Babushkin +# + +pwm_out_sim mode_pwm +mixer load /dev/pwm_output0 etc/mixers/quad_w.main.mix diff --git a/posix-configs/SITL/init/6011_typhoon_h480 b/posix-configs/SITL/init/6011_typhoon_h480 new file mode 100644 index 0000000000..f4ece2b5d9 --- /dev/null +++ b/posix-configs/SITL/init/6011_typhoon_h480 @@ -0,0 +1,15 @@ +#!nsh +# +# @name Typhoon H480 SITL +# +# @type Hexarotor x +# +# @maintainer empty@empty.com +# + +param set MAV_TYPE 13 + +pwm_out_sim mode_pwm16 +mixer load /dev/pwm_output0 etc/mixers/hexa_x.main.mix +mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix +vmount start diff --git a/posix-configs/SITL/init/CMakeLists.txt b/posix-configs/SITL/init/CMakeLists.txt new file mode 100644 index 0000000000..2483929ffa --- /dev/null +++ b/posix-configs/SITL/init/CMakeLists.txt @@ -0,0 +1,59 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ + +set(posix_init_file_names + rcS + rcS_tests + 6011_typhoon_h480 + 10016_iris +) + +# Get absolute paths +set(posix_init_files) +foreach(posix_file ${posix_init_file_names}) + list(APPEND posix_init_files ${CMAKE_CURRENT_SOURCE_DIR}/${posix_file}) +endforeach() + +set(init_directory ${ROOTFS_DIRECTORY}/etc/init) + +add_custom_target(posix_init_dir + COMMAND ${CMAKE_COMMAND} -E make_directory ${init_directory} +) + +# cmake -E copy with version < 3.5 does not support copying multiple files yet, therefore use cp. +add_custom_target(posix_init + COMMAND cp ${posix_init_files} ${init_directory} + DEPENDS posix_init_dir +) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS new file mode 100644 index 0000000000..e3bb2ecd8a --- /dev/null +++ b/posix-configs/SITL/init/rcS @@ -0,0 +1,131 @@ +#!/usr/bin/bash + +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +source px4-alias.sh + +# TODO: In the future we want to share rc.autostart with NuttX +#source rc.autostart + +uorb start +if ! param load +then + # param load fails the first time, set defaults + param set BAT_N_CELLS 3 + param set CAL_ACC0_ID 1376264 + param set CAL_ACC0_XOFF 0.01 + param set CAL_ACC0_XSCALE 1.01 + param set CAL_ACC0_YOFF -0.01 + param set CAL_ACC0_YSCALE 1.01 + param set CAL_ACC0_ZOFF 0.01 + param set CAL_ACC0_ZSCALE 1.01 + param set CAL_ACC1_ID 1310728 + param set CAL_ACC1_XOFF 0.01 + param set CAL_GYRO0_ID 2293768 + param set CAL_GYRO0_XOFF 0.01 + param set CAL_MAG0_ID 196616 + param set CAL_MAG0_XOFF 0.01 + param set COM_DISARM_LAND 3 + param set COM_OBL_ACT 2 + param set COM_OBL_RC_ACT 0 + param set COM_OF_LOSS_T 5 + param set COM_RC_IN_MODE 1 + param set EKF2_AID_MASK 1 + param set EKF2_ANGERR_INIT 0.01 + param set EKF2_GBIAS_INIT 0.01 + param set EKF2_HGT_MODE 0 + param set EKF2_MAG_TYPE 1 + param set MC_PITCH_P 6 + param set MC_PITCHRATE_P 0.2 + param set MC_ROLL_P 6 + param set MC_ROLLRATE_P 0.2 + param set MIS_TAKEOFF_ALT 2.5 + param set MPC_HOLD_MAX_Z 2.0 + param set MPC_Z_VEL_I 0.15 + param set MPC_Z_VEL_P 0.6 + param set NAV_ACC_RAD 2.0 + param set NAV_DLL_ACT 2 + param set RTL_DESCEND_ALT 5.0 + param set RTL_LAND_DELAY 5 + param set RTL_RETURN_ALT 30.0 + param set SDLOG_DIRS_MAX 7 + param set SENS_BOARD_ROT 0 + param set SENS_BOARD_X_OFF 0.000001 + param set SYS_RESTART_TYPE 2 +fi + +# Use environment variable PX4_ESTIMATOR to choose estimator. +if [ "$PX4_ESTIMATOR" == "ekf2" ]; then + param set SYS_MC_EST_GROUP 2 +elif [ "$PX4_ESTIMATOR" == "lpe" ]; then + param set SYS_MC_EST_GROUP 1 +fi + +# Use the variable set by sitl_run.sh to choose the model settings. +if [ "$PX4_SIM_MODEL" == "iris" ]; then + param set SYS_AUTOSTART 10016 + +elif [ "$PX4_SIM_MODEL" == "typhoon_h480" ]; then + param set SYS_AUTOSTART 6011 +else + echo "Unknown model" + exit -1 +fi + +dataman start +replay tryapplyparams +simulator start -s +tone_alarm start +gyrosim start +accelsim start +barosim start +gpssim start +sensors start +commander start +land_detector start multicopter +navigator start + +if param compare SYS_MC_EST_GROUP 1 +then + attitude_estimator_q start + local_position_estimator start +elif param compare SYS_MC_EST_GROUP 2 +then + ekf2 start +else + echo "No estimator chosen" + exit -1 +fi + +mc_pos_control start +mc_att_control start + +# TODO: eventually we want to re-use the existing autostart +# infrastructure already available on NuttX. +if param compare SYS_AUTOSTART 10016 +then + source etc/init/10016_iris + +elif param compare SYS_AUTOSTART 6011 +then + source etc/init/6011_typhoon_h480 +fi + + +mavlink start -x -u 14556 -r 4000000 +mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540 +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 50 -s ATTITUDE -u 14556 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556 +mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 + +logger start -e -t -b 1000 + +mavlink boot_complete +replay trystart diff --git a/posix-configs/SITL/init/rcS_tests b/posix-configs/SITL/init/rcS_tests new file mode 100644 index 0000000000..4bb1cd3f4f --- /dev/null +++ b/posix-configs/SITL/init/rcS_tests @@ -0,0 +1,21 @@ +#!/usr/bin/bash + +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +source px4-alias.sh + +uorb start + +param load +param set SYS_RESTART_TYPE 0 + +dataman start + +rgbledsim start +tone_alarm start + +ver all + +tests all + +shutdown diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS new file mode 100644 index 0000000000..8d8b122584 --- /dev/null +++ b/posix-configs/eagle/init/rcS @@ -0,0 +1,210 @@ +#!/usr/bin/bash + +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +source px4-alias.sh + +uorb start +muorb start + +# We need to wait until the DSP side is ready before +# sending commands with qshell. +sleep 1 + +# default, generic quad platform +if param compare SYS_AUTOSTART 4100 +then + param set MAV_TYPE 2 + + qshell pwm_out_rc_in start -d /dev/tty-2 + + qshell gps start -d /dev/tty-4 + qshell df_hmc5883_wrapper start + qshell df_trone_wrapper start + #qshell df_isl29501_wrapper start + +# Qualcomm 200qx microheli platform +elif param compare SYS_AUTOSTART 4200 +then + param set CAL_GYRO0_ID 100 + param set CAL_ACC0_ID 100 + param set CAL_MAG0_ID 101 + param set MAV_TYPE 2 + param set MC_YAW_P 7.0 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.08 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.0001 + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.0001 + param set RC_MAP_THROTTLE 1 + param set RC_MAP_ROLL 2 + param set RC_MAP_PITCH 3 + param set RC_MAP_YAW 4 + param set RC_MAP_MODE_SW 5 + param set RC_MAP_POSCTL_SW 6 + param set RC1_MAX 1900 + param set RC1_MIN 1099 + param set RC1_TRIM 1099 + param set RC1_REV 1 + param set RC2_MAX 1900 + param set RC2_MIN 1099 + param set RC2_TRIM 1500 + param set RC2_REV -1 + param set RC3_MAX 1900 + param set RC3_MIN 1099 + param set RC3_TRIM 1500 + param set RC3_REV 1 + param set RC4_MAX 1900 + param set RC4_MIN 1099 + param set RC4_TRIM 1500 + param set RC4_REV -1 + param set RC5_MAX 1900 + param set RC5_MIN 1099 + param set RC5_TRIM 1500 + param set RC5_REV 1 + param set RC6_MAX 1900 + param set RC6_MIN 1099 + param set RC6_TRIM 1099 + param set RC6_REV 1 + param set ATT_W_MAG 0.00 + param set PE_MAG_NOISE 1.0f + param set SENS_BOARD_ROT 0 + param set RC_RECEIVER_TYPE 1 + param set UART_ESC_MODEL 2 + param set UART_ESC_BAUDRTE 250000 + param set UART_ESC_MOTOR1 4 + param set UART_ESC_MOTOR2 2 + param set UART_ESC_MOTOR3 1 + param set UART_ESC_MOTOR4 3 + + qshell uart_esc start -D /dev/tty-2 + qshell rc_receiver start -D /dev/tty-1 + +# Qualcomm internal 210qc platform +elif param compare SYS_AUTOSTART 4210 +then + param set CAL_GYRO0_ID 100 + param set CAL_ACC0_ID 100 + param set CAL_MAG0_ID 101 + param set MAV_TYPE 2 + param set MC_YAW_P 12 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.08 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.001 + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.001 + param set RC_MAP_THROTTLE 1 + param set RC_MAP_ROLL 2 + param set RC_MAP_PITCH 3 + param set RC_MAP_YAW 4 + param set RC_MAP_MODE_SW 5 + param set RC_MAP_POSCTL_SW 6 + param set RC1_MAX 1900 + param set RC1_MIN 1099 + param set RC1_TRIM 1099 + param set RC1_REV 1 + param set RC2_MAX 1900 + param set RC2_MIN 1099 + param set RC2_TRIM 1500 + param set RC2_REV -1 + param set RC3_MAX 1900 + param set RC3_MIN 1099 + param set RC3_TRIM 1500 + param set RC3_REV 1 + param set RC4_MAX 1900 + param set RC4_MIN 1099 + param set RC4_TRIM 1500 + param set RC4_REV -1 + param set RC5_MAX 1900 + param set RC5_MIN 1099 + param set RC5_TRIM 1500 + param set RC5_REV 1 + param set RC6_MAX 1900 + param set RC6_MIN 1099 + param set RC6_TRIM 1099 + param set RC6_REV 1 + param set ATT_W_MAG 0.00 + param set PE_MAG_NOISE 1.0f + param set SENS_BOARD_ROT 0 + param set CAL_GYRO0_XOFF -0.0028 + param set CAL_GYRO0_YOFF -0.0047 + param set CAL_GYRO0_ZOFF -0.0034 + param set CAL_GYRO0_XSCALE 1.0000 + param set CAL_GYRO0_YSCALE 1.0000 + param set CAL_GYRO0_ZSCALE 1.0000 + param set CAL_MAG0_XOFF 0.0000 + param set CAL_MAG0_YOFF 0.0000 + param set CAL_MAG0_ZOFF 0.0000 + param set CAL_MAG0_XSCALE 1.0000 + param set CAL_MAG0_YSCALE 1.0000 + param set CAL_MAG0_ZSCALE 1.0000 + param set CAL_ACC0_XOFF -0.0941 + param set CAL_ACC0_YOFF 0.1168 + param set CAL_ACC0_ZOFF -0.0398 + param set CAL_ACC0_XSCALE 1.0004 + param set CAL_ACC0_YSCALE 0.9974 + param set CAL_ACC0_ZSCALE 0.9951 + param set RC_RECEIVER_TYPE 1 + param set UART_ESC_MODEL 2 + param set UART_ESC_BAUDRTE 250000 + param set UART_ESC_MOTOR1 4 + param set UART_ESC_MOTOR2 2 + param set UART_ESC_MOTOR3 1 + param set UART_ESC_MOTOR4 3 + + uart_esc start -D /dev/tty-2 + rc_receiver start -D /dev/tty-1 +else + echo "NO AIRFRAME CHOSEN!" + echo "Please set parameter to select airframe:" + echo " SYS_AUTOSTART 4100: generic (flight) Quad" + echo " SYS_AUTOSTART 4200: Microheli 200QX" +fi + + +qshell df_mpu9250_wrapper start +qshell df_bmp280_wrapper start +qshell sensors start + +if param compare SYS_MC_EST_GROUP 1 +then + qshell attitude_estimator_q start + qshell local_position_estimator start + +elif param compare SYS_MC_EST_GROUP 2 +then + param set EKF2_GBIAS_INIT 0.01 + param set EKF2_ANGERR_INIT 0.01 + qshell ekf2 start +else + echo "No estimator chosen" + exit -1 +fi + +qshell commander start +qshell land_detector start multicopter +qshell mc_pos_control start +qshell mc_att_control start + +logger start -b 200 -t + +param set MAV_BROADCAST 1 +dataman start +navigator start +mavlink start -u 14556 -r 1000000 +mavlink stream -u 14556 -s HIGHRES_IMU -r 50 +mavlink stream -u 14556 -s ATTITUDE -r 50 +mavlink stream -u 14556 -s RC_CHANNELS -r 20 +mavlink boot_complete diff --git a/src/drivers/qshell/posix/qshell.cpp b/src/drivers/qshell/posix/qshell.cpp index bd8492696d..d111db42e0 100644 --- a/src/drivers/qshell/posix/qshell.cpp +++ b/src/drivers/qshell/posix/qshell.cpp @@ -38,20 +38,50 @@ * @author Nicolas de Palezieux */ -#include "qshell.h" #include #include #include #include #include #include +#include +#include + +#include "qshell.h" px4::AppState QShell::appState; +orb_advert_t QShell::_pub_qshell_req = nullptr; +int QShell::_sub_qshell_retval = -1; +uint32_t QShell::_current_sequence = 0; + int QShell::main(std::vector argList) { - appState.setRunning(true); + int ret = _send_cmd(argList); + if (ret != 0) { + PX4_ERR("Could not send command"); + return -1; + } + + ret = _wait_for_retval(); + + if (ret != 0) { + PX4_ERR("Could not get return value"); + return -1; + } + + return 0; +} + +int QShell::_send_cmd(std::vector &argList) +{ + // Let's use a sequence number to check if a return value belongs to the + // command that we just sent and not a previous one that we assumed that + // it had timed out. + ++_current_sequence; + + struct qshell_req_s qshell_req; std::string cmd; for (size_t i = 0; i < argList.size(); i++) { @@ -62,27 +92,60 @@ int QShell::main(std::vector argList) } } - if (cmd.size() > m_qshell_req.MAX_STRLEN) { - PX4_ERR("The provided command exceeds the maximum length of characters: %d > %d", (int) cmd.size(), - (int) m_qshell_req.MAX_STRLEN); + if (cmd.size() >= qshell_req.MAX_STRLEN) { + PX4_ERR("Command too long: %d >= %d", (int) cmd.size(), (int) qshell_req.MAX_STRLEN); return -1; } - PX4_DEBUG("Requesting %s", cmd.c_str()); + PX4_INFO("Send cmd: '%s'", cmd.c_str()); - orb_advert_t pub_id_qshell_req = orb_advertise(ORB_ID(qshell_req), & m_qshell_req); + qshell_req.strlen = cmd.size(); + strcpy((char *)qshell_req.cmd, cmd.c_str()); + qshell_req.sequence = _current_sequence; - m_qshell_req.strlen = cmd.size(); + int instance; + orb_publish_auto(ORB_ID(qshell_req), &_pub_qshell_req, &qshell_req, &instance, ORB_PRIO_DEFAULT); - for (size_t i = 0; i < cmd.size(); i++) { - m_qshell_req.string[i] = (int) cmd[i]; - } - - if (orb_publish(ORB_ID(qshell_req), pub_id_qshell_req, &m_qshell_req) == PX4_ERROR) { - PX4_ERR("Error publishing the qshell_req message"); - return -1; - } - - appState.setRunning(false); return 0; } + +int QShell::_wait_for_retval() +{ + if (_sub_qshell_retval < 0) { + _sub_qshell_retval = orb_subscribe(ORB_ID(qshell_retval)); + + if (_sub_qshell_retval < 0) { + PX4_ERR("could not subscribe to retval"); + return -1; + } + } + + const hrt_abstime time_started_us = hrt_absolute_time(); + + while (hrt_elapsed_time(&time_started_us) < 3000000) { + bool updated; + orb_check(_sub_qshell_retval, &updated); + + if (updated) { + + struct qshell_retval_s retval; + orb_copy(ORB_ID(qshell_retval), _sub_qshell_retval, &retval); + + if (retval.sequence != _current_sequence) { + PX4_WARN("Ignoring return value with wrong sequence"); + + } else { + if (retval.return_value) { + PX4_WARN("cmd returned with: %d", retval.return_value); + } + + return 0; + } + } + + usleep(1000); + } + + PX4_ERR("command timed out"); + return -1; +} diff --git a/src/drivers/qshell/posix/qshell.h b/src/drivers/qshell/posix/qshell.h index 0873e221b2..507cc1dd83 100644 --- a/src/drivers/qshell/posix/qshell.h +++ b/src/drivers/qshell/posix/qshell.h @@ -56,7 +56,10 @@ public: static px4::AppState appState; /* track requests to terminate app */ private: + int _send_cmd(std::vector &argList); + int _wait_for_retval(); - struct qshell_req_s m_qshell_req; - + static orb_advert_t _pub_qshell_req; + static int _sub_qshell_retval; + static uint32_t _current_sequence; }; diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp index 61e039049b..d159be1bc8 100644 --- a/src/drivers/qshell/qurt/qshell.cpp +++ b/src/drivers/qshell/qurt/qshell.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -56,7 +57,7 @@ #include #include -#include "modules/uORB/uORB.h" +#include #include #include "DriverFramework.hpp" @@ -71,7 +72,6 @@ QShell::QShell() int QShell::main() { - int rc; appState.setRunning(true); int sub_qshell_req = orb_subscribe(ORB_ID(qshell_req)); @@ -80,61 +80,65 @@ int QShell::main() return -1; } - int i = 0; + px4_pollfd_struct_t fds[1] = {}; + fds[0].fd = sub_qshell_req; + fds[0].events = POLLIN; + + orb_advert_t qshell_pub = nullptr; while (!appState.exitRequested()) { - bool updated = false; - if (orb_check(sub_qshell_req, &updated) == 0) { - if (updated) { - PX4_DEBUG("[%d]qshell_req status is updated... reading new value", i); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - if (orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req) != 0) { - PX4_ERR("[%d]Error calling orb copy for qshell_req... ", i); - break; - } + if (pret > 0 && fds[0].revents & POLLIN) { - char current_char; - std::string arg; - std::vector appargs; + orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req); - for (size_t str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) { - current_char = m_qshell_req.string[str_idx]; + PX4_INFO("qshell gotten: %s", m_qshell_req.cmd); + char current_char; + std::string arg; + std::vector appargs; - if (isspace(current_char)) { // split at spaces - if (arg.length()) { - appargs.push_back(arg); - arg = ""; - } + for (unsigned str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) { + current_char = m_qshell_req.cmd[str_idx]; - } else { - arg += current_char; + if (isspace(current_char)) { // split at spaces + if (arg.length()) { + appargs.push_back(arg); + arg = ""; } - } - appargs.push_back(arg); // push last argument - - int ret = run_cmd(appargs); - - if (ret) { - PX4_ERR("Failed to execute command"); + } else { + arg += current_char; } } + appargs.push_back(arg); // push last argument + + struct qshell_retval_s retval; + retval.return_value = run_cmd(appargs); + retval.sequence = m_qshell_req.sequence; + + if (retval.return_value) { + PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd); + + } else { + PX4_INFO("Ok executing command: %s", m_qshell_req.cmd); + } + + int instance; + orb_publish_auto(ORB_ID(qshell_retval), &qshell_pub, &retval, &instance, ORB_PRIO_DEFAULT); + + } else if (pret == 0) { + // Timing out is fine. } else { - PX4_ERR("[%d]Error checking the updated status for qshell_req ", i); - break; + // Something is wrong. + usleep(10000); } - - // sleep for 1/2 sec. - usleep(500000); - - ++i; } - return 0; appState.setRunning(false); - return rc; + return 0; } int QShell::run_cmd(const std::vector &appargs) diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index f2064fdfb3..d5bac6b23b 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -246,7 +246,7 @@ static px4_sem_t g_sys_state_mutex_mission; static px4_sem_t g_sys_state_mutex_fence; /* The data manager store file handle and file name */ -#if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) +#ifdef __PX4_POSIX static const char *default_device_path = PX4_ROOTFSDIR"/dataman"; #else static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman"; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index be8d22b89f..c188904bcd 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -829,7 +829,12 @@ void Logger::run() int log_message_sub = orb_subscribe(ORB_ID(log_message)); orb_set_interval(log_message_sub, 20); + +#if __PX4_POSIX + int ntopics = add_topics_from_file(PX4_ROOTFSDIR "/etc/logging/logger_topics.txt"); +#else int ntopics = add_topics_from_file(PX4_ROOTFSDIR "/fs/microsd/etc/logging/logger_topics.txt"); +#endif if (ntopics > 0) { PX4_INFO("logging %d topics from logger_topics.txt", ntopics); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 47c48a3ff4..97749c1d31 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -313,7 +313,7 @@ private: static constexpr size_t MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */ static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ -#if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) +#ifdef __PX4_POSIX static constexpr const char *LOG_ROOT = PX4_ROOTFSDIR"/log"; #else static constexpr const char *LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log"; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 0bfc75661a..94993d22a0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -191,7 +191,11 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p, float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) { +#ifdef __PX4_POSIX + FILE *f = fopen(PX4_ROOTFSDIR"/inav.log", "a"); +#else FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); +#endif if (f) { char *s = malloc(256); diff --git a/src/modules/uORB/uORBDevices.cpp b/src/modules/uORB/uORBDevices.cpp index 98aaa5f960..40073dc469 100644 --- a/src/modules/uORB/uORBDevices.cpp +++ b/src/modules/uORB/uORBDevices.cpp @@ -1059,7 +1059,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) print_active_only = false; // print non-active if -a or some filter given } - printf("\033[2J\n"); //clear screen + PX4_INFO_RAW("\033[2J\n"); //clear screen lock(); @@ -1142,12 +1142,12 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) start_time = current_time; - printf("\033[H"); // move cursor home and clear screen - printf(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics); + PX4_INFO_RAW("\033[H"); // move cursor home and clear screen + PX4_INFO_RAW(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics); #ifdef __PX4_NUTTX - printf(CLEAR_LINE "%*-s INST #SUB #MSG #LOST #QSIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME"); + PX4_INFO_RAW(CLEAR_LINE "%*-s INST #SUB #MSG #LOST #QSIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME"); #else - printf(CLEAR_LINE "%*s INST #SUB #MSG #LOST #QSIZE\n", -(int)max_topic_name_length + 2, "TOPIC NAME"); + PX4_INFO_RAW(CLEAR_LINE "%*s INST #SUB #MSG #LOST #QSIZE\n", -(int)max_topic_name_length + 2, "TOPIC NAME"); #endif cur_node = first_node; @@ -1155,13 +1155,13 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) if (!print_active_only || cur_node->pub_msg_delta > 0) { #ifdef __PX4_NUTTX - printf(CLEAR_LINE "%*-s %2i %4i %4i %5i %i\n", (int)max_topic_name_length, + PX4_INFO_RAW(CLEAR_LINE "%*-s %2i %4i %4i %5i %i\n", (int)max_topic_name_length, #else - printf(CLEAR_LINE "%*s %2i %4i %4i %5i %i\n", -(int)max_topic_name_length, + PX4_INFO_RAW(CLEAR_LINE "%*s %2i %4i %4i %5i %i\n", -(int)max_topic_name_length, #endif - cur_node->node->get_meta()->o_name, (int)cur_node->instance, - (int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta, - (int)cur_node->lost_msg_delta, cur_node->node->get_queue_size()); + cur_node->node->get_meta()->o_name, (int)cur_node->instance, + (int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta, + (int)cur_node->lost_msg_delta, cur_node->node->get_queue_size()); } cur_node = cur_node->next; diff --git a/src/platforms/common/px4_log.c b/src/platforms/common/px4_log.c index fefdb84034..0d57174c5c 100644 --- a/src/platforms/common/px4_log.c +++ b/src/platforms/common/px4_log.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2017-2018 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 @@ -31,13 +31,12 @@ * ****************************************************************************/ -#include - #include #include - +#include #if defined(__PX4_POSIX) && !defined(__PX4_CYGWIN) #include +#include #endif #include @@ -46,9 +45,9 @@ static orb_advert_t orb_log_message_pub = NULL; -__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; +__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "DEBUG", "INFO", "WARN", "ERROR", "PANIC" }; __EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] = -{ PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED }; +{ PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED }; void px4_log_initialize(void) @@ -91,30 +90,74 @@ void px4_backtrace() #endif } + __EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) { - PX4_LOG_COLOR_START - printf(__px4__log_level_fmt __px4__log_level_arg(level)); - PX4_LOG_COLOR_MODULE - printf(__px4__log_modulename_pfmt, moduleName); - PX4_LOG_COLOR_MESSAGE - va_list argptr; - va_start(argptr, fmt); - vprintf(fmt, argptr); - va_end(argptr); - PX4_LOG_COLOR_END - printf("\n"); + +#ifdef __PX4_POSIX + char *buffer; + unsigned max_length; + bool is_atty = false; + + if (get_stdout_pipe_buffer(&buffer, &max_length, &is_atty) == 0) { + if (level >= _PX4_LOG_LEVEL_INFO) { + + unsigned pos = 0; + + if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", __px4_log_level_color[level]); } + + pos += snprintf(buffer + pos, max_length - pos, __px4__log_level_fmt __px4__log_level_arg(level)); + + if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", PX4_ANSI_COLOR_GRAY); } + + pos += snprintf(buffer + pos, max_length - pos, __px4__log_modulename_pfmt, moduleName); + va_list argptr; + + if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", __px4_log_level_color[level]); } + + va_start(argptr, fmt); + pos += vsnprintf(buffer + pos, max_length - pos, fmt, argptr); + va_end(argptr); + pos += snprintf(buffer + pos, max_length - pos, "\n"); + + if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", PX4_ANSI_COLOR_RESET); } + + // +1 for the terminating 0 char. + send_stdout_pipe_buffer(pos + 1); + } + + } else { +#endif + + if (level >= _PX4_LOG_LEVEL_INFO) { + PX4_LOG_COLOR_START + printf(__px4__log_level_fmt __px4__log_level_arg(level)); + PX4_LOG_COLOR_MODULE + printf(__px4__log_modulename_pfmt, moduleName); + PX4_LOG_COLOR_MESSAGE + va_list argptr; + va_start(argptr, fmt); + vprintf(fmt, argptr); + va_end(argptr); + PX4_LOG_COLOR_END + printf("\n"); + } + +#ifdef __PX4_POSIX + } + +#endif /* publish an orb log message */ if (level >= _PX4_LOG_LEVEL_WARN && orb_log_message_pub) { //only publish important messages struct log_message_s log_message; - const unsigned max_length = sizeof(log_message.text); + const unsigned max_length_pub = sizeof(log_message.text); log_message.timestamp = hrt_absolute_time(); const uint8_t log_level_table[] = { - 6, /* _PX4_LOG_LEVEL_ALWAYS */ 7, /* _PX4_LOG_LEVEL_DEBUG */ + 6, /* _PX4_LOG_LEVEL_INFO */ 4, /* _PX4_LOG_LEVEL_WARN */ 3, /* _PX4_LOG_LEVEL_ERROR */ 0 /* _PX4_LOG_LEVEL_PANIC */ @@ -123,14 +166,62 @@ __EXPORT void px4_log_modulename(int level, const char *moduleName, const char * unsigned pos = 0; - pos += snprintf((char *)log_message.text + pos, max_length - pos, __px4__log_modulename_pfmt, moduleName); + va_list argptr; + + pos += snprintf((char *)log_message.text + pos, max_length_pub - pos, __px4__log_modulename_pfmt, moduleName); va_start(argptr, fmt); - pos += vsnprintf((char *)log_message.text + pos, max_length - pos, fmt, argptr); + pos += vsnprintf((char *)log_message.text + pos, max_length_pub - pos, fmt, argptr); va_end(argptr); - log_message.text[max_length - 1] = 0; //ensure 0-termination + log_message.text[max_length_pub - 1] = 0; //ensure 0-termination #if !defined(PARAM_NO_ORB) orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message); #endif /* !PARAM_NO_ORB */ } } + +__EXPORT void px4_log_raw(int level, const char *fmt, ...) +{ + +#ifdef __PX4_POSIX + char *buffer; + unsigned max_length; + bool is_atty = false; + + if (get_stdout_pipe_buffer(&buffer, &max_length, &is_atty) == 0) { + if (level >= _PX4_LOG_LEVEL_INFO) { + + unsigned pos = 0; + + va_list argptr; + + if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", __px4_log_level_color[level]); } + + va_start(argptr, fmt); + pos += vsnprintf(buffer + pos, max_length - pos, fmt, argptr); + va_end(argptr); + + if (is_atty) { pos += snprintf(buffer + pos, max_length - pos, "%s", PX4_ANSI_COLOR_RESET); } + + // +1 for the terminating 0 char. + send_stdout_pipe_buffer(pos + 1); + } + + } else { +#endif + + if (level >= _PX4_LOG_LEVEL_INFO) { + PX4_LOG_COLOR_START + PX4_LOG_COLOR_MESSAGE + va_list argptr; + va_start(argptr, fmt); + vprintf(fmt, argptr); + va_end(argptr); + PX4_LOG_COLOR_END + } + +#ifdef __PX4_POSIX + } + +#endif +} diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 7e2063d4da..30e9bba771 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -110,7 +110,7 @@ typedef param_t px4_param_t; * NuttX specific defines. ****************************************************************************/ -#define PX4_ROOTFSDIR "" +#define PX4_ROOTFSDIR "." #define _PX4_IOC(x,y) _IOC(x,y) // mode for open with O_CREAT @@ -173,7 +173,7 @@ using ::isfinite; // QURT specific # include "dspal_math.h" -# define PX4_ROOTFSDIR "" +# define PX4_ROOTFSDIR "." # define PX4_TICKS_PER_SEC 1000L # define SIOCDEVPRIVATE 999999 @@ -196,7 +196,7 @@ __END_DECLS # elif defined(__PX4_POSIX_BEBOP) # define PX4_ROOTFSDIR "/data/ftp/internal_000" # else -# define PX4_ROOTFSDIR "rootfs" +# define PX4_ROOTFSDIR "." # endif #endif // __PX4_QURT diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 7af9cf8488..dbadbcb46f 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -39,8 +39,8 @@ #pragma once -#define _PX4_LOG_LEVEL_ALWAYS 0 -#define _PX4_LOG_LEVEL_DEBUG 1 +#define _PX4_LOG_LEVEL_DEBUG 0 +#define _PX4_LOG_LEVEL_INFO 1 #define _PX4_LOG_LEVEL_WARN 2 #define _PX4_LOG_LEVEL_ERROR 3 #define _PX4_LOG_LEVEL_PANIC 4 @@ -73,6 +73,7 @@ __END_DECLS #define PX4_ERR(...) ROS_ERROR(__VA_ARGS__) #define PX4_WARN(...) ROS_WARN(__VA_ARGS__) #define PX4_INFO(...) ROS_INFO(__VA_ARGS__) +#define PX4_INFO_RAW(...) printf(__VA_ARGS__) #define PX4_DEBUG(...) ROS_DEBUG(__VA_ARGS__) #define PX4_BACKTRACE() @@ -81,8 +82,8 @@ __END_DECLS /**************************************************************************** * Messages that should never be filtered or compiled out ****************************************************************************/ -#define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) -#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_INFO_RAW(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_INFO, FMT, ##__VA_ARGS__) #define PX4_BACKTRACE() #if defined(TRACE_BUILD) @@ -122,8 +123,8 @@ __END_DECLS #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif -#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) -#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) +#define PX4_LOG_NAMED(name, FMT, ...) qurt_log( _PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) +#define PX4_LOG_NAMED_COND(name, cond, FMT, ...) if( cond ) qurt_log( _PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, "%s " FMT, name, ##__VA_ARGS__) #else @@ -141,6 +142,7 @@ __EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1]; __EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1]; __EXPORT extern void px4_backtrace(void); __EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...); +__EXPORT void px4_log_raw(int level, const char *fmt, ...); __END_DECLS @@ -260,6 +262,22 @@ __END_DECLS do { \ px4_log_modulename(level, MODULE_NAME, fmt, ##__VA_ARGS__); \ } while(0) + +/**************************************************************************** + * __px4_log_raw: + * Convert a message in the form: + * PX4_INFO("val is %d", val); + * to + * printf("val is %d", val); + * + * This can be used for simple printfs with all the formatting control. + ****************************************************************************/ +#define __px4_log_raw(level, fmt, ...) \ + do { \ + px4_log_raw(level, fmt, ##__VA_ARGS__); \ + } while(0) + + /**************************************************************************** * __px4_log_timestamp: * Convert a message in the form: @@ -398,8 +416,13 @@ __END_DECLS /**************************************************************************** * Messages that should never be filtered or compiled out ****************************************************************************/ -#define PX4_LOG(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) -#define PX4_INFO(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_ALWAYS, FMT, ##__VA_ARGS__) +#define PX4_INFO(FMT, ...) __px4_log_modulename(_PX4_LOG_LEVEL_INFO, FMT, ##__VA_ARGS__) + +#ifdef __NUTTX +#define PX4_INFO_RAW printf +#else +#define PX4_INFO_RAW(FMT, ...) __px4_log_raw(_PX4_LOG_LEVEL_INFO, FMT, ##__VA_ARGS__) +#endif #if defined(TRACE_BUILD) /**************************************************************************** diff --git a/src/systemcmds/shutdown/CMakeLists.txt b/src/systemcmds/shutdown/CMakeLists.txt new file mode 100644 index 0000000000..5db004e76b --- /dev/null +++ b/src/systemcmds/shutdown/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +px4_add_module( + MODULE systemcmds__shutdown + MAIN shutdown + STACK_MAIN 800 + COMPILE_FLAGS + SRCS + shutdown.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/systemcmds/shutdown/shutdown.c b/src/systemcmds/shutdown/shutdown.c new file mode 100644 index 0000000000..81432789c0 --- /dev/null +++ b/src/systemcmds/shutdown/shutdown.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 shutdown.c + * Tool similar to UNIX shutdown command. + */ + +#include +//#include + + +__EXPORT int shutdown_main(int argc, char *argv[]); + +int shutdown_main(int argc, char *argv[]) +{ + (void)argc; + (void)argv; + + const bool to_bootloader = false; + px4_systemreset(to_bootloader); +} diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index f9b92a2507..0ac841b5f5 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -54,6 +54,12 @@ #define FLAG_FSYNC 1 #define FLAG_LSEEK 2 +#ifdef __PX4_POSIX +#define LOG_PATH PX4_ROOTFSDIR "/log/" +#else +#define LOG_PATH PX4_ROOTFSDIR "/fs/microsd/" +#endif + /* return a predictable value for any file offset to allow detection of corruption */ @@ -172,7 +178,7 @@ int test_file2(int argc, char *argv[]) { int opt; uint16_t flags = 0; - const char *filename = PX4_ROOTFSDIR "/fs/microsd/testfile2.dat"; + const char *filename = LOG_PATH "testfile2.dat"; uint32_t write_chunk = 64; uint32_t write_size = 5 * 1024; @@ -214,7 +220,7 @@ int test_file2(int argc, char *argv[]) /* check if microSD card is mounted */ struct stat buffer; - if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) { + if (stat(LOG_PATH, &buffer)) { fprintf(stderr, "no microSD card mounted, aborting file test"); return 1; } diff --git a/unittests/px4_log_stub.cpp b/unittests/px4_log_stub.cpp new file mode 100644 index 0000000000..81e41ee741 --- /dev/null +++ b/unittests/px4_log_stub.cpp @@ -0,0 +1,14 @@ +#include + + +__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) +{ + // Don't log Debug + if (level >= 1) { + va_list argptr; + va_start(argptr, fmt); + vprintf(fmt, argptr); + va_end(argptr); + printf("\n"); + } +}