From ae70df27a2adfe01beb563a1aa4cd214d8186b80 Mon Sep 17 00:00:00 2001
From: Isaac <isaacw1925@gmail.com>
Date: Sat, 6 Jan 2024 17:25:02 -0800
Subject: [PATCH] Added an initial GPS position. (#10)

* Added an initial GPS position.

* Fixed meter to GPS conversion.

* Updated comments and fixed longitude calculation.

* Fixed longitude calculation.

* Fixed GPS calculation mistake, reverted starting coordinates to (0.0,0.0).
---
 Assets/Scenes/Simulator.unity |  3 +++
 Assets/Scripts/GpsSensor.cs   | 41 +++++++++++++++++++++++++++++------
 2 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/Assets/Scenes/Simulator.unity b/Assets/Scenes/Simulator.unity
index 4c7106b..b87aa29 100644
--- a/Assets/Scenes/Simulator.unity
+++ b/Assets/Scenes/Simulator.unity
@@ -212,6 +212,9 @@ MonoBehaviour:
   m_EditorClassIdentifier: 
   _noise: 0
   _reportPeriod: 0.05
+  initGPS:
+  - 0
+  - 0
 --- !u!1 &144878289
 GameObject:
   m_ObjectHideFlags: 0
diff --git a/Assets/Scripts/GpsSensor.cs b/Assets/Scripts/GpsSensor.cs
index 4f326c8..62578fc 100644
--- a/Assets/Scripts/GpsSensor.cs
+++ b/Assets/Scripts/GpsSensor.cs
@@ -1,3 +1,4 @@
+using System;
 using System.Collections;
 using UnityEngine;
 using Newtonsoft.Json.Linq;
@@ -22,6 +23,10 @@ public class GpsSensor : MonoBehaviour
     private float _noise;
     [SerializeField]
     private float _reportPeriod;
+    [SerializeField]
+    private double initLat = 0.0;
+    [SerializeField]
+    private double initLon = 0.0;
 
     private RoverSocket _socket;
 
@@ -51,21 +56,43 @@ private IEnumerator StreamPosition()
 
     private void ReportPosition()
     {
-        // Use double precision since geographic degrees are very large.
-        double latitude = CartesianToGeographic(transform.position.z + _noise * Utilities.GaussianRandom());
-        double longitude = CartesianToGeographic(transform.position.x + _noise * Utilities.GaussianRandom());
+        // z+ is north, x+ is east
+        double[] GPS = metersToGPS(new double[] {
+            transform.position.z + _noise * Utilities.GaussianRandom(),
+            transform.position.x + _noise * Utilities.GaussianRandom()});
 
         JObject positionReport = new JObject()
         {
             ["type"] = "simGpsPositionReport",
-            ["latitude"] = latitude,
-            ["longitude"] = longitude
+            ["latitude"] = GPS[0],
+            ["longitude"] = GPS[1]
         };
         _socket.Send(positionReport);
     }
 
-    private double CartesianToGeographic(float meters)
+    private double[] metersToGPS(double[] offset)
     {
-        return Mathf.Rad2Deg * meters / EarthRadius;
+        // Because of our starting position, North is +lat and East is +lon
+
+        // The Earth is not a perfect sphere, so we approximate the Earth's surface with an ellipsoid
+        // https://en.wikipedia.org/wiki/Geodetic_datum#Earth_reference_ellipsoid
+
+        // Data taken from WGS 84:
+        // https://en.wikipedia.org/wiki/World_Geodetic_System#WGS_84
+        double semiMajorAxis = 6378137.0;
+        double semiMinorAxis = 6356752.314245;
+
+        // Math from
+        // https://en.wikipedia.org/wiki/Longitude#Length_of_a_degree_of_longitude
+        // https://en.wikipedia.org/wiki/Latitude#Meridian_distance_on_the_ellipsoid
+        double phi = Math.PI * initLat / 180.0;
+        // Square Eccentricity
+        double eSq = 1 - (Math.Pow(semiMinorAxis, 2)) / (Math.Pow(semiMajorAxis, 2));
+        double var = 1 - eSq * Math.Pow(Math.Sin(phi), 2);
+        double metersPerDegLon = (Math.PI * semiMajorAxis * Math.Cos(phi)) / (180.0 * Math.Sqrt(var));
+        double metersPerDegLat = (Math.PI * semiMajorAxis * (1 - eSq)) / (180.0 * Math.Pow(var, 1.5));
+        double degDiffLat = offset[0] / metersPerDegLat;
+        double degDiffLon = offset[1] / metersPerDegLon;
+        return new double[] {initLat + degDiffLat, initLon + degDiffLon};
     }
 }