Address Paval's comments regarding extracting matrix from uavcan msg, position covariance calculation, and _poll_fds_num

1 files changed, 11 insertions, 12 deletions

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index c0c07be53..5bc437670 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -243,22 +243,21 @@ int UavcanNode::run()
 
 	_node.setStatusOk();
 
-	/*
-	 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
-	 * 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;
-	_poll_fds_num += 1;
-
 	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).
+			 * 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;
+			_poll_fds_num += 1;
 		}
 
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@@ -272,7 +271,7 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 1;
+			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) {