Diff: VIPSSerialProtocol.cpp

Revision:

0:97661408d0f9

Child:

1:dd1f7e162f91

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VIPSSerialProtocol.cpp	Fri Jan 15 11:49:01 2021 +0000
@@ -0,0 +1,342 @@
+#include "LTCApp.h"
+#include <cstdint>
+#include <cstring>
+
+#define _DisableUWB 0x00
+#define _EnableUWB  0x01
+#define _GetPPSTime  0x02
+#define _UWBData_Legacy 0x03
+#define _UWBData_MOCAP 0x11
+#define _UWBFitted  0x04
+#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 Rx, const PinName Tx): _port(Rx,Tx) {
+     _port.baud(115200);
+     _port.attach(callback(this,&VIPSSerial::onSerialRx));
+    messagePrt = 0;
+    messageLength = 0;
+    pointCount = 0;
+    nextPosition = 0;
+    outputPtr = NULL;
+    statusMessage = 0;
+    enableAllUpdates = false;
+    newFormatMsg = false;
+    nextPosition= 0;
+ }
+
+
+void VIPSSerial::onSerialRx() {
+static uint32_t readCount = 0;
+    while (_port.readable()) {
+        led2=!led2;
+        if (messagePrt==0) { // look for header
+           int bytesIn = _port.getc();
+           if ((bytesIn==1) && ((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 (messageInBuffer[1]!=0xd9)
+                        messagePrt=0;
+                    messageLength = *(uint16_t*)(messageInBuffer+2);
+                    if ((messageLength>115) || (messageLength<34))
+                        messagePrt = 0;
+                }
+            } else {
+                messageInBuffer[messagePrt] = _port.getc();
+                messagePrt++;
+                if (messagePrt == messageLength) {
+                    parsePostionInput_mocap();
+                    messagePrt=0;
+                }
+            }
+        } else {
+            if (messagePrt==1) {
+                messageInBuffer[messagePrt] = _port.getc();
+                if (messageInBuffer[1]<128)            { // valid length
+                    messageLength = messageInBuffer[1];
+                    messagePrt = 2;
+                } else {
+                    messagePrt=0;
+                }
+            } else { // in the middle of a message
+                messageInBuffer[messagePrt] = _port.getc();
+                messagePrt++;
+                if (messagePrt==messageLength) {
+                    led3=0;
+                    processRxMessage();
+                    messagePrt=0;
+                }
+            }
+        }
+    }
+}
+
+void VIPSSerial::processRxMessage() {
+    if (!checkCRC(&messageInBuffer[0])) {
+//          statusMessage = 6;
+        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;
+        case _UWBData_Legacy:
+            parsePostionInput_legacy();
+        break;
+        default:
+        break;
+     }
+}
+
+
+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;
+    timeValue[1]= (timeToSend>>8)&0x00ff;
+    timeValue[2]= timeToSend&0x00ff;
+    sendResponse(_GetPPSTime,timeValue,3);
+}
+
+void VIPSSerial::sendAck(unsigned char function) {
+    unsigned char ack=0x01;
+    sendResponse(function,&ack,1);
+}
+
+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);
+   for (int i=0;i<dataLen+5;i++)
+        _port.putc(messageOutBuffer[i]);
+}
+
+void VIPSSerial::getCRC(unsigned char* data, int len, unsigned char* checksum) {
+    uint16_t crc = 0x0;
+    unsigned char x;
+    int byteCount = 0;
+
+    while ((len--) > 0) {
+        x = (unsigned char)(crc >> 8 ^ data[byteCount++]);
+        x ^= (unsigned char)(x >> 4);
+        crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
+    }
+    checksum[0] = crc >> 8;
+    checksum[1] = crc &0x00ff;
+}
+
+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);
+    if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
+        return true;
+    return false;
+}
+
+
+bool VIPSSerial::checkNewPacketRC(unsigned char* data) {
+    unsigned char expectedCRC[2];
+    int length = data[2] | (((int)data[3])<<8);
+    getCRC(data, length-2, expectedCRC);
+    if ((data[length-2]==expectedCRC[0]) && (data[length-1]==expectedCRC[1]))
+        return true;
+    return false;
+}
+
+
+void VIPSSerial::parsePostionInput_legacy() {
+
+    printf("L");
+    uint8_t tmpBuffer[8];
+    int32_t tmpInt;
+    memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+4, 3);
+    tmpInt &= 0x00ffffff;
+    lastPositions[nextPosition].pos.time = tmpInt*10;
+    memcpy(tmpBuffer, messageInBuffer+7, 8);
+    lastPositions[nextPosition].pos.X = *(double *)(tmpBuffer);
+    memcpy(tmpBuffer, messageInBuffer+15, 8);
+    lastPositions[nextPosition].pos.Y = *(double *)(tmpBuffer);
+    memcpy((uint8_t *)(&tmpInt)+1, messageInBuffer+27, 3);
+    if (tmpInt & 0x00800000)
+        tmpInt |= 0xff000000;
+    lastPositions[nextPosition].pos.Height = tmpInt/100.0f;
+    lastPositions[nextPosition].pos.roll = 0;
+    lastPositions[nextPosition].pos.pitch = 0;
+    lastPositions[nextPosition].pos.yaw = 0;
+
+    if (enableAllUpdates) {
+            printf("Add pos\r\n");
+            outputPtr = &outputPosition;
+            memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
+    }
+
+    nextPosition++;
+    if (nextPosition == posHistoryLen) {
+        nextPosition = 0;
+    }
+    pointCount++;
+}
+
+void VIPSSerial::parsePostionInput_mocap() {
+    if (!checkNewPacketRC(&messageInBuffer[0])) {
+        pc.write("CRC error\r\n",11);
+        return;
+    }
+    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;
+    }
+
+    if (mask & 0x0004) {
+        lastPositions[nextPosition].pos.roll = *(float *)(messageInBuffer+offset);
+        lastPositions[nextPosition].pos.pitch = *(float *)(messageInBuffer+offset+4);
+        lastPositions[nextPosition].pos.yaw = *(float *)(messageInBuffer+offset+8);
+        offset+=12;
+    } else {
+        lastPositions[nextPosition].pos.roll = 0;
+        lastPositions[nextPosition].pos.pitch = 0;
+        lastPositions[nextPosition].pos.yaw = 0;
+    }
+
+    if (mask & 0x0008) { // velocity
+
+        offset+=8;
+    } 
+    if (mask & 0x0010) {// vert velocity
+
+        offset+=4; 
+    }
+    if (mask & 0x0020) { // pos uncertainty
+
+        offset+=12; 
+        if (mask & 0x0004) { // orientation uncertainty
+            offset += 12; 
+        }
+        if (mask & 0x0008) { // velocity uncertainty
+            offset += 12; 
+        }
+    }
+    if (mask & 0x0040) {  // accuracy
+
+        offset+=4;
+    }   
+    if (mask & 0x0080) {  // raw UWB 
+
+        offset+=24;
+    }
+    if (mask & 0x0100) { // raw IMU 
+
+        offset+=24;
+    }
+
+    if (mask & 0x0200) {// rover info
+
+        offset+=4; 
+    }
+
+    if (enableAllUpdates) {
+            printf("Add pos\r\n");
+            outputPtr = &outputPosition;
+            memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
+    }
+
+    nextPosition++;
+    if (nextPosition == posHistoryLen) {
+        nextPosition = 0;
+    }
+    pointCount++;
+}
+
+    // 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");
+
+            // 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;
+
+            // 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;
+}
+