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

1 files changed, 15 insertions, 13 deletions

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);