Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Jacob Dahl
2020-03-21 09:43:56 -06:00
committed by GitHub
parent 341dd6ba26
commit b566de2c36
1 changed files with 17 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/UavcanNode.cpp
+17 -3
View File
@@ -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);
}