File content as of revision 0:97661408d0f9:

#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;
}