Various position fixes (meshtastic#3297)

* Guard against no movement * Add newlines * Fix printfs
GUVWAF · Feb 27, 2024 · e6a2c06 · e6a2c06
1 parent ce0e5c0
commit e6a2c06
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 11 deletions.
diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp
@@ -319,7 +319,6 @@ bool GPS::setup()
             delay(250);
             _serial_gps->write("$CFGMSG,6,1,0\r\n");
             delay(250);
-
         } else if (gnssModel == GNSS_MODEL_UBLOX) {
             // Configure GNSS system to GPS+SBAS+GLONASS (Module may restart after this command)
             // We need set it because by default it is GPS only, and we want to use GLONASS too
@@ -458,7 +457,6 @@ bool GPS::setup()
                             LOG_WARN("Unable to enable NMEA 4.10.\n");
                         }
                     }
-
                 } else {
                     if (strncmp(info.hwVersion, "00040007", 8) == 0) { // This PSM mode is only for Neo-6
                         msglen = makeUBXPacket(0x06, 0x11, 0x2, _message_CFG_RXM_ECO);
@@ -642,12 +640,12 @@ void GPS::setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime)
 #endif
 #ifdef PIN_GPS_STANDBY // Specifically the standby pin for L76K and clones
     if (on) {
-        LOG_INFO("Waking GPS");
+        LOG_INFO("Waking GPS\n");
         pinMode(PIN_GPS_STANDBY, OUTPUT);
         digitalWrite(PIN_GPS_STANDBY, 1);
         return;
     } else {
-        LOG_INFO("GPS entering sleep");
+        LOG_INFO("GPS entering sleep\n");
         // notifyGPSSleep.notifyObservers(NULL);
         pinMode(PIN_GPS_STANDBY, OUTPUT);
         digitalWrite(PIN_GPS_STANDBY, 0);

diff --git a/src/gps/GeoCoord.cpp b/src/gps/GeoCoord.cpp
@@ -376,14 +376,17 @@ void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double &
 }
 
 /// Ported from my old java code, returns distance in meters along the globe
-/// surface (by magic?)
+/// surface (by Haversine formula)
 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;
+    // Don't do math if the points are the same
+    if (lat_a == lat_b && lng_a == lng_b)
+        return 0.0;
+
+    double a1 = lat_a / DEG_CONVERT;
+    double a2 = lng_a / DEG_CONVERT;
+    double b1 = lat_b / DEG_CONVERT;
+    double b2 = lng_b / DEG_CONVERT;
     double cos_b1 = cos(b1);
     double cos_a1 = cos(a1);
     double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2);

diff --git a/src/gps/GeoCoord.h b/src/gps/GeoCoord.h
@@ -11,6 +11,7 @@
 
 #define PI 3.1415926535897932384626433832795
 #define OLC_CODE_LEN 11
+#define DEG_CONVERT 180 / PI
 
 // Helper functions
 // Raises a number to an exponent, handling negative exponents.

diff --git a/src/modules/PositionModule.cpp b/src/modules/PositionModule.cpp
@@ -372,7 +372,7 @@ struct SmartPosition PositionModule::getDistanceTraveledSinceLastSend(meshtastic
     LOG_DEBUG("currentPosition.latitude_i=%i, currentPosition.longitude_i=%i\n", lastGpsLatitude, lastGpsLongitude);
 
     LOG_DEBUG("--------SMART POSITION-----------------------------------\n");
-    LOG_DEBUG("hasTraveledOverThreshold=%i, distanceTraveled=%d, distanceThreshold=% u\n",
+    LOG_DEBUG("hasTraveledOverThreshold=%i, distanceTraveled=%f, distanceThreshold=%f\n",
               abs(distanceTraveledSinceLastSend) >= distanceTravelThreshold, abs(distanceTraveledSinceLastSend),
               distanceTravelThreshold);