diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake index 23148c25f1..1ef5202c00 100644 --- a/cmake/configs/posix_bebop_default.cmake +++ b/cmake/configs/posix_bebop_default.cmake @@ -16,6 +16,7 @@ set(config_module_list # Board support modules # drivers/device + drivers/linux_sbus modules/sensors platforms/posix/drivers/df_ms5607_wrapper platforms/posix/drivers/df_mpu6050_wrapper diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake index 0d4bc2c9dd..53dbeec172 100644 --- a/cmake/configs/posix_eagle_hil.cmake +++ b/cmake/configs/posix_eagle_hil.cmake @@ -22,6 +22,7 @@ set(config_module_list drivers/device drivers/boards/sitl drivers/led + drivers/linux_sbus systemcmds/param systemcmds/ver diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index fa79eab85f..e4f45c8fc0 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -74,6 +74,7 @@ set(config_module_list # # PX4 drivers # + drivers/linux_sbus drivers/gps drivers/navio_adc drivers/navio_sysfs_rc_in diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake index 72e1604314..71b6e47ec4 100644 --- a/cmake/configs/posix_sdflight_default.cmake +++ b/cmake/configs/posix_sdflight_default.cmake @@ -30,6 +30,7 @@ set(CONFIG_SHMEM "1") set(config_module_list drivers/device drivers/blinkm + drivers/linux_sbus drivers/pwm_out_sim drivers/rgbled drivers/led diff --git a/cmake/configs/posix_sdflight_legacy.cmake b/cmake/configs/posix_sdflight_legacy.cmake index 8d98adfbcb..b655f8ed0b 100644 --- a/cmake/configs/posix_sdflight_legacy.cmake +++ b/cmake/configs/posix_sdflight_legacy.cmake @@ -22,6 +22,7 @@ set(CONFIG_SHMEM "1") set(config_module_list drivers/device drivers/blinkm + drivers/linux_sbus drivers/pwm_out_sim drivers/rgbled drivers/led diff --git a/src/drivers/linux_pwm_out/PCA9685.cpp b/src/drivers/linux_pwm_out/PCA9685.cpp index 7504d07b9e..6059c645b2 100644 --- a/src/drivers/linux_pwm_out/PCA9685.cpp +++ b/src/drivers/linux_pwm_out/PCA9685.cpp @@ -55,7 +55,7 @@ using namespace linux_pwm_out; -void PCA9685::init(int bus, int address) +int PCA9685::init(int bus, int address) { _fd = open_fd(bus, address); reset(); @@ -65,6 +65,7 @@ void PCA9685::init(int bus, int address) /* 200HZ for 12bit Resolution, supported by most of the esc's */ setPWMFreq(200); usleep(1000 * 1000); + return 0; } int PCA9685::send_output_pwm(const uint16_t *pwm, int num_outputs) diff --git a/src/drivers/linux_pwm_out/PCA9685.h b/src/drivers/linux_pwm_out/PCA9685.h index 128ff92085..d54c263605 100644 --- a/src/drivers/linux_pwm_out/PCA9685.h +++ b/src/drivers/linux_pwm_out/PCA9685.h @@ -87,10 +87,10 @@ public: int send_output_pwm(const uint16_t *pwm, int num_outputs) override; - void init(int bus, int address); + int init(int bus, int address); virtual ~PCA9685(); - /// Sets PCA9685 mode to 00 + /** Sets PCA9685 mode to 00 */ void reset(); /** diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 42c3c66ddc..6c845520fb 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -211,12 +211,18 @@ void task_main(int argc, char *argv[]) PX4_INFO("Starting PWM output in ocpoc_mmap mode"); pwm_out = new OcpocMmapPWMOut(_max_num_outputs); - } else { // navio + } else { /* navio */ PX4_INFO("Starting PWM output in Navio mode"); pwm_out = new NavioSysfsPWMOut(_device, _max_num_outputs); } - if (pwm_out->init() != 0) { + /** + * if the _protocol is "pca9685" and the driver is executed correctly， + * the return value of "pwm_out->init()" will be higher than 0. + */ + bool check_pwm_device = (strcmp(_protocol, "pca9685") == 0) ? (0 > pwm_out->init()) : (pwm_out->init() != 0); + + if (check_pwm_device) { PX4_ERR("PWM output init failed"); delete pwm_out; return; @@ -494,7 +500,7 @@ int linux_pwm_out_main(int argc, char *argv[]) } } - // gets the parameters for the esc's pwm + /** gets the parameters for the esc's pwm */ param_get(param_find("PWM_DISARMED"), &linux_pwm_out::_pwm_disarmed); param_get(param_find("PWM_MIN"), &linux_pwm_out::_pwm_min); param_get(param_find("PWM_MAX"), &linux_pwm_out::_pwm_max); diff --git a/src/drivers/linux_sbus/CMakeLists.txt b/src/drivers/linux_sbus/CMakeLists.txt new file mode 100644 index 0000000000..7f62d5f129 --- /dev/null +++ b/src/drivers/linux_sbus/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2017 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. +# +############################################################################ +px4_add_module( + MODULE drivers__linux_sbus + MAIN linux_sbus + COMPILE_FLAGS + SRCS + linux_sbus.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/linux_sbus/linux_sbus.cpp b/src/drivers/linux_sbus/linux_sbus.cpp new file mode 100644 index 0000000000..44f5292a4a --- /dev/null +++ b/src/drivers/linux_sbus/linux_sbus.cpp @@ -0,0 +1,364 @@ +/**************************************************************************** + * + * Copyright (C) 2017 Mark Charl. All rights reserved. + * Copyright (C) 2017 Fan.zhang. All rights reserved. + * Copyright (C) 2017 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 "linux_sbus.h" +using namespace linux_sbus; +//---------------------------------------------------------------------------------------------------------// +int RcInput::init() +{ + int i; + + /** + * initialize the data of each channel + */ + for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { + _data.values[i] = UINT16_MAX; + } + + _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data); + + if (nullptr == _rcinput_pub) { + PX4_WARN("error: advertise failed"); + return -1; + } + + /** + * open the serial port + */ + _device_fd = open(_device, O_RDWR | O_NONBLOCK | O_CLOEXEC); + + if (-1 == _device_fd) { + PX4_ERR("Open SBUS input %s failed, status %d \n", _device, + (int) _device_fd); + fflush(stdout); + return -1; + } + + struct termios2 tio { }; + + if (0 != ioctl(_device_fd, TCGETS2, &tio)) { + close(_device_fd); + _device_fd = -1; + return -1; + } + + /** + * Setting serial port,8E2, non-blocking.100Kbps + */ + tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL + | IXON); + tio.c_iflag |= (INPCK | IGNPAR); + tio.c_oflag &= ~OPOST; + tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD); + /** + * use BOTHER to specify speed directly in c_[io]speed member + */ + tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD); + tio.c_ispeed = 100000; + tio.c_ospeed = 100000; + tio.c_cc[VMIN] = 25; + tio.c_cc[VTIME] = 0; + + if (0 != ioctl(_device_fd, TCSETS2, &tio)) { + close(_device_fd); + _device_fd = -1; + return -1; + } + + return 0; +} +//---------------------------------------------------------------------------------------------------------// +int RcInput::start(char *device, int channels) +{ + int result = 0; + strcpy(_device, device); + PX4_WARN("Device %s , channels: %d \n", device, channels); + _channels = channels; + result = init(); + + if (0 != result) { + 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() +{ + close(_device_fd); + _shouldExit = true; +} +//---------------------------------------------------------------------------------------------------------// +void RcInput::cycle_trampoline(void *arg) +{ + RcInput *dev = reinterpret_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; + int nread; + fd_set fds; + FD_ZERO(&fds); + FD_SET(_device_fd, &fds); + /** + *error counter to count the lost frame + */ + int count = 0; // + + while (1) { + nread = read(_device_fd, &_sbusData, sizeof(_sbusData)); + + if (25 == nread) { + /** + * Notice: most sbus rx device support sbus1 + */ + if (0x0f == _sbusData[0] && 0x00 == _sbusData[24]) { + break; + } + } + + ++count; + usleep(RCINPUT_MEASURE_INTERVAL_US); + } + + /** + * parse sbus data to pwm + */ + _channels_data[0] = + (uint16_t)(((_sbusData[1] | _sbusData[2] << 8) & 0x07FF) + * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[1] = (uint16_t)(((_sbusData[2] >> 3 | _sbusData[3] << 5) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[2] = (uint16_t)(((_sbusData[3] >> 6 | _sbusData[4] << 2 + | _sbusData[5] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + + SBUS_SCALE_OFFSET; + _channels_data[3] = (uint16_t)(((_sbusData[5] >> 1 | _sbusData[6] << 7) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[4] = (uint16_t)(((_sbusData[6] >> 4 | _sbusData[7] << 4) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[5] = (uint16_t)(((_sbusData[7] >> 7 | _sbusData[8] << 1 + | _sbusData[9] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + + SBUS_SCALE_OFFSET; + _channels_data[6] = (uint16_t)(((_sbusData[9] >> 2 | _sbusData[10] << 6) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[7] = (uint16_t)(((_sbusData[10] >> 5 | _sbusData[11] << 3) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; // & the other 8 + 2 channels if you need them + _channels_data[8] = (uint16_t)(((_sbusData[12] | _sbusData[13] << 8) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[9] = (uint16_t)(((_sbusData[13] >> 3 | _sbusData[14] << 5) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[10] = (uint16_t)(((_sbusData[14] >> 6 | _sbusData[15] << 2 + | _sbusData[16] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + + SBUS_SCALE_OFFSET; + _channels_data[11] = (uint16_t)(((_sbusData[16] >> 1 | _sbusData[17] << 7) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[12] = (uint16_t)(((_sbusData[17] >> 4 | _sbusData[18] << 4) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[13] = (uint16_t)(((_sbusData[18] >> 7 | _sbusData[19] << 1 + | _sbusData[20] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + + SBUS_SCALE_OFFSET; + _channels_data[14] = (uint16_t)(((_sbusData[20] >> 2 | _sbusData[21] << 6) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + _channels_data[15] = (uint16_t)(((_sbusData[21] >> 5 | _sbusData[22] << 3) + & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; + int i = 0; + + for (i = 0; i < _channels; ++i) { + _data.values[i] = _channels_data[i]; + } + + ts = hrt_absolute_time(); + _data.timestamp = ts; + _data.timestamp_last_signal = ts; + _data.channel_count = _channels; + _data.rssi = 100; + _data.rc_lost_frame_count = count; + _data.rc_total_frame_count = 1; + _data.rc_ppm_frame_length = 0; + _data.rc_failsafe = (_sbusData[23] & (1 << 3)) ? true : false; + _data.rc_lost = (_sbusData[23] & (1 << 2)) ? true : false; + _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data); +} +//---------------------------------------------------------------------------------------------------------// +/** + * Print the correct usage. + */ +static void linux_sbus::usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PX4_INFO("Usage: linux_sbus {start|stop|status} -d -c "); +} +//---------------------------------------------------------------------------------------------------------// +int linux_sbus_main(int argc, char **argv) +{ + int start; + int command = -1; + /** + * ttyS1 for default, it can be changed through -d parameter + */ + char device[30] = "/dev/ttyS1"; + /** + * 8 channel for default setting, it can be changed through -c parameter + */ + int max_channel = 8; + + /** + * Parse command line and get device and channels count from consolex + */ + for (start = 0; start < argc; ++start) { + if (0 == strcmp(argv[start], "start")) { + command = 0; + continue; + } + + if (0 == strcmp(argv[start], "stop")) { + command = 1; + continue; + } + + if (0 == strcmp(argv[start], "status")) { + command = 2; + continue; + } + + if (0 == strcmp(argv[start], "-d")) { + if (argc > (start + 1)) { + strcpy(device, argv[start + 1]); + } + + continue; + } + + if (0 == strcmp(argv[start], "-c")) { + if (argc > (start + 1)) { + max_channel = atoi(argv[start + 1]); + } + + continue; + } + } + + /** + * Channels count can't be higher than 16; + */ + max_channel = (max_channel > 16) ? 16 : max_channel; + + if (0 == command) { + if (nullptr != rc_input && rc_input->isRunning()) { + PX4_WARN("running"); + return 0; + } + + rc_input = new RcInput(); + + /** Check if alloc worked. */ + if (nullptr == rc_input) { + PX4_ERR("Sbus driver initialization failed"); + return -1; + } + + int ret = rc_input->start(device, max_channel); + + if (ret != 0) { + PX4_ERR("Linux sbus module failure"); + } + + return 0; + } + + if (1 == command) { + if (rc_input == nullptr || !rc_input->isRunning()) { + PX4_WARN("Not running \n"); + /* this is not an error */ + return 0; + } + + rc_input->stop(); + /** + * Wait for task to die + */ + int i = 0; + + do { + /* wait up to 100ms */ + usleep(100000); + } while (rc_input->isRunning() && ++i < 30); + + delete rc_input; + rc_input = nullptr; + return 0; + } + + if (2 == command) { + if (rc_input != nullptr && rc_input->isRunning()) { + PX4_INFO("running"); + + } else { + PX4_INFO("Not running \n"); + } + + return 0; + } + + linux_sbus::usage( + "Usage: linux_sbus start|stop|status -d -c "); + return 0; +} +//---------------------------------------------------------------------------------------------------------// diff --git a/src/drivers/linux_sbus/linux_sbus.h b/src/drivers/linux_sbus/linux_sbus.h new file mode 100644 index 0000000000..e78e148eaa --- /dev/null +++ b/src/drivers/linux_sbus/linux_sbus.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (C) 2017 Mark Charl. All rights reserved. + * Copyright (C) 2017 Fan.zhang. All rights reserved. + * Copyright (C) 2017 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 +#include +#include +#include +#include +#include +#include +#include +#include +/*For terminal I/O interfaces, termbits.h from asm-generic versions of functions guarantees non-standard communication (100Khz, Non-blocking), not guaranteed by termios.h*/ +#include +#include +#include +#include +#include +#include +#include +#include +/* The interval between each frame is 4700us, do not change it. */ +#define RCINPUT_MEASURE_INTERVAL_US 4700 +/* define range mapping here, -+100% -> 1000..2000 */ +#define SBUS_RANGE_MIN 200.0f +#define SBUS_RANGE_MAX 1800.0f +#define SBUS_TARGET_MIN 1000.0f +#define SBUS_TARGET_MAX 2000.0f +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) +#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) + +namespace linux_sbus +{ +class RcInput +{ +public: + RcInput() : + _shouldExit(false), + _isRunning(false), + _work { }, + _rcinput_pub(nullptr), + _data { }, _sbusData { 0x0f, 0x01, 0x04, 0x20, 0x00, + 0xff, 0x07, 0x40, 0x00, 0x02, + 0x10, 0x80, 0x2c, 0x64, 0x21, + 0x0b, 0x59, 0x08, 0x40, 0x00, + 0x02, 0x10, 0x80, 0x00, 0x00 } + { } + ~RcInput() + { + work_cancel(HPWORK, &_work); + _isRunning = false; + close(_device_fd); + } + /** @return 0 on success, -errno on failure */ + int start(char *device, int channels); + void stop(); + + /** Trampoline for the work queue. */ + static void cycle_trampoline(void *arg); + + bool isRunning() + { + return _isRunning; + } + +private: + void _cycle(); + void _measure(); + bool _shouldExit; + bool _isRunning; + struct work_s _work; + orb_advert_t _rcinput_pub; + struct input_rc_s _data; + uint8_t _sbusData[25]; + int _channels; + int _device_fd; /** serial port device to read SBUS; */ + int _channels_data[16]; /** 16 channels support; */ + uint8_t _buffer[25]; + char _device[30]; + bool _failsafe; + bool _rc_loss; + int init(); +}; + +static void usage(const char *reason); +static RcInput *rc_input = nullptr; +} +extern "C" __EXPORT int linux_sbus_main(int argc, char **argv);