File content as of revision 82:ee6eed2a51bd:

#include "LTCApp.h"
#include "PNTSerial.h"
//#include <cstdint>
//#include <cstring>

//#define IdleTxBuffer (txBuf == TXBuffer1)?TXBuffer2:TXBuffer1
//#define ActiveTxBuffer (txBuf == TXBuffer1)?TXBuffer1:TXBuffer2

const int RLMessageLength = 63;


PNTSerial::PNTSerial(const PinName Tx, const PinName Rx) : PosSource(Tx,Rx)
{
    originSet = false;
}

void PNTSerial::setOrigin(double Latitude, double Longitude, float altitude) {
    origin.SetDecimalDegrees(Latitude, Longitude, altitude);
    dataPoint.SetReferancePosition(&origin);
    originSet = true;
}

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

void PNTSerial::onSerialRx(void)
{
    while (_port.readable()) {
        if (BypassMode) {
            vipsBypassRx(_port.getc());
        } else {
            uint8_t charIn = _port.getc();
            if (messagePrt == 0) {
                if (charIn == '$') {
                    messageInBuffer[messagePrt++] = charIn;
                }
            } else {
                messageInBuffer[messagePrt++] = charIn;
                if (charIn == '/n') {
                    parsePostion();
                    messagePrt = 0;
                } else if (messagePrt == MaxBuffSize) {
                    messagePrt = 0;
                }
            }
        }
    }
}

/*
uint32_t PNTSerial::NMEATimeToMS() {
    int hours = (messageInBuffer[messageParsePtr]-'0')*10+(messageInBuffer[messageParsePtr+1]-'0');
    int minutes = (messageInBuffer[messageParsePtr+2]-'0')*10+(messageInBuffer[messageParsePtr+3]-'0');
    int seconds = (messageInBuffer[messageParsePtr+4]-'0')*10+(messageInBuffer[messageParsePtr+5]-'0');
    int ms = 0;
    messageParsePtr+=6;
    if (messageInBuffer[messageParsePtr] == '.') {
        messageParsePtr++;
        ms = (messageInBuffer[messageParsePtr++]-'0')*100;
        if (messageInBuffer[messageParsePtr] != ',') {
            ms += (messageInBuffer[messageParsePtr++]-'0')*10;
            if (messageInBuffer[messageParsePtr] != ',') {
                ms += (messageInBuffer[messageParsePtr++]-'0');
            }
        }
    }
    while (messageInBuffer[messageParsePtr] != ',')
        messageParsePtr++;
}
*/

bool PNTSerial::CheckValidMessage() {

//check length
    if (messagePrt != RLMessageLength)
        return false;

    // check header
if (messageInBuffer[0] != '$')
        return false;
    if (messageInBuffer[1] != 'R')
        return false;
    if (messageInBuffer[2] != 'L')
        return false;
    if (messageInBuffer[3] != 'P')
        return false;
    if (messageInBuffer[4] != 'N')
        return false;
    if (messageInBuffer[5] != 'T')
        return false;
    if (messageInBuffer[6] != '$')
        return false;
    if (messageInBuffer[7] != ',')
        return false;

// check CRC
    uint16_t Polynomial = 4129;
    uint16_t CRC=0;
    for (int i=0;i<RLMessageLength-2;i++) { 
        int tmp = messageInBuffer[i];
        CRC ^= tmp<<8;
        for (int bit=8;bit>0;bit--) {
            if (CRC & 0x8000) {
                    CRC <<= 1;
                    CRC ^= Polynomial;
            } else {
                 CRC <<= 1;
            }
        }
        CRC &= 0xffff;
    }
    uint16_t messageCRC = ((uint16_t)messageInBuffer[RLMessageLength-2])<<8 | messageInBuffer[RLMessageLength-1];
    return messageCRC == CRC;
}

uint32_t PNTSerial::readUInt32(int &start, int bytes) {
    uint32_t value = 0;
    do {
        value <<= 8;
        value |= messageInBuffer[start++];
       } while (--bytes);
    return value;
}

int32_t PNTSerial::readInt32(int &start, int bytes) {
    int32_t value = 0;
    if (messageInBuffer[start] & 0x80) // negative value
        value = -1; // set to all 1's

    do {
        value <<= 8;
        value |= messageInBuffer[start++];
       } while (--bytes);
    return value;
}

int64_t PNTSerial::readInt64(int &start, int bytes) {
    int64_t value = 0;
    if (messageInBuffer[start] & 0x80) // negative value
        value = -1; // set to all 1's

    do {
        value <<= 8;
        value |= messageInBuffer[start++];
       } while (--bytes);
    
    return value;
}


void PNTSerial::parsePostion() {
    int messageIndex = 0;
    lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
    if (!CheckValidMessage())
        return;
    // 8 byte header ("$RLPNT$,")
    messageIndex = 9;
    // index 9 = 1 byte sat count
    lastPositions[nextPosition].pos.beacons = messageInBuffer[messageIndex++];

    // 3 bytes time since midnight (10ms per tick)   
    lastPositions[nextPosition].pos.time = readUInt32(messageIndex,3)*10;

    // 5 byte lattitude in 0.000000001 degrees steps (9 decimal places)
    int64_t latInt = readInt64(messageIndex,5);
    // 5 byte longitude in 0.000000001 degrees steps (9 decimal places)
    int64_t lonInt = readInt64(messageIndex,5);

    // 3 byte velocity
    messageIndex+=3;
    // 2 byte heading
    messageIndex+=2;
    // 3 byte altitude
    int32_t altInt = readInt32(messageIndex,3);
    if (originSet) {
        dataPoint.SetDecimalDegrees(latInt*0.000000001,lonInt *0.000000001,altInt*0.01);
        double tmpDouble;
        dataPoint.GetENU(&(lastPositions[nextPosition].pos.X),&(lastPositions[nextPosition].pos.Y),&tmpDouble);
        lastPositions[nextPosition].pos.Height = (float)tmpDouble;
        lastPositions[nextPosition].pos.LLAPosition = false;
    } else {
        lastPositions[nextPosition].pos.X = latInt*0.000000001;
        lastPositions[nextPosition].pos.Y = lonInt *0.000000001;
        lastPositions[nextPosition].pos.Height = altInt*0.01f;
        lastPositions[nextPosition].pos.LLAPosition = true;
    }

    // 3 byte vert velocity
    messageIndex+=3;
    
    // 1 byte solution type
    lastPositions[nextPosition].pos.solutionType = messageInBuffer[messageIndex++];

// 2 byte pitch
        lastPositions[nextPosition].pos.pitch = readInt32(messageIndex,2)*0.01;
// 2 byte roll
        lastPositions[nextPosition].pos.roll = readInt32(messageIndex,2)*0.01;
// 2 byte yaw
        lastPositions[nextPosition].pos.yaw = readInt32(messageIndex,2)*0.01;

// 2 byte pitch, roll, yaw rates
messageIndex+=6;

// 2 byte X,Y,Z accelerations
messageIndex+=6;

// 2 byte KF status
        lastPositions[nextPosition].pos.KFStatus = (uint16_t)readUInt32(messageIndex,2);

// 3 byte wheel speed
messageIndex+=3;
// 1 byte spoof/jam mask
messageIndex++;
// 1 byte spoof/jam indicator
messageIndex++;
// 2 byte date
messageIndex+=2;



    if (UserSettings.AutoHyperSmooth) {
        int testValue = (lastPositions[nextPosition].pos.KFStatus & 0xE634);

        if ( ( ( testValue & 0x400) == 0x400) && (!hyperSmoothEnabled)) {
            EnableSmoothing(true);
            //pc.write("Auto HS On\r\n", 12);
        } else if ( ( ( testValue & 0x400) != 0x400) && (hyperSmoothEnabled) && (!forcedHyperSmooth)) {
            EnableSmoothing(false);
            //pc.write("Auto HS Off\r\n", 13);
        } //Auto Hypersmooth
    }

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