diff options
author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-05-08 23:34:23 +0400 |
---|---|---|
committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-05-08 23:34:23 +0400 |
commit | c697aae17a32f25b2f163282b9cb18efedb14d77 (patch) | |
tree | fd2b3bb6474aa8797568266c39f596081a1fa6ab | |
parent | 4a98dae227f3e60f1a220164bce0b995ce303f3d (diff) | |
download | px4-firmware-c697aae17a32f25b2f163282b9cb18efedb14d77.tar.gz px4-firmware-c697aae17a32f25b2f163282b9cb18efedb14d77.tar.bz2 px4-firmware-c697aae17a32f25b2f163282b9cb18efedb14d77.zip |
Proper IO miltiplexing libuavcan + ORB
-rw-r--r-- | src/modules/uavcan/uavcan_main.cpp | 68 | ||||
-rw-r--r-- | src/modules/uavcan/uavcan_main.hpp | 3 |
2 files changed, 44 insertions, 27 deletions
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e7829cbba..6ba596ad0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -35,6 +35,7 @@ #include <cstdlib> #include <cstring> +#include <fcntl.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <systemlib/mixer/mixer.h> @@ -202,9 +203,17 @@ int UavcanNode::init(uavcan::NodeID node_id) return _node.start(); } +void UavcanNode::node_spin_once() +{ + const int spin_res = _node.spin(uavcan::MonotonicTime()); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } +} + int UavcanNode::run() { - _node.setStatusOk(); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count _output_count = 2; @@ -215,42 +224,65 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + const int busevent_fd = ::open(uavcan_stm32::Event::DevName, 0); + if (busevent_fd < 0) + { + warnx("Failed to open %s", uavcan_stm32::Event::DevName); + _task_should_exit = true; + } + /* * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); * IO multiplexing shall be done here. */ + _node.setStatusOk(); + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Actual event type (POLLIN/POLLOUT/...) doesn't matter here. + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN | POLLOUT; + _poll_fds_num += 1; } - int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + + node_spin_once(); // Non-blocking // this would be bad... - if (ret < 0) { + if (poll_ret < 0) { log("poll error %d", errno); continue; - - } else if (ret == 0) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } else { - // get controls for required topics + bool controls_updated = false; unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { + controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } poll_id++; } } + if (!controls_updated) { + // timeout: no control data, switch to failsafe values + // XXX trigger failsafe + } + //can we mix? if (_mixers != nullptr) { @@ -277,15 +309,7 @@ int UavcanNode::run() } } - /* - * Output to the bus - */ - printf("CAN out: "); - for (unsigned i = 0; i < outputs.noutputs; i++) { - printf("%u: %8.4f ", i, outputs.output[i]); - } - printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); - + // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } @@ -303,14 +327,6 @@ int UavcanNode::run() arm_actuators(set_armed); } - - // Output commands and fetch data TODO ORB multiplexing - - const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1)); - - if (spin_res < 0) { - warnx("node spin error %i", spin_res); - } } teardown(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index f4a709c79..751a94a8a 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: private: int init(uavcan::NodeID node_id); + void node_spin_once(); int run(); int _task = -1; ///< handle to the OS task @@ -115,6 +116,6 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; }; |