From 2ba369dd54278b5ab94abb46c2b4d13cc3d07a71 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Thu, 7 Oct 2021 21:50:35 -0600 Subject: [PATCH] Add uavcannode fix2 mode and submode --- .../uavcannode/Publishers/GnssFix2.hpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp index 7d50ec060f..5e4f610e09 100644 --- a/src/drivers/uavcannode/Publishers/GnssFix2.hpp +++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp @@ -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);