fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-07 21:50:35 -06:00 committed by Daniel Agar
parent d8e88aedc6
commit 2ba369dd54
1 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/drivers/uavcannode/Publishers/GnssFix2.hpp
View File

@ -67,6 +67,8 @@ public:
void BroadcastAnyUpdates() override
{
using uavcan::equipment::gnss::Fix2;
// sensor_gps -> uavcan::equipment::gnss::Fix2
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
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
// position variances -- Xx, Yy, Zz
fix2.covariance.push_back(gps.eph);