From 6c5e3d53412fa1cdad687818328b3bfc1a83e9ca Mon Sep 17 00:00:00 2001
From: Andrew Chambers <achamber@gmail.com>
Date: Wed, 2 Jul 2014 19:06:30 -0700
Subject: Address Paval's comments regarding extracting matrix from uavcan msg,
 position covariance calculation, and _poll_fds_num

---
 src/modules/uavcan/gnss_receiver.cpp | 28 +++++++++++++++-------------
 src/modules/uavcan/uavcan_main.cpp   | 23 +++++++++++------------
 2 files changed, 26 insertions(+), 25 deletions(-)

(limited to 'src/modules')

diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp
index 23c221565..65a7b4a2a 100644
--- a/src/modules/uavcan/gnss_receiver.cpp
+++ b/src/modules/uavcan/gnss_receiver.cpp
@@ -43,7 +43,6 @@
 #include <systemlib/err.h>
 
 #define MM_PER_CM 			10	// Millimeters per centimeter
-#define EXPECTED_COV_SIZE 	9 	// Expect a 3x3 matrix for position and velocity covariance
 
 UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
 _node(node),
@@ -75,18 +74,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 	_report.timestamp_position = hrt_absolute_time();
 	_report.lat = msg.lat_1e7;
 	_report.lon = msg.lon_1e7;
-	_report.alt = msg.alt_1e2 * MM_PER_CM;					// Convert from centimeter (1e2) to millimeters (1e3)
+	_report.alt = msg.alt_1e2 * MM_PER_CM;	// Convert from centimeter (1e2) to millimeters (1e3)
 
 	_report.timestamp_variance = _report.timestamp_position;
 
-	// Check that the covariance arrays are valid
-	bool valid_position_covariance = (msg.position_covariance.size() == EXPECTED_COV_SIZE &&
-			msg.position_covariance[0] > 0);
-	bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE &&
-			msg.velocity_covariance[0] > 0);
+
+	// Check if the msg contains valid covariance information
+	const bool valid_position_covariance = !msg.position_covariance.empty();
+	const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
 
 	if (valid_position_covariance) {
-		_report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4];
+	    float pos_cov[9];
+	    msg.position_covariance.unpackSquareMatrix(pos_cov);
+		_report.p_variance_m = std::max(pos_cov[0], pos_cov[4]);
 		_report.eph_m = sqrtf(_report.p_variance_m);
 	} else {
 		_report.p_variance_m = -1.0;
@@ -94,7 +94,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 	}
 
 	if (valid_velocity_covariance) {
-		_report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8];
+	    float vel_cov[9];
+	    msg.velocity_covariance.unpackSquareMatrix(vel_cov);
+		_report.s_variance_m_s = vel_cov[0] + vel_cov[4] + vel_cov[8];
 
 		/* There is a nonlinear relationship between the velocity vector and the heading.
 		 * Use Jacobian to transform velocity covariance to heading covariance
@@ -111,9 +113,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 		float vel_n_sq = vel_n * vel_n;
 		float vel_e_sq = vel_e * vel_e;
 		_report.c_variance_rad =
-				(vel_e_sq * msg.velocity_covariance[0] +
-						-2 * vel_n * vel_e * msg.velocity_covariance[1] +		// Covariance matrix is symmetric
-						vel_n_sq* msg.velocity_covariance[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
+				(vel_e_sq * vel_cov[0] +
+						-2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
+						vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
 
 		_report.epv_m = sqrtf(msg.position_covariance[8]);
 
@@ -138,7 +140,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav
 
 	_report.timestamp_satellites = _report.timestamp_position;
 	_report.satellites_visible = msg.sats_used;
-	_report.satellite_info_available = 0;					// Set to 0 for no info available
+	_report.satellite_info_available = 0;	// Set to 0 for no info available
 
 	if (_report_pub > 0) {
 		orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
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) {
-- 
cgit v1.2.3