AP_HAL_Linux: added RCInput for Navio2

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -28,6 +28,7 @@ namespace Linux {
     class RCInput_AioPRU;
     class RCInput_RPI;
     class RCInput_Raspilot;
+    class RCInput_Navio2;
     class RCInput_ZYNQ;
     class RCInput_UART;
     class RCInput_UDP;

libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h

@@ -20,6 +20,7 @@
 #include "RCInput.h"
 #include "RCInput_AioPRU.h"
 #include "RCInput_RPI.h"
+#include "RCInput_Navio2.h"
 #include "RCInput_UART.h"
 #include "RCInput_UDP.h"
 #include "RCInput_Raspilot.h"

libraries/AP_HAL_Linux/RCInput_Navio2.cpp

@@ -0,0 +1,76 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
+#include <cstdio>
+#include <unistd.h>
+#include <fcntl.h>
+#include <cstdlib>
+
+#include <AP_Common/AP_Common.h>
+
+#include "RCInput_Navio2.h"
+
+using namespace Linux;
+
+extern const AP_HAL::HAL& hal;
+
+#define RCIN_SYSFS_PATH "/sys/kernel/rcio/rcin"
+
+void RCInput_Navio2::init()
+{
+    for (size_t i = 0; i < ARRAY_SIZE(channels); i++) {
+        channels[i] = open_channel(i);
+        if (channels[i] < 0) {
+            AP_HAL::panic("[RCInput_Navio2]: failed to open channels\n");
+        }
+    }
+}
+
+void RCInput_Navio2::_timer_tick(void)
+{
+    if (AP_HAL::micros() - _last_timestamp < 10000) {
+        return;
+    }
+
+    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);
+    }
+
+    _update_periods(periods, ARRAY_SIZE(periods));
+
+    _last_timestamp = AP_HAL::micros();
+}
+
+RCInput_Navio2::RCInput_Navio2()
+{
+
+}
+
+RCInput_Navio2::~RCInput_Navio2()
+{
+}
+
+int RCInput_Navio2::open_channel(int channel)
+{
+    char *channel_path;
+    if (asprintf(&channel_path, "%s/ch%d", RCIN_SYSFS_PATH, channel) == -1) {
+        AP_HAL::panic("[RCInput_Navio2]: not enough memory\n");
+    }
+
+    int fd = ::open(channel_path, O_RDONLY);
+
+    free(channel_path);
+
+    return fd;
+}
+
+
+#endif

libraries/AP_HAL_Linux/RCInput_Navio2.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "RCInput.h"
+
+class Linux::RCInput_Navio2 : public Linux::RCInput
+{
+public:
+    void init() override;
+    void _timer_tick(void) override;
+    RCInput_Navio2();
+    ~RCInput_Navio2();
+
+private:
+    int open_channel(int ch);
+
+    uint64_t _last_timestamp = 0l;
+    static const size_t CHANNEL_COUNT = 8;
+    int channels[CHANNEL_COUNT];
+    uint16_t periods[ARRAY_SIZE(channels)] = {0};
+};