diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp index 82cb914872..be17b084b3 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp +++ b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp @@ -38,13 +38,11 @@ #include #include +using namespace time_literals; + 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) { @@ -57,50 +55,44 @@ NavioSysRCInput::~NavioSysRCInput() _isRunning = false; - for (int i = 0; i < _channels; ++i) { - if (_ch_fd[i] != -1) { - ::close(_ch_fd[i]); - } + for (int i = 0; i < CHANNELS; ++i) { + ::close(_channel_fd[i]); } + + ::close(_connected_fd); + + perf_free(_publish_interval_perf); } int NavioSysRCInput::navio_rc_init() { - int i; - char buf[64]; + _connected_fd = ::open("/sys/kernel/rcio/rcin/connected", O_RDONLY); - for (i = 0; i < _channels; ++i) { - ::snprintf(buf, sizeof(buf), "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i); + for (int i = 0; i < CHANNELS; ++i) { + char buf[80] {}; + ::snprintf(buf, sizeof(buf), "%s/ch%d", "/sys/kernel/rcio/rcin", i); int fd = ::open(buf, O_RDONLY); if (fd < 0) { - PX4_WARN("error: open %d failed", i); + PX4_ERR("open %s (%d) failed", buf, i); break; } - _ch_fd[i] = fd; - } - - for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { - _data.values[i] = UINT16_MAX; + _channel_fd[i] = fd; } - return 0; + return PX4_OK; } int NavioSysRCInput::start() { - int result = navio_rc_init(); - - if (result != 0) { - PX4_WARN("error: RC initialization failed"); - return -1; - } + navio_rc_init(); _should_exit.store(false); - ScheduleOnInterval(RCINPUT_MEASURE_INTERVAL_US); - return result; + ScheduleOnInterval(10_ms); // 100 Hz + + return PX4_OK; } void NavioSysRCInput::stop() @@ -115,38 +107,65 @@ void NavioSysRCInput::Run() return; } - uint64_t ts = 0; - char buf[12] {}; + char connected_buf[12] {}; + int ret_connected = ::pread(_connected_fd, connected_buf, sizeof(connected_buf) - 1, 0); + + if (ret_connected < 0) { + return; + } + + input_rc_s data{}; + + connected_buf[sizeof(connected_buf) - 1] = '\0'; + _connected = (atoi(connected_buf) == 1); - for (int i = 0; i < _channels; ++i) { - int res; + data.rc_lost = !_connected; - if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) { - _data.values[i] = UINT16_MAX; + uint64_t timestamp_sample = hrt_absolute_time(); + + for (int i = 0; i < CHANNELS; ++i) { + char buf[12] {}; + int res = ::pread(_channel_fd[i], buf, sizeof(buf) - 1, 0); + + if (res < 0) { continue; } - ts = hrt_absolute_time(); - buf[sizeof(buf) - 1] = '\0'; - _data.values[i] = atoi(buf); + data.values[i] = atoi(buf); + } + + // check if all channels are 0 + bool all_zero = true; + + for (int i = 0; i < CHANNELS; ++i) { + if (data.values[i] != 0) { + all_zero = false; + } } - 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); + if (all_zero) { + return; } + + data.timestamp_last_signal = timestamp_sample; + data.channel_count = CHANNELS; + data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; + data.timestamp = hrt_absolute_time(); + + _input_rc_pub.publish(data); + perf_count(_publish_interval_perf); +} + +int NavioSysRCInput::print_status() +{ + PX4_INFO("Running"); + PX4_INFO("connected: %d", _connected); + + perf_print_counter(_publish_interval_perf); + + return 0; } }; // 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 index 8d8013de84..08601b9ebe 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.hpp +++ b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.hpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -57,6 +58,8 @@ public: /* @return 0 on success, -errno on failure */ void stop(); + int print_status(); + bool isRunning() { return _isRunning; } private: @@ -70,11 +73,13 @@ private: 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] {}; + static constexpr int CHANNELS{14}; + int _channel_fd[CHANNELS] {}; + int _connected_fd{-1}; - input_rc_s _data{}; + bool _connected{false}; + perf_counter_t _publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")}; }; }; // namespace navio_sysfs_rc_in diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp b/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp index 2b450df91b..ea3c8e3fcb 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp +++ b/boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in_main.cpp @@ -109,11 +109,11 @@ extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - if (rc_input != nullptr && rc_input->isRunning()) { - PX4_INFO("running"); + if (rc_input != nullptr) { + rc_input->print_status(); } else { - PX4_INFO("not running\n"); + PX4_INFO("not running"); } return 0;