From e4f641e9b5aa9175ca322854b51569967288e200 Mon Sep 17 00:00:00 2001 From: Zachary Lowell Date: Fri, 9 Dec 2022 15:49:41 -0600 Subject: [PATCH] Qurt qshell implementation (#20736) --- boards/modalai/voxl2-slpi/default.px4board | 1 + boards/modalai/voxl2/default.px4board | 1 + msg/CMakeLists.txt | 2 + msg/QshellReq.msg | 5 + msg/QshellRetval.msg | 3 + platforms/qurt/CMakeLists.txt | 6 + platforms/qurt/src/px4/CMakeLists.txt | 1 + platforms/qurt/src/px4/main.cpp | 206 ++++++++++++++++++ src/drivers/qshell/Kconfig | 3 + src/drivers/qshell/posix/CMakeLists.txt | 41 ++++ src/drivers/qshell/posix/qshell.cpp | 132 +++++++++++ src/drivers/qshell/posix/qshell.h | 64 ++++++ .../qshell/posix/qshell_start_posix.cpp | 70 ++++++ src/drivers/qshell/qurt/CMakeLists.txt | 41 ++++ src/drivers/qshell/qurt/qshell.cpp | 183 ++++++++++++++++ src/drivers/qshell/qurt/qshell.h | 63 ++++++ src/drivers/qshell/qurt/qshell_start_qurt.cpp | 107 +++++++++ src/modules/muorb/apps/muorb_main.cpp | 10 +- .../muorb/apps/uORBAppsProtobufChannel.cpp | 19 +- .../muorb/slpi/uORBProtobufChannel.cpp | 14 ++ 20 files changed, 964 insertions(+), 8 deletions(-) create mode 100644 msg/QshellReq.msg create mode 100644 msg/QshellRetval.msg create mode 100644 platforms/qurt/src/px4/main.cpp create mode 100644 src/drivers/qshell/Kconfig create mode 100644 src/drivers/qshell/posix/CMakeLists.txt create mode 100644 src/drivers/qshell/posix/qshell.cpp create mode 100644 src/drivers/qshell/posix/qshell.h create mode 100644 src/drivers/qshell/posix/qshell_start_posix.cpp create mode 100644 src/drivers/qshell/qurt/CMakeLists.txt create mode 100644 src/drivers/qshell/qurt/qshell.cpp create mode 100644 src/drivers/qshell/qurt/qshell.h create mode 100644 src/drivers/qshell/qurt/qshell_start_qurt.cpp diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index ae489ecb2f..34700d7dc9 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -4,3 +4,4 @@ CONFIG_MODULES_MUORB_SLPI=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_ORB_COMMUNICATOR=y CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_DRIVERS_QSHELL_QURT=y diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index b3b6bac4ee..ce1f89146e 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -6,3 +6,4 @@ CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_ORB_COMMUNICATOR=y CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_DRIVERS_QSHELL_POSIX=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 18b3c8c0e6..2f6358f9eb 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -145,6 +145,8 @@ set(msg_files PpsCapture.msg PwmInput.msg Px4ioStatus.msg + QshellReq.msg + QshellRetval.msg RadioStatus.msg RateCtrlStatus.msg RcChannels.msg diff --git a/msg/QshellReq.msg b/msg/QshellReq.msg new file mode 100644 index 0000000000..d472e8dd33 --- /dev/null +++ b/msg/QshellReq.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +char[100] cmd +uint32 MAX_STRLEN = 100 +uint32 strlen +uint32 request_sequence diff --git a/msg/QshellRetval.msg b/msg/QshellRetval.msg new file mode 100644 index 0000000000..d42a7715ff --- /dev/null +++ b/msg/QshellRetval.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +int32 return_value +uint32 return_sequence diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index 446d0160d5..d85f3fcb7b 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -33,12 +33,18 @@ get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) +px4_qurt_generate_builtin_commands( + OUT ${PX4_BINARY_DIR}/apps + MODULE_LIST ${module_libraries} +) + include_directories( ${CMAKE_CURRENT_BINARY_DIR} ) add_library(px4 SHARED ${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c + ${PX4_BINARY_DIR}/apps.cpp ) target_link_libraries(px4 diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index d2f19d4917..286c6fa585 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -36,6 +36,7 @@ set(QURT_LAYER_SRCS drv_hrt.cpp tasks.cpp px4_qurt_impl.cpp + main.cpp ) add_library(px4_layer diff --git a/platforms/qurt/src/px4/main.cpp b/platforms/qurt/src/px4/main.cpp new file mode 100644 index 0000000000..1372fa9179 --- /dev/null +++ b/platforms/qurt/src/px4/main.cpp @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "apps.h" + +#define MAX_ARGS 8 // max number of whitespace separated args after app name + +__BEGIN_DECLS +int slpi_main(int argc, char *argv[]); +__END_DECLS + +static void run_cmd(apps_map_type &apps, const std::vector &appargs) +{ + // command is 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) + if (it->first == command) { + // one for command name, one for null terminator + const char *arg[MAX_ARGS + 2]; + + unsigned int i = 0; + + if (appargs.size() > MAX_ARGS + 1) { + PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1)); + return; + } + + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { + arg[i] = (char *)appargs[i].c_str(); + PX4_DEBUG(" arg%d = '%s'\n", i, arg[i]); + ++i; + } + + arg[i] = (char *)0; + + //PX4_DEBUG_PRINTF(i); + if (apps[command] == NULL) { + PX4_ERR("Null function !!\n"); + + } else { + apps[command](i, (char **)arg); + break; + } + + } +} + +void eat_whitespace(const char *&b, int &i) +{ + // Eat whitespace + while (b[i] == ' ' || b[i] == '\t') { ++i; } + + b = &b[i]; + i = 0; +} + +static void process_commands(apps_map_type &apps, const char *cmds) +{ + std::vector appargs; + int i = 0; + const char *b = cmds; + char arg[256]; + + // Eat leading whitespace + eat_whitespace(b, i); + + for (;;) { + // End of command line + if (b[i] == '\n' || b[i] == '\0') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs.push_back(arg); + + // If we have a command to run + if (appargs.size() > 0) { + PX4_DEBUG("Processing command: %s", appargs[0].c_str()); + + for (int ai = 1; ai < (int)appargs.size(); ai++) { + PX4_DEBUG(" > arg: %s", appargs[ai].c_str()); + } + + run_cmd(apps, appargs); + } + + appargs.clear(); + + if (b[i] == '\n') { + eat_whitespace(b, ++i); + continue; + + } else { + break; + } + } + + // End of arg + else if (b[i] == ' ') { + strncpy(arg, b, i); + arg[i] = '\0'; + appargs.push_back(arg); + eat_whitespace(b, ++i); + continue; + } + + ++i; + } +} + + +const char *get_commands() +{ + // 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; +} + + +int slpi_entry(int argc, char *argv[]) +{ + PX4_INFO("Inside slpi_entry"); + apps_map_type apps; + init_app_map(apps); + process_commands(apps, get_commands()); + sleep(1); // give time for all commands to execute before starting external function + + for (;;) { + sleep(1); + } + + return 0; +} + +static void usage() +{ + PX4_INFO("Usage: slpi {start |stop}"); +} + + +extern "C" { + + int slpi_main(int argc, char *argv[]) + { + int ret = 0; + + if (argc == 2 && strcmp(argv[1], "start") == 0) { + (void) px4_task_spawn_cmd("slpi", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + slpi_entry, + argv); + + } else { + usage(); + ret = -1; + } + + return ret; + } +} diff --git a/src/drivers/qshell/Kconfig b/src/drivers/qshell/Kconfig new file mode 100644 index 0000000000..36edef2887 --- /dev/null +++ b/src/drivers/qshell/Kconfig @@ -0,0 +1,3 @@ +menu "qshell" +rsource "*/Kconfig" +endmenu #qshell diff --git a/src/drivers/qshell/posix/CMakeLists.txt b/src/drivers/qshell/posix/CMakeLists.txt new file mode 100644 index 0000000000..874382e2dc --- /dev/null +++ b/src/drivers/qshell/posix/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2022 ModalAI, Inc. 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 drivers__qshell__posix + MAIN qshell + SRCS + qshell_start_posix.cpp + qshell.cpp + DEPENDS + ) diff --git a/src/drivers/qshell/posix/qshell.cpp b/src/drivers/qshell/posix/qshell.cpp new file mode 100644 index 0000000000..c9c223ed0b --- /dev/null +++ b/src/drivers/qshell/posix/qshell.cpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "qshell.h" + +#define QSHELL_RESPONSE_WAIT_TIME_US (20 * 1000000) // 20 sec, for temporary ESC calibration + +// Static variables +px4::AppState QShell::appState; +uint32_t QShell::_current_sequence{0}; + +int QShell::main(std::vector argList) +{ + 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("Command failed"); + return -1; + } + + return 0; +} + +int QShell::_send_cmd(std::vector &argList) +{ + // Let's use a sequence number to check if a return value belongs to the + // command that we just sent and not a previous one that we assumed that + // it had timed out. + ++_current_sequence; + + qshell_req_s qshell_req{}; + std::string cmd; + + for (size_t i = 0; i < argList.size(); i++) { + cmd += argList[i]; + + if (i < argList.size() - 1) { + cmd += " "; + } + } + + 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_INFO("Send cmd: '%s'", cmd.c_str()); + + qshell_req.strlen = cmd.size(); + strcpy((char *)qshell_req.cmd, cmd.c_str()); + qshell_req.request_sequence = _current_sequence; + qshell_req.timestamp = hrt_absolute_time(); + + _qshell_req_pub.publish(qshell_req); + + return 0; +} + +int QShell::_wait_for_retval() +{ + const hrt_abstime time_started_us = hrt_absolute_time(); + qshell_retval_s retval; + memset(&retval, 0, sizeof(qshell_retval_s)); + + while (hrt_elapsed_time(&time_started_us) < QSHELL_RESPONSE_WAIT_TIME_US) { + if (_qshell_retval_sub.update(&retval)) { + if (retval.return_sequence != _current_sequence) { + PX4_WARN("Ignoring return value with wrong sequence"); + + } else { + if (retval.return_value) { + PX4_INFO("cmd returned with: %d", retval.return_value); + } + + PX4_INFO("qshell return value timestamp: %lu, local time: %lu", retval.timestamp, hrt_absolute_time()); + + return retval.return_value; + } + } + + px4_usleep(1000); + } + + PX4_ERR("command timed out"); + return -1; +} diff --git a/src/drivers/qshell/posix/qshell.h b/src/drivers/qshell/posix/qshell.h new file mode 100644 index 0000000000..877b13de52 --- /dev/null +++ b/src/drivers/qshell/posix/qshell.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +class QShell +{ +public: + QShell() = default; + ~QShell() = default; + + int main(std::vector argList); + + static px4::AppState appState; /* track requests to terminate app */ + +private: + int _send_cmd(std::vector &argList); + int _wait_for_retval(); + + uORB::Publication _qshell_req_pub{ORB_ID(qshell_req)}; + + uORB::Subscription _qshell_retval_sub{ORB_ID(qshell_retval)}; + + static uint32_t _current_sequence; +}; diff --git a/src/drivers/qshell/posix/qshell_start_posix.cpp b/src/drivers/qshell/posix/qshell_start_posix.cpp new file mode 100644 index 0000000000..24c38a830f --- /dev/null +++ b/src/drivers/qshell/posix/qshell_start_posix.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +#include "qshell.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//using namespace px4; + +extern "C" __EXPORT int qshell_main(int argc, char *argv[]); + +static void usage() +{ + PX4_DEBUG("usage: qshell cmd [args]"); +} + +int qshell_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + std::vector argList; + + for (int i = 1; i < argc; i++) { + argList.push_back(argv[i]); + } + + + QShell qshell; + return qshell.main(argList); +} diff --git a/src/drivers/qshell/qurt/CMakeLists.txt b/src/drivers/qshell/qurt/CMakeLists.txt new file mode 100644 index 0000000000..60ce10effa --- /dev/null +++ b/src/drivers/qshell/qurt/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2022 ModalAI, Inc. 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 drivers__qshell__qurt + MAIN qshell + SRCS + qshell_start_qurt.cpp + qshell.cpp + DEPENDS + ) diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp new file mode 100644 index 0000000000..fafe04136d --- /dev/null +++ b/src/drivers/qshell/qurt/qshell.cpp @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +#include "qshell.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define MAX_ARGS 8 // max number of whitespace separated args after app name + +px4::AppState QShell::appState; + +QShell::QShell() +{ + PX4_INFO("Init app map initialized"); + init_app_map(m_apps); +} + +int QShell::main() +{ + appState.setRunning(true); + + usleep(2000); + + int sub_qshell_req = orb_subscribe(ORB_ID(qshell_req)); + + if (sub_qshell_req == PX4_ERROR) { + PX4_ERR("Error subscribing to qshell_req topic"); + return -1; + } + + px4_pollfd_struct_t fds[1] = {}; + fds[0].fd = sub_qshell_req; + fds[0].events = POLLIN; + + while (!appState.exitRequested()) { + + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret > 0 && fds[0].revents & POLLIN) { + + orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req); + + PX4_INFO("qshell gotten: %s", m_qshell_req.cmd); + + char current_char; + std::string arg; + std::vector appargs; + + for (unsigned str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) { + current_char = m_qshell_req.cmd[str_idx]; + + 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 + + qshell_retval_s retval{}; + retval.return_value = run_cmd(appargs); + retval.return_sequence = m_qshell_req.request_sequence; + + if (retval.return_value) { + PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd); + + } else { + PX4_INFO("Ok executing command: %s", m_qshell_req.cmd); + } + + retval.timestamp = hrt_absolute_time(); + _qshell_retval_pub.publish(retval); + + } else if (pret == 0) { + + // Timing out is fine. + } else { + // Something is wrong. + usleep(10000); + } + } + + appState.setRunning(false); + return 0; +} + +int QShell::run_cmd(const std::vector &appargs) +{ + // command is appargs[0] + std::string command = appargs[0]; + + if (command.compare("help") == 0) { + list_builtins(m_apps); + return 0; + } + + //replaces app.find with iterator code to avoid null pointer exception + for (apps_map_type::iterator it = m_apps.begin(); it != m_apps.end(); ++it) { + if (it->first == command) { + // one for command name, one for null terminator + const char *arg[MAX_ARGS + 2]; + + unsigned int i = 0; + + if (appargs.size() > MAX_ARGS + 1) { + PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1)); + return 1; + } + + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { + arg[i] = (char *)appargs[i].c_str(); + PX4_INFO(" arg%d = '%s'\n", i, arg[i]); + ++i; + } + + arg[i] = (char *)0; + + //PX4_DEBUG_PRINTF(i); + if (m_apps[command] == NULL) { + PX4_ERR("Null function !!\n"); + + } else { + int x = m_apps[command](i, (char **)arg); + return x; + } + + } + } + + PX4_ERR("Command %s not found", command.c_str()); + return 1; +} diff --git a/src/drivers/qshell/qurt/qshell.h b/src/drivers/qshell/qurt/qshell.h new file mode 100644 index 0000000000..77cd1be8ae --- /dev/null +++ b/src/drivers/qshell/qurt/qshell.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "apps.h" + +class QShell +{ +public: + QShell(); + ~QShell() {} + + int main(); + int run_cmd(const std::vector &appargs); + + static px4::AppState appState; /* track requests to terminate app */ + +private: + + uORB::Publication _qshell_retval_pub{ORB_ID(qshell_retval)}; + + qshell_req_s m_qshell_req{}; + + apps_map_type m_apps; +}; diff --git a/src/drivers/qshell/qurt/qshell_start_qurt.cpp b/src/drivers/qshell/qurt/qshell_start_qurt.cpp new file mode 100644 index 0000000000..34862ee473 --- /dev/null +++ b/src/drivers/qshell/qurt/qshell_start_qurt.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +#include "qshell.h" +#include +#include +#include +#include +#include +#include + +static int daemon_task; /* Handle of deamon task / thread */ + +extern "C" __EXPORT int qshell_main(int argc, char *argv[]); + +int qshell_entry(int argc, char **argv) +{ + PX4_INFO("qshell entry....."); + QShell qshell; + qshell.main(); + + PX4_INFO("goodbye"); + return 0; +} + +static void usage() +{ + PX4_INFO("usage: qshell {start|stop|status}"); +} + +int qshell_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (QShell::appState.isRunning()) { + PX4_INFO("already running"); + /* this is not an error */ + return 0; + } + + PX4_INFO("before starting the qshell_entry task"); + + daemon_task = px4_task_spawn_cmd("qshell", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 8192, + qshell_entry, + (char *const *)argv); + PX4_INFO("after starting the qshell_entry task"); + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + QShell::appState.requestExit(); + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (QShell::appState.isRunning()) { + PX4_INFO("is running"); + + } else { + PX4_INFO("not started"); + } + + return 0; + } + + usage(); + return 1; +} diff --git a/src/modules/muorb/apps/muorb_main.cpp b/src/modules/muorb/apps/muorb_main.cpp index 256a46bb55..6ad21b4a53 100644 --- a/src/modules/muorb/apps/muorb_main.cpp +++ b/src/modules/muorb/apps/muorb_main.cpp @@ -57,6 +57,8 @@ muorb_main(int argc, char *argv[]) // Register the protobuf channel with UORB. uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); + PX4_INFO("Got muorb start command"); + if (channel && channel->Initialize(enable_debug)) { uORB::Manager::get_instance()->set_uorb_communicator(channel); return OK; @@ -65,7 +67,13 @@ muorb_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "test")) { uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); - if (channel && channel->Initialize(enable_debug) && channel->Test()) { return OK; } + PX4_INFO("Got muorb test command"); + + if (channel && channel->Initialize(enable_debug)) { + uORB::Manager::get_instance()->set_uorb_communicator(channel); + + if (channel->Test()) { return OK; } + } } else if (!strcmp(argv[1], "stop")) { if (uORB::AppsProtobufChannel::isInstance() == false) { diff --git a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp index 4687f4b37b..b57bcf81e5 100644 --- a/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp +++ b/src/modules/muorb/apps/uORBAppsProtobufChannel.cpp @@ -219,16 +219,21 @@ bool uORB::AppsProtobufChannel::Test() bool uORB::AppsProtobufChannel::Initialize(bool enable_debug) { - fc_callbacks cb = {&ReceiveCallback, &AdvertiseCallback, - &SubscribeCallback, &UnsubscribeCallback - }; + if (! _Initialized) { + fc_callbacks cb = { &ReceiveCallback, &AdvertiseCallback, + &SubscribeCallback, &UnsubscribeCallback + }; - if (fc_sensor_initialize(enable_debug, &cb) != 0) { - if (enable_debug) { PX4_INFO("Warning: muorb protobuf initalize method failed"); } + if (fc_sensor_initialize(enable_debug, &cb) != 0) { + if (enable_debug) { PX4_INFO("Warning: muorb protobuf initalize method failed"); } + + } else { + PX4_INFO("muorb protobuf initalize method succeeded"); + _Initialized = true; + } } else { - PX4_INFO("muorb protobuf initalize method succeeded"); - _Initialized = true; + PX4_INFO("AppsProtobufChannel already initialized"); } return true; diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index fae8900dcb..ee16f0bafc 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -192,6 +192,10 @@ static void *test_runner(void *) return nullptr; } +__BEGIN_DECLS +extern int slpi_main(int argc, char *argv[]); +__END_DECLS + int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) { hrt_set_absolute_time_offset(clock_offset_us); @@ -226,6 +230,16 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) uORB::Manager::get_instance()->set_uorb_communicator( uORB::ProtobufChannel::GetInstance()); + const char *argv[3] = { "slpi", "start" }; + int argc = 2; + + // Make sure that argv has a NULL pointer in the end. + argv[argc] = NULL; + + if (slpi_main(argc, (char **) argv)) { + PX4_ERR("slpi failed in %s", __FUNCTION__); + } + px4muorb_orb_initialized = true; if (_px4_muorb_debug) { PX4_INFO("px4muorb_orb_initialize called"); }