AP_RCProtocol: move navio2 RC input into AP_RCProtocol

ArduPilot · Feb 10, 2025 · 25f7eeb · 25f7eeb
1 parent 261a804
commit 25f7eeb
Show file tree

Hide file tree

Showing 5 changed files with 118 additions and 1 deletion.
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
@@ -23,6 +23,7 @@
 
 #include "AP_RCProtocol_PPMSum.h"
 #include "AP_RCProtocol_DSM.h"
+#include "AP_RCProtocol_Emlid_RCIO.h"
 #include "AP_RCProtocol_IBUS.h"
 #include "AP_RCProtocol_SBUS.h"
 #include "AP_RCProtocol_SUMD.h"
@@ -114,6 +115,9 @@ void AP_RCProtocol::init()
 #if AP_RCPROTOCOL_RADIO_ENABLED
     backend[AP_RCProtocol::RADIO] = NEW_NOTHROW AP_RCProtocol_Radio(*this);
 #endif
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+    backend[AP_RCProtocol::EMLID_RCIO] = new AP_RCProtocol_Emlid_RCIO(*this);
+#endif
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -478,6 +482,9 @@ bool AP_RCProtocol::new_input()
 #endif
 #if AP_RCPROTOCOL_RADIO_ENABLED
         AP_RCProtocol::RADIO,
+#endif
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+        AP_RCProtocol::EMLID_RCIO,
 #endif
     };
     for (const auto protocol : pollable) {
@@ -630,6 +637,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
 #if AP_RCPROTOCOL_RADIO_ENABLED
     case RADIO:
         return "Radio";
+#endif
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+    case EMLID_RCIO:
+        return "Emlid RCIO";
 #endif
     case NONE:
         break;

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -92,6 +92,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_RADIO_ENABLED
         RADIO = 19,
+#endif
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+        EMLID_RCIO = 21,
 #endif
         NONE    //last enum always is None
     };
@@ -199,6 +202,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_RADIO_ENABLED
         case RADIO:
+#endif
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+        case EMLID_RCIO:
 #endif
         case NONE:
             return false;

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Emlid_RCIO.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Emlid_RCIO.cpp
@@ -0,0 +1,68 @@
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+
+#include "AP_RCProtocol_Emlid_RCIO.h"
+
+#include <cstdio>
+#include <unistd.h>
+#include <fcntl.h>
+#include <cstdlib>
+
+#include <AP_Common/AP_Common.h>
+
+#define RCIN_SYSFS_PATH "/sys/kernel/rcio/rcin"
+
+void AP_RCProtocol_Emlid_RCIO::init()
+{
+    for (size_t i = 0; i < ARRAY_SIZE(channels); i++) {
+        channels[i] = open_channel(i);
+        if (channels[i] < 0) {
+            AP_HAL::panic("AP_RCProtocol_Emlid_RCIO: failed to open channels");
+        }
+    }
+}
+
+void AP_RCProtocol_Emlid_RCIO::update(void)
+{
+    if (AP_HAL::micros() - _last_timestamp < 10000) {
+        return;
+    }
+
+    if (!init_done) {
+        init();
+        init_done = true;
+    }
+
+    uint16_t periods[CHANNEL_COUNT]{};
+
+    char buffer[12];
+    for (size_t i = 0; i < ARRAY_SIZE(channels); i++) {
+        if (::pread(channels[i], buffer, sizeof(buffer) - 1, 0) <= 0) {
+            /* We ignore error in order not to spam the console */
+            continue;
+        }
+        buffer[sizeof(buffer) - 1]  = '\0';
+        periods[i] = atoi(buffer);
+    }
+
+    add_input(ARRAY_SIZE(periods), periods, false);
+
+    _last_timestamp = AP_HAL::micros();
+}
+
+int AP_RCProtocol_Emlid_RCIO::open_channel(int channel)
+{
+    char *channel_path;
+    if (asprintf(&channel_path, "%s/ch%d", RCIN_SYSFS_PATH, channel) == -1) {
+        AP_HAL::panic("AP_RCProtocol_Emlid_RCIO: not enough memory");
+    }
+
+    int fd = ::open(channel_path, O_RDONLY|O_CLOEXEC);
+
+    free(channel_path);
+
+    return fd;
+}
+
+#endif  // AP_RCPROTOCOL_EMLID_RCIO_ENABLED
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Emlid_RCIO.h b/libraries/AP_RCProtocol/AP_RCProtocol_Emlid_RCIO.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+
+#include "AP_RCProtocol_Backend.h"
+
+class AP_RCProtocol_Emlid_RCIO : public AP_RCProtocol_Backend {
+public:
+
+    using AP_RCProtocol_Backend::AP_RCProtocol_Backend;
+
+    void update() override;
+
+private:
+    int open_channel(int channel);
+
+    bool init_done;
+    void init();
+
+    uint32_t _last_timestamp;
+    static const size_t CHANNEL_COUNT = 16;
+    int channels[CHANNEL_COUNT];
+
+};
+
+#endif  // AP_RCPROTOCOL_EMLID_RCIO_ENABLED
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h
@@ -25,6 +25,10 @@
 #define AP_RCPROTOCOL_DSM_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
 #endif
 
+#ifndef AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+#define AP_RCPROTOCOL_EMLID_RCIO_ENABLED 0
+#endif
+
 #ifndef AP_RCPROTOCOL_FPORT_ENABLED
 #define AP_RCPROTOCOL_FPORT_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
 #endif
@@ -90,4 +94,4 @@
 
 #ifndef AP_RCPROTOCOL_FDM_ENABLED
 #define AP_RCPROTOCOL_FDM_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
-#endif
+#endif