From f6693f1f17ea34e86125864bf60bae6ff81bf456 Mon Sep 17 00:00:00 2001 From: davidaroyer Date: Mon, 30 Oct 2017 11:26:56 -0500 Subject: [PATCH] drivers: remove ocpoc_sbus_rc_in --- src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt | 43 --- .../ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp | 306 ------------------ 2 files changed, 349 deletions(-) delete mode 100644 src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt delete mode 100644 src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp diff --git a/src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt b/src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt deleted file mode 100644 index 29ab2e63a5..0000000000 --- a/src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016-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__ocpoc_sbus_rc_in - MAIN ocpoc_sbus_rc_in - STACK_MAIN 1200 - COMPILE_FLAGS - SRCS - ocpoc_sbus_rc_in.cpp - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp b/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp deleted file mode 100644 index 47f3528c45..0000000000 --- a/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp +++ /dev/null @@ -1,306 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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 - -#include -#include - -#include - -namespace ocpoc_sbus_rc_in -{ - -extern "C" __EXPORT int ocpoc_sbus_rc_in_main(int argc, char *argv[]); - -#define RCINPUT_DEVICE_PATH "/dev/ttyS2" - -#define RCINPUT_MEASURE_INTERVAL_US 6500 // FUBATA T8J is 6.8ms frame rate, microseconds - -#define SBUS_INPUT_CHANNELS 8 // FUBATA T8J is 8-channel - -class RcInput -{ -public: - RcInput() : - _shouldExit(false), - _isRunning(false), - _work{}, - _rcinput_pub(nullptr), - _channels(8), //FUBUTA, 8 - _data{} - { - _sbus_fd = -1; - } - ~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(); - - bool _shouldExit; - bool _isRunning; - struct work_s _work; - - orb_advert_t _rcinput_pub; - - int _channels; - int _sbus_fd = -1; - struct input_rc_s _data; - - int ocpoc_subs_rc_init(); -}; - -int RcInput::ocpoc_subs_rc_init() -{ - int i; - - /* S.bus input (USART3) */ - _sbus_fd = sbus_init(RCINPUT_DEVICE_PATH, true); //jly - - - for (i = _channels; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { - _data.values[i] = UINT16_MAX; - } - - _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data); - - if (_rcinput_pub == nullptr) { - PX4_WARN("error: advertise failed"); - return -1; - } - - return 0; -} - -int RcInput::start() -{ - int result = 0; - - result = ocpoc_subs_rc_init(); - - if (result != 0) { - PX4_WARN("error: RC sbus 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 = 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; - - /* - * Gather R/C control inputs from sbus - */ - bool sbus_failsafe, sbus_frame_drop; - uint16_t values[SBUS_INPUT_CHANNELS * 2]; - uint16_t num_values; - - bool sbus_updated = sbus_input(_sbus_fd, values, &num_values, &sbus_failsafe, &sbus_frame_drop, - _channels); - - if (sbus_updated) { - - if (num_values > 8) { - num_values = 8; - } - - // assume r_raw_rc_count may be less than _channels - for (int i = 0; i < num_values; ++i) { - _data.values[i] = values[i]; - } - - ts = hrt_absolute_time(); - _data.timestamp = ts; - _data.timestamp_last_signal = ts; - _data.channel_count = num_values; - _data.rssi = 100; - _data.rc_lost_frame_count = 0; - _data.rc_total_frame_count = 1; - _data.rc_ppm_frame_length = 100; - _data.rc_failsafe = sbus_failsafe; - _data.rc_lost = sbus_frame_drop; - _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; - - orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data); - - } -} - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) { - PX4_ERR("%s", reason); - } - - PX4_INFO("usage: ocpoc_sbus_rc_in {start|stop|status}"); -} - -static RcInput *rc_input = nullptr; - -int ocpoc_sbus_rc_in_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage("missing command"); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (rc_input != nullptr && rc_input->isRunning()) { - PX4_WARN("already running"); - /* this is not an error */ - return 0; - } - - rc_input = new RcInput(); - - // Check if alloc worked. - if (rc_input == nullptr) { - PX4_ERR("alloc failed"); - return -1; - } - - int ret = rc_input->start(); - - if (ret != 0) { - PX4_ERR("start failed"); - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - - if (rc_input == nullptr || !rc_input->isRunning()) { - PX4_WARN("not running"); - /* this is not an error */ - return 0; - } - - rc_input->stop(); - - // Wait for task to die - int i = 0; - - do { - /* wait up to 3s */ - usleep(100000); - - } while (rc_input->isRunning() && ++i < 30); - - delete rc_input; - rc_input = nullptr; - - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (rc_input != nullptr && rc_input->isRunning()) { - PX4_INFO("running"); - - } else { - PX4_INFO("not running\n"); - } - - return 0; - } - - usage("unrecognized command"); - return 1; - -} - -}; // namespace ocpoc_sbus_rc_in