fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 20:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add uavcannode fix2 mode and submode

Browse Source
This commit is contained in:
alexklimaj
2021-10-08 11:00:37 -04:00
committed by Daniel Agar
parent d8e88aedc6
commit 2ba369dd54
1 changed files with 21 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/Publishers/GnssFix2.hpp
+21
View File
@@ -67,6 +67,8 @@ public:
void BroadcastAnyUpdates() override void BroadcastAnyUpdates() override
{ {
using uavcan::equipment::gnss::Fix2;
// sensor_gps -> uavcan::equipment::gnss::Fix2 // sensor_gps -> uavcan::equipment::gnss::Fix2
sensor_gps_s gps; sensor_gps_s gps;
@@ -87,6 +89,25 @@ public:
gps.vdop; // Use pdop for both hdop and vdop since uavcan v0 spec does not support them gps.vdop; // Use pdop for both hdop and vdop since uavcan v0 spec does not support them
fix2.sats_used = gps.satellites_used; fix2.sats_used = gps.satellites_used;
fix2.mode = Fix2::MODE_SINGLE;
fix2.sub_mode = 0;
switch (fix2.status) {
case 4:
fix2.mode = Fix2::MODE_DGPS;
break;
case 5:
fix2.mode = Fix2::MODE_RTK;
fix2.sub_mode = Fix2::SUB_MODE_RTK_FLOAT;
break;
case 6:
fix2.mode = Fix2::MODE_RTK;
fix2.sub_mode = Fix2::SUB_MODE_RTK_FIXED;
break;
}
// Diagonal matrix // Diagonal matrix
// position variances -- Xx, Yy, Zz // position variances -- Xx, Yy, Zz
fix2.covariance.push_back(gps.eph); fix2.covariance.push_back(gps.eph);