Qurt UART ESC driver support (#20784)

boards/modalai/voxl2-slpi/default.px4board

@@ -5,3 +5,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
+CONFIG_DRIVERS_ACTUATORS_MODALAI_ESC=y

boards/modalai/voxl2-slpi/src/board_config.h

@@ -57,3 +57,8 @@
  */
 #include <system_config.h>
 #include <px4_platform_common/board_common.h>
+
+/*
+ *  Default port for the ESC
+ */
+#define MODALAI_ESC_DEFAULT_PORT 	"2"

boards/modalai/voxl2/target/voxl-px4.config

@@ -19,3 +19,5 @@ fi
 muorb start
 
 qshell icm42688p start -s
+
+qshell modalai_esc start

platforms/qurt/include/termios.h

@@ -0,0 +1,37 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+// This file prevents a missing header file error but since Qurt doesn't support
+// termios the actual code will still need to be changed for Qurt
+
+typedef unsigned int speed_t;

src/drivers/actuators/modalai_esc/CMakeLists.txt

@@ -47,4 +47,5 @@ px4_add_module(
 		qc_esc_packet.h
 	DEPENDS
 		px4_work_queue
+		mixer_module
 	)

src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp

@@ -53,13 +53,18 @@ int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
 	}
 
 	/* Open UART */
+#ifdef __PX4_QURT
+	_uart_fd = qurt_uart_open(dev, speed);
+#else
 	_uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
+#endif
 
 	if (_uart_fd < 0) {
 		PX4_ERR("Error opening port: %s (%i)", dev, errno);
 		return -1;
 	}
 
+#ifndef __PX4_QURT
 	/* Back up the original UART configuration to restore it after exit */
 	int termios_state;
 
@@ -98,6 +103,8 @@ int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
 		return -1;
 	}
 
+#endif
+
 	_speed =  speed;
 
 	return 0;
@@ -105,6 +112,8 @@ int ModalaiEscSerial::uart_open(const char *dev, speed_t speed)
 
 int ModalaiEscSerial::uart_set_baud(speed_t speed)
 {
+#ifndef __PX4_QURT
+
 	if (_uart_fd < 0) {
 		return -1;
 	}
@@ -120,10 +129,15 @@ int ModalaiEscSerial::uart_set_baud(speed_t speed)
 	_speed = speed;
 
 	return 0;
+#endif
+
+	return -1;
 }
 
 int ModalaiEscSerial::uart_close()
 {
+#ifndef __PX4_QURT
+
 	if (_uart_fd < 0) {
 		PX4_ERR("invalid state for closing");
 		return -1;
@@ -137,6 +151,8 @@ int ModalaiEscSerial::uart_close()
 		PX4_ERR("error closing uart");
 	}
 
+#endif
+
 	_uart_fd = -1;
 
 	return 0;
@@ -149,7 +165,11 @@ int ModalaiEscSerial::uart_write(FAR void *buf, size_t len)
 		return -1;
 	}
 
+#ifdef __PX4_QURT
+	return qurt_uart_write(_uart_fd, (const char *) buf, len);
+#else
 	return write(_uart_fd, buf, len);
+#endif
 }
 
 int ModalaiEscSerial::uart_read(FAR void *buf, size_t len)
@@ -159,5 +179,13 @@ int ModalaiEscSerial::uart_read(FAR void *buf, size_t len)
 		return -1;
 	}
 
+#ifdef __PX4_QURT
+#define ASYNC_UART_READ_WAIT_US 2000
+	// The UART read on SLPI is via an asynchronous service so specify a timeout
+	// for the return. The driver will poll periodically until the read comes in
+	// so this may block for a while. However, it will timeout if no read comes in.
+	return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US);
+#else
 	return read(_uart_fd, buf, len);
+#endif
 }

src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp

@@ -38,6 +38,10 @@
 #include <fcntl.h>
 #include <termios.h>
 
+#ifdef __PX4_QURT
+#include <drivers/device/qurt/uart.h>
+#define FAR
+#endif
 
 class ModalaiEscSerial
 {
@@ -55,7 +59,11 @@ public:
 
 private:
 	int			_uart_fd = -1;
+
+#if ! defined(__PX4_QURT)
 	struct termios		_orig_cfg;
 	struct termios		_cfg;
+#endif
+
 	int   _speed = -1;
 };

src/lib/drivers/device/CMakeLists.txt

@@ -43,7 +43,7 @@ if (${PX4_PLATFORM} STREQUAL "nuttx")
 elseif((${PX4_PLATFORM} MATCHES "qurt"))
 	# list(APPEND SRCS_PLATFORM qurt/I2C.cpp)
 	list(APPEND SRCS_PLATFORM qurt/SPI.cpp)
-	# list(APPEND SRCS_PLATFORM qurt/uart.c)
+	list(APPEND SRCS_PLATFORM qurt/uart.c)
 elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type
 	# Linux I2Cdev and SPIdev
 	list(APPEND SRCS_PLATFORM

src/modules/muorb/slpi/uORBProtobufChannel.cpp

@@ -38,6 +38,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <drivers/device/spi.h>
+#include <drivers/device/qurt/uart.h>
 #include <pthread.h>
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/log.h>
@@ -280,6 +281,10 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us)
 		// Configure the SPI driver function pointers
 		device::SPI::configure_callbacks(muorb_func_ptrs._config_spi_bus_func_t, muorb_func_ptrs._spi_transfer_func_t);
 
+		// Configure the UART driver function pointers
+		configure_uart_callbacks(muorb_func_ptrs.open_uart_func_t, muorb_func_ptrs.write_uart_func_t,
+					 muorb_func_ptrs.read_uart_func_t);
+
 		// Initialize the interrupt callback registration
 		register_interrupt_callback_initalizer(muorb_func_ptrs.register_interrupt_callback);
 

src/modules/muorb/slpi/uORBProtobufChannel.hpp

@@ -38,6 +38,7 @@
 #include <string>
 #include <map>
 #include <pthread.h>
+#include <termios.h>
 
 #include "uORB/uORBCommunicator.hpp"
 #include "mUORBAggregator.hpp"
@@ -196,7 +197,7 @@ typedef struct {
 	// open_uart_func_t open_uart_func;
 	// write_uart_func_t write_uart_func;
 	// read_uart_func_t read_uart_func;
-	int (*open_uart_func_t)(uint8_t, uint32_t);
+	int (*open_uart_func_t)(uint8_t, speed_t);
 	int (*write_uart_func_t)(int, const void *, size_t);
 	int (*read_uart_func_t)(int,  void *, size_t);
 	int (*register_interrupt_callback)(int (*)(int, void *, void *), void *arg);