From 76ee17e532fdbd28bc6e0a20bf16626c17f52288 Mon Sep 17 00:00:00 2001 From: Hidenori Date: Wed, 29 Jun 2016 13:10:11 -0400 Subject: [PATCH] RC input and PWM output for Navio2 --- cmake/configs/posix_rpi2_release.cmake | 3 + posix-configs/rpi2/mainapp.config | 2 + src/drivers/pwm_out/CMakeLists.txt | 44 ++ src/drivers/pwm_out/pwm_out.cpp | 490 ++++++++++++++++++ src/modules/commander/PreflightCheck.cpp | 3 + src/modules/mavlink/mavlink_main.cpp | 3 + src/modules/rcinput/CMakeLists.txt | 43 ++ src/modules/rcinput/rcinput.cpp | 297 +++++++++++ .../posix/px4_layer/px4_posix_tasks.cpp | 6 +- 9 files changed, 890 insertions(+), 1 deletion(-) create mode 100644 src/drivers/pwm_out/CMakeLists.txt create mode 100644 src/drivers/pwm_out/pwm_out.cpp create mode 100644 src/modules/rcinput/CMakeLists.txt create mode 100644 src/modules/rcinput/rcinput.cpp diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake index 0778efa1ff..a796aa6f15 100644 --- a/cmake/configs/posix_rpi2_release.cmake +++ b/cmake/configs/posix_rpi2_release.cmake @@ -72,9 +72,12 @@ set(config_module_list modules/navigator modules/mavlink + modules/rcinput + # # PX4 drivers # + drivers/pwm_out # # Libraries diff --git a/posix-configs/rpi2/mainapp.config b/posix-configs/rpi2/mainapp.config index 752ea8f66b..4f7ddd9354 100644 --- a/posix-configs/rpi2/mainapp.config +++ b/posix-configs/rpi2/mainapp.config @@ -19,3 +19,5 @@ sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 mavlink boot_complete +rcinput start +pwm_out start diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt new file mode 100644 index 0000000000..ff6158c0cd --- /dev/null +++ b/src/drivers/pwm_out/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ +px4_add_module( + MODULE drivers__pwm_out + MAIN pwm_out + COMPILE_FLAGS + -Os + -DMAVLINK_COMM_NUM_BUFFERS=1 + SRCS + pwm_out.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pwm_out/pwm_out.cpp b/src/drivers/pwm_out/pwm_out.cpp new file mode 100644 index 0000000000..0fb4ebd35e --- /dev/null +++ b/src/drivers/pwm_out/pwm_out.cpp @@ -0,0 +1,490 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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. + * + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace pwm_out +{ +static px4_task_t _task_handle = -1; +volatile bool _task_should_exit = false; +static bool _is_running = false; + +static const int NUM_PWM = 4; +static char _device[32] = "/sys/class/pwm/pwmchip0"; +static int _pwm_fd[NUM_PWM]; + +static const char *MIXER_FILENAME = ""; + +// subscriptions +int _controls_sub; +int _armed_sub; + +// publications +orb_advert_t _outputs_pub = nullptr; +orb_advert_t _rc_pub = nullptr; + +// topic structures +actuator_controls_s _controls; +actuator_outputs_s _outputs; +actuator_armed_s _armed; + +pwm_limit_t _pwm_limit; + +// esc parameters +int32_t _pwm_disarmed; +int32_t _pwm_min; +int32_t _pwm_max; + +MultirotorMixer *_mixer = nullptr; + +void usage(); + +void start(); + +void stop(); + +int pwm_write_sysfs(char *path, int value); + +int pwm_initialize(const char *device); + +void pwm_deinitialize(); + +void send_outputs_pwm(const uint16_t *pwm); + +void task_main_trampoline(int argc, char *argv[]); + +void task_main(int argc, char *argv[]); + +/* mixer initialization */ +int initialize_mixer(const char *mixer_filename); +int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); + + +int mixer_control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls[control_group].control[control_index]; + + return 0; +} + + +int initialize_mixer(const char *mixer_filename) +{ + char buf[2048]; + size_t buflen = sizeof(buf); + + PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); + + int fd_load = ::open(mixer_filename, O_RDONLY); + + if (fd_load != -1) { + int nRead = ::read(fd_load, buf, buflen); + close(fd_load); + + if (nRead > 0) { + _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); + + if (_mixer != nullptr) { + PX4_INFO("Successfully initialized mixer from config file"); + return 0; + + } else { + PX4_ERR("Unable to parse from mixer config file"); + return -1; + } + + } else { + PX4_WARN("Unable to read from mixer config file"); + return -2; + } + + } else { + PX4_WARN("No mixer config file found, using default mixer."); + + /* Mixer file loading failed, fall back to default mixer configuration for + * QUAD_X airframe. */ + float roll_scale = 1; + float pitch_scale = 1; + float yaw_scale = 1; + float deadband = 0; + + _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, + MultirotorGeometry::QUAD_X, + roll_scale, pitch_scale, yaw_scale, deadband); + + // TODO: temporary hack to make this compile + (void)_config_index[0]; + + if (_mixer == nullptr) { + PX4_ERR("Mixer initialization failed"); + return -1; + } + + return 0; + } +} + +int pwm_write_sysfs(char *path, int value) +{ + int fd = ::open(path, O_WRONLY | O_CLOEXEC); + int n; + char *data; + + ::free(path); + if (fd == -1) { + return -errno; + } + n = ::asprintf(&data, "%u", value); + if (n > 0) { + ::write(fd, data, n); + ::free(data); + } + ::close(fd); + + return 0; +} + +int pwm_initialize(const char *device) +{ + int i; + char *path; + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/export", device); + if (pwm_write_sysfs(path, i) < 0) { + PX4_ERR("PWM export failed"); + } + } + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/pwm%u/enable", device, i); + if (pwm_write_sysfs(path, 1) < 0) { + PX4_ERR("PWM enable failed"); + } + } + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/pwm%u/period", device, i); + if (pwm_write_sysfs(path, (int)1e9/50)) { + PX4_ERR("PWM period failed"); + } + } + + for (i = 0; i < NUM_PWM; ++i) { + ::asprintf(&path, "%s/pwm%u/duty_cycle", device, i); + _pwm_fd[i] = ::open(path, O_WRONLY | O_CLOEXEC); + ::free(path); + if (_pwm_fd[i] == -1) { + PX4_ERR("PWM: Failed to open duty_cycle."); + return -errno; + } + } + + return 0; +} + +void pwm_deinitialize() +{ + for (int i = 0; i < NUM_PWM; ++i) { + if (_pwm_fd[i] != -1) { + ::close(_pwm_fd[i]); + } + } +} + +void send_outputs_pwm(const uint16_t *pwm) +{ + int n; + char *data; + + //convert this to duty_cycle in ns + for (unsigned i = 0; i < NUM_PWM; ++i) { + n = ::asprintf(&data, "%u", pwm[i] * 1000); + ::write(_pwm_fd[i], data, n); + } +} + + + +void task_main(int argc, char *argv[]) +{ + _is_running = true; + + if (pwm_initialize(_device) < 0) { + PX4_ERR("Failed to initialize PWM."); + return; + } + + // Subscribe for orb topics + _controls_sub = orb_subscribe(ORB_ID(actuator_controls_3)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + // Start disarmed + _armed.armed = false; + _armed.prearmed = false; + + // Set up poll topic + px4_pollfd_struct_t fds[1]; + fds[0].fd = _controls_sub; + fds[0].events = POLLIN; + /* Don't limit poll intervall for now, 250 Hz should be fine. */ + //orb_set_interval(_controls_sub, 10); + + // Set up mixer + if (initialize_mixer(MIXER_FILENAME) < 0) { + PX4_ERR("Mixer initialization failed."); + return; + } + + pwm_limit_init(&_pwm_limit); + + // 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) { + orb_copy(ORB_ID(actuator_controls_3), _controls_sub, &_controls); + + _outputs.timestamp = _controls.timestamp; + + /* do mixing */ + _outputs.noutputs = _mixer->mix(_outputs.output, + 0 /* not used */, + NULL); + + + /* disable unused ports by setting their output to NaN */ + for (size_t i = _outputs.noutputs; + i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); + i++) { + _outputs.output[i] = NAN; + } + + const uint16_t reverse_mask = 0; + uint16_t disarmed_pwm[4]; + uint16_t min_pwm[4]; + uint16_t max_pwm[4]; + + for (unsigned int i = 0; i < 4; i++) { + disarmed_pwm[i] = _pwm_disarmed; + min_pwm[i] = _pwm_min; + max_pwm[i] = _pwm_max; + } + + uint16_t pwm[4]; + + // TODO FIXME: pre-armed seems broken + pwm_limit_calc(_armed.armed, + false/*_armed.prearmed*/, + _outputs.noutputs, + reverse_mask, + disarmed_pwm, + min_pwm, + max_pwm, + _outputs.output, + pwm, + &_pwm_limit); + + send_outputs_pwm(pwm); + + if (_outputs_pub != nullptr) { + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); + + } else { + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); + } + } + + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } + } + + pwm_deinitialize(); + orb_unsubscribe(_controls_sub); + orb_unsubscribe(_armed_sub); + + _is_running = false; + +} + +void task_main_trampoline(int argc, char *argv[]) +{ + task_main(argc, argv); +} + +void start() +{ + ASSERT(_task_handle == -1); + + _task_should_exit = false; + + /* start the task */ + _task_handle = px4_task_spawn_cmd("pwm_out_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + (px4_main_t)&task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + warn("task start failed"); + return; + } + +} + +void stop() +{ + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; +} + +void usage() +{ + PX4_INFO("usage: pwm_out start -d /sys/class/pwm/pwmchip0"); + PX4_INFO(" pwm_out stop"); + PX4_INFO(" pwm_out status"); +} + +} // namespace pwm_out + +/* driver 'main' command */ +extern "C" __EXPORT int pwm_out_main(int argc, char *argv[]); + +int pwm_out_main(int argc, char *argv[]) +{ + const char *device = nullptr; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + char *verb = nullptr; + + if (argc >= 2) { + verb = argv[1]; + } else { + return 1; + } + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device = myoptarg; + strncpy(pwm_out::_device, device, strlen(device)); + break; + } + } + + // gets the parameters for the esc's pwm + param_get(param_find("PWM_DISARMED"), &pwm_out::_pwm_disarmed); + param_get(param_find("PWM_MIN"), &pwm_out::_pwm_min); + param_get(param_find("PWM_MAX"), &pwm_out::_pwm_max); + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (pwm_out::_is_running) { + PX4_WARN("pwm_out already running"); + return 1; + } + + pwm_out::start(); + } + + else if (!strcmp(verb, "stop")) { + if (!pwm_out::_is_running) { + PX4_WARN("pwm_out is not running"); + return 1; + } + + pwm_out::stop(); + } + + else if (!strcmp(verb, "status")) { + PX4_WARN("pwm_out is %s", pwm_out::_is_running ? "running" : "not running"); + return 0; + + } else { + pwm_out::usage(); + return 1; + } + + return 0; +} diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 1f31586e19..a11432c994 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -391,6 +391,9 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, // all the sensors are supported PX4_WARN("Preflight checks always pass on Snapdragon."); return true; +#elif defined(__LINUX) + PX4_WARN("Preflight checks always pass on Linux (RPI)."); + return true; #endif bool failed = false; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 30ad39d3d3..42c17a8fdc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -863,6 +863,9 @@ Mavlink::get_free_tx_buf() // No FIONWRITE on Linux #if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#else + //Linux cp210x does not support TIOCOUTQ + buf_free = 256; #endif if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { diff --git a/src/modules/rcinput/CMakeLists.txt b/src/modules/rcinput/CMakeLists.txt new file mode 100644 index 0000000000..5008242d19 --- /dev/null +++ b/src/modules/rcinput/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE modules__rcinput + MAIN rcinput + STACK_MAIN 1200 + COMPILE_FLAGS -Os + SRCS + rcinput.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/rcinput/rcinput.cpp b/src/modules/rcinput/rcinput.cpp new file mode 100644 index 0000000000..22a835ee0f --- /dev/null +++ b/src/modules/rcinput/rcinput.cpp @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace rcinput +{ + +extern "C" __EXPORT int rcinput_main(int argc, char *argv[]); + +#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin" + +#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds + + +class RcInput +{ +public: + RcInput() : + _shouldExit(false), + _isRunning(false), + _work{}, + _rcinput_pub(nullptr), + _channels(8), //D8R-II plus + _data{} + { + memset(_ch_fd, 0, sizeof(_ch_fd)); + } + ~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 _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS]; + struct input_rc_s _data; + + int navio_rc_init(); +}; + +int RcInput::navio_rc_init() +{ + int i; + char *buf; + + for (i = 0; i < _channels; ++i) { + ::asprintf(&buf, "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i); + int fd = ::open(buf, O_RDONLY); + ::free(buf); + if (fd < 0) { + PX4_WARN("error: open %d failed", i); + break; + } + _ch_fd[i] = fd; + } + for (; 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 = navio_rc_init(); + + if (result != 0) { + 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() +{ + _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; + char buf[12]; + + for (int i = 0; i < _channels; ++i) { + int res; + if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) { + _data.values[i] = UINT16_MAX; + continue; + } + buf[sizeof(buf) -1] = '\0'; + + _data.values[i] = atoi(buf); + } + + ts = hrt_absolute_time(); + _data.timestamp_publication = ts; + _data.timestamp_last_signal = ts; + _data.channel_count = _channels; + _data.rssi = 100; + _data.rc_lost_frame_count = 0; + _data.rc_total_frame_count = 1; + _data.rc_ppm_frame_length = 100; + _data.rc_failsafe = false; + _data.rc_lost = false; + _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; + + 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: rcinput {start|stop|status}"); +} + +static RcInput *rc_input = nullptr; + +int rcinput_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 rcinput diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index 53cc821b2f..908a444802 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -90,11 +90,15 @@ static void *entry_adapter(void *ptr) pthdata_t *data = (pthdata_t *) ptr; int rv; + // set the threads name #ifdef __PX4_DARWIN rv = pthread_setname_np(data->name); #else - rv = pthread_setname_np(pthread_self(), data->name); + char buf[17]; + snprintf(buf, 16, "%s", data->name); + buf[16] = '0'; + rv = pthread_setname_np(pthread_self(), buf); #endif if (rv) {