diff --git a/src/gps/GeoCoord.cpp b/src/gps/GeoCoord.cpp index b96181069..96c27bdf8 100644 --- a/src/gps/GeoCoord.cpp +++ b/src/gps/GeoCoord.cpp @@ -344,4 +344,25 @@ void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double & osgb_Longitude = atan2(osgbY, osgbX); // leave in radians //osgb height = p*cos(osgb.latitude) + osgbZ*sin(osgb.latitude) - //(airyA*airyA/(airyA / sqrt(1 - airyEcc*sin(osgb.latitude)*sin(osgb.latitude)))); // Not used, no OSTN data +} + +/// Ported from my old java code, returns distance in meters along the globe +/// surface (by magic?) +float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) +{ + double pk = (180 / 3.14169); + double a1 = lat_a / pk; + double a2 = lng_a / pk; + double b1 = lat_b / pk; + double b2 = lng_b / pk; + double cos_b1 = cos(b1); + double cos_a1 = cos(a1); + double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2); + double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2); + double t3 = sin(a1) * sin(b1); + double tt = acos(t1 + t2 + t3); + if (std::isnan(tt)) + tt = 0.0; // Must have been the same point? + + return (float)(6366000 * tt); } \ No newline at end of file diff --git a/src/gps/GeoCoord.h b/src/gps/GeoCoord.h index fcb7aa18b..236d66a9f 100644 --- a/src/gps/GeoCoord.h +++ b/src/gps/GeoCoord.h @@ -112,6 +112,7 @@ class GeoCoord { static void latLongToOSGR(const double lat, const double lon, OSGR &osgr); static void latLongToOLC(const double lat, const double lon, OLC &olc); static void convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude); + static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b); // Lat lon alt getters int32_t getLatitude() const { return _latitude; } diff --git a/src/graphics/Screen.cpp b/src/graphics/Screen.cpp index bd08218fd..8a7997a09 100644 --- a/src/graphics/Screen.cpp +++ b/src/graphics/Screen.cpp @@ -437,27 +437,6 @@ static void drawGPScoordinates(OLEDDisplay *display, int16_t x, int16_t y, const } } -/// Ported from my old java code, returns distance in meters along the globe -/// surface (by magic?) -static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) -{ - double pk = (180 / 3.14169); - double a1 = lat_a / pk; - double a2 = lng_a / pk; - double b1 = lat_b / pk; - double b2 = lng_b / pk; - double cos_b1 = cos(b1); - double cos_a1 = cos(a1); - double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2); - double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2); - double t3 = sin(a1) * sin(b1); - double tt = acos(t1 + t2 + t3); - if (isnan(tt)) - tt = 0.0; // Must have been the same point? - - return (float)(6366000 * tt); -} - /** * Computes the bearing in degrees between two points on Earth. Ported from my * old Gaggle android app. @@ -543,7 +522,7 @@ static float estimatedHeading(double lat, double lon) return b; } - float d = latLongToMeter(oldLat, oldLon, lat, lon); + float d = GeoCoord::latLongToMeter(oldLat, oldLon, lat, lon); if (d < 10) // haven't moved enough, just keep current bearing return b; @@ -667,7 +646,7 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_ // display direction toward node hasNodeHeading = true; Position &p = node->position; - float d = latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i)); + float d = GeoCoord::latLongToMeter(DegD(p.latitude_i), DegD(p.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i)); if (d < 2000) snprintf(distStr, sizeof(distStr), "%.0f m", d); else diff --git a/src/plugins/esp32/RangeTestPlugin.cpp b/src/plugins/esp32/RangeTestPlugin.cpp index b28bd8c9c..47970b88c 100644 --- a/src/plugins/esp32/RangeTestPlugin.cpp +++ b/src/plugins/esp32/RangeTestPlugin.cpp @@ -5,6 +5,7 @@ #include "RTC.h" #include "Router.h" #include "configuration.h" +#include "gps/GeoCoord.h" #include #include //#include @@ -184,27 +185,6 @@ bool RangeTestPluginRadio::handleReceived(const MeshPacket &mp) return true; // Let others look at this message also if they want } -/// Ported from my old java code, returns distance in meters along the globe -/// surface (by magic?) -float RangeTestPluginRadio::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) -{ - double pk = (180 / 3.14169); - double a1 = lat_a / pk; - double a2 = lng_a / pk; - double b1 = lat_b / pk; - double b2 = lng_b / pk; - double cos_b1 = cos(b1); - double cos_a1 = cos(a1); - double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2); - double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2); - double t3 = sin(a1) * sin(b1); - double tt = acos(t1 + t2 + t3); - if (isnan(tt)) - tt = 0.0; // Must have been the same point? - - return (float)(6366000 * tt); -} - bool RangeTestPluginRadio::appendFile(const MeshPacket &mp) { auto &p = mp.decoded; @@ -303,7 +283,7 @@ bool RangeTestPluginRadio::appendFile(const MeshPacket &mp) fileToAppend.printf("%f,", mp.rx_snr); // RX SNR if (n->position.latitude_i && n->position.longitude_i && gpsStatus->getLatitude() && gpsStatus->getLongitude()) { - float distance = latLongToMeter(n->position.latitude_i * 1e-7, n->position.longitude_i * 1e-7, + float distance = GeoCoord::latLongToMeter(n->position.latitude_i * 1e-7, n->position.longitude_i * 1e-7, gpsStatus->getLatitude() * 1e-7, gpsStatus->getLongitude() * 1e-7); fileToAppend.printf("%f,", distance); // Distance in meters } else {