From f9c5c02f91d78954ad52a53d2d39e5b9d6f74a95 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 1 May 2024 13:23:46 +1000 Subject: [PATCH] AP_RCProtocol: move handling of AP_Radio RC input down into AP_RCProtocol --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 12 ++++- libraries/AP_RCProtocol/AP_RCProtocol.h | 13 +++++ .../AP_RCProtocol/AP_RCProtocol_Radio.cpp | 53 +++++++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_Radio.h | 32 +++++++++++ .../AP_RCProtocol/AP_RCProtocol_config.h | 5 ++ 5 files changed, 114 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_Radio.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_Radio.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index b7b6bfdf94..2202dbd5cc 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -38,6 +38,7 @@ #include "AP_RCProtocol_Joystick_SFML.h" #include "AP_RCProtocol_UDP.h" #include "AP_RCProtocol_FDM.h" +#include "AP_RCProtocol_Radio.h" #include #include @@ -110,7 +111,9 @@ void AP_RCProtocol::init() UDP_backend->set_fdm_backend(FDM_backend); #endif // AP_RCPROTOCOL_UDP_ENABLED #endif // AP_RCPROTOCOL_FDM_ENABLED - +#if AP_RCPROTOCOL_RADIO_ENABLED + backend[AP_RCProtocol::RADIO] = new AP_RCProtocol_Radio(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -472,6 +475,9 @@ bool AP_RCProtocol::new_input() #endif #if AP_RCPROTOCOL_FDM_ENABLED AP_RCProtocol::FDM, +#endif +#if AP_RCPROTOCOL_RADIO_ENABLED + AP_RCProtocol::RADIO, #endif }; for (const auto protocol : pollable) { @@ -620,6 +626,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_FDM_ENABLED case FDM: return "FDM"; +#endif +#if AP_RCPROTOCOL_RADIO_ENABLED + case RADIO: + return "Radio"; #endif case NONE: break; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 6234e0a1a1..37f46a28da 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -89,6 +89,9 @@ public: #endif #if AP_RCPROTOCOL_FDM_ENABLED FDM = 18, +#endif +#if AP_RCPROTOCOL_RADIO_ENABLED + RADIO = 19, #endif NONE //last enum always is None }; @@ -125,6 +128,12 @@ public: _disabled_for_pulses |= (1U<<(uint8_t)protocol); } +// in the case we've disabled most backends then the "return true" in +// the following method can never be reached, and the compiler gets +// annoyed at that. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wswitch-unreachable" + // for protocols without strong CRCs we require 3 good frames to lock on bool requires_3_frames(enum rcprotocol_t p) { switch (p) { @@ -185,12 +194,16 @@ public: #endif #if AP_RCPROTOCOL_FDM_ENABLED case FDM: +#endif +#if AP_RCPROTOCOL_RADIO_ENABLED + case RADIO: #endif case NONE: return false; } return false; } +#pragma GCC diagnostic pop uint8_t num_channels(); uint16_t read(uint8_t chan); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Radio.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Radio.cpp new file mode 100644 index 0000000000..f0f37703f3 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Radio.cpp @@ -0,0 +1,53 @@ +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_RADIO_ENABLED + +#include "AP_RCProtocol_Radio.h" +#include + +void AP_RCProtocol_Radio::update() +{ + auto *radio = AP_Radio::get_singleton(); + if (radio == nullptr) { + return; + } + if (!init_done) { + radio->init(); + init_done = true; + } + + // allow the radio to handle mavlink on the main thread: + radio->update(); + + const uint32_t last_recv_us = radio->last_recv_us(); + if (last_recv_us == last_input_us) { + // no new data + return; + } + last_input_us = last_recv_us; + + const auto num_channels = radio->num_channels(); + uint16_t rcin_values[MAX_RCIN_CHANNELS]; + for (uint8_t i=0; iread(i); + } + + add_input( + num_channels, + rcin_values, + false, // failsafe + 0, // check me + 0 // link quality + ); +} + +void AP_RCProtocol_Radio::start_bind() +{ + auto *radio = AP_Radio::get_singleton(); + if (radio == nullptr) { + return; + } + radio->start_recv_bind(); +} + +#endif // AP_RCPROTOCOL_RADIO_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Radio.h b/libraries/AP_RCProtocol/AP_RCProtocol_Radio.h new file mode 100644 index 0000000000..0144e0e426 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Radio.h @@ -0,0 +1,32 @@ +#pragma once + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_RADIO_ENABLED + +/* + * Reads fixed-length packets containing either 8 or 16 2-byte values, + * and interprets them as RC input. + */ + +#include "AP_RCProtocol_Backend.h" + +class AP_RCProtocol_Radio : public AP_RCProtocol_Backend { +public: + + using AP_RCProtocol_Backend::AP_RCProtocol_Backend; + + void update() override; + + void start_bind(void) override; + +private: + + bool init(); + bool init_done; + + uint32_t last_input_us; +}; + + +#endif // AP_RCPROTOCOL_RADIO_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index a648b0969b..4cb2b65124 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -3,6 +3,7 @@ #include #include #include +#include #ifndef AP_RCPROTOCOL_ENABLED #define AP_RCPROTOCOL_ENABLED 1 @@ -43,6 +44,10 @@ #define AP_RCPROTOCOL_PPMSUM_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_RCPROTOCOL_RADIO_ENABLED +#define AP_RCPROTOCOL_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RADIO_ENABLED +#endif + #ifndef AP_RCPROTOCOL_SBUS_ENABLED #define AP_RCPROTOCOL_SBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED #endif