From 0c5c741b1a63ff90b9137d6327ccdd10fab6c084 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 2 Aug 2018 21:32:51 +0200 Subject: [PATCH] add posix shell squashed & rebased version, not including: - listener changes - src/firmware renaming Commits: tag_to_version.py: fix Python3 error subprocess.communicate returns bytes instead of a str which is not the same for Python3. Therefore, we need to decode the bytes. cmake: remove folder src/firmware The folder src/firmware was not intuitive. Why would the binaries for SITL be inside a src and why even inside a src/firmware folder. Also, the rootfs was put there which made it even more confusing. The CMakeLists.txt files are moved into cmake/ and get now called from the main CMakeLists.txt. qshell: support for return value Instead of just sending commands, qshell will now also wait until the command has finished on QURT and sent back a return value. This will allow all modules on the DSP side to be spawned from the Linux side meaning that we only need one config/startup file instead of two. adb_upload: create folders before pushing Previously the script failed if the folder on the destination was not already existing. This therefore makes pushing easier. posix: spawn PX4 modules in bash This adds the possibility to spawn PX4 modules out of bash. Basically, the main executable can now be started as a server/daemon or as a client. The server replaces the existing functionality of the main exe with the pxh shell, however, it also opens a pipe that clients can talk to. Clients can run or spawn PX4 modules or commands by connecting to the server over the pipe. They clients will get the stdout and return value of their commands via a client specific pipe back. This work will allow to start all modules using a bash script similar to the way it is done in NuttX where the NuttShell scripts the startup scripts and starts the modules. SITL: use new client shell in SITL This is a first step to use the new shell capabilities for SITL. The new startup bash script rcS merges (and therefore replaces) the two existing scripts rcS_gazebo_iris and rcS_jmavsim_iris. More cleanup will be necessary for the rest of the SITL startup scripts. Snapdragon: use new shell to start all modules Instead of different mainapp.config and px4.config files, we can now use a unified rcS bash script which starts all the modules based on parameters, mainly the SYS_AUTOSTART param. Snapdragon: fix the airframe description pxh: argv needs to end with a nullptr The comment was wrong that argv needs an additional 0 termination. Instead it needs a nullptr at the end. px4_posix_tasks: variable cleanup The px4_task_spawn_cmd function got a cleanup while debugging, however, no functional changes. Snapdragon: move some drivers to 4100 config These drivers are supported by the community, so they go into the 4100 config. Snapdragon: update 210qc platform px4_daemon: use doxygen comments apps.h_in: fix string printf: use .c_str() px4_daemon: \b -> \n in printf px4_daemon: handle error in generate_uuid (close the file on error) posix main: some clarifications in comment (it's the symlinks not the script aliases) cmake: remove new install command again This one was probably wrong and untested. Installing needs revisiting. POSIX: remove argument USES_TERMINAL POSIX: copy init and mixer files for SITL Instead of using non-working install commands, the mixer and startup files are now copied as part of the build in cmake. adb_upload.sh: remove leftover commented printf POSIX main: just the pointer instead of memmove POSIX main: remove chroot chroot is removed because it hasn't been used anywhere and seems untested. px4_daemon: remove client pipe when cleaning up px4_daemon: fail if the client pipe already exists The client pipe is supposed to be specific (by UUID), so the path shouldn't exist already. history: limit the number of history entries This is a protection to avoid filling the memory if we are entering a lot of commands (e.g. auto-generated). px4_daemon: add a threadsafe map and use it px4_daemon: whitespace px4_daemon: fix client parsing Sometimes the client ends up reading more than one packet in one read. The parsing is not made for this and would require a (ring)buffer for it. The solution of this commit just reads as much as needed from the pipe which avoids having to do buffering and parsing. posix: changes sitl_run.sh and main.cpp cleanup This changes the paths in sitl_run.sh quite a bit to allow the px4 binary to run in the rootfs directory which should make it convenient and very close to the NuttX variant. Also main.cpp got a big cleanup after the big rebase with some conflicts. Quite some functionality was removed but it has yet to be seen if it needs to be re-added. px4_log: cleanup log levels, now they make sense Before DEBUG and INFO log levels where inverted which didn't make much sense in my eyes. dataman: fix path for bash shell logger: fix paths for bash shell mavlink: fix paths for bash shell param: fix path for bash shell inav: fix paths for bash shell sdlog2: fix paths for bash shell ROMFS: add forgotten mixer to list SITL init: more models, more options - Support for different models using the unified startup script rcS. - Support to choose the estimator by setting the environment variable PX4_ESTIMATOR. - Support to choose the logger by setting the environment variable PX4_LOGGER. rcS: fix string comparison listener: use template file Instead of having all of the C++ code inside the Python file it is nicer to have a separate template file with the C++ headers, etc. px4_log: add PX4_INFO_RAW for raw printfs This allows to do custom formatting but is still transported over sockets to clients. topic_listener: use PX4_INFO_RAW instead of printf commander: use PX4_INFO_RAW for status listener: rewrite to classes and factory posix: fix some argument warnings generate_listener.py: by accident changed shebang listener: big refactor of the generator Hopefully this makes it easier to read and change in the future. rcS: manually take over rebase changes listener: remove leftover try listener: properly clean up topic instance rcS: take over some vehicle specific changes posix-configs: vehicle specifics to separate files posix-configs: remove leftover lines uORBDevices: new PX4_INFO_RAW instead of printf px4_log: just use printf on NuttX listener: use less binary space, strip on NuttX generate_listener.py: remove commented code cmake: fix syntax error from merge px4_daemon: fixes after rebase of apps.h/cpp fix px4_daemon: namespace missing posix: only create stub for fsync on QURT unitests: reduce dependencies of param test This makes the unit test compile and link again after the bash changes. QURT: some compile fixes after a rebase SITL: arg change for sitl_run.sh to use rcS_test This allows to use a custom startup file for testing. SITL: add the folder test_data SITL: implement shutdown command as systemcmd The shutdown command needs to be a proper systemcmd, otherwise the alias and symlink generation doesn't work and we end up calling shutdown of the host computer which is to be avoided. px4fmu_test: same IO_pass mixer as px4fmu_default px4fmu_test: use normal quad x mixer There is no good reason to use a specific test mixer, except more cmake code around it. Therefore just use the same mixer as default, and at some point px4fmu_test and px4fmu_default can get merged POSIX: cleanup, dir and symlink fixes This cleans up the logic behind the symlinking and creating directories. POSIX: correct arg order in usage info tests: fix paths for SITL tests POSIX: printf fix sitl_run.sh: try to make this run on Mac as well cmake: try to make jenkins happier Path cleanup, the bin is no longer in src/firmware POSIX: fix symlink logic SITL: prefix all exported env variables cmake: fix path for ROS tests integrationtests: fix log path launch: try to make tets with ROS working again px4_defines: fix after wrong merge deconflicting px4_defines: get paths for POSIX correct cmake: fix cmake arguments This was fine with cmake 3.6 but did not work with cmake 3.2.2 cmake: use cp instead of cmake -E copy cmake -E copy does not support copying multiple files with versions < 3.5. Therefore, just use cp for now. ROMFS: fix build error after rebase cmake: fix paths in configs launch: use `spawn_model` again cmake: various fixes after big rebase param: path fixes after rebase posix platform: fixes after rebase test_mixer: fix screwed up rebase --- ROMFS/px4fmu_test/mixers/IO_pass.mix | 14 + ROMFS/px4fmu_test/mixers/quad_test.mix | 25 - ROMFS/px4fmu_test/mixers/quad_x.main.mix | 7 + Tools/adb_upload.sh | 11 +- Tools/adb_upload_to_bebop.sh | 4 +- Tools/decode_backtrace.py | 4 +- Tools/sitl_multiple_run.sh | 2 +- Tools/sitl_run.sh | 20 +- cmake/configs/posix_rpi_common.cmake | 1 + cmake/configs/posix_sdflight_default.cmake | 1 + cmake/configs/posix_sitl_default.cmake | 4 +- cmake/configs/posix_sitl_ekf2.cmake | 4 - cmake/posix/apps.h_in | 80 ++ cmake/posix/px4-alias.sh_in | 13 + launch/iris_env.sh | 2 + launch/mavros_posix_sitl.launch | 3 +- launch/posix_sitl.launch | 12 +- msg/CMakeLists.txt | 1 + msg/qshell_req.msg | 7 +- msg/qshell_retval.msg | 2 + platforms/posix/CMakeLists.txt | 47 +- platforms/posix/cmake/px4_impl_os.cmake | 59 + .../posix/include/px4_daemon/server_io.h | 65 + platforms/posix/src/CMakeLists.txt | 1 + platforms/posix/src/main.cpp | 1041 +++++++---------- platforms/posix/src/px4_daemon/CMakeLists.txt | 42 + platforms/posix/src/px4_daemon/client.cpp | 345 ++++++ platforms/posix/src/px4_daemon/client.h | 103 ++ platforms/posix/src/px4_daemon/history.cpp | 126 ++ platforms/posix/src/px4_daemon/history.h | 104 ++ .../posix/src/px4_daemon/pipe_protocol.cpp | 67 ++ .../posix/src/px4_daemon/pipe_protocol.h | 98 ++ platforms/posix/src/px4_daemon/pxh.cpp | 293 +++++ platforms/posix/src/px4_daemon/pxh.h | 98 ++ platforms/posix/src/px4_daemon/server.cpp | 304 +++++ platforms/posix/src/px4_daemon/server.h | 128 ++ platforms/posix/src/px4_daemon/server_io.cpp | 135 +++ .../posix/src/px4_daemon/threadsafe_map.h | 104 ++ platforms/posix/src/px4_layer/CMakeLists.txt | 1 + .../posix/src/px4_layer/px4_posix_tasks.cpp | 23 +- platforms/qurt/src/px4_layer/main.cpp | 50 +- posix-configs/SITL/init/10016_iris | 11 + posix-configs/SITL/init/6011_typhoon_h480 | 15 + posix-configs/SITL/init/CMakeLists.txt | 59 + posix-configs/SITL/init/rcS | 131 +++ posix-configs/SITL/init/rcS_tests | 21 + posix-configs/eagle/init/rcS | 210 ++++ src/drivers/qshell/posix/qshell.cpp | 99 +- src/drivers/qshell/posix/qshell.h | 7 +- src/drivers/qshell/qurt/qshell.cpp | 82 +- src/modules/dataman/dataman.cpp | 2 +- src/modules/logger/logger.cpp | 5 + src/modules/logger/logger.h | 2 +- .../position_estimator_inav_main.cpp | 4 + src/modules/uORB/uORBDevices.cpp | 20 +- src/platforms/common/px4_log.c | 135 ++- src/platforms/px4_defines.h | 6 +- src/platforms/px4_log.h | 39 +- src/systemcmds/shutdown/CMakeLists.txt | 43 + src/systemcmds/shutdown/shutdown.c | 52 + src/systemcmds/tests/test_file2.c | 10 +- unittests/px4_log_stub.cpp | 14 + 62 files changed, 3606 insertions(+), 812 deletions(-) delete mode 100644 ROMFS/px4fmu_test/mixers/quad_test.mix create mode 100644 ROMFS/px4fmu_test/mixers/quad_x.main.mix create mode 100644 cmake/posix/apps.h_in create mode 100644 cmake/posix/px4-alias.sh_in create mode 100644 launch/iris_env.sh create mode 100644 msg/qshell_retval.msg create mode 100644 platforms/posix/include/px4_daemon/server_io.h create mode 100644 platforms/posix/src/px4_daemon/CMakeLists.txt create mode 100644 platforms/posix/src/px4_daemon/client.cpp create mode 100644 platforms/posix/src/px4_daemon/client.h create mode 100644 platforms/posix/src/px4_daemon/history.cpp create mode 100644 platforms/posix/src/px4_daemon/history.h create mode 100644 platforms/posix/src/px4_daemon/pipe_protocol.cpp create mode 100644 platforms/posix/src/px4_daemon/pipe_protocol.h create mode 100644 platforms/posix/src/px4_daemon/pxh.cpp create mode 100644 platforms/posix/src/px4_daemon/pxh.h create mode 100644 platforms/posix/src/px4_daemon/server.cpp create mode 100644 platforms/posix/src/px4_daemon/server.h create mode 100644 platforms/posix/src/px4_daemon/server_io.cpp create mode 100644 platforms/posix/src/px4_daemon/threadsafe_map.h create mode 100644 posix-configs/SITL/init/10016_iris create mode 100644 posix-configs/SITL/init/6011_typhoon_h480 create mode 100644 posix-configs/SITL/init/CMakeLists.txt create mode 100644 posix-configs/SITL/init/rcS create mode 100644 posix-configs/SITL/init/rcS_tests create mode 100644 posix-configs/eagle/init/rcS create mode 100644 src/systemcmds/shutdown/CMakeLists.txt create mode 100644 src/systemcmds/shutdown/shutdown.c create mode 100644 unittests/px4_log_stub.cpp 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"); + } +}