dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

uavcan: added add_poll_fd() helper function 

this makes the code clearer and avoids repeated code
This commit is contained in:
Andrew Tridgell 2014-11-21 08:17:50 +11:00
parent a7a68c88a2
commit 7ae4f6d97e
2 changed files with 27 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/modules/uavcan/uavcan_main.cpp
View File

@ -269,6 +269,24 @@ void UavcanNode::node_spin_once()
}
}
/*
add a fd to the list of polled events. This assumes you want
POLLIN for now.
*/
int UavcanNode::add_poll_fd(int fd)
{
int ret = _poll_fds_num;
if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
errx(1, "uavcan: too many poll fds, exiting");
}
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
return ret;
}
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
@ -304,22 +322,14 @@ int UavcanNode::run()
* 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_num = 0;
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
add_poll_fd(busevent_fd);
/*
* setup poll to look for actuator direct input if we are
* subscribed to the topic
*/
if (_actuator_direct_sub != -1) {
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = _actuator_direct_sub;
_poll_fds[_poll_fds_num].events = POLLIN;
_actuator_direct_poll_fd_num = _poll_fds_num;
_poll_fds_num += 1;
_actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
}
while (!_task_should_exit) {
@ -490,7 +500,6 @@ UavcanNode::subscribe()
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
// the first fd used by CAN
_poll_fds_num = 1;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
@ -503,10 +512,7 @@ UavcanNode::subscribe()
}
if (_control_subs[i] > 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_ids[i] = _poll_fds_num;
_poll_fds_num++;
_poll_ids[i] = add_poll_fd(_control_subs[i]);
}
}
}

7
src/modules/uavcan/uavcan_main.hpp
View File

@ -58,6 +58,9 @@
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
// we add two to allow for actuator_direct and busevent
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
/**
* A UAVCAN node.
*/
@ -98,6 +101,8 @@ private:
int init(uavcan::NodeID node_id);
void node_spin_once();
int run();
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
@ -126,7 +131,7 @@ 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 + 1] = {}; ///< +1 for /dev/uavcan/busevent
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
unsigned _poll_fds_num = 0;
int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic