uavcannode: GPS add timestamp, covariance, and corrected integer overflow on lat/lon

src/drivers/uavcannode/UavcanNode.cpp

@@ -361,15 +361,29 @@ void UavcanNode::Run()
 		if (_vehicle_gps_position_sub.copy(&gps)) {
 			uavcan::equipment::gnss::Fix2 fix2{};
 
-			//fix2.gnss_timestamp = gps.time_utc_usec;
-			fix2.latitude_deg_1e8 = gps.lat * 10;
-			fix2.longitude_deg_1e8 = gps.lon * 10;
+			fix2.gnss_time_standard = fix2.GNSS_TIME_STANDARD_UTC;
+			fix2.gnss_timestamp.usec = gps.time_utc_usec;
+			fix2.latitude_deg_1e8 = (int64_t)gps.lat * 10;
+			fix2.longitude_deg_1e8 = (int64_t)gps.lon * 10;
 			fix2.height_msl_mm = gps.alt;
 			fix2.height_ellipsoid_mm = gps.alt_ellipsoid;
 			fix2.status = gps.fix_type;
 			fix2.ned_velocity[0] = gps.vel_n_m_s;
 			fix2.ned_velocity[1] = gps.vel_e_m_s;
 			fix2.ned_velocity[2] = gps.vel_d_m_s;
+			fix2.pdop = gps.hdop > gps.vdop ? gps.hdop :
+				    gps.vdop; // Use pdop for both hdop and vdop since uavcan v0 spec does not support them
+			fix2.sats_used = gps.satellites_used;
+
+			// Diagonal matrix
+			// position variances -- Xx, Yy, Zz
+			fix2.covariance.push_back(gps.eph);
+			fix2.covariance.push_back(gps.eph);
+			fix2.covariance.push_back(gps.eph);
+			// velocity variance -- Vxx, Vyy, Vzz
+			fix2.covariance.push_back(gps.s_variance_m_s);
+			fix2.covariance.push_back(gps.s_variance_m_s);
+			fix2.covariance.push_back(gps.s_variance_m_s);
 
 			_gnss_fix2_publisher.broadcast(fix2);
 		}