From 8a4d93cca33991b2a3cfffb632db855a6da8056c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Feb 2022 21:49:55 -0500 Subject: [PATCH] drivers/rc/ghst_rc: create new standalone ghst_rc driver --- boards/px4/fmu-v5x/default.px4board | 3 +- src/drivers/rc/Kconfig | 9 + src/drivers/rc/ghst_rc/CMakeLists.txt | 47 ++++ src/drivers/rc/ghst_rc/GhstRc.cpp | 241 ++++++++++++++++++ src/drivers/rc/ghst_rc/GhstRc.hpp | 92 +++++++ src/drivers/rc/ghst_rc/Kconfig | 5 + .../ghst_rc}/ghst_telemetry.cpp | 1 + .../ghst_rc}/ghst_telemetry.hpp | 0 src/drivers/rc/ghst_rc/module.yaml | 11 + src/drivers/rc_input/CMakeLists.txt | 1 - src/drivers/rc_input/RCInput.cpp | 55 ---- src/drivers/rc_input/RCInput.hpp | 5 - 12 files changed, 408 insertions(+), 62 deletions(-) create mode 100644 src/drivers/rc/Kconfig create mode 100644 src/drivers/rc/ghst_rc/CMakeLists.txt create mode 100644 src/drivers/rc/ghst_rc/GhstRc.cpp create mode 100644 src/drivers/rc/ghst_rc/GhstRc.hpp create mode 100644 src/drivers/rc/ghst_rc/Kconfig rename src/drivers/{rc_input => rc/ghst_rc}/ghst_telemetry.cpp (99%) rename src/drivers/{rc_input => rc/ghst_rc}/ghst_telemetry.hpp (100%) create mode 100644 src/drivers/rc/ghst_rc/module.yaml diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 878bfd79f7..6d017ca70c 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -38,6 +38,7 @@ CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_RPM=y @@ -60,6 +61,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -81,7 +83,6 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y diff --git a/src/drivers/rc/Kconfig b/src/drivers/rc/Kconfig new file mode 100644 index 0000000000..79e0751200 --- /dev/null +++ b/src/drivers/rc/Kconfig @@ -0,0 +1,9 @@ +menu "RC" + menuconfig COMMON_RC + bool "Common RC" + default n + select DRIVERS_RC_GHST_RC + ---help--- + Enable default set of magnetometer drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/rc/ghst_rc/CMakeLists.txt b/src/drivers/rc/ghst_rc/CMakeLists.txt new file mode 100644 index 0000000000..405e158482 --- /dev/null +++ b/src/drivers/rc/ghst_rc/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__rc__ghst_rc + MAIN ghst_rc + COMPILE_FLAGS + SRCS + ghst_telemetry.cpp + ghst_telemetry.hpp + GhstRc.cpp + GhstRc.hpp + + MODULE_CONFIG + module.yaml + DEPENDS + rc + ) diff --git a/src/drivers/rc/ghst_rc/GhstRc.cpp b/src/drivers/rc/ghst_rc/GhstRc.cpp new file mode 100644 index 0000000000..a4e896ebf4 --- /dev/null +++ b/src/drivers/rc/ghst_rc/GhstRc.cpp @@ -0,0 +1,241 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "GhstRc.hpp" + +#include +#include + +using namespace time_literals; + +GhstRc::GhstRc(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)) +{ + if (device) { + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; + } +} + +GhstRc::~GhstRc() +{ + delete _ghst_telemetry; + + perf_free(_cycle_interval_perf); + perf_free(_publish_interval_perf); +} + +int GhstRc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device = myoptarg; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + if (device == nullptr) { + PX4_ERR("valid device required"); + return PX4_ERROR; + } + + GhstRc *instance = new GhstRc(device); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleNow(); + + return PX4_OK; +} + +void GhstRc::Run() +{ + if (should_exit()) { + ScheduleClear(); + ::close(_rc_fd); + exit_and_cleanup(); + return; + } + + if (_rc_fd < 0) { + _rc_fd = ::open(_device, O_RDWR | O_NONBLOCK); + } + + // poll with 3 second timeout + pollfd fds[1]; + fds[0].fd = _rc_fd; + fds[0].events = POLLIN; + ::poll(fds, 1, 3000); + + perf_count(_cycle_interval_perf); + + const hrt_abstime cycle_timestamp = hrt_absolute_time(); + + // read all available data from the serial RC input UART + int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE); + + if (new_bytes > 0) { + _bytes_rx += new_bytes; + } + + if (_rc_scan_begin == 0) { + _rc_scan_begin = cycle_timestamp; + // Configure serial port for GHST + ghst_config(_rc_fd); + } + + if (_rc_locked || (cycle_timestamp - _rc_scan_begin < 300_ms)) { + + } + + // parse new data + if (new_bytes > 0) { + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; + uint16_t raw_rc_count = 0; + int8_t ghst_rssi = -1; + bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &raw_rc_values[0], &ghst_rssi, &raw_rc_count, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (rc_updated) { + if (!_rc_locked) { + _rc_locked = true; + PX4_INFO("RC input locked"); + } + + // we have a new GHST frame. Publish it. + input_rc_s input_rc{}; + input_rc.timestamp_last_signal = cycle_timestamp; + input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); + input_rc.rssi = ghst_rssi; + input_rc.rc_lost = (raw_rc_count == 0); + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + + for (int i = 0; i < input_rc.channel_count; i++) { + input_rc.values[i] = raw_rc_values[i]; + } + + input_rc.timestamp = hrt_absolute_time(); + _input_rc_pub.publish(input_rc); + perf_count(_publish_interval_perf); + + if (!_ghst_telemetry) { + _ghst_telemetry = new GHSTTelemetry(_rc_fd); + } + + if (_ghst_telemetry) { + _ghst_telemetry->update(cycle_timestamp); + } + } + } + + ScheduleDelayed(4_ms); +} + +int GhstRc::print_status() +{ + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal"); + + if (_rc_locked) { + PX4_INFO("Telemetry: %s", _ghst_telemetry ? "yes" : "no"); + } + + perf_print_counter(_cycle_interval_perf); + perf_print_counter(_publish_interval_perf); + + return 0; +} + +int GhstRc::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int GhstRc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module does the GHST RC input parsing. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ghst_rc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[]) +{ + return GhstRc::main(argc, argv); +} diff --git a/src/drivers/rc/ghst_rc/GhstRc.hpp b/src/drivers/rc/ghst_rc/GhstRc.hpp new file mode 100644 index 0000000000..9501775a21 --- /dev/null +++ b/src/drivers/rc/ghst_rc/GhstRc.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ghst_telemetry.hpp" + +class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + GhstRc(const char *device); + ~GhstRc() override; + + /** @see ModuleBase */ + static int task_spawn(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::print_status() */ + int print_status() override; + +private: + void Run() override; + + hrt_abstime _rc_scan_begin{0}; + + bool _rc_locked{false}; + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + + int _rc_fd{-1}; + char _device[20] {}; ///< device / serial port path + + static constexpr size_t RC_MAX_BUFFER_SIZE{64}; + uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; + uint32_t _bytes_rx{0}; + + GHSTTelemetry *_ghst_telemetry{nullptr}; + + perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + perf_counter_t _publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")}; +}; diff --git a/src/drivers/rc/ghst_rc/Kconfig b/src/drivers/rc/ghst_rc/Kconfig new file mode 100644 index 0000000000..d95449dcda --- /dev/null +++ b/src/drivers/rc/ghst_rc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RC_GHST_RC + bool "ghst_rc" + default n + ---help--- + Enable support for ghst rc diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc/ghst_rc/ghst_telemetry.cpp similarity index 99% rename from src/drivers/rc_input/ghst_telemetry.cpp rename to src/drivers/rc/ghst_rc/ghst_telemetry.cpp index e254cb09ab..6e9d12663b 100644 --- a/src/drivers/rc_input/ghst_telemetry.cpp +++ b/src/drivers/rc/ghst_rc/ghst_telemetry.cpp @@ -41,6 +41,7 @@ */ #include "ghst_telemetry.hpp" + #include using time_literals::operator ""_s; diff --git a/src/drivers/rc_input/ghst_telemetry.hpp b/src/drivers/rc/ghst_rc/ghst_telemetry.hpp similarity index 100% rename from src/drivers/rc_input/ghst_telemetry.hpp rename to src/drivers/rc/ghst_rc/ghst_telemetry.hpp diff --git a/src/drivers/rc/ghst_rc/module.yaml b/src/drivers/rc/ghst_rc/module.yaml new file mode 100644 index 0000000000..5398361220 --- /dev/null +++ b/src/drivers/rc/ghst_rc/module.yaml @@ -0,0 +1,11 @@ +module_name: GHST RC Input Driver +serial_config: + - command: "ghst_rc start -d ${SERIAL_DEV}" + port_config_param: + name: GHST_RC_PRT_CFG + group: Serial + #default: RC + #depends_on_port: RC + description_extended: | + Ghost RC input driver. + diff --git a/src/drivers/rc_input/CMakeLists.txt b/src/drivers/rc_input/CMakeLists.txt index 6c58fb2605..1ad8262481 100644 --- a/src/drivers/rc_input/CMakeLists.txt +++ b/src/drivers/rc_input/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( SRCS RCInput.cpp crsf_telemetry.cpp - ghst_telemetry.cpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 071a678d0d..0c7ba33a9d 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -74,7 +74,6 @@ RCInput::~RCInput() dsm_deinit(); delete _crsf_telemetry; - delete _ghst_telemetry; perf_free(_cycle_perf); perf_free(_publish_interval_perf); @@ -665,56 +664,6 @@ void RCInput::Run() } } - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_GHST); - } - - break; - - case RC_SCAN_GHST: - if (_rc_scan_begin == 0) { - _rc_scan_begin = cycle_timestamp; - // Configure serial port for GHST - ghst_config(_rcs_fd); - - // flush serial buffer and any existing buffered data - tcflush(_rcs_fd, TCIOFLUSH); - memset(_rcs_buf, 0, sizeof(_rcs_buf)); - - } else if (_rc_scan_locked - || cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - // parse new data - if (newBytes > 0) { - int8_t ghst_rssi = -1; - rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi, - &_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS); - - if (rc_updated) { - // we have a new GHST frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); - - // ghst telemetry works on fmu-v5 - // on other Pixhawk (-related) boards we cannot write to the RC UART - // another option is to use a different UART port -#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT - - if (!_rc_scan_locked && !_ghst_telemetry) { - _ghst_telemetry = new GHSTTelemetry(_rcs_fd); - } - -#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */ - - _rc_scan_locked = true; - - if (_ghst_telemetry) { - _ghst_telemetry->update(cycle_timestamp); - } - } - } - } else { // Scan the next protocol set_rc_scan_state(RC_SCAN_SBUS); @@ -827,10 +776,6 @@ int RCInput::print_status() PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no"); break; - case RC_SCAN_GHST: - PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no"); - break; - case RC_SCAN_SBUS: PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames()); break; diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 41c7dbbea1..028d3afc46 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -61,7 +60,6 @@ #include #include "crsf_telemetry.h" -#include "ghst_telemetry.hpp" #ifdef HRT_PPM_CHANNEL # include @@ -97,7 +95,6 @@ private: RC_SCAN_SUMD, RC_SCAN_ST24, RC_SCAN_CRSF, - RC_SCAN_GHST } _rc_scan_state{RC_SCAN_SBUS}; static constexpr char const *RC_SCAN_STRING[7] { @@ -107,7 +104,6 @@ private: "SUMD", "ST24", "CRSF", - "GHST" }; void Run() override; @@ -159,7 +155,6 @@ private: uint16_t _raw_rc_count{}; CRSFTelemetry *_crsf_telemetry{nullptr}; - GHSTTelemetry *_ghst_telemetry{nullptr}; perf_counter_t _cycle_perf; perf_counter_t _publish_interval_perf;