Conversion between LLA and ECEF. New example sketch to set survey fixed mode using LLA.

Author: mikalhart

USERGUIDE.md

@@ -335,6 +335,12 @@ This library provides an interface for controlling and configuring an LG290P GNS
 - **`uint32_t getEcefAgeMs();`**  
   Retrieves the age of the last RTCM 1005 report in milliseconds.
 
+- **`void geodeticToEcef(double lat, double lon, double alt, double &xOut, double &yOut, double &zOut)`**
+  Converts the geodetic position coordinate given by (lat/long/altitude) into an ECEF format coordinate
+
+- **`void ecefToGeodetic(double x, double y, double z, double &latOut, double &lonOut, double &altOut);`**  
+  Converts the ECEF position coordinate given by (x, y, z) into a geodetic (lat/long/altitude) format
+
 ## EPE Error Domain
 
 - **`double getNorthError();`**  

examples/15.Survey.Fixed.Mode/15.Survey.Fixed.Mode.ino

@@ -60,7 +60,8 @@ void setup()
   verify(myGNSS.setModeBase(false)); // don't reset, because setSurveyInMode() is going to do it
 
   Serial.println("Setting 'Survey' Mode fixed to top of Eiffel Tower");
-  verify(myGNSS.setSurveyFixedMode(4200944.016, 168364.025, 4780802.825));
+  double eiffelEcefX = 4201152.7587, eiffelEcefY = 168331.7945, eiffelEcefZ = 4780461.5607;
+  verify(myGNSS.setSurveyFixedMode(eiffelEcefX, eiffelEcefY, eiffelEcefZ));
   if (!myGNSS.isConnected())
   {
     Serial.println("reconnection failed; halting");

examples/17.Survey.Fixed.Mode.LLA/17.Survey.Fixed.Mode.LLA.ino

@@ -0,0 +1,106 @@
+/*
+  Setting "Survey Fixed" Mode
+  By: Nathan Seidle + Mikal Hart
+  SparkFun Electronics
+  Date: 29 September 2024
+  License: MIT. Please see LICENSE.md for more information.
+
+  This example shows how enable the fixed survey mode, where you define the BASE station's
+  location by supplying setSurveyFixedMode with Latitude/Longitude/Altitude coordinates.
+
+  These examples are targeted for an ESP32 platform but any platform that has multiple
+  serial UARTs should be compatible.
+
+  Feel like supporting open source hardware?
+  Buy a board from SparkFun!
+  SparkFun Quadband GNSS RTK Breakout - LG290P (GPS-26620) https://www.sparkfun.com/products/26620
+
+  Hardware Connections:
+  Connect RX3 (green wire) of the LG290P to pin 14 on the ESP32
+  Connect TX3 (orange wire) of the LG290P to pin 13 on the ESP32
+  To make this easier, a 4-pin locking JST cable can be purchased here: https://www.sparkfun.com/products/17240
+  Note: Almost any ESP32 pins can be used for serial.
+  Connect a multi-band GNSS antenna: https://www.sparkfun.com/products/21801
+*/
+
+#include <SparkFun_LG290P_GNSS.h> // Click here to get the library: http://librarymanager/All#SparkFun_LG290P
+
+// Adjust these values according to your configuration
+int pin_UART1_TX = 14;
+int pin_UART1_RX = 13;
+int gnss_baud = 460800;
+
+LG290P myGNSS;
+HardwareSerial SerialGNSS(1); // Use UART1 on the ESP32
+int mode;
+
+void setup()
+{
+  Serial.begin(115200);
+  delay(250);
+  Serial.println();
+  Serial.println("SparkFun Survey In Mode example");
+  Serial.println("Initializing device...");
+
+  // We must start the serial port before using it in the library
+  // Increase buffer size to handle high baud rate streams
+  SerialGNSS.setRxBufferSize(1024);
+  SerialGNSS.begin(gnss_baud, SERIAL_8N1, pin_UART1_RX, pin_UART1_TX);
+  
+  // myGNSS.enableDebugging(Serial); // Print all debug to Serial
+  if (myGNSS.begin(SerialGNSS) == false)     // Give the serial port over to the library
+  {
+    Serial.println("LG290P failed to respond. Check ports and baud rates. Freezing...");
+    while (true);
+  }
+  Serial.println("LG290P detected!");
+
+  // You MUST be in BASE mode to set Survey In Mode
+  Serial.println("Setting base station mode");
+  verify(myGNSS.setModeBase(false)); // don't reset, because setSurveyInMode() is going to do it
+  
+  double eiffelLat = 48.8584, eiffelLong = 2.2945, eiffelAlt = 365;
+  double ecefX, ecefY, ecefZ;
+
+  // Convert Eiffel Tower lat/long/alt to ECEF coordinates
+  LG290P::geodeticToEcef(eiffelLat, eiffelLong, eiffelAlt, ecefX, ecefY, ecefZ);
+
+  Serial.println("Setting 'Survey' Mode fixed to top of Eiffel Tower");
+  Serial.printf("(%.3f,%.3f,%.3f)\r\n", ecefX, ecefY, ecefZ);
+  verify(myGNSS.setSurveyFixedMode(ecefX, ecefY, ecefZ));
+  if (!myGNSS.isConnected())
+  {
+    Serial.println("reconnection failed; halting");
+    while (true);
+  }
+  Serial.println("Online!");
+
+  verify(myGNSS.getMode(mode));
+}
+
+unsigned long lastUpdate = 0;
+
+void loop()
+{
+  myGNSS.update(); // Regularly call to parse any new data
+  if (millis() - lastUpdate >= 1000)
+  {
+    lastUpdate = millis();
+    int svinStatus = myGNSS.getSurveyInStatus();
+    const char *status = svinStatus == 1 ? "In Progress" : svinStatus == 2 ? "Valid" : "Unknown/Invalid";
+    Serial.printf("%02d:%02d:%02d: Mode = '%s' Status = '%s' (%d/%d) (%.4f,%.4f,%.4f) Mean Accuracy = %.4f\r\n",
+      myGNSS.getHour(), myGNSS.getMinute(), myGNSS.getSecond(), mode == 2 ? "BASE" : "ROVER", status,
+      myGNSS.getSurveyInObservations(), myGNSS.getSurveyInCfgDuration(),
+      myGNSS.getSurveyInMeanX(), myGNSS.getSurveyInMeanY(), myGNSS.getSurveyInMeanZ(), 
+      myGNSS.getSurveyInMeanAccuracy());
+  }
+}
+
+void verify(bool event)
+{
+  if (!event)
+  {
+    Serial.println("Operation failed: freezing!");
+    while (true);
+  }
+}

keywords.txt

@@ -127,6 +127,8 @@ getEcefZ    KEYWORD2
 
 getGeodeticAgeMs	KEYWORD2
 getEcefAgeMs	KEYWORD2
+geodeticToEcef	KEYWORD2
+ecefToGeodetic	KEYWORD2
 
 getNorthError	KEYWORD2
 getEastError	KEYWORD2

src/SparkFun_LG290P_GNSS.cpp

@@ -1342,6 +1342,86 @@ uint32_t LG290P::getGeodeticAgeMs()
     return millis() - lastUpdatePvtDomain;
 }
 
+/* static */
+void LG290P::geodeticToEcef(double lat, double lon, double alt, double &xOut, double &yOut, double &zOut)
+{
+    const double WGS84_A = 6378137;           // https://geographiclib.sourceforge.io/html/Constants_8hpp_source.html
+    const double WGS84_E = 0.081819190842622; // http://docs.ros.org/en/hydro/api/gps_common/html/namespacegps__common.html
+                                              // and https://gist.github.com/uhho/63750c4b54c7f90f37f958cc8af0c718
+    double clat = cos(lat * DEG_TO_RAD);
+    double slat = sin(lat * DEG_TO_RAD);
+    double clon = cos(lon * DEG_TO_RAD);
+    double slon = sin(lon * DEG_TO_RAD);
+
+    double N = WGS84_A / sqrt(1.0 - WGS84_E * WGS84_E * slat * slat);
+
+    xOut = (N + alt) * clat * clon;
+    yOut = (N + alt) * clat * slon;
+    zOut = (N * (1.0 - WGS84_E * WGS84_E) + alt) * slat;
+}
+
+// Convert ECEF to LLH (geodetic)
+// From: https://danceswithcode.net/engineeringnotes/geodetic_to_ecef/geodetic_to_ecef.html
+/* static */
+void LG290P::ecefToGeodetic(double x, double y, double z, double &latOut, double &lonOut, double &altOut)
+{
+    double a = 6378137.0;              // WGS-84 semi-major axis
+    double e2 = 6.6943799901377997e-3; // WGS-84 first eccentricity squared
+    double a1 = 4.2697672707157535e+4; // a1 = a*e2
+    double a2 = 1.8230912546075455e+9; // a2 = a1*a1
+    double a3 = 1.4291722289812413e+2; // a3 = a1*e2/2
+    double a4 = 4.5577281365188637e+9; // a4 = 2.5*a2
+    double a5 = 4.2840589930055659e+4; // a5 = a1+a3
+    double a6 = 9.9330562000986220e-1; // a6 = 1-e2
+
+    double zp, w2, w, r2, r, s2, c2, s, c, ss;
+    double g, rg, rf, u, v, m, f, p;
+
+    zp = abs(z);
+    w2 = x * x + y * y;
+    w = sqrt(w2);
+    r2 = w2 + z * z;
+    r = sqrt(r2);
+    lonOut = atan2(y, x); // Lon (final)
+
+    s2 = z * z / r2;
+    c2 = w2 / r2;
+    u = a2 / r;
+    v = a3 - a4 / r;
+    if (c2 > 0.3)
+    {
+        s = (zp / r) * (1.0 + c2 * (a1 + u + s2 * v) / r);
+        latOut = asin(s); // Lat
+        ss = s * s;
+        c = sqrt(1.0 - ss);
+    }
+    else
+    {
+        c = (w / r) * (1.0 - s2 * (a5 - u - c2 * v) / r);
+        latOut = acos(c); // Lat
+        ss = 1.0 - c * c;
+        s = sqrt(ss);
+    }
+
+    g = 1.0 - e2 * ss;
+    rg = a / sqrt(g);
+    rf = a6 * rg;
+    u = w - rg * c;
+    v = zp - rf * s;
+    f = c * u + s * v;
+    m = c * v - s * u;
+    p = m / (rf / g + f);
+    latOut = latOut + p;        // Lat
+    altOut = f + m * p / 2.0; // Altitude
+    if (z < 0.0)
+    {
+        latOut *= -1.0; // Lat
+    }
+
+    latOut *= RAD_TO_DEG; // Convert to degrees
+    lonOut *= RAD_TO_DEG;
+}
+
 uint16_t LG290P::getYear()
 {
     ensurePvtEnabled();

src/SparkFun_LG290P_GNSS.h

@@ -959,6 +959,15 @@ class LG290P
      */
     double getSurveyInMeanAccuracy() { ensureSvinStatusEnabled(); return svinStatusDomain.meanAcc; }
 
+    // Convert LLH (geodetic) to ECEF
+    // From: https://stackoverflow.com/questions/19478200/convert-latitude-and-longitude-to-ecef-coordinates-system
+    static void geodeticToEcef(double lat, double lon, double alt, double &xOut, double &yOut, double &zOut);
+
+    // Convert ECEF to LLH (geodetic)
+    // From: https://danceswithcode.net/engineeringnotes/geodetic_to_ecef/geodetic_to_ecef.html
+    static void ecefToGeodetic(double x, double y, double z, double &latOut, double &lonOut, double &altOut);
+
+
   #if false // TODO
 
     float getLatitudeDeviation();