diff --git a/ROMFS/px4fmu_test/mixers/IO_pass.mix b/ROMFS/px4fmu_test/mixers/IO_pass.mix
index 42321f0ad938f1897aa7f8a1d4804fba9d092e6f..39f875ddb947eec9e86d8eb1e926d52a0ad376c3 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 a4876bea2127f15ff566859813a7b24c5d41bd51..0000000000000000000000000000000000000000
--- 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 0000000000000000000000000000000000000000..bf034e7c1f1dcf5d66556e5bf4002347a3b1cb60
--- /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 b4e4f649cbe0142e10b1bfa262b80e1b762a0e49..5b9475abcc14770b7a340f816472d768d50a9e62 100755
--- a/Tools/adb_upload.sh
+++ b/Tools/adb_upload.sh
@@ -5,13 +5,16 @@ if [[ "$#" < 2 ]]; then
exit
fi
+# Get last argument
+for last; do true; done
+
echo "Wait for device..."
adb wait-for-device
-echo "Uploading..."
-# Get last argument
-for last; do true; done
+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 48edf0007ecdd1da1450ed56e7f1ca8217b17546..b54f938a01e78f74c78382dce61580fff4ae217f 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 64f8a8ea3078b4322d7d5c3cbac7ee18cc66ebae..a9eedb5c293544dd6026f04e7b9d0bbb2a548505 100755
--- a/Tools/decode_backtrace.py
+++ b/Tools/decode_backtrace.py
@@ -63,7 +63,7 @@ def usage():
msg = """
Usage: Tools/decode_backtrace.py <builddir>
-This will load the symbols for <builddir>/src/firmware/posix/px4
+This will load the symbols for <builddir>/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 7f590d87cbb615e844b6640d8fbdb491299cc348..a15ab1d3ec575e9f84114d041f587e27262d551f 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 4a516b88144056d7cccfe232f22dfce6c4ff56e7..53c9e3c6ae2d9e47d3932e68ba4a3268fdecabfe 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 c04108b2612a23cd247d34e89962283d8817bd21..0bba4a1cfeafee772d0540c9a08a93026a085285 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 70e1377317fc44a6a54214f8d7bfe42137a1d015..5aee64186cf8b24d61a33ece0d11868e05f8cc8c 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 544d43f2b2a994d50d7997e11b160c063c1137d1..b27477d63b8e0a9bfa3059e0e43aa3a24c503403 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 ae6191e508825c990c0adc308cfbc0ae04311c22..d8ee1acc8a45cda8bb05eab8f39b1b7900eb657c 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 0000000000000000000000000000000000000000..bf76b9725d5de707cd1c4b62df318b95bd2d4485
--- /dev/null
+++ b/cmake/posix/apps.h_in
@@ -0,0 +1,80 @@
+/* builtin command list - automatically generated, do not edit */
+#include <string>
+#include <map>
+#include <stdio.h>
+
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_log.h>
+#include <stdlib.h>
+
+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<string,px4_main_t> app_map(void)
+{
+ static map<string,px4_main_t> 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<string,px4_main_t> apps = app_map();
+
+static void list_builtins(void)
+{
+ printf("Builtin Commands:\n");
+ for (map<string,px4_main_t>::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 <seconds>\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 0000000000000000000000000000000000000000..e6ebb886818e6f3e0804c5778bf54a1ff10989fe
--- /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 0000000000000000000000000000000000000000..bfaeeb39cc86cfe60b9e22dc68edfe5910bdc33a
--- /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 70bb33818156ba681acad54cbfbc5796de85f9ab..408122c9c8798edcb3de2e74d410af8c54662b04 100644
--- a/launch/mavros_posix_sitl.launch
+++ b/launch/mavros_posix_sitl.launch
@@ -14,7 +14,8 @@
<arg name="vehicle" default="iris"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
- <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)"/>
+ <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/rcS"/>
+
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
diff --git a/launch/posix_sitl.launch b/launch/posix_sitl.launch
index 64df6d349dcf44a3cb278d008b1f6a98bb7896da..4a8d00cecac3a18d39b45311993f0acf96d21ca5 100644
--- a/launch/posix_sitl.launch
+++ b/launch/posix_sitl.launch
@@ -14,7 +14,8 @@
<arg name="vehicle" default="iris"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
- <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)"/>
+ <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/rcS"/>
+
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
@@ -26,7 +27,9 @@
<!-- PX4 SITL -->
<arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
<arg if="$(arg interactive)" name="px4_command_arg1" value=""/>
- <node name="sitl" pkg="px4" type="px4" output="screen" args="$(find px4) $(arg rcS) $(arg px4_command_arg1)" required="true"/>
+ <node name="sitl" pkg="px4" type="px4" output="screen"
+ args="$(arg rootfs) $(arg rcS) $(arg px4_command_arg1)" />
+
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
@@ -39,3 +42,8 @@
<!-- gazebo model -->
<node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf -file $(arg sdf) -model $(arg vehicle) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
</launch>
+
+ <!-- This will set the environment variable needed to select iris in the startup. -->
+ <machine name="px4" env-loader="iris_env.sh" address="none" />
+</launch>
+
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 45997df36730a1e1dd379685dab184aade4a1694..4adc266dd39e47619ce5a2685d7c92703b306751 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 fa75390edd556420eab53a635300ecc19424fc5c..478d1e702878bb8223b323bfc33c574193921f5f 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 0000000000000000000000000000000000000000..f51d16ea7f3b4bb3d450b9ac6cf6796a482e1eb8
--- /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 228b1b2b1f85bd23d8019ba73ca059d2b59edbf7..a93613ce825a7c29a10dbf917c73451a092441c7 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 c7ed95297ec00e24e42a18b1dbf0af25917b166e..217d1662a509790ec0d1c82f039db1308eb315fd 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 0000000000000000000000000000000000000000..88c417501dea2ec81065328017d40c6ad813028a
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+#pragma once
+
+#include <stdio.h>
+
+__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 602b1b710744f71f932377fcc0f524e855cf4cd6..00bab5f2defe7135584c8f8e69069d2afb0377bf 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 54478e838d34e8923b8e28bbcfaa68ac38b459da..42bb1be99a19a64dd269eec328d629456500a84e 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,656 +32,489 @@
****************************************************************************/
/**
* @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 <charlebm@gmail.com>
* @author Roman Bapst <bapstroman@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
*/
-#include <iostream>
-#include <fstream>
#include <string>
-#include <sstream>
-#include <vector>
+#include <fstream>
#include <signal.h>
-#include <unistd.h>
#include <stdio.h>
-#include "apps.h"
-#include "px4_middleware.h"
-#include "px4_posix.h"
-#include "px4_log.h"
-#include "DriverFramework.hpp"
-#include <termios.h>
+#include <errno.h>
+#include <fcntl.h>
#include <sys/stat.h>
+
+#include <px4_log.h>
#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_log.h>
-namespace px4
-{
-void init_once();
-}
+#include "apps.h"
+#include "px4_middleware.h"
+#include "DriverFramework.hpp"
+#include "px4_middleware.h"
+#include "px4_daemon/client.h"
+#include "px4_daemon/server.h"
+#include "px4_daemon/pxh.h"
-using namespace std;
-#define CMD_BUFF_SIZE 100
+static const char *LOCK_FILE_PATH = "/tmp/px4_lock";
-#ifdef PATH_MAX
-const unsigned path_max_len = PATH_MAX;
-#else
-const unsigned path_max_len = 1024;
+#ifndef PATH_MAX
+#define PATH_MAX 1024
#endif
-static volatile bool _ExitFlag = false;
-
-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();
- }
+static volatile bool _exit_requested = false;
- void _SigSegvHandler(int sig_num);
- void _SigSegvHandler(int sig_num)
- {
- cout.flush();
- cout << endl << "segmentation fault" << endl;
- PX4_BACKTRACE();
- cout.flush();
- }
-}
-static inline bool fileExists(const string &name)
-{
- struct stat buffer;
- return (stat(name.c_str(), &buffer) == 0);
-}
-
-static inline bool dirExists(const string &path)
-{
- struct stat info;
-
- if (stat(path.c_str(), &info) != 0) {
- return false;
-
- } else if (info.st_mode & S_IFDIR) {
- return true;
-
- } else {
- return false;
- }
-}
-
-static inline void touch(const string &name)
-{
- fstream fs;
- fs.open(name, ios::out);
- fs.close();
-}
-
-static int mkpath(const char *path, mode_t mode);
-
-static int do_mkdir(const char *path, mode_t mode)
+namespace px4
{
- 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);
+void init_once(void);
}
-/**
-** 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 = '/';
- }
+static void sig_int_handler(int sig_num);
+static void sig_fpe_handler(int sig_num);
+static void sig_segv_handler(int sig_num);
- pp = sp + 1;
- }
+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();
- 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(""));
-}
+#ifdef __PX4_SITL_MAIN_OVERRIDE
+int SITL_MAIN(int argc, char **argv);
-static void print_prompt()
+int SITL_MAIN(int argc, char **argv)
+#else
+int main(int argc, char **argv)
+#endif
{
- cout.flush();
- cout << "pxh> ";
- cout.flush();
-}
+ bool is_client = false;
+ bool pxh_off = false;
-static void run_cmd(const vector<string> &appargs, bool exit_on_fail, bool silently_fail = false)
-{
- static apps_map_type apps;
- static bool initialized = false;
+ /* Symlinks point to all commands that can be used as a client with a prefix. */
+ const char prefix[] = PX4_BASH_PREFIX;
- if (!initialized) {
- init_app_map(apps);
- initialized = true;
+ if (strstr(argv[0], prefix)) {
+ is_client = true;
}
- // command is appargs[0]
- string command = appargs[0];
-
- if (apps.find(command) != apps.end()) {
- const char *arg[appargs.size() + 2];
+ if (is_client) {
- unsigned int i = 0;
-
- while (i < appargs.size() && appargs[i] != "") {
- arg[i] = (char *)appargs[i].c_str();
- ++i;
+ if (!is_already_running()) {
+ PX4_ERR("PX4 daemon not running yet");
+ return -1;
}
- arg[i] = (char *)nullptr;
-
- int retval = apps[command](i, (char **)arg);
+ /* Remove the prefix. */
+ argv[0] += strlen(prefix);
- if (retval) {
- cout << "Command '" << command << "' failed, returned " << retval << endl;
+ px4_daemon::Client client;
+ client.generate_uuid();
+ client.register_sig_handler();
+ return client.process_args(argc, (const char **)argv);
- if (exit_on_fail && retval) {
- exit(retval);
- }
+ } else {
+ if (is_already_running()) {
+ PX4_ERR("PX4 daemon already running");
+ return -1;
}
- } else if (command == "help") {
- list_builtins(apps);
+ /* Server/daemon apps need to parse the command line arguments. */
- } else if (command.length() == 0 || command[0] == '#') {
- // Do nothing
+ int positional_arg_count = 0;
+ std::string data_path = "";
+ std::string commands_file = "";
+ std::string node_name = "";
- } else if (!silently_fail) {
- cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl;
+ // parse arguments
+ for (int i = 1; i < argc; ++i) {
+ if (argv[i][0] == '-') {
- }
-}
+ if (strcmp(argv[i], "-h") == 0) {
+ print_usage();
+ return 0;
-static void usage()
-{
+ } else if (strcmp(argv[i], "-d") == 0) {
+ pxh_off = true;
- 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 << "<data_directory> - directory where ROMFS and posix-configs are located (if not given, CWD is used)" << endl;
- cout << "<startup_config> - config file for starting/stopping px4 modules" << endl;
- cout << " -h - help/usage information" << endl;
-}
+ } else {
+ printf("Unknown/unhandled parameter: %s\n", argv[i]);
+ print_usage();
+ return 1;
+ }
-static void process_line(string &line, bool exit_on_fail)
-{
- vector<string> appargs(20);
+ } else if (!strncmp(argv[i], "__", 2)) {
- 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);
-}
+ // 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());
+ }
-static void restore_term()
-{
- cout << "Restoring terminal\n";
- tcsetattr(0, TCSANOW, &orig_term);
-}
+ } else {
+ //printf("positional argument\n");
-bool px4_exit_requested(void)
-{
- return _ExitFlag;
-}
+ positional_arg_count += 1;
-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");
+ if (positional_arg_count == 1) {
+ commands_file = argv[i];
- // 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
-}
+ } else if (positional_arg_count == 2) {
+ data_path = commands_file;
+ commands_file = argv[i];
+ }
+ }
-#ifdef __PX4_SITL_MAIN_OVERRIDE
-int SITL_MAIN(int argc, char **argv);
+ 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;
+ }
+ }
-int SITL_MAIN(int argc, char **argv)
-#else
-int main(int argc, char **argv)
-#endif
-{
- bool daemon_mode = false;
- bool chroot_on = false;
+ if (!file_exists(commands_file)) {
+ PX4_ERR("Error opening startup file, does not exist: %s", commands_file.c_str());
+ return -1;
+ }
- tcgetattr(0, &orig_term);
- atexit(restore_term);
+ PX4_INFO("startup file: %s", commands_file.c_str());
- // SIGINT
- struct sigaction sig_int {};
- sig_int.sa_handler = _SigIntHandler;
- sig_int.sa_flags = 0;// not SA_RESTART!;
- sigaction(SIGINT, &sig_int, nullptr);
+ register_sig_handler();
+ set_cpu_scaling();
- // SIGFPE
- struct sigaction sig_fpe {};
- sig_fpe.sa_handler = _SigFpeHandler;
- sig_fpe.sa_flags = 0;// not SA_RESTART!;
- sigaction(SIGFPE, &sig_fpe, nullptr);
+ px4_daemon::Server server;
+ server.start();
- // SIGSEGV
- struct sigaction sig_segv {};
- sig_segv.sa_handler = _SigSegvHandler;
- sig_segv.sa_flags = SA_RESTART | SA_SIGINFO;
- sigaction(SIGSEGV, &sig_segv, nullptr);
+ int ret = create_symlinks_if_needed(data_path);
- 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 (ret != PX4_OK) {
+ return ret;
+ }
- if (argv[index][0] == '-') {
- // the arg starts with -
- if (strncmp(argv[index], "-d", 2) == 0) {
- daemon_mode = true;
+ ret = create_dirs();
- } else if (strncmp(argv[index], "-h", 2) == 0) {
- usage();
- return 0;
+ if (ret != PX4_OK) {
+ return ret;
+ }
- } else if (strncmp(argv[index], "-c", 2) == 0) {
- chroot_on = true;
+ DriverFramework::Framework::initialize();
- } else {
- PX4_ERR("Unknown/unhandled parameter: %s", argv[index]);
- return 1;
- }
+ px4::init_once();
+ px4::init(argc, argv, "px4");
- } else if (!strncmp(argv[index], "__", 2)) {
- //cout << "ros argument" << endl;
+ ret = run_startup_bash_script(commands_file.c_str());
- // 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;
- }
+ // We now block here until we need to exit.
+ if (pxh_off) {
+ wait_to_exit();
} 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];
- }
+ px4_daemon::Pxh pxh;
+ pxh.run_pxh();
}
- ++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;
- }
+ // When we exit, we need to stop muorb on Snapdragon.
- bool symlinks_needed = true;
+#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
- if (positional_arg_count == 1) { //data path is optional
- commands_file = data_path;
- symlinks_needed = false;
+ std::string cmd("shutdown");
+ px4_daemon::Pxh::process_line(cmd, true);
- } else {
- cout << "data path: " << data_path << endl;
}
- cout << "commands file: " << commands_file << endl;
+ return PX4_OK;
+}
- if (commands_file.empty()) {
- PX4_ERR("Error commands file not specified");
- return -1;
- }
+int create_symlinks_if_needed(std::string &data_path)
+{
+ std::string current_path = pwd();
- if (!fileExists(commands_file)) {
- PX4_ERR("Error opening commands file, does not exist: %s", commands_file.c_str());
- return -1;
+ 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;
}
- // create sym-links
- if (symlinks_needed) {
- vector<string> 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());
- }
- }
+ 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;
}
- // setup rootfs
- const string eeprom_path = "./rootfs/eeprom/";
- const string microsd_path = "./rootfs/fs/microsd/";
+ std::vector<std::string> path_sym_links;
+ path_sym_links.push_back("etc");
+ path_sym_links.push_back("test_data");
- 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");
- }
+ for (const auto &it = path_sym_links.begin(); it != path_sym_links.end(); ++it) {
- // initialize
- DriverFramework::Framework::initialize();
- px4::init_once();
+ PX4_DEBUG("path sym link: %s", it->c_str());;
- px4::init(argc, argv, "px4");
+ std::string src_path = data_path + "/" + *it;
+ std::string dest_path = current_path + "/" + *it;
- // if commandfile is present, process the commands from the file
- if (!commands_file.empty()) {
- ifstream infile(commands_file.c_str());
+ if (dir_exists(dest_path.c_str())) {
+ continue;
+ }
- if (infile.is_open()) {
- for (string line; getline(infile, line, '\n');) {
+ PX4_INFO("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str());
- if (px4_exit_requested()) {
- break;
- }
+ // create sym-links
+ int ret = symlink(src_path.c_str(), dest_path.c_str());
- // TODO: this should be true but for that we have to check all startup files
- process_line(line, false);
- }
+ if (ret != 0) {
+ PX4_ERR("Error creating symlink %s -> %s", src_path.c_str(), dest_path.c_str());
+ return ret;
} else {
- PX4_ERR("Error opening commands file: %s", commands_file.c_str());
+ PX4_DEBUG("Successfully created symlink %s -> %s", src_path.c_str(), dest_path.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.
+ return PX4_OK;
+}
+
+int create_dirs()
+{
+ std::string current_path = pwd();
- char pwd_path[path_max_len];
- const char *folderpath = "/rootfs/";
+ std::vector<std::string> dirs;
+ dirs.push_back("log");
- if (nullptr == getcwd(pwd_path, sizeof(pwd_path))) {
- PX4_ERR("Failed acquiring working dir, abort.");
- exit(1);
- }
+ 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 (nullptr == strcat(pwd_path, folderpath)) {
- PX4_ERR("Failed completing path, abort.");
- exit(1);
+ if (dir_exists(dir_path)) {
+ continue;
}
- if (chroot(pwd_path)) {
- PX4_ERR("Failed chrooting application, path: %s, error: %s.", pwd_path, strerror(errno));
- exit(1);
- }
+ // create dirs
+ int ret = mkdir(dir_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
- if (chdir("/")) {
- PX4_ERR("Failed changing to root dir, path: %s, error: %s.", pwd_path, strerror(errno));
- exit(1);
+ 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());
}
}
- 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)
+ return PX4_OK;
+}
- 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);
+void register_sig_handler()
+{
+ // SIGINT
+ struct sigaction sig_int {};
+ sig_int.sa_handler = sig_int_handler;
+ sig_int.sa_flags = 0;// not SA_RESTART!;
- while (!_ExitFlag) {
+ // SIGFPE
+ struct sigaction sig_fpe {};
+ sig_int.sa_handler = sig_fpe_handler;
+ sig_int.sa_flags = 0;// not SA_RESTART!;
- int c = getchar();
- string add_string; // string to add at current cursor position
- bool update_prompt = true;
+ // SIGPIPE
+ // We want to ignore if a PIPE has been closed.
+ struct sigaction sig_pipe {};
+ sig_pipe.sa_handler = SIG_IGN;
- switch (c) {
- case EOF:
- _ExitFlag = true;
- break;
+ // SIGSEGV
+ struct sigaction sig_segv {};
+ sig_segv.sa_handler = sig_segv_handler;
+ sig_segv.sa_flags = SA_RESTART | SA_SIGINFO;
- case 127: // backslash
- if (mystr.length() - cursor_position > 0) {
- mystr.erase(mystr.length() - cursor_position - 1, 1);
+ 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;
+}
- break;
+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;
+}
- case '\n': // user hit enter
- if (buf_ptr_write == CMD_BUFF_SIZE) {
- buf_ptr_write = 0;
- }
+void sig_segv_handler(int sig_num)
+{
+ fflush(stdout);
+ printf("\nSegmentation Fault\n");
+ PX4_BACKTRACE();
+ fflush(stdout);
+}
- if (buf_ptr_write > 0) {
- if (!mystr.empty() && mystr != string_buffer[buf_ptr_write - 1]) {
- string_buffer[buf_ptr_write] = mystr;
- buf_ptr_write++;
- }
+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");
- } else {
- if (!mystr.empty() && mystr != string_buffer[CMD_BUFF_SIZE - 1]) {
- string_buffer[buf_ptr_write] = mystr;
- buf_ptr_write++;
- }
- }
+ // 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
+}
- cout << endl;
- process_line(mystr, false);
- // reset string and cursor position
- mystr = "";
- cursor_position = 0;
- buf_ptr_read = buf_ptr_write;
+int run_startup_bash_script(const char *commands_file)
+{
+ std::string bash_command("bash ");
- print_prompt();
- break;
+ bash_command += commands_file;
- case '\033': { // arrow keys
- c = getchar(); // skip first one, does not have the info
- c = getchar();
+ PX4_INFO("Calling bash script: %s", bash_command.c_str());
- if (c == 'A') { // arrow up
- buf_ptr_read--;
+ int ret = 0;
- } else if (c == 'B') { // arrow down
- if (buf_ptr_read < buf_ptr_write) {
- buf_ptr_read++;
- }
+ if (!bash_command.empty()) {
+ ret = system(bash_command.c_str());
- } else if (c == 'C') { // arrow right
- if (cursor_position > 0) {
- printf("\033[1C");
- cursor_position--;
- }
+ if (ret == 0) {
+ PX4_INFO("Startup script returned successfully");
- break;
+ } else {
+ PX4_WARN("Startup script returned with return value: %d", ret);
+ }
- } else if (c == 'D') { // arrow left
- if (cursor_position < mystr.length()) {
- printf("\033[1D");
- cursor_position++;
- }
+ } else {
+ PX4_INFO("Startup script empty");
+ }
- break;
+ return ret;
+}
- } else if (c == 'H') { // Home (go to the beginning of the command)
- cursor_position = mystr.length();
- break;
+void wait_to_exit()
+{
+ while (!_exit_requested) {
+ usleep(100000);
+ }
+}
- } else if (c == '1') { // Home (go to the beginning of the command, Editing key)
- (void)getchar(); // swallow '~'
- cursor_position = mystr.length();
- break;
+void print_usage()
+{
+ printf("Usage for Server/daemon process: \n");
+ printf("\n");
+ printf(" px4 [-h|-d] [rootfs_directory] startup_file\n");
+ printf("\n");
+ printf(" <rootfs_directory> directory where startup files and mixers are located,\n");
+ printf(" (if not given, CWD is used)\n");
+ printf(" <startup_file> 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");
+}
- } else if (c == 'F') { // End (go to the end of the command)
- cursor_position = 0;
- break;
+bool is_already_running()
+{
+ struct flock fl;
+ int fd = open(LOCK_FILE_PATH, O_RDWR | O_CREAT, 0666);
- } else if (c == '4') { // End (go to the end of the command, Editing key)
- (void)getchar(); // swallow '~'
- cursor_position = 0;
- break;
- }
+ if (fd < 0) {
+ return false;
+ }
- if (buf_ptr_read < 0) {
- buf_ptr_read = 0;
- }
+ fl.l_type = F_WRLCK;
+ fl.l_whence = SEEK_SET;
+ fl.l_start = 0;
+ fl.l_len = 0;
+ fl.l_pid = getpid();
- string saved_cmd = string_buffer[buf_ptr_read];
- mystr = saved_cmd;
- cursor_position = 0; // move cursor to end of line
+ if (fcntl(fd, F_SETLK, &fl) == -1) {
+ // We failed to create a file lock, must be already locked.
- break;
- }
+ if (errno == EACCES || errno == EAGAIN) {
+ return true;
+ }
+ }
- default: // any other input
- if (c > 3) {
- add_string += (char)c;
+ return false;
+}
- } else {
- update_prompt = false;
- }
+bool px4_exit_requested(void)
+{
+ return _exit_requested;
+}
- break;
- }
+bool file_exists(const std::string &name)
+{
+ struct stat buffer;
+ return (stat(name.c_str(), &buffer) == 0);
+}
- 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);
- }
- }
+bool dir_exists(const std::string &path)
+{
+ struct stat info;
- }
+ if (stat(path.c_str(), &info) != 0) {
+ return false;
- } else {
- while (!_ExitFlag) {
- usleep(100000);
- }
- }
+ } else if (info.st_mode & S_IFDIR) {
+ return true;
- // 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<string> muorb_stop_cmd = { "muorb", "stop" };
- run_cmd(muorb_stop_cmd, !daemon_mode, true);
+ } else {
+ return false;
}
-
- vector<string> 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 : */
+std::string pwd()
+{
+ char temp[PATH_MAX];
+ return (getcwd(temp, PATH_MAX) ? std::string(temp) : std::string(""));
+}
diff --git a/platforms/posix/src/px4_daemon/CMakeLists.txt b/platforms/posix/src/px4_daemon/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9704af02c19910d1ece1999ffb581cd661cb4eea
--- /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 0000000000000000000000000000000000000000..bd064fe34eaf7f291bdd9eb09d2e969c1958fe94
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <errno.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <signal.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/un.h>
+
+#include <string>
+
+#include <px4_log.h>
+#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 0000000000000000000000000000000000000000..b0a606cfcbe31b26a52ae8eacbb5f4cf2273052f
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+#pragma once
+
+#include <stdint.h>
+
+#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 0000000000000000000000000000000000000000..0d601bae7b7f6929fc5bf36542ced919b9312a95
--- /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 <julian@oes.ch>
+ */
+
+#include <string>
+#include <vector>
+
+#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 0000000000000000000000000000000000000000..b29ca8c53107378bfc3cd44b5b6ce8e4add01986
--- /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 <julian@oes.ch>
+ */
+#pragma once
+
+#include <vector>
+#include <string>
+
+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<std::string> _history;
+ std::vector<std::string>::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 0000000000000000000000000000000000000000..0cdf49c94bbaeeaceaa4ee8e93294875f8fc7c97
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <inttypes.h>
+
+#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 0000000000000000000000000000000000000000..b0bcd0ba37952c1e02850c08ec9700e2c500bdf8
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+#pragma once
+
+#include <stdint.h>
+
+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 0000000000000000000000000000000000000000..60f840ee372a75bcc2efdbfd2ae2b6adba9c88b5
--- /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 <charlebm@gmail.com>
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string>
+#include <sstream>
+#include <vector>
+#include <stdio.h>
+
+#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<std::string> 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 0000000000000000000000000000000000000000..0fd6018c6239cc9c2180d05761a06dd4878eaf44
--- /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 <charlebm@gmail.com>
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ */
+#pragma once
+
+#include <vector>
+#include <string>
+#include <termios.h>
+
+#include <platforms/posix/apps.h>
+#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 0000000000000000000000000000000000000000..cb6e2f19085211d0a76b835aa14fb2907729530c
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <string>
+#include <pthread.h>
+#include <poll.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <px4_log.h>
+
+#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<uint64_t, pthread_t>
+ (packet.header.client_uuid, new_pthread));
+ _pthread_to_pipe_fd.insert(std::pair<pthread_t, int>
+ (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 0000000000000000000000000000000000000000..44bcf0918ab0b5d2cdf619605b99016aaf7aa090
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <pthread.h>
+#include <map>
+
+#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_t, int> _pthread_to_pipe_fd;
+ ThreadsafeMap <uint64_t, pthread_t> _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 0000000000000000000000000000000000000000..67e9e138be8c7fc62b24be2cd1d9f63d216bc4b7
--- /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 <julian@oes.ch>
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <string>
+#include <pthread.h>
+#include <poll.h>
+#include <assert.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <px4_log.h>
+
+#include "server.h"
+#include <px4_daemon/server_io.h>
+#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 0000000000000000000000000000000000000000..c3a9955a7d4660a7af1dceff5b224284693ed78d
--- /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 <julian@oes.ch>
+ */
+#pragma once
+
+#include <pthread.h>
+#include <map>
+
+
+namespace px4_daemon
+{
+
+template <class Key, class Type>
+class ThreadsafeMap
+{
+public:
+ ThreadsafeMap() :
+ _mutex(PTHREAD_MUTEX_INITIALIZER) {};
+ ~ThreadsafeMap() {};
+
+ /**
+ * Insert an entry into the map.
+ */
+ void
+ insert(std::pair<Key, Type> 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<Key, Type> _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 cf566b52b051eea903dd25978edd17b55a01191b..dc752e595badfc258dac8f73170b3e6015e86338 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 3e69b542a39359efc6a246bb8d7794651d34ff45..f8570fcc917b306223cb5335a9073f4d31e3e208 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 dffc84e9a86ba3a7a94f871ec715beffdcee6c10..2fe6b57aac508f19572ab914570177a2acf51677 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 <charlebm@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
*/
#include <px4_middleware.h>
@@ -47,19 +49,20 @@
#include <map>
#include <stdio.h>
#include <stdlib.h>
+
#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_<target>.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<string> &appargs)
+static void run_cmd(apps_map_type &apps, const std::vector<std::string> &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<string> appargs;
+ std::vector<std::string> 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 0000000000000000000000000000000000000000..5ad6462f84897b24826d6c71df3de205f0788e02
--- /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 <julian@oes.ch>
+#
+
+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 0000000000000000000000000000000000000000..f4ece2b5d95e0f196f754c70781e109b36c0cf91
--- /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 0000000000000000000000000000000000000000..2483929ffa5839f831edec08fe275a05da656c68
--- /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 0000000000000000000000000000000000000000..e3bb2ecd8a44681858318a8cfea141875aecf3b5
--- /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 0000000000000000000000000000000000000000..4bb1cd3f4f6b7c5581f43cbb5ee4ddd3ab55a3dc
--- /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 0000000000000000000000000000000000000000..8d8b1225844b6eeaa308ca4187968bbd5464712c
--- /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 bd8492696dfbae413fa7cdffacbe6792fc6feee4..d111db42e05dba1d169be2c206aa5a1ecc822c81 100644
--- a/src/drivers/qshell/posix/qshell.cpp
+++ b/src/drivers/qshell/posix/qshell.cpp
@@ -38,20 +38,50 @@
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
-#include "qshell.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <string>
#include <stdlib.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/qshell_retval.h>
+
+#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<std::string> 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<std::string> &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<std::string> 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];
+ 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;
+ }
}
- 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;
+ 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);
}
- appState.setRunning(false);
- return 0;
+ PX4_ERR("command timed out");
+ return -1;
}
diff --git a/src/drivers/qshell/posix/qshell.h b/src/drivers/qshell/posix/qshell.h
index 0873e221b2d446a4c8a1f285b9e2bc579b2ca3de..507cc1dd83fcac1c7f7a0c6b148abf7537af4cb2 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<std::string> &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 61e039049b070fb64848bd53ad6bdad2639f4708..d159be1bc81106aa461e61a259675bbee5567195 100644
--- a/src/drivers/qshell/qurt/qshell.cpp
+++ b/src/drivers/qshell/qurt/qshell.cpp
@@ -44,6 +44,7 @@
#include <px4_time.h>
#include <px4_posix.h>
#include <px4_middleware.h>
+#include <px4_defines.h>
#include <dspal_platform.h>
#include <unistd.h>
@@ -56,7 +57,7 @@
#include <stdio.h>
#include <stdlib.h>
-#include "modules/uORB/uORB.h"
+#include <uORB/topics/qshell_retval.h>
#include <drivers/drv_hrt.h>
#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<std::string> 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<std::string> 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 = "";
}
+
+ } else {
+ arg += current_char;
}
+ }
- appargs.push_back(arg); // push last argument
+ appargs.push_back(arg); // push last argument
- int ret = run_cmd(appargs);
+ struct qshell_retval_s retval;
+ retval.return_value = run_cmd(appargs);
+ retval.sequence = m_qshell_req.sequence;
- if (ret) {
- PX4_ERR("Failed to execute command");
- }
+ 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<std::string> &appargs)
diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp
index f2064fdfb307dbb09bacc602d995f950e062d878..d5bac6b23b750b46d53bdc02ef8edcd3feacc5b9 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 be8d22b89fd530184eabf5aa64de31d4fe7b668e..c188904bcd15b100daed36540f806afd486c7e5e 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 47c48a3ff4ba4cc4dea73dcb247cbdcd111cb966..97749c1d318582d0281e28996b6442ab03224e6e 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 0bfc75661af4b843160605b88dfd81ae77497d7f..94993d22a0300d80ae638f21b38c50f33dcbd263 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 98aaa5f9605e878ec05bf3d704bbab49ac5983e5..40073dc4694478a3cdd6cb38c98d857c4ad4895c 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 fefdb840344fcef5a51845944d6919dac33e2d1c..0d57174c5ccc4055ca17fc01f5aec5e5b3c98af9 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 <px4_log.h>
-
#include <stdlib.h>
#include <string.h>
-
+#include <px4_log.h>
#if defined(__PX4_POSIX) && !defined(__PX4_CYGWIN)
#include <execinfo.h>
+#include <px4_daemon/server_io.h>
#endif
#include <uORB/uORB.h>
@@ -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 7e2063d4da52d4b87261e3779cb7097e1199940d..30e9bba771953cc9755b12b34c7b57a57643c2a0 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 7af9cf848867c8c81f3f4383bf2a6fb8ba765060..dbadbcb46f3ef13be465a3c6df3c77c1943b64bf 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 0000000000000000000000000000000000000000..5db004e76b6b539b34390ad6536a6c64bc3e3515
--- /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 0000000000000000000000000000000000000000..81432789c0babb2ce6b1c9e2304b9578de7f16ae
--- /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 <px4_tasks.h>
+//#include <systemlib/systemlib.h>
+
+
+__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 f9b92a2507e0f590ac676f6b999d98fc1cca70d7..0ac841b5f5d1e6bffd18a162a315d04132de3b11 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 0000000000000000000000000000000000000000..81e41ee7415a772e41238d61a923e4cb372e2ca2
--- /dev/null
+++ b/unittests/px4_log_stub.cpp
@@ -0,0 +1,14 @@
+#include <px4_log.h>
+
+
+__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");
+ }
+}