uavcan: added add_poll_fd() helper function

this makes the code clearer and avoids repeated code

2 files changed, 27 insertions, 16 deletions

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 8fa765633..8147a8b89 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -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]);
 		}
 	}
 }
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 5a036fcd7..98f2e5ad4 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -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