diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index f8bc3b55b385b..397ce952604d9 100644 --- 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 index bcf7038ba96bd..8a075f5c8d0c1 100644 --- 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 new file mode 100644 index 0000000000000..08729870ff1e1 --- /dev/null +++ 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 +#include +#include +#include + +#include + +#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 new file mode 100644 index 0000000000000..19e07255efacf --- /dev/null +++ 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 index 4cb2b65124611..2ea39731393df 100644 --- 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 \ No newline at end of file +#endif