Diff: PNTSerial.cpp

Revision:

82:ee6eed2a51bd

Child:

83:f0d1d948c306

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PNTSerial.cpp	Mon Nov 14 14:53:12 2022 +0000
@@ -0,0 +1,248 @@
+#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++;
+}