AP_RCProtocol: add support for RC input from SITL FDM data

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 <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -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;

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;

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 <AP_HAL/AP_HAL.h>
+#include <SITL/SITL.h>
+
+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<count; i++) {
+        pwm_input[i] = 1000 + fdm.rcin[i] * 1000;
+    }
+    add_input(
+        count,
+        pwm_input,
+        false,  // failsafe
+        0, // check me
+        0  // link quality
+        );
+}
+
+#endif // AP_RCPROTOCOL_FDM_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_FDM.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_FDM_ENABLED
+
+#include "AP_RCProtocol_Backend.h"
+
+class AP_RCProtocol_FDM : public AP_RCProtocol_Backend {
+public:
+
+    using AP_RCProtocol_Backend::AP_RCProtocol_Backend;
+
+    void update() override;
+
+    bool active() const { return last_input_ms > 0; }
+
+private:
+
+    uint32_t last_input_ms;
+};
+
+
+#endif  // AP_RCPROTOCOL_FDM_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp

@@ -8,6 +8,10 @@
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <SITL/SITL.h>
 
+#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;

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
 };
 
 

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