File content as of revision 10:053bac3e326b:

#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 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;
    _outputMask = 0x44;
    _port.baud(115200);
}

void VIPSSerial::run(void)
{
    _port.attach(callback(this, &VIPSSerial::onSerialRx));
}

void VIPSSerial::onSerialRx(void)
{
    while (_port.readable()) {
        messageInBuffer[messagePrt] = _port.getc();
        if (messagePrt==0) { // look for header
            if ((messageInBuffer[0]==0x2A) || (messageInBuffer[0]==0x24)) {
                messagePrt=1;
                newFormatMsg=(messageInBuffer[0]==0x24);
            }
        } else if (newFormatMsg) {
            if (messagePrt < 4) {
                messagePrt++;
                if (messagePrt==4) {
                    if (messageInBuffer[1]!=0xd9)
                        messagePrt=0;
                    messageLength = *(uint16_t*)(messageInBuffer+2);
                    if ((messageLength>115) || (messageLength<34))
                        messagePrt = 0;
                }
            } else {
                messagePrt++;
                if (messagePrt == messageLength) {
                    parsePostionInput_mocap();
                    messagePrt=0;
                }
            }
        } else {
            if (messagePrt==1) {
                if (messageInBuffer[1]<128)            { // valid length
                    messageLength = messageInBuffer[1];
                    messagePrt = 2;
                } else {
                    messagePrt=0;
                }
            } else { // in the middle of a message
                messagePrt++;
                if (messagePrt==messageLength) {
                    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);
//    pc.printf("Sending time = %d ",VBOXTicks);
}

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];
    VIPSSerial::getCRC(messageOutBuffer,dataLen+3,messageOutBuffer+dataLen+3);
    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;
    uint8_t *crcPtr = (uint8_t *)checksum;
    uint16_t crc = 0x0;
    unsigned char x;
    int byteCount = 0;

    while ((len--) > 0) {
        x = (unsigned char)(crc >> 8 ^ dataPtr[byteCount++]);
        x ^= (unsigned char)(x >> 4);
        crc = (uint16_t)((crc << 8) ^ (x << 12) ^ (x << 5) ^ x);
    }
    crcPtr[0] = crc >> 8;
    crcPtr[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++;
    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)
{
    unsigned char expectedCRC[2];
    int length = data[2] | (((int)data[3])<<8);
    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()
{
    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
        lastPositions[nextPosition].pos.beacons = messageInBuffer[offset++];
        lastPositions[nextPosition].pos.solutionType = messageInBuffer[offset++];
        lastPositions[nextPosition].pos.KFStatus = *(uint16_t*)(messageInBuffer + offset);
        offset +=2;
    }

    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
        lastPositions[nextPosition].pos.ID = *(messageInBuffer+offset+3);
        offset+=4;
    }
    if (mask & 0x0080) {  // raw UWB

        offset+=24;
    }
    if (mask & 0x0100) { // raw IMU

        offset+=24;
    }

    if (mask & 0x0200) {// rover info

        offset+=4;
    }
    if (mask & 0x0400) {// FIZ data

        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;

    // calculate timestamps as a function of time since last frame

    uint64_t LastTimeMark = lastPositions[lastPoint].time;
    uint64_t PrevTimeMark = lastPositions[prevPoint].time;
    uint64_t timeValueUnwrap = timeValue;
    if (PrevTimeMark > LastTimeMark)
        LastTimeMark += ((uint64_t)1)<<32;
    if (LastTimeMark > timeValueUnwrap)
        timeValueUnwrap += ((uint64_t)1)<<32;

    outputPosition.time = timeValueUnwrap-PrevTimeMark;

    int32_t lTime = lastPositions[lastPoint].pos.time;
    int32_t pTime = lastPositions[prevPoint].pos.time;

    lastPositions[lastPoint].pos.time = LastTimeMark-PrevTimeMark;
    lastPositions[prevPoint].pos.time = 0;

    // interpolate position to requested time.
    if (position::interp(&outputPosition, &(lastPositions[lastPoint].pos), &(lastPositions[prevPoint].pos))) {
        lastPositions[lastPoint].pos.time = lTime;
        lastPositions[prevPoint].pos.time = pTime;
        return &outputPosition;
    }

    lastPositions[lastPoint].pos.time = lTime;
    lastPositions[prevPoint].pos.time = pTime;

    return NULL;
}