From 3ec508169a296d110b215bc8e5880d1f6e4a9b77 Mon Sep 17 00:00:00 2001
From: Jm Casler <jm@casler.org>
Date: Fri, 26 Nov 2021 19:12:00 -0800
Subject: [PATCH] Implementation of smart positioning #934

Initial check in for https://github.com/meshtastic/Meshtastic-device/issues/934
---
 src/plugins/PositionPlugin.cpp        | 32 +++++++++++++++++++++++++++
 src/plugins/PositionPlugin.h          |  4 ++++
 src/plugins/esp32/RangeTestPlugin.cpp |  2 ++
 3 files changed, 38 insertions(+)

diff --git a/src/plugins/PositionPlugin.cpp b/src/plugins/PositionPlugin.cpp
index 509750c8..a64ead3b 100644
--- a/src/plugins/PositionPlugin.cpp
+++ b/src/plugins/PositionPlugin.cpp
@@ -4,6 +4,8 @@
 #include "NodeDB.h"
 #include "RTC.h"
 #include "Router.h"
+#include "gps/GeoCoord.h"
+
 
 PositionPlugin *positionPlugin;
 
@@ -144,6 +146,9 @@ int32_t PositionPlugin::runOnce()
 
         lastGpsSend = now;
 
+        lastGpsLatitude = node->position.latitude_i;
+        lastGpsLongitude = node->position.longitude_i;
+
         // If we changed channels, ask everyone else for their latest info
         bool requestReplies = currentGeneration != radioGeneration;
         currentGeneration = radioGeneration;
@@ -151,6 +156,33 @@ int32_t PositionPlugin::runOnce()
         DEBUG_MSG("Sending pos@%x:6 to mesh (wantReplies=%d)\n", 
                     node->position.pos_timestamp, requestReplies);
         sendOurPosition(NODENUM_BROADCAST, requestReplies);
+    } else if (radioConfig.preferences.position_broadcast_smart == true) {
+        // radioConfig.preferences.position_broadcast_smart
+        //NodeInfo *node = service.refreshMyNodeInfo(); // should guarantee there is now a position
+        
+        if (node->has_position && (node->position.latitude_i != 0 || node->position.longitude_i != 0) ) {
+            float distance = GeoCoord::latLongToMeter(lastGpsLatitude * 1e-7, lastGpsLongitude * 1e-7,
+                                node->position.latitude_i * 1e-7, node->position.longitude_i * 1e-7);
+
+            // Please don't change this value. This accomodates for possible poor positioning
+            //   in the event the GPS has a poor satelite lock.
+            const uint8_t distanceTravel = 100; 
+
+            // Minimum time between position updates.
+            const uint8_t timeTravel = 60; 
+
+            // If the distance traveled since the last update is greater than 100 meters
+            //   and it's been at least 60 seconds since the last update
+            if ((abs(distance) >= distanceTravel) && (lastGpsSend == 0 || now - timeTravel >= getPref_position_broadcast_secs() * 1000)) {
+                bool requestReplies = currentGeneration != radioGeneration;
+                currentGeneration = radioGeneration;
+
+                DEBUG_MSG("Sending pos@%x:6 to mesh (wantReplies=%d)\n", 
+                            node->position.pos_timestamp, requestReplies);
+                sendOurPosition(NODENUM_BROADCAST, requestReplies);
+
+            }
+        }
     }
 
     return 5000; // to save power only wake for our callback occasionally
diff --git a/src/plugins/PositionPlugin.h b/src/plugins/PositionPlugin.h
index 3e4034d4..5ffe0480 100644
--- a/src/plugins/PositionPlugin.h
+++ b/src/plugins/PositionPlugin.h
@@ -13,6 +13,10 @@ class PositionPlugin : public ProtobufPlugin<Position>, private concurrency::OST
     /// We limit our GPS broadcasts to a max rate
     uint32_t lastGpsSend = 0;
 
+    // Store the latest good lat / long
+    uint32_t lastGpsLatitude = 0;
+    uint32_t lastGpsLongitude = 0;
+
     /// We force a rebroadcast if the radio settings change
     uint32_t currentGeneration = 0;
 
diff --git a/src/plugins/esp32/RangeTestPlugin.cpp b/src/plugins/esp32/RangeTestPlugin.cpp
index 1252fe74..f5cc87d1 100644
--- a/src/plugins/esp32/RangeTestPlugin.cpp
+++ b/src/plugins/esp32/RangeTestPlugin.cpp
@@ -151,6 +151,7 @@ ProcessMessage RangeTestPluginRadio::handleReceived(const MeshPacket &mp)
                 appendFile(mp);
             }
 
+            /*
             DEBUG_MSG("-----------------------------------------\n");
             DEBUG_MSG("p.payload.bytes  \"%s\"\n", p.payload.bytes);
             DEBUG_MSG("p.payload.size   %d\n", p.payload.size);
@@ -174,6 +175,7 @@ ProcessMessage RangeTestPluginRadio::handleReceived(const MeshPacket &mp)
             DEBUG_MSG("gpsStatus->getHasLock()      %d\n", gpsStatus->getHasLock());
             DEBUG_MSG("gpsStatus->getDOP()          %d\n", gpsStatus->getDOP());
             DEBUG_MSG("-----------------------------------------\n");
+            */
         }
 
     } else {