From 73f4fb11de72c5e3d9b717c15c516aeca9fe0d2c Mon Sep 17 00:00:00 2001
From: Rodrigo Amaral <roaa1g19@soton.ac.uk>
Date: Wed, 3 Mar 2021 15:21:39 +0000
Subject: [PATCH] made _rodGPS_Distance() function for accurate distance calcs

Signed-off-by: Rodrigo Amaral <roaa1g19@soton.ac.uk>
---
 rodGPS/src/main.cpp   |  2 --
 rodGPS/src/rodGPS.cpp | 13 +++++++++----
 rodGPS/src/rodGPS.hpp | 18 +++++++++++-------
 3 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/rodGPS/src/main.cpp b/rodGPS/src/main.cpp
index 03dd7b8..2600736 100644
--- a/rodGPS/src/main.cpp
+++ b/rodGPS/src/main.cpp
@@ -14,6 +14,4 @@ void setup()
 void loop() 
 {
   _rodGPS_MakeReading(&gprmc_buff, &test);
-  Serial.print(std::atof("32.575"));
-  Serial.write("\n\n");
 }
diff --git a/rodGPS/src/rodGPS.cpp b/rodGPS/src/rodGPS.cpp
index 6c2a646..84b5941 100644
--- a/rodGPS/src/rodGPS.cpp
+++ b/rodGPS/src/rodGPS.cpp
@@ -26,10 +26,10 @@ void _rodGPS_PrintGPRMC(simple_GPRMC* gprmc_data)
   Serial.write(gprmc_data->DATE[5]);
 
   Serial.write("\nLatitude: ");
-  Serial.write((uint8_t*)&(gprmc_data->LATI),10);
+  Serial.write((uint8_t*)&(gprmc_data->LATI),LATI_SIZE);
   Serial.write(gprmc_data->LATI_DIR);
   Serial.write("  Longitude: ");
-  Serial.write((uint8_t*)&(gprmc_data->LONG),11);
+  Serial.write((uint8_t*)&(gprmc_data->LONG),LONG_SIZE);
   Serial.write(gprmc_data->LONG_DIR);
 
   Serial.write("\n\n");
@@ -42,8 +42,13 @@ double _rodGPS_SimpleDistance(simple_GPRMC* coord_1, simple_GPRMC* coord_2)
   return sqrt( dx*dx + dy*dy );
 }
 
+double _rodGPS_Distance(simple_GPRMC* coord_1, simple_GPRMC* coord_2)
+{
+  return 6378388 * acos(sin(std::atof(coord_1->LATI)) * sin(std::atof(coord_2->LATI)) + cos(std::atof(coord_1->LATI)) * cos(std::atof(coord_2->LATI)) * cos(std::atof(coord_2->LONG) - std::atof(coord_1->LONG)));
+}
+
 static char char_buff[5];
-byte _rodGPS_MakeReading(simple_GPRMC* gprmc1, simple_GPRMC* gprmc2)
+uint8_t _rodGPS_MakeReading(simple_GPRMC* gprmc1, simple_GPRMC* gprmc2)
 {
   while(Serial2.available() == 0) {;}
 
@@ -102,7 +107,7 @@ byte _rodGPS_MakeReading(simple_GPRMC* gprmc1, simple_GPRMC* gprmc2)
 
       _rodGPS_PrintGPRMC(gprmc1);
       Serial.write("Distance: ");
-      Serial.println(String((_rodGPS_SimpleDistance(gprmc1,gprmc2)),2));
+      Serial.println(String((_rodGPS_Distance(gprmc1,gprmc2)),2));
       Serial.write("\n\n");
 
       for(byte i = 0; i < 10; i++)
diff --git a/rodGPS/src/rodGPS.hpp b/rodGPS/src/rodGPS.hpp
index 418a258..fdba9a3 100644
--- a/rodGPS/src/rodGPS.hpp
+++ b/rodGPS/src/rodGPS.hpp
@@ -1,9 +1,12 @@
+#ifndef RODGPS_HPP
+#define RODGPS_HPP
+
 #include <Arduino.h>
 #include <HardwareSerial.h>
 #include <string.h>
 
-#ifndef RODGPS_HPP
-#define RODGPS_HPP
+#define LATI_SIZE 10
+#define LONG_SIZE 11
 
 typedef struct simple_GPRMC
 {
@@ -11,14 +14,15 @@ typedef struct simple_GPRMC
   char DATE[6];
   char VALID;
 
-  char LATI[10];
+  char LATI[LATI_SIZE];
   char LATI_DIR;
-  char LONG[11];
+  char LONG[LONG_SIZE];
   char LONG_DIR;
 };
 
-void _rodGPS_PrintGPRMC(simple_GPRMC* gprmc_data);
-double _rodGPS_SimpleDistance(simple_GPRMC* coord_1, simple_GPRMC* coord_2);
-byte _rodGPS_MakeReading(simple_GPRMC* gprmc1, simple_GPRMC* gprmc2);
+void _rodGPS_PrintGPRMC(simple_GPRMC* gprmc_data); /* Print out the GPRMC data */
+double _rodGPS_SimpleDistance(simple_GPRMC* coord_1, simple_GPRMC* coord_2); /* Quicker distance calculation, good enough for small distances */
+double _rodGPS_Distance(simple_GPRMC* coord_1, simple_GPRMC* coord_2); /* Accurate distance calculations */
+uint8_t _rodGPS_MakeReading(simple_GPRMC* gprmc1, simple_GPRMC* gprmc2); /* Make a GPRMC reading from GPS IC */
 
 #endif /* RODGPS_HPP */
\ No newline at end of file
-- 
GitLab