From 8572e446e7284d834b6b80c48147acedafd93327 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 14 Mar 2024 16:47:43 +1100 Subject: [PATCH] AP_RCProtocol: add support for RC input from SITL FDM data --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 27 +++++++++- libraries/AP_RCProtocol/AP_RCProtocol.h | 6 +++ libraries/AP_RCProtocol/AP_RCProtocol_FDM.cpp | 50 +++++++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_FDM.h | 24 +++++++++ libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp | 11 ++++ libraries/AP_RCProtocol/AP_RCProtocol_UDP.h | 10 ++++ .../AP_RCProtocol/AP_RCProtocol_config.h | 4 ++ 7 files changed, 131 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_FDM.cpp create mode 100644 libraries/AP_RCProtocol/AP_RCProtocol_FDM.h diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index ea5d6cb70ffcf..b7b6bfdf945da 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -37,6 +37,7 @@ #include "AP_RCProtocol_MAVLinkRadio.h" #include "AP_RCProtocol_Joystick_SFML.h" #include "AP_RCProtocol_UDP.h" +#include "AP_RCProtocol_FDM.h" #include #include @@ -98,8 +99,18 @@ void AP_RCProtocol::init() backend[AP_RCProtocol::JOYSTICK_SFML] = new AP_RCProtocol_Joystick_SFML(*this); #endif #if AP_RCPROTOCOL_UDP_ENABLED - backend[AP_RCProtocol::UDP] = new AP_RCProtocol_UDP(*this); + const auto UDP_backend = new AP_RCProtocol_UDP(*this); + backend[AP_RCProtocol::UDP] = UDP_backend; #endif +#if AP_RCPROTOCOL_FDM_ENABLED + const auto FDM_backend = new AP_RCProtocol_FDM(*this);; + backend[AP_RCProtocol::FDM] = FDM_backend; +#if AP_RCPROTOCOL_UDP_ENABLED + // the UDP-Packed16Bit backend gives way to the FDM backend: + UDP_backend->set_fdm_backend(FDM_backend); +#endif // AP_RCPROTOCOL_UDP_ENABLED +#endif // AP_RCPROTOCOL_FDM_ENABLED + } AP_RCProtocol::~AP_RCProtocol() @@ -114,6 +125,13 @@ AP_RCProtocol::~AP_RCProtocol() bool AP_RCProtocol::should_search(uint32_t now_ms) const { +#if AP_RCPROTOCOL_FDM_ENABLED && AP_RCPROTOCOL_UDP_ENABLED + // force re-detection when FDM is active and active backend is UDP values + if (_detected_protocol == AP_RCProtocol::UDP && + ((AP_RCProtocol_FDM*)backend[AP_RCProtocol::FDM])->active()) { + return true; + } +#endif // AP_RCPROTOCOL_FDM_ENABLED && AP_RCPROTOCOL_UDP_ENABLED #if AP_RC_CHANNEL_ENABLED && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) if (_detected_protocol != AP_RCProtocol::NONE && !rc().option_is_enabled(RC_Channels::Option::MULTI_RECEIVER_SUPPORT)) { return false; @@ -451,6 +469,9 @@ bool AP_RCProtocol::new_input() #endif #if AP_RCPROTOCOL_UDP_ENABLED AP_RCProtocol::UDP, +#endif +#if AP_RCPROTOCOL_FDM_ENABLED + AP_RCProtocol::FDM, #endif }; for (const auto protocol : pollable) { @@ -595,6 +616,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_UDP_ENABLED case UDP: return "UDP"; +#endif +#if AP_RCPROTOCOL_FDM_ENABLED + case FDM: + return "FDM"; #endif case NONE: break; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 7557db7907bee..6234e0a1a1309 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -86,6 +86,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_UDP_ENABLED UDP = 17, +#endif +#if AP_RCPROTOCOL_FDM_ENABLED + FDM = 18, #endif NONE //last enum always is None }; @@ -179,6 +182,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_UDP_ENABLED case UDP: +#endif +#if AP_RCPROTOCOL_FDM_ENABLED + case FDM: #endif case NONE: return false; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FDM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FDM.cpp new file mode 100644 index 0000000000000..5463c508ae18a --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FDM.cpp @@ -0,0 +1,50 @@ +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_FDM_ENABLED + +#include "AP_RCProtocol_FDM.h" + +#include +#include + +extern const AP_HAL::HAL& hal; + +void AP_RCProtocol_FDM::update() +{ + const auto sitl = AP::sitl(); + if (sitl == nullptr) { + return; + } + + const auto &fdm = sitl->state; + + // see if there's fresh input. Until timestamps are worked out, + // just check for non-zero values: + if (fdm.rcin_chan_count == 0) { + return; + } + + // simulate RC input at 50Hz + if (AP_HAL::millis() - last_input_ms < 20) { + return; + } + + last_input_ms = AP_HAL::millis(); + + // scale from FDM 0-1 floats to PWM values + // these are the values that will be fed into the autopilot. + uint16_t pwm_input[16]; + const uint8_t count = MIN(ARRAY_SIZE(pwm_input), fdm.rcin_chan_count); + for (uint8_t i=0; i 0; } + +private: + + uint32_t last_input_ms; +}; + + +#endif // AP_RCPROTOCOL_FDM_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp index 68d8b782f52b9..78b7a80cabce4 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp @@ -8,6 +8,10 @@ #include #include +#if AP_RCPROTOCOL_FDM_ENABLED +#include "AP_RCProtocol_FDM.h" +#endif + extern const AP_HAL::HAL& hal; void AP_RCProtocol_UDP::set_default_pwm_input_values() @@ -59,6 +63,13 @@ bool AP_RCProtocol_UDP::init() void AP_RCProtocol_UDP::update() { +#if AP_RCPROTOCOL_FDM_ENABLED + // yield to the FDM backend if it is getting data + if (fdm_backend->active()) { + return; + } +#endif + if (!init_done) { if (!init()) { return; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_UDP.h b/libraries/AP_RCProtocol/AP_RCProtocol_UDP.h index d903393060758..0b80e8d92cb9d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_UDP.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_UDP.h @@ -21,6 +21,12 @@ class AP_RCProtocol_UDP : public AP_RCProtocol_Backend { void update() override; +#if AP_RCPROTOCOL_FDM_ENABLED + void set_fdm_backend(class AP_RCProtocol_FDM *_fdm_backend) { + fdm_backend = _fdm_backend; + } +#endif + private: bool init(); @@ -38,6 +44,10 @@ class AP_RCProtocol_UDP : public AP_RCProtocol_Backend { uint8_t num_channels; void set_default_pwm_input_values(); + +#if AP_RCPROTOCOL_FDM_ENABLED + AP_RCProtocol_FDM *fdm_backend; +#endif }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index ad85efd4a28da..04382fc946026 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -82,3 +82,7 @@ #ifndef AP_RCPROTOCOL_UDP_ENABLED #define AP_RCPROTOCOL_UDP_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif + +#ifndef AP_RCPROTOCOL_FDM_ENABLED +#define AP_RCPROTOCOL_FDM_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif