Revision 83:f0d1d948c306, committed 19 months ago

Comitter:

AndyA

Date:

Tue Nov 15 10:28:47 2022 +0000

Parent:

82:ee6eed2a51bd

Child:

84:8094a1e64ecf

Commit message:

improve message parsing. Efficiency improvement in position conversion.

Changed in this revision

--- a/GeoPosition.cpp	Mon Nov 14 14:53:12 2022 +0000
+++ b/GeoPosition.cpp	Tue Nov 15 10:28:47 2022 +0000
@@ -31,6 +31,11 @@
   llaValid = false;
   ecefValid = false;
   enuValid = false;
+
+  sinRefLong = 0;
+  cosRefLong = 0;
+  sinRefLat = 0;
+  cosRefLat = 0;
 }
 
 GeoPosition::GeoPosition(GeoPosition *posToClone) {
@@ -50,6 +55,15 @@
     enuValid = true;
     enuPosition = posToClone->GetENU();
   }
+
+  struct LLA refPosLLA = referencePosition->GetLLA();
+  if (!(referencePosition->LLAKnown()))
+    return;
+  sinRefLong = sin(DEG2RAD(refPosLLA.longitude));
+  cosRefLong = cos(DEG2RAD(refPosLLA.longitude));
+  sinRefLat = sin(DEG2RAD(refPosLLA.latitude));
+  cosRefLat = cos(DEG2RAD(refPosLLA.latitude));
+
 }
 
 GeoPosition::~GeoPosition() {
@@ -155,6 +169,14 @@
   ecefValid = false;
   enuValid = true;
   referencePosition = refPos;
+
+  struct LLA refPosLLA = referencePosition->GetLLA();
+  if (!(referencePosition->LLAKnown()))
+    return;
+  sinRefLong = sin(DEG2RAD(refPosLLA.longitude));
+  cosRefLong = cos(DEG2RAD(refPosLLA.longitude));
+  sinRefLat = sin(DEG2RAD(refPosLLA.latitude));
+  cosRefLat = cos(DEG2RAD(refPosLLA.latitude));
 }
 
 void GeoPosition::SetReferancePosition(GeoPosition *refPos) {
@@ -163,6 +185,13 @@
   if ((llaValid) || (ecefValid))
     enuValid = false;
 
+  struct LLA refPosLLA = referencePosition->GetLLA();
+  if (!(referencePosition->LLAKnown()))
+    return;
+  sinRefLong = sin(DEG2RAD(refPosLLA.longitude));
+  cosRefLong = cos(DEG2RAD(refPosLLA.longitude));
+  sinRefLat = sin(DEG2RAD(refPosLLA.latitude));
+  cosRefLat = cos(DEG2RAD(refPosLLA.latitude));
 }
 
 
@@ -401,31 +430,19 @@
     return;
   }
 
-
   void GeoPosition::ENU2ECEF() {
 
   if (!enuValid || ecefValid || !referencePosition)
     return;
 
-  struct LLA refPosLLA = referencePosition->GetLLA();
   struct ECEF refPosECEF = referencePosition->GetECEF();
-
-  if (!(referencePosition->LLAKnown()))
-    return;
   if (!(referencePosition->ECEFKnown()))
     return;
 
-  double sinRefLong = sin(DEG2RAD(refPosLLA.longitude));
-  double cosRefLong = cos(DEG2RAD(refPosLLA.longitude));
-  double sinRefLat = sin(DEG2RAD(refPosLLA.latitude));
-  double cosRefLat = cos(DEG2RAD(refPosLLA.latitude));
-
   ecefPosition.X = -sinRefLong * enuPosition.E - sinRefLat*cosRefLong*enuPosition.N + cosRefLat*cosRefLong*enuPosition.U + refPosECEF.X;
   ecefPosition.Y = cosRefLong* enuPosition.E - sinRefLat*sinRefLong*enuPosition.N + cosRefLat*sinRefLong*enuPosition.U + refPosECEF.Y;
   ecefPosition.Z = cosRefLat*enuPosition.N + sinRefLat*enuPosition.U + refPosECEF.Z;
   ecefValid = true;
-
-
 }
 
 void GeoPosition::ECEF2ENU() {
@@ -433,24 +450,15 @@
   if (enuValid || !ecefValid || !referencePosition)
     return;
 
-  struct LLA refPosLLA = referencePosition->GetLLA();
   struct ECEF refPosECEF = referencePosition->GetECEF();
 
-  if (!(referencePosition->LLAKnown()))
-    return;
   if (!(referencePosition->ECEFKnown()))
     return;
 
-  double sinRefLong = sin(DEG2RAD(refPosLLA.longitude));
-  double cosRefLong = cos(DEG2RAD(refPosLLA.longitude));
-  double sinRefLat = sin(DEG2RAD(refPosLLA.latitude));
-  double cosRefLat = cos(DEG2RAD(refPosLLA.latitude));
-
   double xDelta = ecefPosition.X - refPosECEF.X;
   double yDelta = ecefPosition.Y - refPosECEF.Y;
   double zDelta = ecefPosition.Z - refPosECEF.Z;
 
-
   enuPosition.E = -sinRefLong * xDelta + cosRefLong * yDelta;
   enuPosition.N = -sinRefLat * cosRefLong * xDelta - sinRefLat*sinRefLong*yDelta + cosRefLat*zDelta;
   enuPosition.U = cosRefLat * cosRefLong * xDelta + cosRefLat*sinRefLong*yDelta + sinRefLat*zDelta;

--- a/GeoPosition.h	Mon Nov 14 14:53:12 2022 +0000
+++ b/GeoPosition.h	Tue Nov 15 10:28:47 2022 +0000
@@ -67,7 +67,7 @@
   // Changing reference position will flag ENU as invalid if LLA or ECEF are known
   // If you want to manitain ENU then first call SetENU(GetENU());
   // this will flag LLA and ECEF as invalid and maintain the ENU over a reference change.
-  // Directly changing the values of the GeoPosition being used as a reference after using it for conversions could have some weird effects.
+  // Changing the values of the GeoPosition being used as a reference after setting it as reference will give invalid results due to speed optimisations.
   void SetReferancePosition(GeoPosition *refPos);
   
 
@@ -116,6 +116,11 @@
 
   GeoPosition *referencePosition;
 
+  double sinRefLong;
+  double cosRefLong;
+  double sinRefLat;
+  double cosRefLat;
+
   struct LLA  llaPosition;
   struct ECEF ecefPosition;
   struct ENU  enuPosition;

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

--- a/PNTSerial.h	Mon Nov 14 14:53:12 2022 +0000
+++ b/PNTSerial.h	Tue Nov 15 10:28:47 2022 +0000
@@ -10,6 +10,11 @@
 
 struct UserSettings_s;
 
+namespace PNTSerialConstants {
+// full message length
+const int MessageLength = 7+1+1+3+5+5+3+2+3+3+1+2*3+2*3+2*3+1+1+2+2;
+}
+
 class PNTSerial : public PosSource {
 
 public:
@@ -53,6 +58,5 @@
     GeoPosition origin;
     GeoPosition dataPoint;
 
-
 };