Merge pull request #848 from srichs/gps-coordinate-formats

GPS Coordinate Formats
2021-09-01 10:21:27 -07:00 · 2021-09-01 10:21:27 -07:00 · b1b3d9df6e
commit b1b3d9df6e
--- a/src/graphics/Screen.cpp
+++ b/src/graphics/Screen.cpp
@ -380,10 +380,165 @@ static void drawGPSAltitude(OLEDDisplay *display, int16_t x, int16_t y, const GP
    }
 }

+static inline double toRadians(double deg)
+{
+    return deg * PI / 180;
+}
+
+static inline double toDegrees(double r)
+{
+    return r * 180 / PI;
+}
+
+// A struct to hold the data for a DMS coordinate.
+struct DMS
+{
+    byte latDeg;
+    byte latMin;
+    double latSec;
+    char latCP;
+    byte lonDeg;
+    byte lonMin;
+    double lonSec;
+    char lonCP;
+};
+
+// A struct to hold the data for a UTM coordinate, this is also used when creating an MGRS coordinate.
+struct UTM 
+{
+    byte zone;
+    char band;
+    double easting;
+    double northing;
+};
+
+// A struct to hold the data for a MGRS coordinate.
+struct MGRS
+{
+    byte zone;
+    char band;
+    char east100k;
+    char north100k;
+    uint32_t easting;
+    uint32_t northing;
+};
+
+/**
+ * Converts lat long coordinates to UTM.
+ * based on this: https://github.com/walvok/LatLonToUTM/blob/master/latlon_utm.ino
+ */
+static struct UTM latLongToUTM(const double lat, const double lon)
+{
+    const String latBands = "CDEFGHJKLMNPQRSTUVWXX";
+    UTM utm;
+    utm.zone = int((lon + 180)/6 + 1);
+    utm.band = latBands.charAt(int(lat/8 + 10));
+    double a = 6378137; // WGS84 - equatorial radius
+    double k0 = 0.9996; // UTM point scale on the central meridian
+    double eccSquared = 0.00669438;   // eccentricity squared
+    double lonTemp = (lon + 180) - int((lon + 180)/360) * 360 - 180; //Make sure the longitude is between -180.00 .. 179.9
+    double latRad = toRadians(lat);
+    double lonRad = toRadians(lonTemp);
+
+    // Special Zones for Norway and Svalbard
+    if( lat >= 56.0 && lat < 64.0 && lonTemp >= 3.0 && lonTemp < 12.0 ) // Norway
+        utm.zone = 32;
+    if( lat >= 72.0 && lat < 84.0 ) { // Svalbard
+        if     ( lonTemp >= 0.0  && lonTemp <  9.0 ) utm.zone = 31;
+        else if( lonTemp >= 9.0  && lonTemp < 21.0 ) utm.zone = 33;
+        else if( lonTemp >= 21.0 && lonTemp < 33.0 ) utm.zone = 35;
+        else if( lonTemp >= 33.0 && lonTemp < 42.0 ) utm.zone = 37;
+    }
+    
+    double lonOrigin = (utm.zone - 1)*6 - 180 + 3;  // puts origin in middle of zone
+    double lonOriginRad = toRadians(lonOrigin);
+    double eccPrimeSquared = (eccSquared)/(1 - eccSquared);
+    double N = a/sqrt(1 - eccSquared*sin(latRad)*sin(latRad));
+    double T = tan(latRad)*tan(latRad);
+    double C = eccPrimeSquared*cos(latRad)*cos(latRad);
+    double A = cos(latRad)*(lonRad - lonOriginRad);
+    double M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*latRad 
+        - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*latRad)
+        + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*latRad) 
+        - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*latRad));
+    utm.easting = (double)(k0*N*(A+(1-T+C)*pow(A, 3)/6 + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) 
+        + 500000.0);
+    utm.northing = (double)(k0*(M+N*tan(latRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+        + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
+            
+    if(lat < 0)
+        utm.northing += 10000000.0; //10000000 meter offset for southern hemisphere
+
+    return utm;
+}
+
+// Converts lat long coordinates to an MGRS.
+static struct MGRS latLongToMGRS(double lat, double lon)
+{
+    const String e100kLetters[3] = { "ABCDEFGH", "JKLMNPQR", "STUVWXYZ" };
+    const String n100kLetters[2] = { "ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE" };
+    UTM utm = latLongToUTM(lat, lon);
+    MGRS mgrs;
+    mgrs.zone = utm.zone;
+    mgrs.band = utm.band;
+    double col = floor(utm.easting / 100000);
+    mgrs.east100k = e100kLetters[(mgrs.zone - 1) % 3].charAt(col - 1);
+    double row = (int)floor(utm.northing / 100000.0) % 20;
+    mgrs.north100k = n100kLetters[(mgrs.zone-1)%2].charAt(row);
+    mgrs.easting = (int)utm.easting % 100000;
+    mgrs.northing = (int)utm.northing % 100000;
+    return mgrs;
+}
+
+/**
+ * Converts lat long coordinates from decimal degrees to degrees minutes seconds format. 
+ * DD°MM'SS"C DDD°MM'SS"C
+ * 
+ * Possible TODO - As it is currently implemented there will be a loss of fidelity due 
+ * to the space constraint of 22 characters. One solution to this is to add a scrolling 
+ * text capability for the coordinates, where if the string is too long to fit the space
+ * it will be displayed for a couple of seconds then scroll over to the rest of the string
+ * for a couple of seconds.
+ */
+static struct DMS latLongToDMS(double lat, double lon)
+{
+    DMS dms;
+    
+    if (lat < 0) dms.latCP = 'S';
+    else dms.latCP = 'N';
+
+    double latDeg = lat;
+
+    if (lat < 0)
+        latDeg = latDeg * -1;
+
+    dms.latDeg = floor(latDeg);
+    double latMin = (latDeg - dms.latDeg) * 60;
+    dms.latMin = floor(latMin);
+    dms.latSec = (latMin - dms.latMin) * 60;
+
+    if (lon < 0) dms.lonCP = 'W';
+    else dms.lonCP = 'E';
+
+    double lonDeg = lon;
+
+    if (lon < 0)
+        lonDeg = lonDeg * -1;
+
+    dms.lonDeg = floor(lonDeg);
+    double lonMin = (lonDeg - dms.lonDeg) * 60;
+    dms.lonMin = floor(lonMin);
+    dms.lonSec = (lonMin - dms.lonMin) * 60;
+
+    return dms;
+}
+
 // Draw GPS status coordinates
 static void drawGPScoordinates(OLEDDisplay *display, int16_t x, int16_t y, const GPSStatus *gps)
 {
+    auto gpsFormat = radioConfig.preferences.gps_format;
    String displayLine = "";
+
    if (!gps->getIsConnected()) {
        displayLine = "No GPS Module";
        display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(displayLine))) / 2, y, displayLine);
@ -392,7 +547,21 @@ static void drawGPScoordinates(OLEDDisplay *display, int16_t x, int16_t y, const
        display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(displayLine))) / 2, y, displayLine);
    } else {
        char coordinateLine[22];
-        sprintf(coordinateLine, "%f %f", gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
+
+        if (gpsFormat == GpsCoordinateFormat_GpsFormatDMS) { // Degrees Minutes Seconds
+            DMS dms = latLongToDMS(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
+            sprintf(coordinateLine, "%2i°%2i'%2.0f\"%1c%3i°%2i'%2.0f\"%1c", dms.latDeg, dms.latMin, dms.latSec, dms.latCP, 
+                    dms.lonDeg, dms.lonMin, dms.lonSec, dms.lonCP);
+        } else if (gpsFormat == GpsCoordinateFormat_GpsFormatUTM) { // Universal Transverse Mercator
+            UTM utm = latLongToUTM(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
+            sprintf(coordinateLine, "%2i%1c %06.0f %07.0f", utm.zone, utm.band, utm.easting, utm.northing);
+        } else if (gpsFormat == GpsCoordinateFormat_GpsFormatMGRS) { // Military Grid Reference System
+            MGRS mgrs = latLongToMGRS(gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
+            sprintf(coordinateLine, "%2i%1c %1c%1c %05i %05i", mgrs.zone, mgrs.band, mgrs.east100k, mgrs.north100k, 
+                    mgrs.easting, mgrs.northing);
+        } else // Defaults to decimal degrees
+            sprintf(coordinateLine, "%f %f", gps->getLatitude() * 1e-7, gps->getLongitude() * 1e-7);
+        
        display->drawString(x + (SCREEN_WIDTH - (display->getStringWidth(coordinateLine))) / 2, y, coordinateLine);
    }
 }
@ -418,16 +587,6 @@ static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng
    return (float)(6366000 * tt);
 }

-static inline double toRadians(double deg)
-{
-    return deg * PI / 180;
-}
-
-static inline double toDegrees(double r)
-{
-    return r * 180 / PI;
-}
-
 /**
 * Computes the bearing in degrees between two points on Earth.  Ported from my
 * old Gaggle android app.
--- a/src/mesh/generated/radioconfig.pb.h
+++ b/src/mesh/generated/radioconfig.pb.h
@ -51,6 +51,13 @@ typedef enum _GpsOperation {
    GpsOperation_GpsOpDisabled = 4
 } GpsOperation;

+typedef enum _GpsCoordinateFormat {
+    GpsCoordinateFormat_GpsFormatDec = 0,
+    GpsCoordinateFormat_GpsFormatDMS = 1,
+    GpsCoordinateFormat_GpsFormatUTM = 2,
+    GpsCoordinateFormat_GpsFormatMGRS = 3,
+} GpsCoordinateFormat;
+
 typedef enum _LocationSharing {
    LocationSharing_LocUnset = 0,
    LocationSharing_LocEnabled = 1,
@ -89,6 +96,7 @@ typedef struct _RadioConfig_UserPreferences {
    float frequency_offset;
    char mqtt_server[32];
    bool mqtt_disabled;
+    GpsCoordinateFormat gps_format;
    bool factory_reset;
    bool debug_log_enabled;
    pb_size_t ignore_incoming_count;
@ -139,6 +147,10 @@ typedef struct _RadioConfig {
 #define _GpsOperation_MAX GpsOperation_GpsOpDisabled
 #define _GpsOperation_ARRAYSIZE ((GpsOperation)(GpsOperation_GpsOpDisabled+1))

+#define _GpsCoordinateFormat_MIN GpsCoordinateFormat_GpsFormatDec
+#define _GpsCoordinateFormat_MAX GpsCoordinateFormat_GpsFormatMGRS
+#define _GpsCoordinateFormat_ARRAYSIZE ((GpsCoordinateFormat)(GpsCoordinateFormat_GpsFormatMGRS+1))
+
 #define _LocationSharing_MIN LocationSharing_LocUnset
 #define _LocationSharing_MAX LocationSharing_LocDisabled
 #define _LocationSharing_ARRAYSIZE ((LocationSharing)(LocationSharing_LocDisabled+1))
@ -154,9 +166,9 @@ extern "C" {

 /* Initializer values for message structs */
 #define RadioConfig_init_default                 {false, RadioConfig_UserPreferences_init_default}
-#define RadioConfig_UserPreferences_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", 0, _RegionCode_MIN, _ChargeCurrent_MIN, _LocationSharing_MIN, _GpsOperation_MIN, 0, 0, 0, 0, 0, 0, 0, "", 0, 0, 0, 0, {0, 0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, _RadioConfig_UserPreferences_EnvironmentalMeasurementSensorType_MIN, 0, 0}
+#define RadioConfig_UserPreferences_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", 0, _RegionCode_MIN, _ChargeCurrent_MIN, _LocationSharing_MIN, _GpsOperation_MIN, 0, 0, 0, 0, 0, 0, 0, "", 0, _GpsCoordinateFormat_MIN, 0, 0, 0, {0, 0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, _RadioConfig_UserPreferences_EnvironmentalMeasurementSensorType_MIN, 0, 0}
 #define RadioConfig_init_zero                    {false, RadioConfig_UserPreferences_init_zero}
-#define RadioConfig_UserPreferences_init_zero    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", 0, _RegionCode_MIN, _ChargeCurrent_MIN, _LocationSharing_MIN, _GpsOperation_MIN, 0, 0, 0, 0, 0, 0, 0, "", 0, 0, 0, 0, {0, 0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, _RadioConfig_UserPreferences_EnvironmentalMeasurementSensorType_MIN, 0, 0}
+#define RadioConfig_UserPreferences_init_zero    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", 0, _RegionCode_MIN, _ChargeCurrent_MIN, _LocationSharing_MIN, _GpsOperation_MIN, 0, 0, 0, 0, 0, 0, 0, "", 0, _GpsCoordinateFormat_MIN, 0, 0, 0, {0, 0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, _RadioConfig_UserPreferences_EnvironmentalMeasurementSensorType_MIN, 0, 0}

 /* Field tags (for use in manual encoding/decoding) */
 #define RadioConfig_UserPreferences_position_broadcast_secs_tag 1
@ -185,6 +197,7 @@ extern "C" {
 #define RadioConfig_UserPreferences_frequency_offset_tag 41
 #define RadioConfig_UserPreferences_mqtt_server_tag 42
 #define RadioConfig_UserPreferences_mqtt_disabled_tag 43
+#define RadioConfig_UserPreferences_gps_format_tag 44
 #define RadioConfig_UserPreferences_factory_reset_tag 100
 #define RadioConfig_UserPreferences_debug_log_enabled_tag 101
 #define RadioConfig_UserPreferences_ignore_incoming_tag 103
@ -249,6 +262,7 @@ X(a, STATIC,   SINGULAR, BOOL,     serial_disabled,  40) \
 X(a, STATIC,   SINGULAR, FLOAT,    frequency_offset,  41) \
 X(a, STATIC,   SINGULAR, STRING,   mqtt_server,      42) \
 X(a, STATIC,   SINGULAR, BOOL,     mqtt_disabled,    43) \
+X(a, STATIC,   SINGULAR, UENUM,     gps_format,      44) \
 X(a, STATIC,   SINGULAR, BOOL,     factory_reset,   100) \
 X(a, STATIC,   SINGULAR, BOOL,     debug_log_enabled, 101) \
 X(a, STATIC,   REPEATED, UINT32,   ignore_incoming, 103) \