diff --git a/src/graphics/Screen.cpp b/src/graphics/Screen.cpp index f2fc1e1e..4b1be858 100644 --- 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. diff --git a/src/mesh/generated/radioconfig.pb.h b/src/mesh/generated/radioconfig.pb.h index afccd937..0d349224 100644 --- 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) \