Diff: PNTSerial.cpp

Revision:

83:f0d1d948c306

Parent:

82:ee6eed2a51bd

diff -r ee6eed2a51bd -r f0d1d948c306 PNTSerial.cpp
--- a/PNTSerial.cpp	Mon Nov 14 14:53:12 2022 +0000
+++ b/PNTSerial.cpp	Tue Nov 15 10:28:47 2022 +0000
@@ -1,248 +1,219 @@
+#include "PNTSerial.h"
 #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;
+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;
+  origin.SetDecimalDegrees(Latitude, Longitude, altitude);
+  dataPoint.SetReferancePosition(&origin);
+  originSet = true;
 }
 
-void PNTSerial::run(void)
-{
-    _port.attach(callback(this, &PNTSerial::onSerialRx));
+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;
-                }
-            }
+void PNTSerial::onSerialRx(void) {
+  while (_port.readable()) {
+    if (BypassMode) {
+      vipsBypassRx(_port.getc());
+    } else {
+      uint8_t charIn = _port.getc();
+      if (messagePrt == 0) { // wait for start character
+        if (charIn == '$') {
+          messageInBuffer[messagePrt++] = charIn;
         }
+      } else {
+        messageInBuffer[messagePrt++] = charIn;
+        // basic sanity check that second byte is expected value.
+        if ((messagePrt==2) && (messageInBuffer[1] != 'R')) 
+            messagePrt = 0;
+        // got the full message 
+        else if (messagePrt == PNTSerialConstants::MessageLength) {
+          parsePostion();
+          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 length
+  if (messagePrt != PNTSerialConstants::MessageLength)
+    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 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;
+  // check CRC
+  uint16_t Polynomial = 4129;
+  uint16_t CRC = 0;
+  for (int i = 0; i < PNTSerialConstants::MessageLength - 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;
+      }
     }
-    uint16_t messageCRC = ((uint16_t)messageInBuffer[RLMessageLength-2])<<8 | messageInBuffer[RLMessageLength-1];
-    return messageCRC == CRC;
+    CRC &= 0xffff;
+  }
+  uint16_t messageCRC = ((uint16_t)messageInBuffer[PNTSerialConstants::MessageLength - 2]) << 8 |
+                        messageInBuffer[PNTSerialConstants::MessageLength - 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;
+  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
+  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;
+  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
+  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);
 
-    do {
-        value <<= 8;
-        value |= messageInBuffer[start++];
-       } while (--bytes);
-    
-    return value;
+  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++];
+  int messageIndex = 0;
+  lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
+  if (!CheckValidMessage())
+    return;
+  // 8 byte header ("$RLPNT$,")
+  messageIndex = 8;
+  // index 8 = 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;
+  // 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);
+  // 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 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++];
+  // 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
+  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 pitch, roll, yaw rates
+  messageIndex += 2*3;
 
-// 2 byte KF status
-        lastPositions[nextPosition].pos.KFStatus = (uint16_t)readUInt32(messageIndex,2);
+  // 2 byte X,Y,Z accelerations
+  messageIndex += 2*3;
+
+  // 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;
-
-
+  // 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 (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
-    }
+    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));
+  smoothOutputPacket(&(lastPositions[nextPosition].pos));
+
+  if (enableAllUpdates) {
+    //        printf("Add pos\r\n");
+    outputPtr = &outputPosition;
+    memcpy(outputPtr, &(lastPositions[nextPosition].pos), sizeof(position));
+  }
 
-    if (enableAllUpdates) {
-//        printf("Add pos\r\n");
-        outputPtr = &outputPosition;
-        memcpy(outputPtr,&(lastPositions[nextPosition].pos),sizeof(position));
-    }
+  nextPosition++;
+  if (nextPosition == posHistoryLen) {
+    nextPosition = 0;
+  }
+  pointCount++;
+}
 
-    nextPosition++;
-    if (nextPosition == posHistoryLen) {
-        nextPosition = 0;
-    }
-    pointCount++;
-}