Qurt qshell implementation (#20736)

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

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

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

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

msg/QshellRetval.msg

@@ -0,0 +1,3 @@
+uint64 timestamp		# time since system start (microseconds)
+int32 return_value
+uint32 return_sequence

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

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

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 <px4_platform_common/tasks.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/defines.h>
+#include <vector>
+#include <string>
+#include <map>
+#include <stdio.h>
+#include <stdlib.h>
+
+#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<std::string> &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<std::string> 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;
+	}
+}

src/drivers/qshell/Kconfig

@@ -0,0 +1,3 @@
+menu "qshell"
+rsource "*/Kconfig"
+endmenu #qshell

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
+	)

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 <px4_platform_common/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"
+
+#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<std::string> 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<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;
+
+	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;
+}

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 <px4_platform_common/app.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/qshell_req.h>
+#include <uORB/topics/qshell_retval.h>
+
+#include <vector>
+#include <string>
+
+class QShell
+{
+public:
+	QShell() = default;
+	~QShell() = default;
+
+	int main(std::vector<std::string> argList);
+
+	static px4::AppState appState; /* track requests to terminate app */
+
+private:
+	int _send_cmd(std::vector<std::string> &argList);
+	int _wait_for_retval();
+
+	uORB::Publication<qshell_req_s>	_qshell_req_pub{ORB_ID(qshell_req)};
+
+	uORB::Subscription		_qshell_retval_sub{ORB_ID(qshell_retval)};
+
+	static uint32_t			_current_sequence;
+};

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 <px4_platform_common/log.h>
+#include <px4_platform_common/app.h>
+#include <px4_platform_common/tasks.h>
+#include <stdio.h>
+#include <string.h>
+#include <sched.h>
+#include <cstdlib>
+#include <string>
+#include <vector>
+
+//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<std::string> argList;
+
+	for (int i = 1; i < argc; i++) {
+		argList.push_back(argv[i]);
+	}
+
+
+	QShell qshell;
+	return qshell.main(argList);
+}

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
+	)

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 <px4_platform_common/log.h>
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/defines.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <uORB/topics/qshell_retval.h>
+#include <drivers/drv_hrt.h>
+
+#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<std::string> 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<std::string> &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;
+}

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 <px4_platform_common/app.h>
+#include <string>
+#include <vector>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/qshell_retval.h>
+#include <uORB/topics/qshell_req.h>
+
+#include "apps.h"
+
+class QShell
+{
+public:
+	QShell();
+	~QShell() {}
+
+	int main();
+	int run_cmd(const std::vector<std::string> &appargs);
+
+	static px4::AppState appState; /* track requests to terminate app */
+
+private:
+
+	uORB::Publication<qshell_retval_s>	_qshell_retval_pub{ORB_ID(qshell_retval)};
+
+	qshell_req_s m_qshell_req{};
+
+	apps_map_type m_apps;
+};

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 <px4_platform_common/log.h>
+#include <px4_platform_common/app.h>
+#include <px4_platform_common/tasks.h>
+#include <stdio.h>
+#include <string.h>
+#include <sched.h>
+
+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;
+}

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) {

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;

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"); }