AP_RCProtocol: move navio2 RC input into AP_RCProtocol

Author: peterbarker

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

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
     };
@@ -197,6 +200,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;

libraries/AP_RCProtocol/AP_RCProtocol_Emlid_RCIO.cpp

@@ -0,0 +1,75 @@
+#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>
+
+#include <GCS_MAVLink/GCS.h>
+
+#define RCIN_SYSFS_PATH "/sys/kernel/rcio/rcin"
+
+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\n");
+    }
+
+    int fd = ::open(channel_path, O_RDONLY|O_CLOEXEC);
+
+    free(channel_path);
+
+    return fd;
+}
+
+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\n");
+        }
+    }
+}
+
+void AP_RCProtocol_Emlid_RCIO::update(void)
+{
+    if (!init_done) {
+        init();
+        init_done = true;
+    }
+
+    if (AP_HAL::micros() - _last_timestamp < 10000) {
+        return;
+    }
+    _last_timestamp = AP_HAL::micros();
+
+    uint16_t periods[16];
+
+    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;
+        }
+        periods[i] = atoi(buffer);
+    }
+
+    add_input(
+        ARRAY_SIZE(periods),
+        periods,
+        false,
+        -1,
+        0
+        );
+
+}
+
+#endif  // AP_RCPROTOCOL_EMLID_RCIO_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_Emlid_RCIO.h

@@ -0,0 +1,27 @@
+#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:
+    bool init_done;
+    void init();
+
+    uint32_t _last_timestamp;
+
+    int channels[16];
+
+    int open_channel(int channel);
+};
+
+#endif  // AP_RCPROTOCOL_EMLID_RCIO_ENABLED

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