From de9cb46a56f64404b79aa95a3e0fcd82bc2b50c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 4 Dec 2016 18:12:37 +0100 Subject: [PATCH] spektrum_rc: move and clean up (untested) --- cmake/configs/qurt_sdflight_default.cmake | 4 +- src/drivers/spektrum_rc/.CMakeLists.txt.swo | Bin 0 -> 12288 bytes .../spektrum_rc}/CMakeLists.txt | 9 +- src/drivers/spektrum_rc/spektrum_rc.cpp | 304 +++++++++++++++++ .../drivers/df_spektrum_rc/df_spektrum_rc.cpp | 311 ------------------ 5 files changed, 309 insertions(+), 319 deletions(-) create mode 100644 src/drivers/spektrum_rc/.CMakeLists.txt.swo rename src/{platforms/posix/drivers/df_spektrum_rc => drivers/spektrum_rc}/CMakeLists.txt (91%) create mode 100644 src/drivers/spektrum_rc/spektrum_rc.cpp delete mode 100644 src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake index 381b293688..84d78b1ec8 100644 --- a/cmake/configs/qurt_sdflight_default.cmake +++ b/cmake/configs/qurt_sdflight_default.cmake @@ -68,8 +68,8 @@ set(config_module_list # PX4 drivers # drivers/gps - #drivers/pwm_out_rc_in - platforms/posix/drivers/df_spektrum_rc + drivers/pwm_out_rc_in + drivers/spektrum_rc drivers/qshell/qurt drivers/snapdragon_pwm_out diff --git a/src/drivers/spektrum_rc/.CMakeLists.txt.swo b/src/drivers/spektrum_rc/.CMakeLists.txt.swo new file mode 100644 index 0000000000000000000000000000000000000000..020203150d3e7c4d8d11c9fc1601686a424170f1 GIT binary patch literal 12288 zcmeI2J&fE$6vtnX5a;_up`ZYyN8H?@&0fBQqC_ct_T24AUVE7t-{mCG;j?FRCTq`P z&)y{s6!aAIbQB2DAc}~Zjur_~(I6yJBT-SH@oujTG{45)dE+;K|KFRHRy&6m zc4;54b}s+`ybN%b1ULWu<#qV{eSi-pqav-8Y)tGa4C^%0>F6mlrhN2I!p)}6Hn)qq zJV|TWe#qzRkH7K_DVu~;k?i^XEGSpL7H zIlvz;0o+@d{{R2r_y4zR0N<Tm~*;CLn`($-bSAIJq*0Qw?n$%q8IJsp0|&2B1}~D2qv6NZz)F}Z;L&Tkq(kw z5+U72FW9A=1hdKDVo1Gd_727jMa%@Q)5s5#fJQqQk)4q4kjN({8jBubdz6z6^cdx4 z0*zV~0me})M)XL`TBUP|_j<%Ui8Mk#js)|3(LpX4^#!;_O2SD815bFZhyjbYso>Ym zl3NL<5#bzZBnV^4K+rhaz+SvZb_qk@OE?L@#jQkO6blS#p9&J7h&v{apD@xVQELuQ zwz!}o5d?SQIN&BdCv2DcgkQ%n<{Vt4kxv5>2`}s*9}u6K8*-EM2cgHN9{F*^$vX*& z1ogsME%=;i9dOZ)2m6ff^aRFx5n*Pj_RZ;5B4WmIi?sfP1h-?>8W&JTj4d>p-GMue z;lN{p`bp?993*TIbAlct7*Otq9_^E$YmANJw#EW^&kI9ShqkzQw3Zc) zWnC6cQ>e;p4@8_aGDodYR=KJhiK@nNtxBb(x#p8hH<(Lv6gkIXtxD8KMT3nRYI9H- zT)f=HNEZ4?)~JspR%t15aPtaQ>U5w%HN-+Un5jzF#oaUlN?GaDjAL5mo2o|DE>@;m)$_;aII)~G8f!UqO1A0J zLv<`MQ{%gJaqCF`cP*w+7avukhS;b{otd?A`9O(Ii|X+*z{RwdSW0~jTr5>?x}){A z(+1kk&|2oFe(IxCKV;FVf8ai3PHX6-k;bE$GGpuEfmOOIZ|WO04QWycF`M+u(h?pS~jvimU-9Yc!t2=403BB literal 0 HcmV?d00001 diff --git a/src/platforms/posix/drivers/df_spektrum_rc/CMakeLists.txt b/src/drivers/spektrum_rc/CMakeLists.txt similarity index 91% rename from src/platforms/posix/drivers/df_spektrum_rc/CMakeLists.txt rename to src/drivers/spektrum_rc/CMakeLists.txt index 15d9a3dad6..50a0d58ecf 100644 --- a/src/platforms/posix/drivers/df_spektrum_rc/CMakeLists.txt +++ b/src/drivers/spektrum_rc/CMakeLists.txt @@ -31,15 +31,12 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) - px4_add_module( - MODULE platforms__posix__drivers__df_spektrum_rc - MAIN df_spektrum_rc + MODULE drivers__spektrum_rc + MAIN spektrum_rc SRCS - df_spektrum_rc.cpp + spektrum_rc.cpp DEPENDS platforms__common - df_driver_framework ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/spektrum_rc/spektrum_rc.cpp b/src/drivers/spektrum_rc/spektrum_rc.cpp new file mode 100644 index 0000000000..8866e761a2 --- /dev/null +++ b/src/drivers/spektrum_rc/spektrum_rc.cpp @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + + +/** + * @file spektrum_rc.cpp + * + * This is a driver for a Spektrum satellite receiver connected to a Snapdragon + * on the serial port. By default port J15 (next to USB) is used. + */ + +#include +#include + +#include +#include +#include + +#include +#include + +// Snapdraogon: use J15 (next to USB) +#define SPEKTRUM_UART_DEVICE_PATH "/dev/tty-1" + +#define UNUSED(x) (void)(x) + +extern "C" { __EXPORT int spektrum_rc_main(int argc, char *argv[]); } + + +namespace spektrum_rc +{ + +volatile bool _task_should_exit = false; +static bool _is_running = false; +static px4_task_t _task_handle = -1; + +int start(); +int stop(); +int info(); +void usage(); +void task_main(int argc, char *argv[]); + +void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi, + input_rc_s &input_rc); + +void task_main(int argc, char *argv[]) +{ + int uart_fd = dsm_init(SPEKTRUM_UART_DEVICE_PATH); + + if (uart_fd < 1) { + PX4_ERR("dsm init failed"); + return; + } + + orb_advert_t rc_pub = nullptr; + + // Use a buffer size of the double of the minimum, just to be safe. + uint8_t rx_buf[2 * DSM_BUFFER_SIZE]; + + // Set up poll topic + px4_pollfd_struct_t fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; + + _is_running = true; + + // Main loop + while (!_task_should_exit) { + + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); + + /* Timed out, do a periodic check for _task_should_exit. */ + if (pret == 0) { + continue; + } + + if (pret < 0) { + PX4_WARN("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + if (!(fds[0].revents & POLLIN)) { + // There is only one fd in poll, so this should be impossible. + PX4_WARN("poll bit not set"); + continue; + } + + int newbytes = ::read(uart_fd, &rx_buf[0], sizeof(rx_buf)); + + if (newbytes < 0) { + PX4_WARN("read failed"); + continue; + } + + if (newbytes == 0) { + PX4_WARN("read nothing"); + continue; + } + + + const hrt_abstime now = hrt_absolute_time(); + + bool dsm_11_bit; + unsigned frame_drops; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + + // parse new data + bool rc_updated = dsm_parse(now, rx_buf, newbytes, &raw_rc_values[0], &raw_rc_count, + &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); + UNUSED(dsm_11_bit); + + if (rc_updated) { + + input_rc_s input_rc = {}; + + // We don't know RSSI. + const int rssi = -1; + + fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, rssi, + input_rc); + + if (rc_pub == nullptr) { + rc_pub = orb_advertise(ORB_ID(input_rc), &input_rc); + + } else { + orb_publish(ORB_ID(input_rc), rc_pub, &input_rc); + } + } + } + + orb_unadvertise(rc_pub); + dsm_deinit(); + + _is_running = false; +} + +void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi, + input_rc_s &input_rc) +{ + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_QURT; + + input_rc.channel_count = raw_rc_count; + + if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < input_rc.channel_count; ++i) { + input_rc.values[i] = raw_rc_values[i]; + + if (raw_rc_values[i] != UINT16_MAX) { + valid_chans++; + } + } + + input_rc.timestamp = now; + input_rc.timestamp_last_signal = input_rc.timestamp; + input_rc.rc_ppm_frame_length = 0; + + /* fake rssi if no value was provided */ + if (rssi == -1) { + + input_rc.rssi = 255; + + } else { + input_rc.rssi = rssi; + } + + if (valid_chans == 0) { + input_rc.rssi = 0; + } + + input_rc.rc_failsafe = failsafe; + input_rc.rc_lost = (valid_chans == 0); + input_rc.rc_lost_frame_count = frame_drops; + input_rc.rc_total_frame_count = 0; +} + +int start() +{ + if (_is_running) { + PX4_WARN("already running"); + return -1; + } + + ASSERT(_task_handle == -1); + + _task_should_exit = false; + + _task_handle = px4_task_spawn_cmd("spektrum_rc_main", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t)&task_main, + nullptr); + + if (_task_handle < 0) { + PX4_ERR("task start failed"); + return -1; + } + + return 0; +} + +int stop() +{ + if (!_is_running) { + PX4_WARN("not running"); + return -1; + } + + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; + return 0; +} + +int info() +{ + PX4_INFO("running: %s", _is_running ? "yes" : "no"); + + return 0; +} + +void +usage() +{ + PX4_INFO("Usage: spektrum_rc 'start', 'info', 'stop'"); +} + +} // namespace spektrum_rc + + +int spektrum_rc_main(int argc, char *argv[]) +{ + int myoptind = 1; + + if (argc <= 1) { + spektrum_rc::usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + + if (!strcmp(verb, "start")) { + return spektrum_rc::start(); + } + + else if (!strcmp(verb, "stop")) { + return spektrum_rc::stop(); + } + + else if (!strcmp(verb, "info")) { + return spektrum_rc::info(); + } + + else { + spektrum_rc::usage(); + return 1; + } +} diff --git a/src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp b/src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp deleted file mode 100644 index d7293bca6a..0000000000 --- a/src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - - -/** - * @file df_spektrum_rc.cpp - * - * This is a wrapper around the Parrot Bebop bus driver of the DriverFramework. It sends the - * motor and contol commands to the Bebop and reads its status and informations. - */ - -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include - -#define SPEKTRUM_UART_DEVICE_PATH "/dev/serialABC" -#define SBUS_BUFFER_SIZE 128 - -extern "C" { __EXPORT int df_spektrum_rc_main(int argc, char *argv[]); } - -namespace df_spektrum_rc -{ - -volatile bool _task_should_exit = false; // flag indicating if bebop esc control task should exit -static bool _is_running = false; // flag indicating if bebop esc app is running -static px4_task_t _task_handle = -1; // handle to the task main thread - -input_rc_s _rc_in; -float _analog_rc_rssi_volt; -bool _analog_rc_rssi_stable; - -int start(); -int stop(); -int info(); -void usage(); -void task_main(int argc, char *argv[]); -void fill_rc_in(uint16_t raw_rc_count, - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool frame_drop, bool failsafe, - unsigned frame_drops, int rssi = -1); - -void task_main(int argc, char *argv[]) -{ - // publications - orb_advert_t rc_pub = nullptr; - uint8_t _rcs_buf[50]; - - // important to keep these buffers out of the stack - // as they might need to be accumulated over multiple - // iterations of the inner loop - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; - uint16_t raw_rc_count; - unsigned frame_drops; - bool dsm_11_bit; - - int uart_fd = dsm_init(SPEKTRUM_UART_DEVICE_PATH); - - _is_running = true; - - // Set up poll topic - px4_pollfd_struct_t fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; - - // Main loop - while (!_task_should_exit) { - - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); - - /* Timed out, do a periodic check for _task_should_exit. */ - if (pret == 0) { - continue; - } - - /* This is undesirable but not much we can do. */ - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } - - if (fds[0].revents & POLLIN) { - - int newbytes = ::read(uart_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); - - if (newbytes > 0) { - - hrt_abstime now = hrt_absolute_time(); - - // parse new data - bool rc_updated = dsm_parse(now, &_rcs_buf[0], newbytes, &raw_rc_values[0], &raw_rc_count, - &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS); - - if (rc_updated) { - - // we have a new DSM frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; - fill_rc_in(raw_rc_count, raw_rc_values, now, - false, false, frame_drops); - - if (rc_pub == nullptr) { - rc_pub = orb_advertise(ORB_ID(input_rc), &_rc_in); - - } else { - orb_publish(ORB_ID(input_rc), rc_pub, &_rc_in); - } - } - } - } - } - - orb_unadvertise(rc_pub); - - _is_running = false; - -} - -void -fill_rc_in(uint16_t raw_rc_count, - uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool frame_drop, bool failsafe, - unsigned frame_drops, int rssi) -{ - // fill rc_in struct for publishing - _rc_in.channel_count = raw_rc_count; - - if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { - _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; - } - - unsigned valid_chans = 0; - - for (unsigned i = 0; i < _rc_in.channel_count; i++) { - _rc_in.values[i] = raw_rc_values[i]; - - if (raw_rc_values[i] != UINT16_MAX) { - valid_chans++; - } - } - - _rc_in.timestamp = now; - _rc_in.timestamp_last_signal = _rc_in.timestamp; - _rc_in.rc_ppm_frame_length = 0; - - /* fake rssi if no value was provided */ - if (rssi == -1) { - - /* set RSSI if analog RSSI input is present */ - if (_analog_rc_rssi_stable) { - float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f; - - if (rssi_analog > 100.0f) { - rssi_analog = 100.0f; - } - - if (rssi_analog < 0.0f) { - rssi_analog = 0.0f; - } - - _rc_in.rssi = rssi_analog; - - } else { - _rc_in.rssi = 255; - } - - } else { - _rc_in.rssi = rssi; - } - - if (valid_chans == 0) { - _rc_in.rssi = 0; - } - - _rc_in.rc_failsafe = failsafe; - _rc_in.rc_lost = (valid_chans == 0); - _rc_in.rc_lost_frame_count = frame_drops; - _rc_in.rc_total_frame_count = 0; -} - -int start() -{ - // Start the task to handle RC - ASSERT(_task_handle == -1); - - /* start the task */ - _task_handle = px4_task_spawn_cmd("spektrum_rc_main", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - (px4_main_t)&task_main, - nullptr); - - if (_task_handle < 0) { - warn("task start failed"); - return -1; - } - - _is_running = true; - return 0; -} - -int stop() -{ - // Stop bebop motor control task - _task_should_exit = true; - - while (_is_running) { - usleep(200000); - PX4_INFO("."); - } - - _task_handle = -1; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - PX4_INFO("info"); - - return 0; -} - -void -usage() -{ - PX4_INFO("Usage: df_spektrum_rc 'start', 'info', 'stop'"); -} - -} /* df_spektrum_rc */ - - -int -df_spektrum_rc_main(int argc, char *argv[]) -{ - int ret = 0; - int myoptind = 1; - - if (argc <= 1) { - df_spektrum_rc::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = df_spektrum_rc::start(); - } - - else if (!strcmp(verb, "stop")) { - ret = df_spektrum_rc::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_spektrum_rc::info(); - } - - else { - df_spektrum_rc::usage(); - return 1; - } - - return ret; -}