Qurt specific drivers for testing and some updates to the startup process (#20917)

* Added a couple of Qurt specific drivers for testing and some updates to Qurt startup code

boards/modalai/voxl2-slpi/src/CMakeLists.txt

@@ -44,3 +44,5 @@ add_library(drivers_board
 
 # Add custom drivers for SLPI
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)

boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2022 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.
+#
+############################################################################
+
+message(STATUS "Mavlink include directory: ${PX4_SOURCE_DIR}/../build/modalai_voxl2_default/mavlink/standard")
+
+px4_add_module(
+	MODULE drivers__modalai__mavlink_rc_in
+	MAIN mavlink_rc_in
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+		-Wno-address-of-packed-member # TODO: fix in c_library_v2
+	INCLUDES
+		${PX4_SOURCE_DIR}/src/drivers/rc_input
+		${PX4_SOURCE_DIR}/build/modalai_voxl2_default/mavlink/common
+	SRCS
+		mavlink_rc_in.cpp
+	DEPENDS
+		px4_work_queue
+	)

boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp

@@ -0,0 +1,499 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#include <string>
+#include <stdint.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/getopt.h>
+#include <drivers/device/qurt/uart.h>
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/input_rc.h>
+#include "uORB/uORBManager.hpp"
+#include <mavlink.h>
+#include <px4_log.h>
+#include <lib/rc/crsf.h>
+#include <lib/perf/perf_counter.h>
+
+
+#define ASYNC_UART_READ_WAIT_US 500
+#define RC_INPUT_RSSI_MAX	100
+
+#define TX_BUFFER_LEN 20
+
+extern "C" { __EXPORT int mavlink_rc_in_main(int argc, char *argv[]); }
+
+namespace mavlink_rc_in
+{
+
+static bool _is_running = false;
+volatile bool _task_should_exit = false;
+
+static px4_task_t _task_handle = -1;
+static px4_task_t _mav_task_handle = -1;
+
+static int _uart_fd = -1;
+
+static bool debug = false;
+static bool mav_en = true;
+static bool crsf_en = false;
+static bool fake_heartbeat_enable = true;
+static bool filter_local_rc_messages = true;
+static bool dump_received_messages = false;
+static bool dump_transmitted_messages = false;
+
+std::string port = "7";
+uint32_t baudrate = 115200;
+
+uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
+
+perf_counter_t	_perf_rx_rate = nullptr;
+
+static uint8_t _rc_lq;
+static uint8_t _rc_rssi_dbm;
+
+int open_port(const char *dev, speed_t speed);
+int close_port();
+
+int write_response(void *buf, size_t len);
+
+int start(int argc, char *argv[]);
+int stop();
+int status();
+
+void usage();
+void task_main(int argc, char *argv[]);
+
+void handle_message_dsp(mavlink_message_t *msg);
+void handle_message_rc_channels_override_dsp(mavlink_message_t *msg);
+
+void handle_message_dsp(mavlink_message_t *msg)
+{
+	if (debug) {
+		if (filter_local_rc_messages) {
+			if ((msg->msgid != MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE) &&
+			    (msg->msgid != MAVLINK_MSG_ID_RADIO_STATUS)) {
+				PX4_INFO("**^^ MSG ID: %d ^^*****************************************************", msg->msgid);
+			}
+
+		} else {
+			PX4_INFO("msg ID: %d", msg->msgid);
+		}
+	}
+
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
+		handle_message_rc_channels_override_dsp(msg);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void mavlink_out_task(int argc, char *argv[])
+{
+	uint64_t last_heartbeat_timestamp = hrt_absolute_time();
+
+	while (! _task_should_exit) {
+		uint64_t timestamp = hrt_absolute_time();
+		int nwrite = 0;
+
+		// We need to provide heartbeat messages to the receiver. Otherwise it
+		// won't send us anything!
+		if ((timestamp - last_heartbeat_timestamp) > 1000000) {
+			mavlink_heartbeat_t hb = {};
+			mavlink_message_t hb_message = {};
+			hb.type = MAV_TYPE_QUADROTOR;
+			hb.autopilot = MAV_AUTOPILOT_PX4;
+			mavlink_msg_heartbeat_encode(1, 1, &hb_message, &hb);
+
+			uint8_t  hb_newBuf[MAVLINK_MAX_PACKET_LEN];
+			uint16_t hb_newBufLen = 0;
+			hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
+
+			nwrite = qurt_uart_write(_uart_fd, (char *) &hb_newBuf[0], hb_newBufLen);
+
+			if (nwrite != hb_newBufLen) {
+				PX4_ERR("Heartbeat write failed. Expected %d, got %d", hb_newBufLen, nwrite);
+
+			} else if (debug) {
+				PX4_INFO("Sending local heartbeat to TBS");
+
+				if (dump_transmitted_messages) {
+					for (int i = 0; i <= hb_newBufLen; i += 16) {
+						PX4_INFO("  %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x",
+							 hb_newBuf[i + 0],  hb_newBuf[i + 1],  hb_newBuf[i + 2],  hb_newBuf[i + 3],
+							 hb_newBuf[i + 4],  hb_newBuf[i + 5],  hb_newBuf[i + 6],  hb_newBuf[i + 7],
+							 hb_newBuf[i + 8],  hb_newBuf[i + 9],  hb_newBuf[i + 10], hb_newBuf[i + 11],
+							 hb_newBuf[i + 12], hb_newBuf[i + 13], hb_newBuf[i + 14], hb_newBuf[i + 15]);
+					}
+				}
+			}
+
+			last_heartbeat_timestamp = timestamp;
+		}
+
+		px4_usleep(100000);
+	}
+}
+
+void task_main(int argc, char *argv[])
+{
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "rtdmclfp:b:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			debug = true;
+			PX4_INFO("Setting debug flag on");
+			break;
+
+		case 'm':
+			mav_en = true;
+			crsf_en = false;
+
+			if (debug) { PX4_INFO("Using MAVLink mode"); }
+
+			break;
+
+		case 'c':
+			mav_en = false;
+			crsf_en = true;
+
+			if (debug) { PX4_INFO("Using CRSF mode"); }
+
+			break;
+
+		case 'p':
+			port = myoptarg;
+
+			if (debug) { PX4_INFO("Setting port to %s", port.c_str()); }
+
+			break;
+
+		case 'b':
+			baudrate = atoi(myoptarg);
+
+			if (debug) { PX4_INFO("Setting baudrate to %u", baudrate); }
+
+			break;
+
+		case 'f':
+			fake_heartbeat_enable = false;
+
+			if (debug) { PX4_INFO("Disabling fake heartbeats"); }
+
+			break;
+
+		case 'l':
+			filter_local_rc_messages = true;
+
+			if (debug) { PX4_INFO("Filtering debug messages from the RC receiver"); }
+
+			break;
+
+		case 'r':
+			dump_received_messages = true;
+
+			if (debug) { PX4_INFO("Enabling hex dump of received messages"); }
+
+			break;
+
+		case 't':
+			dump_transmitted_messages = true;
+
+			if (debug) { PX4_INFO("Enabling hex dump of transmitted messages"); }
+
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	if (open_port(port.c_str(), (speed_t) baudrate) == -1) {
+		PX4_ERR("Failed to open UART");
+		return;
+	}
+
+	_mav_task_handle = px4_task_spawn_cmd("mavlink_rc_in_mav",
+					      SCHED_DEFAULT,
+					      SCHED_PRIORITY_DEFAULT,
+					      2000,
+					      (px4_main_t)&mavlink_out_task,
+					      (char *const *)argv);
+
+	if (_mav_task_handle < 0) {
+		PX4_ERR("_mav_task_handle task start failed");
+		return;
+	}
+
+	_is_running = true;
+	_perf_rx_rate = perf_alloc(PC_INTERVAL, "mavlink_rc_in: rx interval");
+
+	uint8_t rx_buf[1024];
+	mavlink_message_t msg;
+	mavlink_status_t status{};
+
+	while (! _task_should_exit) {
+		// Check for incoming messages from the TBS Crossfire receiver
+		int nread = qurt_uart_read(_uart_fd, (char *) rx_buf, sizeof(rx_buf), ASYNC_UART_READ_WAIT_US);
+
+		if (nread) {
+			if (debug) {
+				PX4_INFO("TBS Crossfire (MAVLink mode): read %d bytes", nread);
+
+				if (dump_received_messages) {
+					for (int i = 0; i <= nread; i += 16) {
+						PX4_INFO("  %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x %.2x",
+							 rx_buf[i + 0],  rx_buf[i + 1],  rx_buf[i + 2],  rx_buf[i + 3],
+							 rx_buf[i + 4],  rx_buf[i + 5],  rx_buf[i + 6],  rx_buf[i + 7],
+							 rx_buf[i + 8],  rx_buf[i + 9],  rx_buf[i + 10], rx_buf[i + 11],
+							 rx_buf[i + 12], rx_buf[i + 13], rx_buf[i + 14], rx_buf[i + 15]);
+					}
+				}
+			}
+
+			//Take buffer and convert it into mavlink msg
+			for (int i = 0; i < nread; i++) {
+				if (mavlink_parse_char(MAVLINK_COMM_0, rx_buf[i], &msg, &status)) {
+					handle_message_dsp(&msg);
+				}
+			}
+		}
+
+		px4_usleep(5000);
+	}
+}
+
+void handle_message_rc_channels_override_dsp(mavlink_message_t *msg)
+{
+	mavlink_rc_channels_override_t man;
+	mavlink_msg_rc_channels_override_decode(msg, &man);
+
+	// if (debug) PX4_INFO("RC channels override msg received");
+
+	// Check target
+	if (man.target_system != 0) {
+		PX4_ERR("Message has incorrect target system %u", man.target_system);
+		return;
+	}
+
+	// fill uORB message
+	input_rc_s rc{};
+
+	// metadata
+	rc.timestamp = hrt_absolute_time();
+	rc.timestamp_last_signal = rc.timestamp;
+	rc.rssi = RC_INPUT_RSSI_MAX;
+	rc.rc_failsafe = false;
+	rc.rc_lost = false;
+	rc.rc_lost_frame_count = 0;
+	rc.rc_total_frame_count = 1;
+	rc.rc_ppm_frame_length = 0;
+	rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
+
+	// channels
+	rc.values[0] = man.chan1_raw;
+	rc.values[1] = man.chan2_raw;
+	rc.values[2] = man.chan3_raw;
+	rc.values[3] = man.chan4_raw;
+	rc.values[4] = man.chan5_raw;
+	rc.values[5] = man.chan6_raw;
+	rc.values[6] = man.chan7_raw;
+	rc.values[7] = man.chan8_raw;
+	rc.values[8] = man.chan9_raw;
+	rc.values[9] = man.chan10_raw;
+	rc.values[10] = man.chan11_raw;
+	rc.values[11] = man.chan12_raw;
+	rc.values[12] = man.chan13_raw;
+	rc.values[13] = man.chan14_raw;
+	rc.values[14] = man.chan15_raw;
+	rc.values[15] = man.chan16_raw;
+	rc.values[16] = man.chan17_raw;
+	rc.values[17] = man.chan18_raw;
+
+	// check how many channels are valid
+	for (int i = 17; i >= 0; i--) {
+		const bool ignore_max = rc.values[i] == UINT16_MAX; // ignore any channel with value UINT16_MAX
+		const bool ignore_zero = (i > 7) && (rc.values[i] == 0); // ignore channel 8-18 if value is 0
+
+		if (ignore_max || ignore_zero) {
+			// set all ignored values to zero
+			rc.values[i] = 0;
+
+		} else {
+			// first channel to not ignore -> set count considering zero-based index
+			rc.channel_count = i + 1;
+			break;
+		}
+	}
+
+	// publish uORB message
+	_rc_pub.publish(rc);
+}
+
+int open_port(const char *dev, speed_t speed)
+{
+	if (_uart_fd >= 0) {
+		PX4_ERR("Port already in use: %s", dev);
+		return -1;
+	}
+
+	_uart_fd = qurt_uart_open(dev, speed);
+
+	if (_uart_fd < 0) {
+		PX4_ERR("Error opening port: %s (%i)", dev, errno);
+		return -1;
+
+	} else if (debug) { PX4_INFO("qurt uart opened successfully"); }
+
+	return 0;
+}
+
+int close_port()
+{
+	_uart_fd = -1;
+
+	return 0;
+}
+
+int write_response(void *buf, size_t len)
+{
+	if (_uart_fd < 0 || buf == NULL) {
+		PX4_ERR("invalid state for writing or buffer");
+		return -1;
+	}
+
+	return qurt_uart_write(_uart_fd, (const char *) buf, len);
+}
+
+int start(int argc, char *argv[])
+{
+	if (_is_running) {
+		PX4_WARN("already running");
+		return -1;
+	}
+
+	_task_should_exit = false;
+
+	_task_handle = px4_task_spawn_cmd("mavlink_rc_in_main",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_DEFAULT,
+					  2000,
+					  (px4_main_t)&task_main,
+					  (char *const *)argv);
+
+	if (_task_handle < 0) {
+		PX4_ERR("task start failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+int stop()
+{
+	if (!_is_running) {
+		PX4_WARN("not running");
+		return -1;
+	}
+
+	_task_should_exit = true;
+
+	while (_is_running) {
+		usleep(200000);
+		PX4_INFO(".");
+	}
+
+	_task_handle = -1;
+	return 0;
+}
+
+int status()
+{
+	PX4_INFO("running: %s", _is_running ? "yes" : "no");
+	PX4_INFO("");
+	PX4_INFO(" RSSI: %i", _rc_rssi_dbm);
+	PX4_INFO(" LQ:   %i", _rc_lq);
+	PX4_INFO("");
+	perf_print_counter(_perf_rx_rate);
+	return 0;
+}
+
+void
+usage()
+{
+	PX4_INFO("Usage: mavlink_rc_in {start|info|stop} [options]");
+	PX4_INFO("Options: -d             enable debug messages");
+	PX4_INFO("         -p <number>    uart port number");
+	PX4_INFO("         -b <number>    uart baudrate");
+	PX4_INFO("         -c             use CRSF protocol over the wire");
+	PX4_INFO("         -m             use MAVLink protocol over the wire");
+	PX4_INFO("         -f             disable fake hearbeats");
+	PX4_INFO("         -l             filter out local rc received debug messages");
+	PX4_INFO("         -r             enable received messages hex dump");
+	PX4_INFO("         -t             enable transmitted messages hex dump");
+}
+
+}
+
+int mavlink_rc_in_main(int argc, char *argv[])
+{
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		mavlink_rc_in::usage();
+		return -1;
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return mavlink_rc_in::start(argc - 1, argv + 1);
+
+	} else if (!strcmp(verb, "stop")) {
+		return mavlink_rc_in::stop();
+
+	} else if (!strcmp(verb, "status")) {
+		return mavlink_rc_in::status();
+
+	} else {
+		mavlink_rc_in::usage();
+		return -1;
+	}
+}

boards/modalai/voxl2-slpi/src/drivers/rc_controller/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2021 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 drivers__modalai__rc_controller
+	MAIN rc_controller
+	SRCS
+		rc_controller.cpp
+	)

boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.cpp

@@ -0,0 +1,256 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2019 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 px4_simple_app.c
+ * Minimal application example for PX4 autopilot
+ *
+ * @author Example User <mail@example.com>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/posix.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+
+#include <px4_platform_common/getopt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/input_rc.h>
+
+#include "rc_controller.hpp"
+
+int RC_ControllerModule::print_status()
+{
+	PX4_INFO("Running");
+	// TODO: print additional runtime information about the state of the module
+
+	return 0;
+}
+
+int RC_ControllerModule::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		print_usage("not running");
+		return 1;
+	}
+
+	if (!strcmp(argv[0], "throttle")) {
+		uint16_t val = atoi(argv[1]);
+		get_instance()->set_throttle(val);
+		PX4_INFO("Setting throttle to %u", val);
+		return 0;
+	}
+
+	if (!strcmp(argv[0], "yaw")) {
+		uint16_t val = atoi(argv[1]);
+		get_instance()->set_yaw(val);
+		PX4_INFO("Setting yaw to %u", val);
+		return 0;
+	}
+
+	if (!strcmp(argv[0], "pitch")) {
+		uint16_t val = atoi(argv[1]);
+		get_instance()->set_pitch(val);
+		PX4_INFO("Setting pitch to %u", val);
+		return 0;
+	}
+
+	if (!strcmp(argv[0], "roll")) {
+		uint16_t val = atoi(argv[1]);
+		get_instance()->set_roll(val);
+		PX4_INFO("Setting roll to %u", val);
+		return 0;
+	}
+
+	return print_usage("unknown command");
+}
+
+
+int RC_ControllerModule::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("RC_ControllerModule",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_MAX,
+				      1024,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+RC_ControllerModule *RC_ControllerModule::instantiate(int argc, char *argv[])
+{
+	// int example_param = 0;
+	// bool example_flag = false;
+	// bool error_flag = false;
+	//
+	// int myoptind = 1;
+	// int ch;
+	// const char *myoptarg = nullptr;
+	//
+	// // parse CLI arguments
+	// while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) {
+	// 	switch (ch) {
+	// 	case 'p':
+	// 		example_param = (int)strtol(myoptarg, nullptr, 10);
+	// 		break;
+	//
+	// 	case 'f':
+	// 		example_flag = true;
+	// 		break;
+	//
+	// 	case '?':
+	// 		error_flag = true;
+	// 		break;
+	//
+	// 	default:
+	// 		PX4_WARN("unrecognized flag");
+	// 		error_flag = true;
+	// 		break;
+	// 	}
+	// }
+	//
+	// if (error_flag) {
+	// 	return nullptr;
+	// }
+
+	// RC_ControllerModule *instance = new RC_ControllerModule(example_param, example_flag);
+	RC_ControllerModule *instance = new RC_ControllerModule();
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+	}
+
+	return instance;
+}
+
+// RC_ControllerModule::RC_ControllerModule(int example_param, bool example_flag)
+RC_ControllerModule::RC_ControllerModule()
+	: ModuleParams(nullptr)
+{
+	_throttle = 800;
+	// _yaw      = 1500;
+	// _pitch    = 1500;
+	// _roll     = 1500;
+	_yaw      = 800;
+	_pitch    = 800;
+	_roll     = 800;
+}
+
+void RC_ControllerModule::run()
+{
+	PX4_INFO("ModalAI RC_ControllerModule starting");
+
+	input_rc_s rc_data;
+	memset(&rc_data, 0, sizeof(input_rc_s));
+	orb_advert_t input_rc_pub_fd = orb_advertise(ORB_ID(input_rc), &rc_data);
+
+	while (!should_exit()) {
+
+		usleep(10000);
+
+		rc_data.input_source = input_rc_s::RC_INPUT_SOURCE_QURT;
+		rc_data.channel_count = 4;
+		// for (unsigned i = 0; i < rc_data.channel_count; ++i) {
+		// 	rc_data.values[i] = 1024;
+		// }
+		rc_data.values[0] = _throttle;
+		rc_data.values[1] = _yaw;
+		rc_data.values[2] = _pitch;
+		rc_data.values[3] = _roll;
+
+		rc_data.timestamp = hrt_absolute_time();
+		rc_data.timestamp_last_signal = rc_data.timestamp;
+		rc_data.rc_ppm_frame_length = 0;
+		rc_data.rssi = 100;
+		rc_data.rc_failsafe = false;
+		rc_data.rc_lost = 0;
+		rc_data.rc_lost_frame_count = 0;
+		rc_data.rc_total_frame_count = 0;
+
+		orb_publish(ORB_ID(input_rc), input_rc_pub_fd, &rc_data);
+
+		parameters_update();
+	}
+
+	PX4_INFO("RC_ControllerModule Exiting");
+}
+
+void RC_ControllerModule::parameters_update(bool force)
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s update;
+		_parameter_update_sub.copy(&update);
+
+		// update parameters from storage
+		updateParams();
+	}
+}
+
+int RC_ControllerModule::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	// TODO: PRINT_MODULE_DESCRIPTION
+
+	PRINT_MODULE_USAGE_NAME("rc_controller", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("rc");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+int rc_controller_main(int argc, char *argv[])
+{
+	return RC_ControllerModule::main(argc, argv);
+}

boards/modalai/voxl2-slpi/src/drivers/rc_controller/rc_controller.hpp

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+
+extern "C" __EXPORT int rc_controller_main(int argc, char *argv[]);
+
+class RC_ControllerModule : public ModuleBase<RC_ControllerModule>, public ModuleParams
+{
+public:
+	RC_ControllerModule();
+
+	virtual ~RC_ControllerModule() = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static RC_ControllerModule *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void set_throttle(uint16_t val) { _throttle = val; }
+	void set_yaw(uint16_t val) { _yaw = val; }
+	void set_pitch(uint16_t val) { _pitch = val; }
+	void set_roll(uint16_t val) { _roll = val; }
+
+private:
+
+	/**
+	 * Check for parameter changes and update them if needed.
+	 * @param parameter_update_sub uorb subscription to parameter_update
+	 * @param force for a parameter update
+	 */
+	void parameters_update(bool force = false);
+
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
+		(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */
+	)
+
+	uint16_t _throttle;
+	uint16_t _yaw;
+	uint16_t _pitch;
+	uint16_t _roll;
+
+	// Subscriptions
+	// uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1000000};
+
+};

platforms/common/work_queue/work_thread.c

@@ -107,6 +107,14 @@ static void work_process(struct wqueue_s *wqueue, int lock_id)
 
 	next  = CONFIG_SCHED_WORKPERIOD;
 
+#ifdef __PX4_QURT
+	// In Posix certain signals wake up a sleeping thread but it isn't the case
+	// with the Qurt POSIX implementation. So rather than assume we can come out
+	// of the sleep early by a signal we just wake up more often.
+	next = 1000;
+#endif
+
+
 	work_lock(lock_id);
 
 	work  = (struct work_s *)wqueue->q.head;

platforms/qurt/src/px4/main.cpp

@@ -164,6 +164,8 @@ const char *get_commands()
 int slpi_entry(int argc, char *argv[])
 {
 	PX4_INFO("Inside slpi_entry");
+	sleep(1); // give time for apps side to finish initialization
+
 	apps_map_type apps;
 	init_app_map(apps);
 	process_commands(apps, get_commands());

platforms/qurt/src/px4/tasks.cpp

@@ -70,6 +70,35 @@ static pthread_mutex_t task_mutex;
 extern "C" {
 
 	int pthread_setname_np(pthread_t __target_thread, const char *__name) {	return 0; }
+
+	// This function is in pthread.h but is not, apparently, defined in the
+	// Qurt system image. So, it is being defined here. Unfortunately it doesn't
+	// seem to work. It looks like the Qurt pthread implementation likes to allocate
+	// the stack itself and not give us that ability.
+	// int pthread_attr_setstackaddr(pthread_attr_t *attr, void * stackaddr) {
+	// if (attr == NULL) return -1;
+	// if (stackaddr == NULL) return -1;
+	// attr->stackaddr = stackaddr;
+	// attr->internal_stack = 0;
+	// return 0;
+	// }
+
+	// This function is in pthread.h but is not, apparently, defined in the
+	// Qurt system image. So, it is being defined here.
+	int pthread_attr_setthreadname(pthread_attr_t *attr, const char *name)
+	{
+		if (attr == NULL) { return -1; }
+
+		if (&attr->name[0] == NULL) { return -1; }
+
+		if (name == NULL) { return -1; }
+
+		memcpy(attr->name, name, PTHREAD_NAME_LEN);
+		attr->name[PTHREAD_NAME_LEN - 1] = 0;
+		return 0;
+	}
+
+	// Qurt only has one scheduling policy so this just returns a success
 	int pthread_attr_setschedpolicy(pthread_attr_t *attr, int policy) { return 0; }
 
 }
@@ -172,11 +201,30 @@ static px4_task_t px4_task_spawn_internal(const char *name, int priority, px4_ma
 	strcpy(&taskmap[task_index].name[4], name);
 
 	struct sched_param param;
+
+	// Qurt threads have different priority numbers. 1 is the highest
+	// priority and 255 is the lowest. But we are using the pthread
+	// implementation on Qurt which returns 255 when you call sched_get_priority_max.
+	// However, the Qurt pthread implementation deals with this properly when
+	// creating the underlying Qurt task by mapping the high number into a low number.
+
+	// For high priorities bump everything down a little so that critical Qurt
+	// threads are not impacted.
+	if (priority > 128) {
+		priority -= 16;
+	}
+
+	// Likewise, for low priorities, bump everything up a little.
+	else if (priority < 128) {
+		priority += 10;
+	}
+
 	param.sched_priority = priority;
 
 	pthread_attr_init(&taskmap[task_index].attr);
-	//pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name);
-	//pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack);
+	pthread_attr_setthreadname(&taskmap[task_index].attr, taskmap[task_index].name);
+	// See note above about the pthread_attr_setstackaddr function
+	// pthread_attr_setstackaddr(&taskmap[task_index].attr, taskmap[task_index].stack);
 	pthread_attr_setstacksize(&taskmap[task_index].attr, PX4_TASK_STACK_SIZE);
 	pthread_attr_setschedparam(&taskmap[task_index].attr, &param);
 

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -268,6 +268,8 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
 			return -1;
 		}
 
+		hrt_init();
+
 		uORB::Manager::initialize();
 		uORB::Manager::get_instance()->set_uorb_communicator(
 			uORB::ProtobufChannel::GetInstance());
@@ -288,6 +290,7 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
 		// Initialize the interrupt callback registration
 		register_interrupt_callback_initalizer(muorb_func_ptrs.register_interrupt_callback);
 
+		work_queues_init();
 		hrt_work_queue_init();
 
 		const char *argv[3] = { "slpi", "start" };
@@ -304,6 +307,9 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
 		qurt_thread_attr_init(&aggregator_attr);
 		qurt_thread_attr_set_stack_addr(&aggregator_attr, aggregator_stack);
 		qurt_thread_attr_set_stack_size(&aggregator_attr, aggregator_stack_size);
+		char thread_name[QURT_THREAD_ATTR_NAME_MAXLEN];
+		strncpy(thread_name, "PX4_muorb_agg", QURT_THREAD_ATTR_NAME_MAXLEN);
+		qurt_thread_attr_set_name(&aggregator_attr, thread_name);
 		qurt_thread_attr_set_priority(&aggregator_attr, aggregator_thread_priority);
 		(void) qurt_thread_create(&aggregator_tid, &aggregator_attr, aggregator_thread_func, NULL);