Diff: VIPSSerialProtocol.cpp

Revision:

3:14d241e29be3

Parent:

2:a79201e302d7

Child:

7:87aea27cc68b

--- a/VIPSSerialProtocol.cpp	Mon Jan 18 09:15:04 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Thu Jan 28 15:13:05 2021 +0000
@@ -29,6 +29,7 @@
     newFormatMsg = false;
 
     nextPosition= 0;
+    _outputMask = 0x44;
     _port.baud(115200);
 }
 
@@ -71,7 +72,7 @@
                 } else {
                     messagePrt=0;
                 }
-            } else { // in the middle of a message
+            } else { // in the middle of a message                    
                 messagePrt++;
                 if (messagePrt==messageLength) {
                     processRxMessage();
@@ -85,7 +86,7 @@
 void VIPSSerial::processRxMessage()
 {
     if (!checkCRC(&messageInBuffer[0])) {
-//          statusMessage = 6;
+        statusMessage = 6;
         return;
     }
 
@@ -153,14 +154,14 @@
     for (int i=0; i<dataLen; i++)
         messageOutBuffer[i+3] = data[i];
     VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
-   for (int i=0;i<dataLen+5;i++)
+    for (int i=0; i<dataLen+5; i++)
         _port.putc(messageOutBuffer[i]);
 }
 
 void VIPSSerial::getCRC(void *data, int len, void *checksum)
 {
     uint8_t *dataPtr = (uint8_t *)data;
-    uint16_t *crcPtr = (uint16_t *)checksum;
+    uint8_t *crcPtr = (uint8_t *)checksum;
     uint16_t crc = 0x0;
     unsigned char x;
     int byteCount = 0;
@@ -296,6 +297,10 @@
 
         offset+=4;
     }
+    if (mask & 0x0400) {// FIZ data
+
+        offset+=4;
+    }
 
     if (enableAllUpdates) {
 //        printf("Add pos\r\n");
@@ -329,6 +334,8 @@
 
     // calculate timestamps as a function of time since last frame
     outputPosition.time = timeValue;
+    int32_t lTime = lastPositions[lastPoint].pos.time;
+        
     int32_t LastTimeMark = lastPositions[lastPoint].time;
     int32_t PrevTimeMark = lastPositions[prevPoint].time;
     if (PrevTimeMark > LastTimeMark) {
@@ -343,10 +350,9 @@
 
     // interpolate position to requested time.
     if (position::interp(&outputPosition, &(lastPositions[lastPoint].pos), &(lastPositions[prevPoint].pos))) {
-        printf("G\r\n");
+        outputPosition.time = lTime + (timeValue-LastTimeMark)/1000; // last time plus delta
         return &outputPosition;
     }
-    printf(" B\r\n");
 
     return NULL;
 }