diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index c5c85bcf84..99f022024a 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/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; diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index 8f8290049e..21e32a9a57 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/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" diff --git a/libraries/AP_HAL_Linux/RCInput_Navio2.cpp b/libraries/AP_HAL_Linux/RCInput_Navio2.cpp new file mode 100644 index 0000000000..c32adaf2f0 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_Navio2.cpp @@ -0,0 +1,76 @@ +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 +#include +#include +#include +#include + +#include + +#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 diff --git a/libraries/AP_HAL_Linux/RCInput_Navio2.h b/libraries/AP_HAL_Linux/RCInput_Navio2.h new file mode 100644 index 0000000000..ad5eecd2b1 --- /dev/null +++ b/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}; +};