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 0000000000..020203150d Binary files /dev/null and b/src/drivers/spektrum_rc/.CMakeLists.txt.swo differ 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; -}