From b566de2c36c6c9e2acc765b40229109055e2886d Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Sat, 21 Mar 2020 09:43:56 -0600 Subject: [PATCH] uavcannode: GPS add timestamp, covariance, and corrected integer overflow on lat/lon --- src/drivers/uavcannode/UavcanNode.cpp | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 71d2a0fd4f..2835ef2457 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/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); }