From 17ba5dd04a507d744d786ea08487ec2c34ab6ce9 Mon Sep 17 00:00:00 2001 From: crossa <421395590@qq.com> Date: Thu, 17 Aug 2017 21:37:09 +0800 Subject: [PATCH] 1.Add sbus driver for linux . This driver can be used to read the inverted S.bus signal and fetch the data of each channel and publish it 2. Fix the bug of linux_pwm_out, when the protocol is pca9685, after the init method is executed,the method of determining the return value of init method is incorrect,this will cause the driver to fail 3. Add linux_sbus driver to other posix prefixes cmake files --- cmake/configs/posix_bebop_default.cmake | 1 + cmake/configs/posix_eagle_hil.cmake | 1 + cmake/configs/posix_rpi_common.cmake | 1 + cmake/configs/posix_sdflight_default.cmake | 1 + cmake/configs/posix_sdflight_legacy.cmake | 1 + src/drivers/linux_pwm_out/PCA9685.cpp | 3 +- src/drivers/linux_pwm_out/PCA9685.h | 4 +- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 12 +- src/drivers/linux_sbus/CMakeLists.txt | 42 +++ src/drivers/linux_sbus/linux_sbus.cpp | 364 ++++++++++++++++++++ src/drivers/linux_sbus/linux_sbus.h | 121 +++++++ 11 files changed, 545 insertions(+), 6 deletions(-) create mode 100644 src/drivers/linux_sbus/CMakeLists.txt create mode 100644 src/drivers/linux_sbus/linux_sbus.cpp create mode 100644 src/drivers/linux_sbus/linux_sbus.h 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);