Move navio2 RC input into AP_RCProtocol

libraries/AP_HAL/board/linux.h

@@ -146,6 +146,7 @@
     #define HAL_MAG_PROBE_LIST HAL_MAG_PROBE1; HAL_MAG_PROBE2
     #define HAL_PROBE_EXTERNAL_I2C_COMPASSES
     #define AP_NOTIFY_SYSFS_LED_ENABLED 1
+    #define AP_RCPROTOCOL_EMLID_RCIO_ENABLED 1
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
     #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_YAW_270)
     #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611")
@@ -284,6 +285,7 @@
     #define HAL_GPS1_TYPE_DEFAULT 9
     #define HAL_CAN_DRIVER_DEFAULT 1
     #define AP_NOTIFY_RCOUTPUTRGBLEDINVERTED_LED_ENABLED 1
+    #define AP_RCPROTOCOL_EMLID_RCIO_ENABLED 1
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
     #define HAL_INS_PROBE_LIST PROBE_IMU_SPI2(Invensense, "rst_g", "rst_a", ROTATION_ROLL_180_YAW_90, ROTATION_ROLL_180_YAW_90)
     #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611")

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -22,7 +22,6 @@
 #include "OpticalFlow_Onboard.h"
 #include "RCInput.h"
 #include "RCInput_AioPRU.h"
-#include "RCInput_Navio2.h"
 #include "RCInput_PRU.h"
 #include "RCInput_RPI.h"
 #include "RCInput_SoloLink.h"
@@ -183,13 +182,12 @@ static RCInput_UDP  rcinDriver;
 static RCInput_Multi rcinDriver{2, NEW_NOTHROW RCInput_RCProtocol("/dev/uart-sbus", "/dev/uart-sumd"), NEW_NOTHROW RCInput_UDP()};
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
 static RCInput_SoloLink rcinDriver;
-#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
-      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
-static RCInput_Navio2 rcinDriver;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
 static RCInput_RCProtocol rcinDriver{"/dev/ttyPS0", NULL};
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_VNAV || \
-      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
+      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \
+      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
+      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
 // this is needed to allow for RC input using SERIALn_PROTOCOL=23. No fd is opened
 // in the linux driver and instead user needs to provide a uart via SERIALn_PROTOCOL
 static RCInput_RCProtocol rcinDriver{nullptr, nullptr};

libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp

@@ -34,7 +34,9 @@
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \
     CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
     CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_VNAV || \
-    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
 
 extern const AP_HAL::HAL& hal;
 

libraries/AP_HAL_Linux/RCInput_RCProtocol.h

@@ -23,7 +23,9 @@
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \
     CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
     CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_VNAV || \
-    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
+    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
 
 namespace Linux {
 

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;

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;

libraries/AP_HAL_Linux/RCInput_Navio2.cpp → libraries/AP_RCProtocol/AP_RCProtocol_Emlid_RCIO.cpp

@@ -1,7 +1,8 @@
-#include <AP_HAL/AP_HAL.h>
+#include "AP_RCProtocol_config.h"
 
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
-    CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
+#if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
+
+#include "AP_RCProtocol_Emlid_RCIO.h"
 
 #include <cstdio>
 #include <unistd.h>
@@ -10,61 +11,51 @@
 
 #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()
+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("[RCInput_Navio2]: failed to open channels");
+            AP_HAL::panic("AP_RCProtocol_Emlid_RCIO: failed to open channels");
         }
     }
 }
 
-void RCInput_Navio2::_timer_tick(void)
+void AP_RCProtocol_Emlid_RCIO::update(void)
 {
     if (AP_HAL::micros() - _last_timestamp < 10000) {
         return;
     }
 
-    char buffer[12];
+    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);
     }
 
-    _update_periods(periods, ARRAY_SIZE(periods));
+    add_input(ARRAY_SIZE(periods), periods, false);
 
     _last_timestamp = AP_HAL::micros();
 }
 
-RCInput_Navio2::RCInput_Navio2()
-{
-
-}
-
-RCInput_Navio2::~RCInput_Navio2()
-{
-}
-
-int RCInput_Navio2::open_channel(int channel)
+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("[RCInput_Navio2]: not enough memory");
+        AP_HAL::panic("AP_RCProtocol_Emlid_RCIO: not enough memory");
     }
 
     int fd = ::open(channel_path, O_RDONLY|O_CLOEXEC);
@@ -74,5 +65,4 @@ int RCInput_Navio2::open_channel(int channel)
     return fd;
 }
 
-
-#endif
+#endif  // AP_RCPROTOCOL_EMLID_RCIO_ENABLED

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

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