Diff: VIPSSerialProtocol.cpp

Revision:

14:76083dc18b0d

Parent:

13:c2d9e975841b

Child:

16:a8d3a0dbe4bf

--- a/VIPSSerialProtocol.cpp	Mon Feb 22 10:44:27 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Thu Feb 25 17:54:04 2021 +0000
@@ -11,8 +11,6 @@
 #define _VBOXReady  0x05
 #define _SetDeltaTime  0x07
 
-const char* VIPSStatusMessages[] = { "Got UWBFitted", "Got UWBEnabled", "Got GetTime", "Ack to VBOX ready", "Nak to VBOX ready", "CRC error", "VIPSAlive", NULL};
-
 
 VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
 {
@@ -86,32 +84,27 @@
 void VIPSSerial::processRxMessage()
 {
     if (!checkCRC(&messageInBuffer[0])) {
-        statusMessage = 6;
+        pc.puts("VIPS CRC error\r\n");
         return;
     }
 
     switch (messageInBuffer[2]) {
         case _UWBFitted:
-            statusMessage = 1;
             sendAck(messageInBuffer[2]);
             break;
         case _EnableUWB:
-            statusMessage=2;
         case _DisableUWB: // for all of these just send an ack.
         case _SetDeltaTime:
             sendAck(messageInBuffer[2]);
             break;
         case _GetPPSTime:
-            statusMessage=3;
             sendVBOXTime();
             // send vbox tick counter
             break;
         case _VBOXReady:
             if (ppsActive) {
-                statusMessage=4;
                 sendAck(_VBOXReady);
             } else {
-                statusMessage=5;
                 sendNack(_VBOXReady);
             }
             break;
@@ -239,6 +232,9 @@
         pc.write("CRC error\r\n",11);
         return;
     }
+
+    led1=!led1;
+
     lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
     uint32_t mask = *(uint32_t*)(messageInBuffer+4);
     int offset = 32; // end of position
@@ -325,12 +321,6 @@
     static uint32_t lastPoints = 0;
     if (pointCount < 2)
         return NULL;
-    if (lastPoints == pointCount)
-        return NULL;
-    lastPoints = pointCount;
-
-    struct posAndTime_s lastPos; // the most recent position received
-    struct posAndTime_s prevPos; // the most last but one position received
 
     __disable_irq();   // disable IRQ and make a copy of the data we're going to interpolate.
     int lastPoint = nextPosition - 1;
@@ -364,6 +354,8 @@
     if (position::interp(&outputPosition, &(lastPos.pos), &(prevPos.pos))) {
         return &outputPosition;
     }
-    return NULL;
+
+    // interpolation failed. Return most recent location
+    return &lastPos.pos;
 }