From 5e220b23b223cf13b5150056123da12e60e715f5 Mon Sep 17 00:00:00 2001
From: Bertrand Lemasle <blemasle@gmail.com>
Date: Wed, 16 May 2018 16:34:09 +1200
Subject: [PATCH] [Untested] Reset StoppedInARow if gap between the previous
 and current position is more than a default value (try to detect missed
 positions)

---
 GpsTracker/Config.h       |  1 +
 GpsTracker/Core.cpp       | 14 +++++++++----
 GpsTracker/Core.h         |  2 +-
 GpsTracker/Gps.cpp        | 42 +++++++++++++++++++++++++++++++++++++++
 GpsTracker/Gps.h          |  3 +++
 GpsTracker/GpsTracker.ino |  2 +-
 6 files changed, 58 insertions(+), 6 deletions(-)

diff --git a/GpsTracker/Config.h b/GpsTracker/Config.h
index bb0db05..54e776e 100644
--- a/GpsTracker/Config.h
+++ b/GpsTracker/Config.h
@@ -36,6 +36,7 @@
 
 #define GPS_DEFAULT_INTERMEDIATE_TIMEOUT_MS				10000L
 #define GPS_DEFAULT_TOTAL_TIMEOUT_MS					80000L
+#define GPS_DEFAULT_MISSED_POSITION_GAP_KM				2
 
 #define NETWORK_DEFAULT_INTERMEDIATE_TIMEOUT_MS			10000L
 #define NETWORK_DEFAULT_TOTAL_TIMEOUT_MS				80000L
diff --git a/GpsTracker/Core.cpp b/GpsTracker/Core.cpp
index e03dc09..7f662d3 100644
--- a/GpsTracker/Core.cpp
+++ b/GpsTracker/Core.cpp
@@ -16,23 +16,29 @@ namespace core {
 		PositionEntryMetadata metadata;
 		if (positions::acquire(metadata)) {
 			positions::appendLast(metadata);
-			forceBackup = updateSleepTime(gps::getVelocity());
+			forceBackup = updateSleepTime();
+
+			gps::preserveCurrentCoordinates();
 		}
 
 		positions::doBackup(forceBackup);
 		mainunit::deepSleep(sleepTime);
 	}
 
-	bool updateSleepTime(uint8_t velocity) {
+	bool updateSleepTime() {
+		uint8_t velocity = gps::getVelocity();
 		uint16_t result = mapSleepTime(velocity);
 		bool goingLongSleep = false;
 
 		if (velocity < SLEEP_TIMING_MIN_MOVING_VELOCITY) {
-			stoppedInARow++;
+			float distance = gps::getDistanceFromPrevious(); //did we missed positions because we were sleeping ?
+			if (distance > GPS_DEFAULT_MISSED_POSITION_GAP_KM) stoppedInARow = 0;
+			else stoppedInARow = max(stoppedInARow + 1, SLEEP_DEFAULT_STOPPED_THRESHOLD + 1); //avoid overflow on REALLY long stops
+			
 			if (stoppedInARow < SLEEP_DEFAULT_STOPPED_THRESHOLD) {
 				result = SLEEP_DEFAULT_PAUSING_TIME_SECONDS;
 			}
-			else if(stoppedInARow == SLEEP_DEFAULT_STOPPED_THRESHOLD) goingLongSleep = true;
+			else if (stoppedInARow == SLEEP_DEFAULT_STOPPED_THRESHOLD) goingLongSleep = true;
 		}
 		else stoppedInARow = 0;
 
diff --git a/GpsTracker/Core.h b/GpsTracker/Core.h
index 635e2f5..8e4e5b2 100644
--- a/GpsTracker/Core.h
+++ b/GpsTracker/Core.h
@@ -14,6 +14,6 @@ namespace core {
 	extern uint16_t sleepTime;
 
 	void main();
-	bool updateSleepTime(uint8_t velocity);
+	bool updateSleepTime();
 	uint16_t mapSleepTime(uint8_t velocity);
 }
\ No newline at end of file
diff --git a/GpsTracker/Gps.cpp b/GpsTracker/Gps.cpp
index adcea68..ee5ad8f 100644
--- a/GpsTracker/Gps.cpp
+++ b/GpsTracker/Gps.cpp
@@ -3,6 +3,7 @@
 #include "Debug.h"
 #include "Hardware.h"
 #include "MainUnit.h"
+#include "math.h"
 
 #define LOGGER_NAME "Gps"
 
@@ -13,6 +14,8 @@
 #define TIME_MINUTE_OFFSET	10
 #define TIME_SECOND_OFFSET	12
 
+#define EARTH_RADIUS 6371 //kilometers
+
 namespace gps {
 
 	namespace details {
@@ -25,6 +28,9 @@ namespace gps {
 	char lastPosition[GPS_POSITION_SIZE];
 	SIM808_GPS_STATUS lastStatus;
 
+	float previousLat = 0;
+	float previousLng = 0;
+
 	SIM808_GPS_STATUS acquireCurrentPosition(int32_t timeout) {
 		SIM808_GPS_STATUS currentStatus = SIM808_GPS_STATUS::OFF;
 
@@ -46,6 +52,42 @@ namespace gps {
 		return currentStatus;
 	}
 
+	void preserveCurrentCoordinates() {
+		float lat, lng;
+		if(!hardware::sim808::device.getGpsField(lastPosition, SIM808_GPS_FIELD::LATITUDE, &lat)) lat = 0;
+		if(!hardware::sim808::device.getGpsField(lastPosition, SIM808_GPS_FIELD::LONGITUDE, &lng)) lng = 0;
+
+		if (lat == 0 || lng == 0) return;
+		previousLat = lat;
+		previousLng = lng;
+	}
+
+	float getDistanceFromPrevious() {
+		float lat1, lng1, lat2, lng2;
+
+		if(!hardware::sim808::device.getGpsField(lastPosition, SIM808_GPS_FIELD::LATITUDE, &lat2)) return 0;
+		if(!hardware::sim808::device.getGpsField(lastPosition, SIM808_GPS_FIELD::LONGITUDE, &lng2)) return 0;
+		
+		lat1 = radians(previousLat);
+		lng1 = radians(previousLng);
+
+		lat2 = radians(lat2);
+		lng2 = radians(lng2);
+
+		float dlat = lat2 - lat1;
+		float dlng = lng2 - lng1;
+		float a = (
+				pow(sin(dlat / 2), 2) +
+				cos(lat1) * cos(lat2) * pow(sin(dlng / 2), 2)
+			);
+
+
+		a = EARTH_RADIUS * (2 * atan2(sqrt(a), sqrt(1 - a))); //kilometers
+
+		NOTICE_FORMAT("distanceFromPrevious", "%fkm", a);
+		return a;
+	}
+
 	uint8_t getVelocity() {
 		uint8_t velocity;
 		if (!hardware::sim808::device.getGpsField(lastPosition, SIM808_GPS_FIELD::SPEED, &velocity)) velocity = 0;
diff --git a/GpsTracker/Gps.h b/GpsTracker/Gps.h
index 2ca0e76..27a6cc0 100644
--- a/GpsTracker/Gps.h
+++ b/GpsTracker/Gps.h
@@ -18,5 +18,8 @@ namespace gps {
 	SIM808_GPS_STATUS acquireCurrentPosition(int32_t timeout);
 
 	uint8_t getVelocity();
+	float getDistanceFromPrevious();
 	void getTime(tmElements_t &time);
+
+	void preserveCurrentCoordinates();
 }
\ No newline at end of file
diff --git a/GpsTracker/GpsTracker.ino b/GpsTracker/GpsTracker.ino
index 259c656..806f815 100644
--- a/GpsTracker/GpsTracker.ino
+++ b/GpsTracker/GpsTracker.ino
@@ -78,7 +78,7 @@ void loop() {
 			debug::addLastPositionToEeprom();
 			break;
 		case debug::GPSTRACKER_DEBUG_COMMAND::EEPROM_BACKUP_ENTRIES:
-			positions::doBackup(false);
+			positions::doBackup(true);
 			break;
 		case debug::GPSTRACKER_DEBUG_COMMAND::SLEEP:
 			mainunit::sleep(period_t::SLEEP_8S);