From 678607117a1d907a95968d5a1599337bdff5a524 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Mon, 19 Dec 2022 22:25:12 -0800 Subject: [PATCH] Qurt UART ESC driver support (#20784) --- boards/modalai/voxl2-slpi/default.px4board | 1 + boards/modalai/voxl2-slpi/src/board_config.h | 5 +++ boards/modalai/voxl2/target/voxl-px4.config | 2 + platforms/qurt/include/termios.h | 37 +++++++++++++++++++ .../actuators/modalai_esc/CMakeLists.txt | 1 + .../modalai_esc/modalai_esc_serial.cpp | 28 ++++++++++++++ .../modalai_esc/modalai_esc_serial.hpp | 8 ++++ src/lib/drivers/device/CMakeLists.txt | 2 +- .../muorb/slpi/uORBProtobufChannel.cpp | 5 +++ .../muorb/slpi/uORBProtobufChannel.hpp | 3 +- 10 files changed, 90 insertions(+), 2 deletions(-) create mode 100644 platforms/qurt/include/termios.h diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 34700d7dc9..d27d6a6f20 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/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 diff --git a/boards/modalai/voxl2-slpi/src/board_config.h b/boards/modalai/voxl2-slpi/src/board_config.h index cd7ddb8557..45e9d09539 100644 --- a/boards/modalai/voxl2-slpi/src/board_config.h +++ b/boards/modalai/voxl2-slpi/src/board_config.h @@ -57,3 +57,8 @@ */ #include #include + +/* + * Default port for the ESC + */ +#define MODALAI_ESC_DEFAULT_PORT "2" diff --git a/boards/modalai/voxl2/target/voxl-px4.config b/boards/modalai/voxl2/target/voxl-px4.config index e84e24275f..a6784ccc26 100755 --- a/boards/modalai/voxl2/target/voxl-px4.config +++ b/boards/modalai/voxl2/target/voxl-px4.config @@ -19,3 +19,5 @@ fi muorb start qshell icm42688p start -s + +qshell modalai_esc start diff --git a/platforms/qurt/include/termios.h b/platforms/qurt/include/termios.h new file mode 100644 index 0000000000..d36d80ee93 --- /dev/null +++ b/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; diff --git a/src/drivers/actuators/modalai_esc/CMakeLists.txt b/src/drivers/actuators/modalai_esc/CMakeLists.txt index 2e1bc60a27..1190937583 100644 --- a/src/drivers/actuators/modalai_esc/CMakeLists.txt +++ b/src/drivers/actuators/modalai_esc/CMakeLists.txt @@ -47,4 +47,5 @@ px4_add_module( qc_esc_packet.h DEPENDS px4_work_queue + mixer_module ) diff --git a/src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp b/src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp index 93ab40c7bb..720257507e 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc_serial.cpp +++ b/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 } diff --git a/src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp b/src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp index fdecbddc4f..461b4e97bc 100644 --- a/src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp +++ b/src/drivers/actuators/modalai_esc/modalai_esc_serial.hpp @@ -38,6 +38,10 @@ #include #include +#ifdef __PX4_QURT +#include +#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; }; diff --git a/src/lib/drivers/device/CMakeLists.txt b/src/lib/drivers/device/CMakeLists.txt index e589d1393d..819f45b229 100644 --- a/src/lib/drivers/device/CMakeLists.txt +++ b/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 diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.cpp b/src/modules/muorb/slpi/uORBProtobufChannel.cpp index fa1dcdbe22..f32470c0c2 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.cpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -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); diff --git a/src/modules/muorb/slpi/uORBProtobufChannel.hpp b/src/modules/muorb/slpi/uORBProtobufChannel.hpp index 7bc35aa088..ecd1d02780 100644 --- a/src/modules/muorb/slpi/uORBProtobufChannel.hpp +++ b/src/modules/muorb/slpi/uORBProtobufChannel.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #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);