diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt b/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt index 6bb2164df1..d05dabc85e 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt +++ b/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,11 +31,12 @@ # ############################################################################ px4_add_module( - MODULE drivers__navio_sysfs_rc_in + MODULE navio_sysfs_rc_in MAIN navio_sysfs_rc_in - COMPILE_FLAGS SRCS - navio_sysfs_rc_in.cpp + NavioSysRCInput.cpp + NavioSysRCInput.hpp + navio_sysfs_rc_in_main.cpp DEPENDS - ) - + px4_work_queue +) diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp new file mode 100644 index 0000000000..82cb914872 --- /dev/null +++ b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "NavioSysRCInput.hpp" + +#include +#include +#include + +namespace navio_sysfs_rc_in +{ + +#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin" + +#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds + +NavioSysRCInput::NavioSysRCInput() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ + _isRunning = true; +}; + +NavioSysRCInput::~NavioSysRCInput() +{ + ScheduleClear(); + + _isRunning = false; + + for (int i = 0; i < _channels; ++i) { + if (_ch_fd[i] != -1) { + ::close(_ch_fd[i]); + } + } +} + +int NavioSysRCInput::navio_rc_init() +{ + int i; + char buf[64]; + + for (i = 0; i < _channels; ++i) { + ::snprintf(buf, sizeof(buf), "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i); + int fd = ::open(buf, O_RDONLY); + + if (fd < 0) { + PX4_WARN("error: open %d failed", i); + break; + } + + _ch_fd[i] = fd; + } + + for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { + _data.values[i] = UINT16_MAX; + } + + return 0; +} + +int NavioSysRCInput::start() +{ + int result = navio_rc_init(); + + if (result != 0) { + PX4_WARN("error: RC initialization failed"); + return -1; + } + + _should_exit.store(false); + ScheduleOnInterval(RCINPUT_MEASURE_INTERVAL_US); + + return result; +} + +void NavioSysRCInput::stop() +{ + _should_exit.store(true); +} + +void NavioSysRCInput::Run() +{ + if (_should_exit.load()) { + ScheduleClear(); + return; + } + + uint64_t ts = 0; + char buf[12] {}; + + for (int i = 0; i < _channels; ++i) { + int res; + + if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) { + _data.values[i] = UINT16_MAX; + continue; + } + + ts = hrt_absolute_time(); + + buf[sizeof(buf) - 1] = '\0'; + + _data.values[i] = atoi(buf); + } + + if (ts > 0) { + _data.timestamp_last_signal = ts; + _data.channel_count = _channels; + _data.rssi = 100; + _data.rc_lost_frame_count = 0; + _data.rc_total_frame_count = 1; + _data.rc_ppm_frame_length = 100; + _data.rc_failsafe = false; + _data.rc_lost = false; + _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; + _data.timestamp = hrt_absolute_time(); + + _input_rc_pub.publish(_data); + } +} + +}; // namespace navio_sysfs_rc_in diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.hpp b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.hpp new file mode 100644 index 0000000000..8d8013de84 --- /dev/null +++ b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace navio_sysfs_rc_in +{ + +class NavioSysRCInput : public px4::ScheduledWorkItem +{ +public: + NavioSysRCInput(); + ~NavioSysRCInput() override; + + /* @return 0 on success, -errno on failure */ + int start(); + + /* @return 0 on success, -errno on failure */ + void stop(); + + bool isRunning() { return _isRunning; } + +private: + void Run() override; + + int navio_rc_init(); + + px4::atomic_bool _should_exit{false}; + + bool _isRunning{false}; + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + + int _channels{8}; // D8R-II plus + int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; + + input_rc_s _data{}; + +}; + +}; // namespace navio_sysfs_rc_in diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp b/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp similarity index 50% rename from boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp rename to boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp index d5f1cbd7c4..2b450df91b 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp +++ b/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp @@ -32,179 +32,15 @@ * ****************************************************************************/ -#include -#include -#include -#include -#include -#include -#include -#include - #include -#include -#include +#include -#include - -#include -#include +#include "NavioSysRCInput.hpp" namespace navio_sysfs_rc_in { -extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[]); - -#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin" - -#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds - - -class RcInput -{ -public: - RcInput() = default; - - ~RcInput() - { - work_cancel(HPWORK, &_work); - _isRunning = false; - } - - /* @return 0 on success, -errno on failure */ - int start(); - - /* @return 0 on success, -errno on failure */ - void stop(); - - /* Trampoline for the work queue. */ - static void cycle_trampoline(void *arg); - - bool isRunning() { return _isRunning; } - -private: - void _cycle(); - void _measure(); - - int navio_rc_init(); - - bool _shouldExit{false}; - bool _isRunning{false}; - work_s _work{}; - - uORB::PublicationMulti _rcinput_pub{ORB_ID(input_rc)}; - - int _channels{8}; // D8R-II plus - int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; - - input_rc_s _data{}; - -}; - -int RcInput::navio_rc_init() -{ - int i; - char buf[64]; - - for (i = 0; i < _channels; ++i) { - ::snprintf(buf, sizeof(buf), "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i); - int fd = ::open(buf, O_RDONLY); - - if (fd < 0) { - PX4_WARN("error: open %d failed", i); - break; - } - - _ch_fd[i] = fd; - } - - for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { - _data.values[i] = UINT16_MAX; - } - - return 0; -} - -int RcInput::start() -{ - int result = navio_rc_init(); - - if (result != 0) { - PX4_WARN("error: RC initialization failed"); - return -1; - } - - _isRunning = true; - result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0); - - if (result == -1) { - _isRunning = false; - } - - return result; -} - -void RcInput::stop() -{ - _shouldExit = true; -} - -void RcInput::cycle_trampoline(void *arg) -{ - RcInput *dev = static_cast(arg); - dev->_cycle(); -} - -void RcInput::_cycle() -{ - _measure(); - - if (!_shouldExit) { - work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, - USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); - } -} - -void RcInput::_measure(void) -{ - uint64_t ts; - char buf[12]; - - for (int i = 0; i < _channels; ++i) { - int res; - - if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) { - _data.values[i] = UINT16_MAX; - continue; - } - - buf[sizeof(buf) - 1] = '\0'; - - _data.values[i] = atoi(buf); - } - - ts = hrt_absolute_time(); - _data.timestamp = ts; - _data.timestamp_last_signal = ts; - _data.channel_count = _channels; - _data.rssi = 100; - _data.rc_lost_frame_count = 0; - _data.rc_total_frame_count = 1; - _data.rc_ppm_frame_length = 100; - _data.rc_failsafe = false; - _data.rc_lost = false; - _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; - - _rcinput_pub.publish(_data); -} - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) +static void usage(const char *reason) { if (reason) { PX4_ERR("%s", reason); @@ -213,9 +49,9 @@ usage(const char *reason) PX4_INFO("usage: navio_sysfs_rc_in {start|stop|status}"); } -static RcInput *rc_input = nullptr; +static NavioSysRCInput *rc_input = nullptr; -int navio_sysfs_rc_in_main(int argc, char *argv[]) +extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); @@ -230,7 +66,7 @@ int navio_sysfs_rc_in_main(int argc, char *argv[]) return 0; } - rc_input = new RcInput(); + rc_input = new NavioSysRCInput(); // Check if alloc worked. if (rc_input == nullptr) {