File content as of revision 36:fff2a71bbf86:

#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


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;

    queueLen = 0;

    SmoothBy = 500;
    XSmoothTotal = 0;
    YSmoothTotal = 0;
    ZSmoothTotal = 0;
    SmoothRunning = false;

    BypassMode = false;

    nextPosition= 0;
    _outputMask = 0x44;
    _port.baud(115200);
}

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

void VIPSSerial::bypassTx(char byte)
{
    _port.putc(byte);
}

void VIPSSerial::onSerialRx(void)
{
    while (_port.readable()) {
        if (BypassMode) {
            vipsBypassRx(_port.getc());
        } else {
            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])) {
        pc.puts("VIPS CRC error\r\n");
        return;
    }

    switch (messageInBuffer[2]) {
        case _UWBFitted:
            sendAck(messageInBuffer[2]);
            break;
        case _EnableUWB:
        case _DisableUWB: // for all of these just send an ack.
        case _SetDeltaTime:
            sendAck(messageInBuffer[2]);
            break;
        case _GetPPSTime:
            sendVBOXTime();
            // send vbox tick counter
            break;
        case _VBOXReady:
            if (ppsActive) {
                sendAck(_VBOXReady);
            } else {
                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&0x00ff;
    timeValue[1]= (timeToSend>>8)&0x00ff;
    timeValue[2]= (timeToSend>>16)&0x00ff;
    queueResponse(_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::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)
{
    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;
    }

//   led1=!led1;

    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 ( ( ( lastPositions[nextPosition].pos.KFStatus & 0xE634) == 0x0400) && (!hyperSmoothEnabled)) {
        EnableSmoothing(true);
        pc.write("Auto HS On\r\n", 12);
    } else if (hyperSmoothEnabled) {
        EnableSmoothing(false);
        pc.write("Auto HS Off\r\n", 13);
    }*/ //Auto Hypersmooth, test in later commit after Ethernet Thread tested

    smoothOutputPacket(&(lastPositions[nextPosition].pos));

    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;

    __disable_irq();   // disable IRQ and make a copy of the data we're going to interpolate.
    int lastPoint = nextPosition - 1;
    int prevPoint = nextPosition - 2;
    if (lastPoint<0)
        lastPoint+=posHistoryLen;
    if (prevPoint<0)
        prevPoint+=posHistoryLen;

    memcpy(&lastPos,&lastPositions[lastPoint], sizeof(struct posAndTime_s));
    memcpy(&prevPos,&lastPositions[prevPoint], sizeof(struct posAndTime_s));
    __enable_irq();

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

    uint64_t LastTimeMark = lastPos.time;
    uint64_t PrevTimeMark = prevPos.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; // should be between 10,000 and 20,000

    // interpolate uses the position times. Replace them with the internal clock counts.
    lastPos.pos.time = LastTimeMark-PrevTimeMark; // should be very close to 10,000
    prevPos.pos.time = 0;

    // interpolate position to requested time.
    if (position::interp(&outputPosition, &(lastPos.pos), &(prevPos.pos))) {
        return &outputPosition;
    }

    // interpolation failed. Return most recent location
    return &lastPos.pos;
}

void VIPSSerial::smoothOutputPacket(position *posPtr)
{
    xFilter.addPoint(posPtr->X);
    yFilter.addPoint(posPtr->Y);
    zFilter.addPoint(posPtr->Height);

    if (hyperSmoothEnabled) {
        if (!SmoothRunning) {
            XSmoothTotal = posPtr->X * (SmoothBy - 1);
            YSmoothTotal = posPtr->Y * (SmoothBy - 1);
            ZSmoothTotal = posPtr->Height * (SmoothBy - 1);
            SmoothRunning = true;
            pc.write("Seeded Filter\r\n",11);
        }
        //smooth the KF_X and KF_Y positions
        XSmoothTotal += posPtr->X;
        posPtr->X = XSmoothTotal / SmoothBy;
        XSmoothTotal -= posPtr->X;

        YSmoothTotal += posPtr->Y;
        posPtr->Y = YSmoothTotal / SmoothBy;
        YSmoothTotal -= posPtr->Y;

        ZSmoothTotal += posPtr->Height;
        posPtr->Height = ZSmoothTotal / SmoothBy;
        ZSmoothTotal -= posPtr->Height;
    } else {
        SmoothRunning = false;
 //       pc.printf("filterX = %f\r\n",xFilter.lastValue());
        posPtr->X = xFilter.lastValue();
        posPtr->Y = yFilter.lastValue();
        posPtr->Height = zFilter.lastValue();
    }
    posPtr->roll = rollFilter.addPoint(posPtr->roll);
    posPtr->pitch = pitchFilter.addPoint(posPtr->pitch);
    posPtr->yaw = yawFilter.addPoint(posPtr->yaw);
}

bool VIPSSerial::setFilters(struct UserSettings_s *settings)
{
    if (settings->FilterXY) {
        if (!xFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
            return false;
        if (!yFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
            return false;
    } else {
        xFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
        yFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
    }

    if (settings->FilterZ) {
        if (!zFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
            return false;
    } else {
        zFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
    }

    if (settings->FilterRoll) {
        if (!rollFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
            return false;
    } else {
        rollFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
    }

    if (settings->FilterPitch) {
        if (!pitchFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
            return false;
    } else {
        pitchFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
    }

    if (settings->FilterYaw) {
        if (!yawFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
            return false;
    } else {
        yawFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
    }
    return true;
}