Diff: VIPSSerialProtocol.cpp

Revision:

18:ad407a2ed4c9

Parent:

16:a8d3a0dbe4bf

Child:

22:0dd9c1b5664a

--- a/VIPSSerialProtocol.cpp	Fri Apr 30 12:21:01 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Mon May 17 11:41:57 2021 +0000
@@ -25,6 +25,8 @@
 
     enableAllUpdates = false;
     newFormatMsg = false;
+    
+    queueLen = 0;
 
     SmoothBy = 500;
     XSmoothTotal = 0;
@@ -138,10 +140,10 @@
 {
     unsigned char timeValue[3];
     uint32_t timeToSend = VBOXTicks-1; // we track time at next PPS, message requires time at the last PPS.
-    timeValue[0]= (timeToSend>>16)&0x00ff;
+    timeValue[0]= timeToSend&0x00ff;
     timeValue[1]= (timeToSend>>8)&0x00ff;
-    timeValue[2]= timeToSend&0x00ff;
-    sendResponse(_GetPPSTime,timeValue,3);
+    timeValue[2]= (timeToSend>>16)&0x00ff;
+    queueResponse(_GetPPSTime,timeValue,3);
 //    pc.printf("Sending time = %d ",VBOXTicks);
 }
 
@@ -168,6 +170,25 @@
     for (int i=0; i<dataLen+5; i++)
         _port.putc(messageOutBuffer[i]);
 }
+void VIPSSerial::queueResponse(unsigned char function, unsigned char* data, int dataLen)
+{
+    messageOutBuffer[0]=0xff;
+    messageOutBuffer[1]=dataLen+4;
+    messageOutBuffer[2]=function;
+    for (int i=0; i<dataLen; i++)
+        messageOutBuffer[i+3] = data[i];
+    VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
+    queueLen = dataLen+5;
+}
+
+void VIPSSerial::sendQueued()
+{
+    if (queueLen) {
+        for (int i=0; i<queueLen; i++)
+            _port.putc(messageOutBuffer[i]);
+        queueLen = 0;
+    }
+}
 
 void VIPSSerial::getCRC(void *data, int len, void *checksum)
 {