Diff: VIPSSerialProtocol.cpp

Revision:

44:fe7fcf62c75d

Parent:

36:fff2a71bbf86

Child:

49:df05784d9a5c

--- a/VIPSSerialProtocol.cpp	Tue Aug 10 14:55:37 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Thu Aug 12 11:30:14 2021 +0000
@@ -282,6 +282,9 @@
     lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20);
     lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28);
 
+    if (mask & 0x0001)
+        lastPositions[nextPosition].pos.LLAPosition = true;
+
     if (mask & 0x0002) { // parse status
         lastPositions[nextPosition].pos.beacons = messageInBuffer[offset++];
         lastPositions[nextPosition].pos.solutionType = messageInBuffer[offset++];
@@ -323,22 +326,28 @@
         offset+=4;
     }
     if (mask & 0x0080) {  // raw UWB
-
         offset+=24;
     }
     if (mask & 0x0100) { // raw IMU
-
         offset+=24;
     }
 
     if (mask & 0x0200) {// rover info
-
         offset+=4;
     }
     if (mask & 0x0400) {// FIZ data
-
         offset+=4;
     }
+    if (mask & 0x0800) {// Origin
+        offset+=24;
+    }
+    if (mask & 0x1000) {// Beacon list
+        memcpy(lastPositions[nextPosition].pos.UsedBeacons, messageInBuffer+offset, 12);
+        lastPositions[nextPosition].pos.UsedBeaconsValid = true;
+        offset+=12;
+    } else
+        lastPositions[nextPosition].pos.UsedBeaconsValid = false;
+
 
     /*if ( ( ( lastPositions[nextPosition].pos.KFStatus & 0xE634) == 0x0400) && (!hyperSmoothEnabled)) {
         EnableSmoothing(true);
@@ -436,7 +445,7 @@
         ZSmoothTotal -= posPtr->Height;
     } else {
         SmoothRunning = false;
- //       pc.printf("filterX = %f\r\n",xFilter.lastValue());
+//       pc.printf("filterX = %f\r\n",xFilter.lastValue());
         posPtr->X = xFilter.lastValue();
         posPtr->Y = yFilter.lastValue();
         posPtr->Height = zFilter.lastValue();