From 9612e40464e31485f07efb3ecbd05fb2280969cd Mon Sep 17 00:00:00 2001 From: SalimTerryLi Date: Mon, 3 Feb 2020 21:40:27 +0800 Subject: [PATCH] purge drivers/linux_sbus - rc_input now provides reliable parsing for SBUS on Linux platform. linux_sbus can be fully removed. --- boards/aerotenna/ocpoc/default.cmake | 1 - boards/atlflight/eagle/default.cmake | 1 - boards/atlflight/excelsior/default.cmake | 1 - boards/beaglebone/blue/default.cmake | 1 - boards/emlid/navio2/default.cmake | 1 - boards/px4/raspberrypi/default.cmake | 1 - posix-configs/bbblue/px4.config | 3 +- posix-configs/bbblue/px4_fw.config | 5 +- posix-configs/ocpoc/px4.config | 3 +- src/drivers/linux_sbus/CMakeLists.txt | 41 --- src/drivers/linux_sbus/linux_sbus.cpp | 364 ----------------------- src/drivers/linux_sbus/linux_sbus.h | 119 -------- 12 files changed, 4 insertions(+), 537 deletions(-) delete mode 100644 src/drivers/linux_sbus/CMakeLists.txt delete mode 100644 src/drivers/linux_sbus/linux_sbus.cpp delete mode 100644 src/drivers/linux_sbus/linux_sbus.h diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index 9cccabb512..4f09cda49e 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -28,7 +28,6 @@ px4_add_board( imu/mpu9250 lights/rgbled linux_pwm_out - linux_sbus #magnetometer # all available magnetometer drivers magnetometer/hmc5883 pwm_out_sim diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index f317dd7163..1105043870 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -50,7 +50,6 @@ px4_add_board( gps #imu # all available imu drivers #lights/rgbled - linux_sbus #magnetometer # all available magnetometer drivers pwm_out_sim qshell/posix diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index c5701519ca..65d5ab4611 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -50,7 +50,6 @@ px4_add_board( gps #imu # all available imu drivers #lights/rgbled - linux_sbus #magnetometer # all available magnetometer drivers pwm_out_sim qshell/posix diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index af78070f1f..a9b5a29039 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -20,7 +20,6 @@ px4_add_board( #imu # all available imu drivers imu/mpu9250 linux_pwm_out - linux_sbus #magnetometer # all available magnetometer drivers magnetometer/hmc5883 pwm_out_sim diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index b7b051440d..e4c11fe3f1 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -21,7 +21,6 @@ px4_add_board( imu/mpu9250 imu/st/lsm9ds1 linux_pwm_out - linux_sbus #magnetometer # all available magnetometer drivers magnetometer/hmc5883 magnetometer/lsm9ds1_mag diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index b316bc3605..a4a7e1fa4b 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -19,7 +19,6 @@ px4_add_board( #imu # all available imu drivers imu/mpu9250 linux_pwm_out - linux_sbus #magnetometer # all available magnetometer drivers magnetometer/hmc5883 pca9685_pwm_out diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index 7bd79d6a72..0d205ac7c8 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -77,8 +77,7 @@ mavlink start -n SoftAp -x -u 14556 -r 1000000 sleep 1 -linux_sbus start -d /dev/ttyS4 -c 16 -# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus) +# RC port is mapped to /dev/ttyS4 (auto-detected) #rc_input start -d /dev/ttyS4 # default: ROMFS/px4fmu_common/mixers/quad_x.main.mix, 8 output channels diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index e4c656f20d..654eb99ddc 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -76,9 +76,8 @@ mavlink start -n SoftAp -x -u 14556 -r 1000000 sleep 1 -linux_sbus start -d /dev/ttyS4 -c 16 -# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus) -#rc_input start -d /dev/ttyS4 +# RC port is mapped to /dev/ttyS4 (auto-detected) +rc_input start -d /dev/ttyS4 # default: ROMFS/px4fmu_common/mixers/quad_x.main.mix, 8 output channels #linux_pwm_out start -p bbblue_rc -m ROMFS/px4fmu_common/mixers/quad_x.main.mix diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 95e5006610..623ab1f81a 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -40,8 +40,7 @@ mavlink start -x -d /dev/ttyPS1 mavlink stream -d /dev/ttyPS1 -s HIGHRES_IMU -r 50 mavlink stream -d /dev/ttyPS1 -s ATTITUDE -r 50 -linux_sbus start -d /dev/ttyS2 -c 10 -#rc_input start -d /dev/ttyS2 +rc_input start -d /dev/ttyS2 linux_pwm_out start -p ocpoc_mmap -m ROMFS/px4fmu_common/mixers/ocpoc_quad_x.main.mix logger start -t -b 200 diff --git a/src/drivers/linux_sbus/CMakeLists.txt b/src/drivers/linux_sbus/CMakeLists.txt deleted file mode 100644 index 8a7d597828..0000000000 --- a/src/drivers/linux_sbus/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# 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 - ) - diff --git a/src/drivers/linux_sbus/linux_sbus.cpp b/src/drivers/linux_sbus/linux_sbus.cpp deleted file mode 100644 index 5cbff31f49..0000000000 --- a/src/drivers/linux_sbus/linux_sbus.cpp +++ /dev/null @@ -1,364 +0,0 @@ -/**************************************************************************** - * - * 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" - -#include -#include - -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_INFO("Device %s , channels: %d \n", device, channels); - _channels = channels; - result = init(); - - if (0 != result) { - PX4_WARN("error: RC initialization failed"); - return -1; - } - - _isRunning = true; - - ScheduleNow(); - - if (result == -1) { - _isRunning = false; - } - - return result; -} -//---------------------------------------------------------------------------------------------------------// -void RcInput::stop() -{ - close(_device_fd); - _shouldExit = true; -} - -//---------------------------------------------------------------------------------------------------------// -void RcInput::Run() -{ - _measure(); - - if (!_shouldExit) { - ScheduleDelayed(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 deleted file mode 100644 index 4ccd024a77..0000000000 --- a/src/drivers/linux_sbus/linux_sbus.h +++ /dev/null @@ -1,119 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -#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 px4::ScheduledWorkItem -{ -public: - RcInput() : - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _shouldExit(false), - _isRunning(false), - _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() - { - ScheduleClear(); - _isRunning = false; - close(_device_fd); - } - /** @return 0 on success, -errno on failure */ - int start(char *device, int channels); - void stop(); - - bool isRunning() - { - return _isRunning; - } - -private: - void Run() override; - void _measure(); - bool _shouldExit; - bool _isRunning; - 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);