Revision 82:ee6eed2a51bd, committed 19 months ago

Comitter:

AndyA

Date:

Mon Nov 14 14:53:12 2022 +0000

Parent:

81:aee60dcce61b

Child:

83:f0d1d948c306

Commit message:

First pass at adding PNT position source support.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/.hgignore	Mon Nov 14 14:53:12 2022 +0000
@@ -0,0 +1,9 @@
+^BUILD$
+^.mbed$
+^compile_commands.json$
+^.clangd$
+^.cache$
+^mbed$
+^FatFileSystem$
+^BufferedSerial$
+^BufferedSerial/Buffer$

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GeoPosition.cpp	Mon Nov 14 14:53:12 2022 +0000
@@ -0,0 +1,460 @@
+#include "GeoPosition.h"
+#include <math.h>
+
+// macros used.
+
+//trig functions
+#define PI 3.1415926535897931
+#define DEG2RAD(x) (PI * x / 180.0)
+#define RAD2DEG(x) (180.0 * x / PI)
+
+
+// the earth to WGS84...
+#define WGS84_A	(6378137.0)				// WGS 84 semi-major axis constant in meters
+#define WGS84_B (6356752.3142)        // WGS 84 earth semiminor in meters
+#define WGS84_F (0.0033528106718309896) // WGS84 flattenting
+#define WGS84_InvF (298.2572229328697) // inverse of WGS84 flattenting
+
+#define WGS84_E (0.08181919092890624) // WGS 84 first eccentricity
+#define WGS84_ESqr (0.006694380004260827) // WGS 84 first eccentricity squared 
+
+#define WGS84_E2 (0.08209443803685366) // WGS 84 second eccentricity
+#define WGS84_E2Sqr (0.006739496756586903) // WGS 84 second eccentricity squared 
+
+
+// a shortcut for square for the lazy.
+#define SQR(x) ((x) * (x))
+
+
+GeoPosition::GeoPosition() {
+  referencePosition = 0;
+  llaValid = false;
+  ecefValid = false;
+  enuValid = false;
+}
+
+GeoPosition::GeoPosition(GeoPosition *posToClone) {
+
+  referencePosition = posToClone->GetReferancePosition();
+  if (posToClone->LLAKnown()) {
+    llaValid = true;
+    llaPosition = posToClone->GetLLA();
+  }
+
+  if (posToClone->ECEFKnown()) {
+    ecefValid = true;
+    ecefPosition = posToClone->GetECEF();
+  }
+
+  if (posToClone->ENUKnown()) {
+    enuValid = true;
+    enuPosition = posToClone->GetENU();
+  }
+}
+
+GeoPosition::~GeoPosition() {
+}
+
+
+void GeoPosition::SetDecimalDegrees(double latitude, double longitude, double altitude) {
+
+  llaPosition.latitude = latitude;
+  llaPosition.longitude = longitude;
+  llaPosition.altitude = altitude;
+  llaValid = true;
+  ecefValid = false;
+  enuValid = false;
+
+}
+
+
+void GeoPosition::SetDegreesMinutesSeconds(int latitudeDegrees, double latitudeMinutes, float latitudeSeconds, int longitudeDegrees, double longitudeMinutes, float longitudeSeconds, double altitude) {
+
+  llaPosition.latitude = (double)latitudeDegrees + (double)latitudeMinutes*60.0 + (double)latitudeSeconds*3600.0;
+  llaPosition.longitude = (double)longitudeDegrees + (double)longitudeMinutes*60.0 + (double)longitudeSeconds*3600.0;
+  llaPosition.altitude = altitude;
+  llaValid = true;
+  ecefValid = false;
+  enuValid = false;
+
+}
+
+void GeoPosition::SetLLA(struct LLA llaStruct) {
+
+  llaPosition.latitude = llaStruct.latitude;
+  llaPosition.longitude = llaStruct.longitude;
+  llaPosition.altitude = llaStruct.altitude;
+  llaValid = true;
+  ecefValid = false;
+  enuValid = false;
+
+}
+
+void GeoPosition::SetECEF(double x, double y, double z) {
+
+  ecefPosition.X = x;
+  ecefPosition.Y = y;
+  ecefPosition.Z = z;
+  llaValid = false;
+  ecefValid = true;
+  enuValid = false;
+
+}
+
+void GeoPosition::SetECEF(struct ECEF ecefStruct) {
+
+  ecefPosition.X = ecefStruct.X;
+  ecefPosition.Y = ecefStruct.Y;
+  ecefPosition.Z = ecefStruct.Z;
+  llaValid = false;
+  ecefValid = true;
+  enuValid = false;
+
+}
+
+void GeoPosition::SetENU(double east, double north, double up) {
+
+  enuPosition.E = east;
+  enuPosition.N = north;
+  enuPosition.U = up;
+  llaValid = false;
+  ecefValid = false;
+  enuValid = true;
+
+}
+
+void GeoPosition::SetENU(double east, double north, double up, GeoPosition *refPos) {
+
+  enuPosition.E = east;
+  enuPosition.N = north;
+  enuPosition.U = up;
+  llaValid = false;
+  ecefValid = false;
+  enuValid = true;
+  referencePosition = refPos;
+}
+
+
+void GeoPosition::SetENU(struct ENU enuStruct) {
+
+  enuPosition.E = enuStruct.E;
+  enuPosition.N = enuStruct.N;
+  enuPosition.U = enuStruct.U;
+  llaValid = false;
+  ecefValid = false;
+  enuValid = true;
+
+}
+
+void GeoPosition::SetENU(struct ENU enuStruct, GeoPosition *refPos) {
+
+  enuPosition.E = enuStruct.E;
+  enuPosition.N = enuStruct.N;
+  enuPosition.U = enuStruct.U;
+  llaValid = false;
+  ecefValid = false;
+  enuValid = true;
+  referencePosition = refPos;
+}
+
+void GeoPosition::SetReferancePosition(GeoPosition *refPos) {
+  referencePosition = refPos;
+
+  if ((llaValid) || (ecefValid))
+    enuValid = false;
+
+}
+
+
+double GeoPosition::GetLatitude() {
+  if (!llaValid)
+    calcLLA();
+
+  if (llaValid)
+    return llaPosition.latitude;
+  else
+    return 0.0;
+}
+
+double GeoPosition::GetLongitude() {
+  if (!llaValid)
+    calcLLA();
+
+  if (llaValid)
+    return llaPosition.longitude;
+  else
+    return 0.0;
+}
+
+double GeoPosition::GetAltitude() {
+  if (!llaValid)
+    calcLLA();
+
+  if (llaValid)
+    return llaPosition.altitude;
+  else
+    return 0.0;
+}
+
+
+bool GeoPosition::GetENU(double *east, double *north, double *up) {
+
+  if (!enuValid)
+    calcENU();
+
+  *east = enuPosition.E;
+  *north = enuPosition.N;
+  *up = enuPosition.U;
+  
+  return enuValid;
+}
+
+bool GeoPosition::GetECEF(double *x, double *y, double *z) {
+
+  if (!ecefValid)
+    calcECEF();
+
+  *x = ecefPosition.X;
+  *y = ecefPosition.Y;
+  *z = ecefPosition.Z;
+
+  return ecefValid;
+
+}
+
+
+bool GeoPosition::GetLLA(double *latitude, double *longitude, double *altitude) {
+
+  if (!llaValid) {
+    calcLLA();
+  }
+
+  *latitude = llaPosition.latitude;
+  *longitude = llaPosition.longitude;
+  if (altitude)
+    *altitude = llaPosition.altitude;
+  
+  return true;
+}
+
+
+struct ENU GeoPosition::GetENU() {
+  if (!enuValid) {
+    calcENU();
+  }
+  return enuPosition;
+}
+
+struct ECEF GeoPosition::GetECEF() {
+  if (!ecefValid) {
+    calcECEF();
+  }
+  return ecefPosition;
+}
+
+struct LLA GeoPosition::GetLLA() {
+  if (!llaValid) {
+    calcLLA();
+  }
+  return llaPosition;
+}
+
+
+// conversion functions.
+
+bool GeoPosition::calcLLA() {
+
+  if (ecefValid)
+    ECEF2LLA();
+  else if (enuValid && referencePosition) {
+    ENU2ECEF();
+    ECEF2LLA();
+  }
+
+  return llaValid;
+
+}
+
+bool GeoPosition::calcECEF() {
+  if (llaValid)
+    LLA2ECEF();
+  else if (enuValid && referencePosition)
+    ENU2ECEF();
+  return ecefValid;
+}
+
+bool GeoPosition::calcENU() {
+
+	if (referencePosition) {
+		if (ecefValid) {
+			ECEF2ENU();
+		}
+		else if (llaValid) {
+			LLA2ECEF();
+			ECEF2ENU();
+		}
+	}
+
+  return enuValid;
+
+}
+
+
+void GeoPosition::LLA2ECEF() {
+
+  if (!llaValid)
+    return;
+
+ 
+  double clat = cos(DEG2RAD(llaPosition.latitude));
+  double slat = sin(DEG2RAD(llaPosition.latitude));
+  double clon = cos(DEG2RAD(llaPosition.longitude));
+  double slon = sin(DEG2RAD(llaPosition.longitude));
+
+  double N = WGS84_A / sqrt(1.0 - WGS84_E * WGS84_E * slat * slat);
+
+  
+  ecefPosition.X = (N + llaPosition.altitude) * clat * clon;
+  ecefPosition.Y = (N + llaPosition.altitude) * clat * slon;
+  ecefPosition.Z = (N * (1.0 - WGS84_E * WGS84_E) + llaPosition.altitude) * slat;
+
+  ecefValid = true;
+
+}
+
+//
+//double GeoPosition::RadiusOfCurvature(double longitudeRadians) {
+//
+//  return (WGS84_A / sqrt(1 - SQR(WGS84_E)*SQR(sin(longitudeRadians))));
+//
+//}
+
+
+void GeoPosition::ECEF2LLA() {
+  // this conversion code based on https://www.ngs.noaa.gov/PC_PROD/XYZWIN/ with constants changed to WGS84
+
+
+  if (llaValid || !ecefValid)
+    return;
+
+    double SemiMinor;
+
+    if (ecefPosition.Z < 0.0)
+      SemiMinor = -WGS84_B;
+    else
+      SemiMinor = WGS84_B;
+
+    /*
+    *   2.0 compute intermediate values for latitude
+    */
+    double r = sqrt(SQR(ecefPosition.X) + SQR(ecefPosition.Y));
+    double e = (SemiMinor*ecefPosition.Z - (SQR(WGS84_A) - SQR(WGS84_B))) / (WGS84_A*r);
+    double f = (SemiMinor*ecefPosition.Z + (SQR(WGS84_A) - SQR(WGS84_B))) / (WGS84_A*r); 
+
+    /*
+    *   3.0 find solution to:
+    *       t^4 + 2*E*t^3 + 2*F*t - 1 = 0
+    */
+    long double p = (4.0/3.0) * (e*f + 1.0);
+    double q = 2.0 * (e*e - f*f);
+    double d = p*p*p + q*q;
+
+    double v;
+
+    if (d >= 0.0) {
+      v = pow((sqrt(d) - q), (1.0 / 3.0))
+        - pow((sqrt(d) + q), (1.0 / 3.0));
+    } else {
+      v = 2.0 * sqrt(-p)
+        * cos(acos(q / (p * sqrt(-p))) / 3.0);
+    }
+
+    /*
+    *   4.0 improve v
+    *       NOTE: not really necessary unless point is near pole
+    */
+    if (v*v < fabs(p)) {
+      v = -(v*v*v + 2.0*q) / (3.0*p);
+    }
+    double g = (sqrt(e*e + v) + e) / 2.0;
+    double t = sqrt(g*g + (f - v*g) / (2.0*g - e)) - g;
+
+    llaPosition.latitude = atan((WGS84_A*(1.0 - t*t)) / (2.0*SemiMinor*t));
+
+    /*
+    *   5.0 compute height above ellipsoid
+    */
+    llaPosition.altitude = (r - WGS84_A*t)*cos(llaPosition.latitude) + (ecefPosition.Z - SemiMinor)*sin(llaPosition.latitude);
+
+    /*
+    *   6.0 compute longitude east of Greenwich
+    */
+    double zlong = atan2(ecefPosition.Y, ecefPosition.X);
+
+    /*
+    *   7.0 convert latitude and longitude to degrees
+    */
+    llaPosition.longitude = RAD2DEG(zlong);
+    llaPosition.latitude = RAD2DEG(llaPosition.latitude);
+
+    llaValid = true;
+    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() {
+
+  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;
+  enuValid = true;
+  
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GeoPosition.h	Mon Nov 14 14:53:12 2022 +0000
@@ -0,0 +1,129 @@
+/****************************************************************************************
+/  GeoPosition.h
+/  (c) 2014 Racelogic Ltd.
+/
+/  A class to convert between LLA (latitude, longitude, altitude), ECEF (Earth centered, earth fixed) and ENU (East, north, up).
+/
+/  Conversions are only calculated when requested. Converted to LLA is EXPENSIVE, don't do it for fun.
+/  All conversions are to WGS84.
+/  LLA altitude, ECEF and ENU are all in meters.
+/  ENU calculations require a reference point with a valid ECEF or LLA location in order to work.
+/  Conversion code based on these sources:
+/  http://www.gmat.unsw.edu.au/snap/gps/clynch_pdfs/coordcvt.pdf
+/  http://en.wikipedia.org/wiki/Geodetic_datum#From_ECEF_to_ENU
+/
+****************************************************************************************/
+
+#ifndef __GEOPOSITION_H__
+#define __GEOPOSITION_H__
+
+// structures to hold position data as a convenience.
+struct LLA {
+  double longitude;
+  double latitude;
+  double altitude;
+};
+
+struct ECEF {
+  double X;
+  double Y;
+  double Z;
+};
+
+struct ENU {
+  double E;
+  double N;
+  double U;
+};
+
+class GeoPosition {
+
+public:
+
+  // constructor
+  GeoPosition();
+  GeoPosition(GeoPosition *posToClone);
+
+  ~GeoPosition();
+
+  // sets position in LLA
+  void SetDecimalDegrees(double latitude, double longitude, double altitude = 0);
+
+  // for degrees and decimal minutes set seconds to 0.
+  void SetDegreesMinutesSeconds(int latitudeDegrees, double latitudeMinutes, float latitudeSeconds, int longitudeDegrees, double longitudeMinutes, float longitudeSeconds, double altitude = 0.0);
+  void SetLLA(struct LLA llaStruct);
+
+  // sets position in ECEF in m
+  void SetECEF(double x, double y, double z);
+  void SetECEF(struct ECEF ecefStruct);
+
+  // sets position in ENU. ENU conversions MUST have a reference position set to work.
+  void SetENU(double east, double north, double up);
+  void SetENU(double east, double north, double up, GeoPosition *refPos);
+  void SetENU(struct ENU enuStruct);
+  void SetENU(struct ENU enuStruct, GeoPosition *refPos);
+
+  // Sets the reference 0,0,0 position for ENU conversions.
+  // 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.
+  void SetReferancePosition(GeoPosition *refPos);
+  
+
+  // get the location. Conversions will be done as needed. Will return false if conversion wasn't possible.
+  bool GetENU(double *east, double *north, double *up);
+  bool GetECEF(double *x, double *y, double *z);
+  bool GetLLA(double *latitude, double *longitude, double *altitude = 0);
+  
+
+  // NOTE - functions below could result in invalid data if there is no refernce position and ENUs are involved.
+  // Check the known flag after a call to verify data is valid.
+  struct ENU GetENU();
+  struct ECEF GetECEF();
+  struct LLA GetLLA();
+  
+  double GetLatitude();
+  double GetLongitude();
+  double GetAltitude();
+
+
+  // Checks to see if various formats have already been calculated or not.
+  bool LLAKnown() { return llaValid; };
+  bool ECEFKnown() { return ecefValid; };
+  bool ENUKnown() { return enuValid; };
+  
+  void ClearLocation() {
+    llaValid = false;
+    ecefValid = false;
+    enuValid = false;
+};
+
+  // returns a const pointer to the reference position being used.
+  GeoPosition *GetReferancePosition() { return referencePosition; };
+
+private:
+
+  bool calcLLA();
+  bool calcECEF();
+  bool calcENU();
+
+  void LLA2ECEF();
+  void ECEF2LLA();
+
+  void ENU2ECEF();
+  void ECEF2ENU();
+
+  GeoPosition *referencePosition;
+
+  struct LLA  llaPosition;
+  struct ECEF ecefPosition;
+  struct ENU  enuPosition;
+
+  bool llaValid;
+  bool ecefValid;
+  bool enuValid;
+
+};
+
+#endif

--- a/LTCApp.h	Wed Aug 31 15:46:41 2022 +0000
+++ b/LTCApp.h	Mon Nov 14 14:53:12 2022 +0000
@@ -4,6 +4,7 @@
 #include "mbed.h"
 #include "LTCDecode.h"
 #include "VIPSSerialProtocol.h"
+#include "PNTSerial.h"
 #include "frameRates.h"
 #include "BufferedSerial.h"
 #include "FIZ_DISNEY.h"
@@ -29,6 +30,8 @@
 
 extern void vipsBypassRx(char byte);
 
+enum positionSource_e {VIPSSource, PNTSource};
+
 typedef struct UserSettings_s {
     int FIZmode;
     int SerialOutMode;
@@ -74,6 +77,10 @@
     bool absolute_focus;
     bool absolute_iris;
     bool absolute_zoom;
+    enum positionSource_e PositionSource;
+    double originLat;
+    double originLon;
+    float originAlt;
     vector<unsigned int> focus_encoder_map;
     vector<unsigned int> focus_absolute_map;
     vector<unsigned int> iris_encoder_map;

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PNTSerial.h	Mon Nov 14 14:53:12 2022 +0000
@@ -0,0 +1,58 @@
+#pragma once
+
+#include "PosSource.h"
+#include "mbed.h"
+#include "position.h"
+#include "BufferedSerial.h"
+#include "LTCApp.h"
+#include "GeoPosition.h"
+
+
+struct UserSettings_s;
+
+class PNTSerial : public PosSource {
+
+public:
+
+    PNTSerial(const PinName Tx, const PinName Rx);
+    void run(void);
+
+    // send all position outputs rather than just when requested.
+    void sendAllUpdated(bool enable);
+
+
+// PNT specific Rx/Tx
+    void sendQueued(void) {};
+    void sendDirectTX(unsigned char* data, int dataLen) {};
+    int getWaitingBuffer(unsigned char **TXBuffer, int *bytesToSend) {return 0;};
+    void sendQuiet(void) {};
+
+    void setOrigin(double Latitude, double Longitude, float altitude);
+
+private:
+    void onSerialRx(void);
+    void parsePostion(void);
+    bool CheckValidMessage(void);
+    uint32_t readUInt32(int &start, int bytes);
+    int32_t readInt32(int &start, int bytes);
+    int64_t readInt64(int &start, int bytes);
+
+//    void processRxMessage();
+//    bool checkCRC(unsigned char* data);
+
+    int messagePrt;
+    int messageParsePtr;
+//    int messageLength;
+//    int statusMessage;
+//    bool enableAllUpdates;
+//    bool newFormatMsg;
+
+    int queueLen;
+
+    bool originSet;
+    GeoPosition origin;
+    GeoPosition dataPoint;
+
+
+};
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PosSource.cpp	Mon Nov 14 14:53:12 2022 +0000
@@ -0,0 +1,151 @@
+#include "PosSource.h"
+#include "LTCApp.h"
+
+PosSource::PosSource(const PinName Tx, const PinName Rx) : _port(Tx,Rx) {
+    nextPosition = 0;
+    outputPtr = NULL;
+SmoothBy = 100; //was 500
+    XSmoothTotal = 0;
+    YSmoothTotal = 0;
+    ZSmoothTotal = 0;
+    SmoothRunning = false;
+
+    BypassMode = false;
+    directTx = false;
+    txBuf = TXBuffer1;
+    waitingBytes = 0;
+    TransmitFinished=false;
+    //self=this;
+
+    nextPosition= 0;
+    _outputMask = 0x44;
+    _port.baud(115200);
+    }
+
+void PosSource::bypassTx(char byte)
+{
+    _port.putc(byte);
+}
+
+// send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
+position* PosSource::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 PosSource::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 PosSource::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;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PosSource.h	Mon Nov 14 14:53:12 2022 +0000
@@ -0,0 +1,136 @@
+#pragma once
+#include "mbed.h"
+#include "position.h"
+#include "BufferedSerial.h"
+#include "LowPassFilter.h"
+
+
+#define MaxBuffSize 272 //The Get Unit Info LPC command (G N) returns 266 bytes
+
+// common interface for all position sources
+
+extern const char* VIPSStatusMessages[];
+struct UserSettings_s;
+
+class PosSource
+{
+public:
+
+    PosSource(const PinName Tx, const PinName Rx);
+
+    virtual void run(void) = 0;
+
+    bool setFilters(struct UserSettings_s *settings);
+
+    // send all position outputs rather than just when requested.
+    void sendAllUpdated(bool enable);
+
+    // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
+    virtual position* sendPositionForTime(uint32_t timeValue);
+
+    position* getWaitingPostion()
+    {
+        position *ptr = outputPtr;
+        outputPtr=NULL;
+        return ptr;
+    }
+
+    static void getCRC(void *data, int len, void *checksum);
+
+//  void setOutMask(uint32_t outputMask) {_outputMask = outputMask;};
+
+    bool EnableSmoothing(bool enabled)
+    {
+        hyperSmoothEnabled = enabled;
+        return hyperSmoothEnabled;
+    };
+
+    bool SmoothingEnabled(void)
+    {
+        return hyperSmoothEnabled;
+    };
+    bool ForceSmoothing(bool enabled)
+    {
+        forcedHyperSmooth = enabled;
+        hyperSmoothEnabled = enabled;
+        return hyperSmoothEnabled;
+    };
+    bool ForceSmoothingEnabled(bool enabled)
+    {
+        return forcedHyperSmooth;
+    };
+    int GetSmoothLevel(void)
+    {
+        return SmoothBy;
+    };
+    bool SetSmoothLevel (const int newSmooth)
+    {
+        if (newSmooth == SmoothBy) return false;
+        SmoothBy = newSmooth;
+        SmoothRunning = false;
+        return true;
+    };
+    void EnableBypass(bool enable)
+    {
+        BypassMode = enable;
+    };
+    bool EnableDirectTX(bool enabled)
+    {
+        directTx = enabled;
+        return directTx;
+    };
+
+    void bypassTx(char byte);
+
+    virtual void sendQueued(void) = 0;
+    virtual void sendDirectTX(unsigned char* data, int dataLen) = 0;
+    virtual int getWaitingBuffer(unsigned char **TXBuffer, int *bytesToSend) = 0;
+    virtual void sendQuiet(void) = 0;
+protected:
+    struct posAndTime_s {
+        uint32_t time;
+        position pos;
+    };
+    void smoothOutputPacket(position *posPtr);
+
+    RawSerial _port;
+    unsigned char messageInBuffer[128];
+    unsigned char messageOutBuffer[16];
+    unsigned char TXBuffer1[MaxBuffSize];
+    unsigned char TXBuffer2[MaxBuffSize];
+    unsigned char *txBuf;
+    int waitingBytes;
+#define posHistoryLen 3
+    struct posAndTime_s lastPositions[posHistoryLen];
+    int nextPosition;
+    struct posAndTime_s lastPos; // the most recent position received
+    struct posAndTime_s prevPos; // the most last but one position received
+
+    uint32_t _outputMask;
+    uint32_t pointCount;
+
+    position outputPosition;
+    position *outputPtr;
+    bool hyperSmoothEnabled;
+    bool forcedHyperSmooth;
+    bool directTx;
+    int SmoothBy;
+    // total as a float we would start to see rounding errors at valuses of ~20m
+    double XSmoothTotal;
+    double YSmoothTotal;
+    double ZSmoothTotal;
+    bool SmoothRunning;
+    bool BypassMode;
+    LowPassFilter yFilter;
+    LowPassFilter xFilter;
+    LowPassFilter zFilter;
+    LowPassFilter rollFilter;
+    LowPassFilter pitchFilter;
+    LowPassFilter yawFilter;
+    Timeout TxTimeout;
+    bool TransmitFinished;
+        bool enableAllUpdates;
+
+
+};
+

--- a/VIPSSerialProtocol.cpp	Wed Aug 31 15:46:41 2022 +0000
+++ b/VIPSSerialProtocol.cpp	Mon Nov 14 14:53:12 2022 +0000
@@ -1,4 +1,6 @@
+
 #include "LTCApp.h"
+#include "VIPSSerialProtocol.h"
 #include <cstdint>
 #include <cstring>
 
@@ -15,15 +17,12 @@
 #define ActiveTxBuffer (txBuf == TXBuffer1)?TXBuffer1:TXBuffer2
 
 
-VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
+VIPSSerial::VIPSSerial(const PinName Tx, const PinName Rx) : PosSource(Tx,Rx)
 {
     messagePrt = 0;
     messageLength = 0;
 
     pointCount = 0;
-    nextPosition = 0;
-
-    outputPtr = NULL;
     statusMessage = 0;
 
     enableAllUpdates = false;
@@ -31,22 +30,7 @@
 
     queueLen = 0;
 
-    SmoothBy = 100; //was 500
-    XSmoothTotal = 0;
-    YSmoothTotal = 0;
-    ZSmoothTotal = 0;
-    SmoothRunning = false;
-
-    BypassMode = false;
     directTx = false;
-    txBuf = TXBuffer1;
-    waitingBytes = 0;
-    TransmitFinished=false;
-    //self=this;
-
-    nextPosition= 0;
-    _outputMask = 0x44;
-    _port.baud(115200);
 }
 
 void VIPSSerial::run(void)
@@ -54,10 +38,6 @@
     _port.attach(callback(this, &VIPSSerial::onSerialRx));
 }
 
-void VIPSSerial::bypassTx(char byte)
-{
-    _port.putc(byte);
-}
 
 void VIPSSerial::onTxTimeout()
 {
@@ -427,126 +407,4 @@
 }
 
 
-// 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;
-}
\ No newline at end of file

--- a/VIPSSerialProtocol.h	Wed Aug 31 15:46:41 2022 +0000
+++ b/VIPSSerialProtocol.h	Mon Nov 14 14:53:12 2022 +0000
@@ -1,97 +1,35 @@
 #ifndef __VIPSSERIALPROTOCOL_H__
 #define __VIPSSERIALPROTOCOL_H__
 
+#include "PosSource.h"
 #include "mbed.h"
 #include "position.h"
 #include "BufferedSerial.h"
 #include "LTCApp.h"
-#include "LowPassFilter.h"
-
-#define MaxBuffSize 272 //The Get Unit Info LPC command (G N) returns 266 bytes
-
 
 
 extern const char* VIPSStatusMessages[];
 struct UserSettings_s;
 
-class VIPSSerial
-{
+class VIPSSerial : public PosSource {
 
 public:
 
     VIPSSerial(const PinName Tx, const PinName Rx);
     void run(void);
-    bool setFilters(struct UserSettings_s *settings);
 
     // send all position outputs rather than just when requested.
     void sendAllUpdated(bool enable);
 
-    // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
-    position* sendPositionForTime(uint32_t timeValue);
-    position* getWaitingPostion()
-    {
-        position *ptr = outputPtr;
-        outputPtr=NULL;
-        return ptr;
-    }
 
+// VIPS specific Rx/Tx
     static void getCRC(void *data, int len, void *checksum);
-
-//  void setOutMask(uint32_t outputMask) {_outputMask = outputMask;};
-
-    bool EnableSmoothing(bool enabled)
-    {
-        hyperSmoothEnabled = enabled;
-        return hyperSmoothEnabled;
-    };
-    bool SmoothingEnabled(void)
-    {
-        return hyperSmoothEnabled;
-    };
-    bool ForceSmoothing(bool enabled)
-    {
-        forcedHyperSmooth = enabled;
-        hyperSmoothEnabled = enabled;
-        return hyperSmoothEnabled;
-    };
-    bool ForceSmoothingEnabled(bool enabled)
-    {
-        return forcedHyperSmooth;
-    };
-    int GetSmoothLevel(void)
-    {
-        return SmoothBy;
-    };
-    bool SetSmoothLevel (const int newSmooth)
-    {
-        if (newSmooth == SmoothBy) return false;
-        SmoothBy = newSmooth;
-        SmoothRunning = false;
-        return true;
-    };
-    void EnableBypass(bool enable)
-    {
-        BypassMode = enable;
-    };
-    void bypassTx(char byte);
     void sendQueued(void);
-    bool EnableDirectTX(bool enabled)
-    {
-        directTx = enabled;
-        return directTx;
-    };
     void sendDirectTX(unsigned char* data, int dataLen);
     int getWaitingBuffer(unsigned char **TXBuffer, int *bytesToSend);
     void sendQuiet(void);
 
 private:
-
-    struct posAndTime_s {
-        uint32_t time;
-        position pos;
-    };
-
-    void smoothOutputPacket(position *posPtr);
     void onSerialRx(void);
     void processRxMessage();
     bool checkCRC(unsigned char* data);
@@ -104,50 +42,14 @@
     void parsePostionInput_mocap();
     void onTxTimeout();
     bool checkNewPacketRC(unsigned char* data);
-    RawSerial _port;
-    unsigned char messageInBuffer[128];
-    unsigned char messageOutBuffer[16];
-    unsigned char TXBuffer1[MaxBuffSize];
-    unsigned char TXBuffer2[MaxBuffSize];
-    unsigned char *txBuf;
-    int waitingBytes;
-#define posHistoryLen 3
-    struct posAndTime_s lastPositions[posHistoryLen];
-    int nextPosition;
-    struct posAndTime_s lastPos; // the most recent position received
-    struct posAndTime_s prevPos; // the most last but one position received
-
-    position outputPosition;
-    position *outputPtr;
 
     int messagePrt;
     int messageLength;
     int statusMessage;
-    bool enableAllUpdates;
     bool newFormatMsg;
-    bool hyperSmoothEnabled;
-    bool forcedHyperSmooth;
-    bool directTx;
-    uint32_t pointCount;
-    uint32_t _outputMask;
 
     int queueLen;
-    int SmoothBy;
-    // total as a float we would start to see rounding errors at valuses of ~20m
-    double XSmoothTotal;
-    double YSmoothTotal;
-    double ZSmoothTotal;
-    bool SmoothRunning;
-    bool BypassMode;
-    LowPassFilter yFilter;
-    LowPassFilter xFilter;
-    LowPassFilter zFilter;
-    LowPassFilter rollFilter;
-    LowPassFilter pitchFilter;
-    LowPassFilter yawFilter;
     
-    Timeout TxTimeout;
-    bool TransmitFinished;
     //VIPSSerial* self;
 };
 

--- a/main.cpp	Wed Aug 31 15:46:41 2022 +0000
+++ b/main.cpp	Mon Nov 14 14:53:12 2022 +0000
@@ -1,4 +1,4 @@
-#define APP_VERSION 0.32
+#define APP_VERSION 0.40
 
 /*
 Settings file options
@@ -22,6 +22,12 @@
     n = 3 - Canon
     n = 4 - Arri
 
+Position_Source=n
+    Sets the position input format
+    n = 0 - VIPS serial
+    n = 1 - PNT RLPNT message (see origin options)
+
+
 FreeD_Port=pppp
     Sets the UDP port for FreeD network output.
     Data is sent as a UDP broadcast on the select port number.
@@ -61,6 +67,11 @@
 Enable extra fields in VIPS output.
 
 
+Origin used for PNT GPS to local conversion
+OriginLat=52.00000
+OriginLon=-1.00000
+OriginAlt=160.0
+
 All settings are case sensitive.
 Do NOT include spaces in the options lines.
 All options default to a value of 0 is omitted from the file.
@@ -84,7 +95,9 @@
 #endif
 
 BufferedSerial pc(USBTX, USBRX);
-VIPSSerial VIPS(p28, p27);
+
+PosSource* PositionSource;
+
 BufferedSerial COM1(p13, p14);
 FIZReader *FIZPort;
 EthernetInterface eth;
@@ -194,7 +207,7 @@
 
 void filterOff(void)
 {
-    VIPS.EnableSmoothing(false);
+    PositionSource->EnableSmoothing(false);
     pc.puts("FilterTimeout");
 }
 
@@ -235,12 +248,12 @@
             return;
 
         if (C1InputBuffer[15] == 'N') {
-            VIPS.ForceSmoothing(true);
+            PositionSource->ForceSmoothing(true);
             filteringTimeout.attach(callback(&filterOff),30.0f);
             pc.puts("FilterOn\n");
             return;
         } else {
-            VIPS.ForceSmoothing(false);
+            PositionSource->ForceSmoothing(false);
             filteringTimeout.detach();
             pc.puts("FilterOFF\n");
             return;
@@ -252,7 +265,7 @@
 
 void ExitBypass(void)
 {
-    VIPS.EnableBypass(false);
+    PositionSource->EnableBypass(false);
     bypassMode = false;
 }
 
@@ -269,7 +282,7 @@
     led1=!led1;
     while (COM1.RawSerial::readable()) {
         if (bypassMode) {
-            VIPS.bypassTx(COM1.RawSerial::getc());
+            PositionSource->bypassTx(COM1.RawSerial::getc());
             BypassTimeout.attach(&ExitBypass,5);
         } else {
             C1InputBuffer[C1InputPtr] = COM1.RawSerial::getc();
@@ -278,8 +291,8 @@
                 if (got0x07) {
                     got0x07 = false;
                     if ((C1InputBuffer[0] >= 5) && (C1InputBuffer[0] <=11)) {
-                        VIPS.bypassTx(0x07);
-                        VIPS.bypassTx(C1InputBuffer[0]);
+                        PositionSource->bypassTx(0x07);
+                        PositionSource->bypassTx(C1InputBuffer[0]);
                         bypassMode = true;
                         BypassTimeout.attach(&ExitBypass,5);
                     }
@@ -561,7 +574,7 @@
     led3 = !led3;
     uint32_t outputTime = TimeSinceLastFrame.read_us() + UserSettings.InterpolationOffset_uS;
     TimeSinceLastFrame.reset();
-    sendPosition(VIPS.sendPositionForTime(outputTime));
+    sendPosition(PositionSource->sendPositionForTime(outputTime));
     FIZPort->requestCurrent();
 }
 
@@ -785,12 +798,18 @@
     UserSettings.absolute_focus = false;
     UserSettings.absolute_iris = false;
     UserSettings.absolute_zoom = false;
+    UserSettings.PositionSource = VIPSSource;
+    UserSettings.originLat=0;
+    UserSettings.originLon=0;
+    UserSettings.originAlt=0;
+
 
 //    LocalFileSystem localFS("local");
     FILE *LSFile= fopen("/local/settings.txt","r");
     char lineBuffer[128];
     int valueIn;
     float floatIn;
+    double doubleIn;
     char lensfile[128] = "/local/";//7 characters for filepath, minus 12 chars of settings string, hence 128 shouldnt cause buffer overflow
     /*MAXIMUM FILENAME LENGTH IS 8 Characters*/
 
@@ -964,6 +983,26 @@
                     pc.printf("Found filename of %s \r\n", lensfile);
                 }
 
+                if (sscanf(lineBuffer,"Position_Source=%d",&valueIn) == 1) {
+                    pc.printf("Got Position_Source value from file of %d\r\n",valueIn);
+                    if (valueIn == 1)
+                        UserSettings.PositionSource = PNTSource;
+                    else
+                        UserSettings.PositionSource = VIPSSource;
+                }
+
+                if (sscanf(lineBuffer,"OriginLatitude=%lf",&doubleIn) == 1) {
+                    pc.printf("Got Origin latitude of %lf\r\n",doubleIn);
+                    UserSettings.originLat = doubleIn;
+                }
+                if (sscanf(lineBuffer,"OriginLongitude=%lf",&doubleIn) == 1) {
+                    pc.printf("Got Origin longitude of %lf\r\n",doubleIn);
+                    UserSettings.originLon = doubleIn;
+                }
+                if (sscanf(lineBuffer,"OriginAltitude=%f",&floatIn) == 1) {
+                    pc.printf("Got Origin altitude of %f\r\n",floatIn);
+                    UserSettings.originAlt = floatIn;
+                }
             }
         }
         fclose(LSFile);
@@ -1015,27 +1054,27 @@
                                 (settingsInBuffer[2] == 0x02)) {
                             validated_connection = true;
                             printf("Validated Connection - Sending 'set quiet' command\r\n");
-                            VIPS.sendQuiet();
+                            PositionSource->sendQuiet();
                             Thread::wait(50);
-                            VIPS.EnableDirectTX(true);
+                            PositionSource->EnableDirectTX(true);
                             GreenLED = LED_ON;
-                            VIPS.sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
+                            PositionSource->sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
                             printf("Sent first %d bytes to VIPS\r\n",bytesIn); //also a set-quiet command but good to send twice
                         } else {
                             printf("Invalid: %X %X %X", settingsInBuffer[0], settingsInBuffer[1], settingsInBuffer[2]);
                         }
                     } else if (validated_connection) {
-                        VIPS.sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
+                        PositionSource->sendDirectTX((unsigned char *)settingsInBuffer, bytesIn);
                         printf("Sent %d bytes to VIPS\r\n",bytesIn);
                     }
                 }
-                if (VIPS.getWaitingBuffer(&bufferToSend, &bytesToSend)) {
+                if (PositionSource->getWaitingBuffer(&bufferToSend, &bytesToSend)) {
                     connection.send((char *)bufferToSend, bytesToSend);
                     printf("Recieved %d bytes from VIPS\r\n",bytesIn);
                 }
             } while (connection.is_connected());
 
-            VIPS.EnableDirectTX(false);
+            PositionSource->EnableDirectTX(false);
             GreenLED = LED_OFF;
             validated_connection = false;
             pc.puts("Disconnected TCP \r\n");
@@ -1043,7 +1082,7 @@
             Thread::wait(50);
         }
     }
-    VIPS.EnableDirectTX(false);
+    PositionSource->EnableDirectTX(false);
     GreenLED = LED_OFF;
     pc.puts("Ending TCP Task\r\n");
 }
@@ -1253,33 +1292,41 @@
 
     readSettingsFile();
 
+    if (UserSettings.PositionSource == PNTSource) {
+        PositionSource = (PosSource*) new PNTSerial(p28, p27); 
+        if ((UserSettings.originLat != 0) || (UserSettings.originLon != 0)) {
+            ((PNTSerial*)PositionSource)->setOrigin(UserSettings.originLat,UserSettings.originLon,UserSettings.originAlt);
+        }
+    } else
+        PositionSource = (PosSource*) new VIPSSerial(p28, p27);
+
     if (UserSettings.bypassBaud) {
         XBEEBypassmode(UserSettings.bypassBaud);
     }
 
     switch(UserSettings.FIZmode) {
         case formatPreston :
-            FIZPort = new FIZDisney(p9, p10);
+            FIZPort = (FIZReader*) new FIZDisney(p9, p10);
             pc.printf("Set Preston");
             break;
         case formatFujiPassive :
-            FIZPort = new FIZDigiPower(p9, p10);
+            FIZPort = (FIZReader*) new FIZDigiPower(p9, p10);
             pc.printf("Set FujiPassive");
             break;
         case formatFujiActive :
-            FIZPort = new FIZDigiPowerActive(p9, p10);
+            FIZPort = (FIZReader*) new FIZDigiPowerActive(p9, p10);
             pc.printf("Set FujiActive\r\n");
             break;
         case formatCanon :
-            FIZPort = new FIZCanon(p9, p10);
+            FIZPort = (FIZReader*) new FIZCanon(p9, p10);
             pc.printf("Set Canon\r\n");
             break;
         case formatArri:
-            FIZPort = new FIZ_ArriCmotion(p9, p10);
+            FIZPort = (FIZReader*) new FIZ_ArriCmotion(p9, p10);
             pc.printf("Set Arri\r\n");
             break;
         default:
-            FIZPort = new FIZDisney(p9, p10); //preston
+            FIZPort = (FIZReader*) new FIZDisney(p9, p10); //preston
             pc.printf("Set Default - Preston");
 
     }
@@ -1295,10 +1342,10 @@
 
     LTCInput.enable(true);
 
-    if (!VIPS.setFilters(&UserSettings))
+    if (!PositionSource->setFilters(&UserSettings))
         pc.puts("Failed to create VIPS filters\r\n");
 
-    VIPS.run();
+    PositionSource->run();
 
     pc.printf("System init complete\r\n");
     setBLUE();
@@ -1406,7 +1453,7 @@
             NewFramePulse = false;
             framePositionOutput();
         }
-        VIPS.sendQueued(); // should ideally be called on 100Hz PPS edge but we don't have that in this code...
+        PositionSource->sendQueued(); // should ideally be called on 100Hz PPS edge but we don't have that in this code...
 
         // if (pos_value) {
         //     pc.printf("Z: %d - (%d, %d)\r\n", pos_value, pos_lower, pos_upper);

--- a/settings.txt	Wed Aug 31 15:46:41 2022 +0000
+++ b/settings.txt	Mon Nov 14 14:53:12 2022 +0000
@@ -25,7 +25,16 @@
 --------------------------------
 FIZ_Format=3
 --------------------------------
- 
+
+[Position source]
+# Sets the position source format
+# 0 - VIPS
+# 1 - PNT in racelogic serial mode
+--------------------------------
+Position_Source=0
+--------------------------------
+
+
 [Ethernet Port]
 # Sets the UDP port for FreeD network output.
 # Data is sent as a UDP broadcast on the select port number.
@@ -68,6 +77,20 @@
 //FilterYaw=0
 --------------------------------
 
+[Origin settings]
+# When using PNT the output is in Latitude / Longitude
+# The output will then be converted to meters east (X), north (Y) and up (height)
+# relative to this origin location.
+# Latitude and longitude are in decimal degrees.
+# Altitude is in meters.
+# If no origin (or a 0 origin) is given output will be in latitude/longitude
+# For VIPS this option has no effect.
+OriginLatitude=52.000
+OriginLongitude=-1.000
+OriginAltitude=160.000
+
+
+
 # NOTE-The filter will add latency so a filtered channel will be delayed relative to an unfiltered one.
  
 [Additional outputs]