diff --git a/boards/modalai/voxl2-slpi/src/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/CMakeLists.txt index 51dbb0c311..7d1867d1ec 100644 --- a/boards/modalai/voxl2-slpi/src/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/CMakeLists.txt @@ -47,3 +47,4 @@ add_library(drivers_board 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) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc) diff --git a/src/drivers/snapdragon_spektrum_rc/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/CMakeLists.txt similarity index 95% rename from src/drivers/snapdragon_spektrum_rc/CMakeLists.txt rename to boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/CMakeLists.txt index 7626afd716..6d8087695f 100644 --- a/src/drivers/snapdragon_spektrum_rc/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/CMakeLists.txt @@ -32,10 +32,13 @@ ############################################################################ px4_add_module( - MODULE drivers__snapdragon_spektrum_rc + MODULE drivers__spektrum_rc MAIN spektrum_rc + INCLUDES + ${PX4_SOURCE_DIR}/src/drivers/rc_input SRCS spektrum_rc.cpp DEPENDS rc ) + diff --git a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/drv_rc_input.h b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/drv_rc_input.h new file mode 100644 index 0000000000..f9a915a1dc --- /dev/null +++ b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/drv_rc_input.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 drv_rc_input.h + * + * R/C input interface. + */ + +#ifndef _DRV_RC_INPUT_H +#define _DRV_RC_INPUT_H + +#include +#include +#include + +// #include "drv_orb_dev.h" + +/** + * Path for the default R/C input device. + * + * Note that on systems with more than one R/C input path (e.g. + * PX4FMU with PX4IO connected) there may be other devices that + * respond to this protocol. + * + * Input data may be obtained by subscribing to the input_rc + * object, or by poll/reading from the device. + */ +#define RC_INPUT0_DEVICE_PATH "/dev/input_rc0" + +/** + * Maximum RSSI value + */ +#define RC_INPUT_RSSI_MAX 100 + +/** + * Input signal type, value is a control position from zero to 100 + * percent. + */ +typedef uint16_t rc_input_t; + +#define _RC_INPUT_BASE 0x2b00 + +/** Enable RSSI input via ADC */ +#ifdef __PX4_POSIX +#define RC_INPUT_ENABLE_RSSI_ANALOG (_RC_INPUT_BASE | 1) +#else +#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1) +#endif + +/** Enable RSSI input via PWM signal */ +#ifdef __PX4_POSIX +#define RC_INPUT_ENABLE_RSSI_PWM (_RC_INPUT_BASE | 2) +#else +#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2) +#endif + +#endif /* _DRV_RC_INPUT_H */ diff --git a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp similarity index 67% rename from src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp rename to boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp index ee39c6d970..c2405de1fe 100644 --- a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp @@ -46,13 +46,23 @@ #include #include +#include +#include "drv_rc_input.h" #include +#ifdef __PX4_QURT +#include +#endif + #include #include // Snapdraogon: use J12 (next to J13, power module side) +#ifdef __PX4_QURT +#define SPEKTRUM_UART_DEVICE_PATH "7" +#else #define SPEKTRUM_UART_DEVICE_PATH "/dev/tty-3" +#endif #define UNUSED(x) (void)(x) @@ -82,13 +92,19 @@ void task_main(int argc, char *argv[]) int ch; int myoptind = 1; const char *myoptarg = NULL; + bool verbose = false; - while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "vd:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': device_path = myoptarg; break; + case 'v': + PX4_INFO("Spektrum RC: Enabling verbose mode"); + verbose = true; + break; + default: break; } @@ -96,9 +112,12 @@ void task_main(int argc, char *argv[]) int uart_fd = dsm_init(device_path); - if (uart_fd < 1) { + if (uart_fd < 0) { PX4_ERR("dsm init failed"); return; + + } else if (verbose) { + PX4_INFO("Spektrum RC: dsm_init succeeded"); } orb_advert_t rc_pub = nullptr; @@ -109,50 +128,94 @@ void task_main(int argc, char *argv[]) _is_running = true; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; uint16_t raw_rc_count = 0; + uint32_t loop_counter = 0; + bool print_msg = false; + bool first_correct_frame_received = false; + int newbytes = 0; // Main loop while (!_task_should_exit) { - int newbytes = ::read(uart_fd, &rx_buf[0], sizeof(rx_buf)); + if (((loop_counter % 20) == 0) && verbose) { print_msg = true; } - if (newbytes < 0) { - PX4_WARN("read failed"); - continue; - } + loop_counter++; - if (newbytes == 0) { - continue; - } +#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. + newbytes = qurt_uart_read(uart_fd, (char *) &rx_buf[0], sizeof(rx_buf), ASYNC_UART_READ_WAIT_US); +#else + newbytes = read(uart_fd, &rx_buf[0], sizeof(rx_buf)); +#endif - const hrt_abstime now = hrt_absolute_time(); + if (newbytes <= 0) { + if (print_msg) { PX4_INFO("Spektrum RC: Read no bytes from UART"); } - bool dsm_11_bit; - unsigned frame_drops; - int8_t dsm_rssi; + } else if (((newbytes != DSM_FRAME_SIZE) || ((rx_buf[1] & 0x0F) != 0x02)) && (! first_correct_frame_received)) { + PX4_ERR("Spektrum RC: Read something other than correct DSM frame on read. Got %d bytes. Protocol byte is 0x%.2x", + newbytes, rx_buf[1]); - // parse new data - bool rc_updated = dsm_parse(now, rx_buf, newbytes, &raw_rc_values[0], &raw_rc_count, - &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS); - UNUSED(dsm_11_bit); + } else { + if (print_msg) { PX4_INFO("Spektrum RC: Read %d bytes from UART", newbytes); } - if (rc_updated) { + first_correct_frame_received = true; - input_rc_s input_rc = {}; + const hrt_abstime now = hrt_absolute_time(); - fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, dsm_rssi, - input_rc); + bool dsm_11_bit; + unsigned frame_drops; + int8_t dsm_rssi; - if (rc_pub == nullptr) { - rc_pub = orb_advertise(ORB_ID(input_rc), &input_rc); + // parse new data + bool rc_updated = dsm_parse(now, rx_buf, newbytes, &raw_rc_values[0], &raw_rc_count, + &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS); + UNUSED(dsm_11_bit); - } else { - orb_publish(ORB_ID(input_rc), rc_pub, &input_rc); + if (rc_updated) { + if (print_msg) { PX4_INFO("Spektrum RC: DSM message parsed successfully"); } + + input_rc_s input_rc = {}; + + fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, dsm_rssi, + input_rc); + + if (rc_pub == nullptr) { + rc_pub = orb_advertise(ORB_ID(input_rc), &input_rc); + + } else { + if (print_msg) { PX4_INFO("Spektrum RC: Publishing input_rc"); } + + orb_publish(ORB_ID(input_rc), rc_pub, &input_rc); + } + } + + if (print_msg) { + PX4_INFO("0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x", + rx_buf[0], + rx_buf[1], + rx_buf[2], + rx_buf[3], + rx_buf[4], + rx_buf[5], + rx_buf[6], + rx_buf[7], + rx_buf[8], + rx_buf[9], + rx_buf[10], + rx_buf[11], + rx_buf[12], + rx_buf[13], + rx_buf[14], + rx_buf[15]); } } + print_msg = false; + // sleep since no poll for qurt usleep(10000); - } orb_unadvertise(rc_pub); @@ -204,9 +267,6 @@ void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_ input_rc.rc_lost = (valid_chans == 0); input_rc.rc_lost_frame_count = frame_drops; input_rc.rc_total_frame_count = 0; - - input_rc.link_quality = -1; - input_rc.rssi_dbm = NAN; } int start(int argc, char *argv[]) diff --git a/src/drivers/spektrum_rc/Kconfig b/src/drivers/spektrum_rc/Kconfig deleted file mode 100644 index 0d67ce18b2..0000000000 --- a/src/drivers/spektrum_rc/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_SPEKTRUM_RC - bool "spektrum_rc" - default n - ---help--- - Enable support for spektrum_rc \ No newline at end of file