Diff: VIPSSerialProtocol.cpp

Revision:

1:dd1f7e162f91

Parent:

0:97661408d0f9

Child:

2:a79201e302d7

--- a/VIPSSerialProtocol.cpp	Fri Jan 15 11:49:01 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Fri Jan 15 15:44:39 2021 +0000
@@ -11,40 +11,45 @@
 #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};
+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 Rx, const PinName Tx): _port(Rx,Tx) {
-     _port.baud(115200);
-     _port.attach(callback(this,&VIPSSerial::onSerialRx));
+VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
+{
     messagePrt = 0;
     messageLength = 0;
+
     pointCount = 0;
     nextPosition = 0;
+
     outputPtr = NULL;
     statusMessage = 0;
+
     enableAllUpdates = false;
     newFormatMsg = false;
+
     nextPosition= 0;
- }
+    _port.baud(115200);
+}
 
+void VIPSSerial::run(void)
+{
+    _port.attach(callback(this, &VIPSSerial::onSerialRx));
+}
 
-void VIPSSerial::onSerialRx() {
-static uint32_t readCount = 0;
+void VIPSSerial::onSerialRx(void)
+{
     while (_port.readable()) {
-        led2=!led2;
+        messageInBuffer[messagePrt] = _port.getc();
         if (messagePrt==0) { // look for header
-           int bytesIn = _port.getc();
-           if ((bytesIn==1) && ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24))) {
+            if ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24)) {
                 messagePrt=1;
-                led3=1;
                 newFormatMsg=(messageInBuffer[0]==0x24);
-           }
+            }
         } else if (newFormatMsg) {
             if (messagePrt < 4) {
-                messageInBuffer[messagePrt] = _port.getc();
                 messagePrt++;
-                if (messagePrt>=4) {
+                if (messagePrt==4) {
                     if (messageInBuffer[1]!=0xd9)
                         messagePrt=0;
                     messageLength = *(uint16_t*)(messageInBuffer+2);
@@ -52,7 +57,6 @@
                         messagePrt = 0;
                 }
             } else {
-                messageInBuffer[messagePrt] = _port.getc();
                 messagePrt++;
                 if (messagePrt == messageLength) {
                     parsePostionInput_mocap();
@@ -61,7 +65,6 @@
             }
         } else {
             if (messagePrt==1) {
-                messageInBuffer[messagePrt] = _port.getc();
                 if (messageInBuffer[1]<128)            { // valid length
                     messageLength = messageInBuffer[1];
                     messagePrt = 2;
@@ -69,10 +72,8 @@
                     messagePrt=0;
                 }
             } else { // in the middle of a message
-                messageInBuffer[messagePrt] = _port.getc();
                 messagePrt++;
                 if (messagePrt==messageLength) {
-                    led3=0;
                     processRxMessage();
                     messagePrt=0;
                 }
@@ -81,47 +82,49 @@
     }
 }
 
-void VIPSSerial::processRxMessage() {
+void VIPSSerial::processRxMessage()
+{
     if (!checkCRC(&messageInBuffer[0])) {
 //          statusMessage = 6;
         return;
     }
-    
+
     switch (messageInBuffer[2]) {
         case _UWBFitted:
-          statusMessage = 1;
+            statusMessage = 1;
             sendAck(messageInBuffer[2]);
             break;
         case _EnableUWB:
-           statusMessage=2;
+            statusMessage=2;
         case _DisableUWB: // for all of these just send an ack.
         case _SetDeltaTime:
             sendAck(messageInBuffer[2]);
             break;
         case _GetPPSTime:
-        statusMessage=3;
+            statusMessage=3;
             sendVBOXTime();
             // send vbox tick counter
             break;
         case _VBOXReady:
             if (ppsActive) {
-                        statusMessage=4;
+                statusMessage=4;
                 sendAck(_VBOXReady);
             } else {
-                        statusMessage=5;
+                statusMessage=5;
                 sendNack(_VBOXReady);
             }
             break;
         case _UWBData_Legacy:
             parsePostionInput_legacy();
-        break;
+            break;
         default:
-        break;
-     }
+            break;
+    }
 }
 
 
-void VIPSSerial::sendVBOXTime() {
+void VIPSSerial::sendVBOXTime()
+{
     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;
@@ -130,65 +133,73 @@
     sendResponse(_GetPPSTime,timeValue,3);
 }
 
-void VIPSSerial::sendAck(unsigned char function) {
+void VIPSSerial::sendAck(unsigned char function)
+{
     unsigned char ack=0x01;
     sendResponse(function,&ack,1);
 }
 
-void VIPSSerial::sendNack(unsigned char function) {
-        unsigned char nack=0x00;
+void VIPSSerial::sendNack(unsigned char function)
+{
+    unsigned char nack=0x00;
     sendResponse(function,&nack,1);
 }
 
-void VIPSSerial::sendResponse(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];
-   getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
+void VIPSSerial::sendResponse(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);
    for (int i=0;i<dataLen+5;i++)
         _port.putc(messageOutBuffer[i]);
 }
 
-void VIPSSerial::getCRC(unsigned char* data, int len, unsigned char* checksum) {
+void VIPSSerial::getCRC(void *data, int len, void *checksum)
+{
+    uint8_t *dataPtr = (uint8_t *)data;
+    uint16_t *crcPtr = (uint16_t *)checksum;
     uint16_t crc = 0x0;
     unsigned char x;
     int byteCount = 0;
 
     while ((len--) > 0) {
-        x = (unsigned char)(crc >> 8 ^ data[byteCount++]);
+        x = (unsigned char)(crc >> 8 ^ dataPtr[byteCount++]);
         x ^= (unsigned char)(x >> 4);
         crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
     }
-    checksum[0] = crc >> 8;
-    checksum[1] = crc &0x00ff;
+    crcPtr[0] = crc >> 8;
+    crcPtr[1] = crc &0x00ff;
 }
 
-bool VIPSSerial::checkCRC(unsigned char* data) {
+bool VIPSSerial::checkCRC(unsigned char* data)
+{
     unsigned char expectedCRC[2];
     int length = data[1];
     if (data[0] == 0xff) // for response length doesn't include the header
         length++;
-    getCRC(data, length-2, expectedCRC);
+    VIPSSerial::getCRC(data, length-2, expectedCRC);
     if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
         return true;
     return false;
 }
 
 
-bool VIPSSerial::checkNewPacketRC(unsigned char* data) {
+bool VIPSSerial::checkNewPacketRC(unsigned char* data)
+{
     unsigned char expectedCRC[2];
     int length = data[2] | (((int)data[3])<<8);
-    getCRC(data, length-2, expectedCRC);
+    VIPSSerial::getCRC(data, length-2, expectedCRC);
     if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
         return true;
     return false;
 }
 
 
-void VIPSSerial::parsePostionInput_legacy() {
-
+void VIPSSerial::parsePostionInput_legacy()
+{
     printf("L");
     uint8_t tmpBuffer[8];
     int32_t tmpInt;
@@ -208,9 +219,9 @@
     lastPositions[nextPosition].pos.yaw = 0;
 
     if (enableAllUpdates) {
-            printf("Add pos\r\n");
-            outputPtr = &outputPosition;
-            memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
+        printf("Add pos\r\n");
+        outputPtr = &outputPosition;
+        memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
     }
 
     nextPosition++;
@@ -220,7 +231,8 @@
     pointCount++;
 }
 
-void VIPSSerial::parsePostionInput_mocap() {
+void VIPSSerial::parsePostionInput_mocap()
+{
     if (!checkNewPacketRC(&messageInBuffer[0])) {
         pc.write("CRC error\r\n",11);
         return;
@@ -228,13 +240,13 @@
     lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
     uint32_t mask = *(uint32_t*)(messageInBuffer+4);
     int offset = 32; // end of position
-    
+
     lastPositions[nextPosition].pos.time = *(uint32_t*)(messageInBuffer+8);
     lastPositions[nextPosition].pos.X = *(double *)(messageInBuffer+12);
     lastPositions[nextPosition].pos.Y = *(double *)(messageInBuffer+20);
     lastPositions[nextPosition].pos.Height = *(float *)(messageInBuffer+28);
     if (mask & 0x0002) { // parse status
-        
+
         offset +=4;
     }
 
@@ -252,43 +264,43 @@
     if (mask & 0x0008) { // velocity
 
         offset+=8;
-    } 
+    }
     if (mask & 0x0010) {// vert velocity
 
-        offset+=4; 
+        offset+=4;
     }
     if (mask & 0x0020) { // pos uncertainty
 
-        offset+=12; 
+        offset+=12;
         if (mask & 0x0004) { // orientation uncertainty
-            offset += 12; 
+            offset += 12;
         }
         if (mask & 0x0008) { // velocity uncertainty
-            offset += 12; 
+            offset += 12;
         }
     }
     if (mask & 0x0040) {  // accuracy
 
         offset+=4;
-    }   
-    if (mask & 0x0080) {  // raw UWB 
+    }
+    if (mask & 0x0080) {  // raw UWB
 
         offset+=24;
     }
-    if (mask & 0x0100) { // raw IMU 
+    if (mask & 0x0100) { // raw IMU
 
         offset+=24;
     }
 
     if (mask & 0x0200) {// rover info
 
-        offset+=4; 
+        offset+=4;
     }
 
     if (enableAllUpdates) {
-            printf("Add pos\r\n");
-            outputPtr = &outputPosition;
-            memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
+//        printf("Add pos\r\n");
+        outputPtr = &outputPosition;
+        memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
     }
 
     nextPosition++;
@@ -298,45 +310,44 @@
     pointCount++;
 }
 
-    // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
-position* VIPSSerial::sendPositionForTime(uint32_t timeValue) {
+// send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
+position* VIPSSerial::sendPositionForTime(uint32_t timeValue)
+{
     static uint32_t lastPoints = 0;
     if (pointCount < 2)
         return NULL;
     if (lastPoints == pointCount)
         return NULL;
     lastPoints = pointCount;
-    
-        int lastPoint = nextPosition - 1;
-        int prevPoint = nextPosition - 2;
-        if (lastPoint<0)
-            lastPoint+=posHistoryLen;
-        if (prevPoint<0)
-            prevPoint+=posHistoryLen;
-        
-            printf("i");
+
+    int lastPoint = nextPosition - 1;
+    int prevPoint = nextPosition - 2;
+    if (lastPoint<0)
+        lastPoint+=posHistoryLen;
+    if (prevPoint<0)
+        prevPoint+=posHistoryLen;
 
-            // calculate timestamps as a function of time since last frame
-                outputPosition.time = timeValue;
-                int32_t LastTimeMark = lastPositions[lastPoint].time;
-                int32_t PrevTimeMark = lastPositions[prevPoint].time;
-                if (PrevTimeMark > LastTimeMark) {
-                    LastTimeMark += TimeSinceLastFrameWrap;
-                    outputPosition.time += TimeSinceLastFrameWrap;
-                }
-                if (LastTimeMark > outputPosition.time)
-                    outputPosition.time += TimeSinceLastFrameWrap;
-            
-                lastPositions[lastPoint].pos.time = LastTimeMark;
-                lastPositions[prevPoint].pos.time = PrevTimeMark;
+    // calculate timestamps as a function of time since last frame
+    outputPosition.time = timeValue;
+    int32_t LastTimeMark = lastPositions[lastPoint].time;
+    int32_t PrevTimeMark = lastPositions[prevPoint].time;
+    if (PrevTimeMark > LastTimeMark) {
+        LastTimeMark += TimeSinceLastFrameWrap;
+        outputPosition.time += TimeSinceLastFrameWrap;
+    }
+    if (LastTimeMark > outputPosition.time)
+        outputPosition.time += TimeSinceLastFrameWrap;
 
-            // interpolate position to requested time.
-                if (position::interp(&outputPosition, &(lastPositions[lastPoint].pos), &(lastPositions[prevPoint].pos))) {
-                    printf(" G\r\n");
-                    return &outputPosition;
-                }
-                                    printf(" B\r\n");
+    lastPositions[lastPoint].pos.time = LastTimeMark;
+    lastPositions[prevPoint].pos.time = PrevTimeMark;
 
-        return NULL;
+    // interpolate position to requested time.
+    if (position::interp(&outputPosition, &(lastPositions[lastPoint].pos), &(lastPositions[prevPoint].pos))) {
+        printf("G\r\n");
+        return &outputPosition;
+    }
+    printf(" B\r\n");
+
+    return NULL;
 }