From 5c5929d9b0a1d63c4b42e45c7443555a59f7fbc4 Mon Sep 17 00:00:00 2001
From: Rajat Singhal <rajatsinghal564@gmail.com>
Date: Tue, 29 Jun 2021 23:56:10 +0530
Subject: [PATCH 1/3] Add small helper methods for GeodeticConverter

---
 AirLib/include/common/GeodeticConverter.hpp | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/AirLib/include/common/GeodeticConverter.hpp b/AirLib/include/common/GeodeticConverter.hpp
index 30d27b0f41..cffd886625 100644
--- a/AirLib/include/common/GeodeticConverter.hpp
+++ b/AirLib/include/common/GeodeticConverter.hpp
@@ -20,6 +20,11 @@ namespace airlib
             setHome(home_latitude, home_longitude, home_altitude);
         }
 
+        GeodeticConverter(const GeoPoint& home_geopoint)
+        {
+            setHome(home_geopoint);
+        }
+
         void setHome(double home_latitude, double home_longitude, float home_altitude)
         {
             home_latitude_ = home_latitude;
@@ -40,6 +45,11 @@ namespace airlib
             ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();
         }
 
+        void setHome(const GeoPoint& home_geopoint)
+        {
+            setHome(home_geopoint.latitude, home_geopoint.longitude, home_geopoint.altitude);
+        }
+
         void getHome(double* latitude, double* longitude, float* altitude)
         {
             *latitude = home_latitude_;
@@ -139,6 +149,11 @@ namespace airlib
             *altitude = home_altitude_ - down;
         }
 
+        void ned2Geodetic(const Vector3r& ned_pos, GeoPoint& geopoint)
+        {
+            ned2Geodetic(ned_pos[0], ned_pos[1], ned_pos[2], &geopoint.latitude, &geopoint.longitude, &geopoint.altitude);
+        }
+
         void geodetic2Enu(const double latitude, const double longitude, const double altitude,
                           double* east, double* north, double* up)
         {

From 06d42b14b7c95dd9997eed2c5803516670b844c8 Mon Sep 17 00:00:00 2001
From: Rajat Singhal <rajatsinghal564@gmail.com>
Date: Wed, 30 Jun 2021 00:18:27 +0530
Subject: [PATCH 2/3] Fix some old bugs in GeodeticConverter

---
 AirLib/include/common/GeodeticConverter.hpp | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/AirLib/include/common/GeodeticConverter.hpp b/AirLib/include/common/GeodeticConverter.hpp
index cffd886625..cbeb223931 100644
--- a/AirLib/include/common/GeodeticConverter.hpp
+++ b/AirLib/include/common/GeodeticConverter.hpp
@@ -39,10 +39,8 @@ namespace airlib
             geodetic2Ecef(home_latitude_, home_longitude_, home_altitude_, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_);
 
             // Compute ECEF to NED and NED to ECEF matrices
-            double phiP = atan2(home_ecef_z_, sqrt(pow(home_ecef_x_, 2) + pow(home_ecef_y_, 2)));
-
-            ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
-            ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();
+            ecef_to_ned_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_);
+            ned_to_ecef_matrix_ = ecef_to_ned_matrix_.inverse();
         }
 
         void setHome(const GeoPoint& home_geopoint)
@@ -143,10 +141,6 @@ namespace airlib
             double x, y, z;
             ned2Ecef(north, east, down, &x, &y, &z);
             ecef2Geodetic(x, y, z, latitude, longitude, altitude);
-
-            //TODO: above returns wrong altitude if down was positive. This is because sqrt return value would be -ve
-            //but normal sqrt only return +ve. For now we just override it.
-            *altitude = home_altitude_ - down;
         }
 
         void ned2Geodetic(const Vector3r& ned_pos, GeoPoint& geopoint)

From 4accf0a4907aa228b0f76fd3958e7eb3a6a3be3f Mon Sep 17 00:00:00 2001
From: Rajat Singhal <rajatsinghal564@gmail.com>
Date: Wed, 30 Jun 2021 00:47:42 +0530
Subject: [PATCH 3/3] Use GeodeticConverter for NED to GPS conversion

---
 AirLib/include/physics/Environment.hpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/AirLib/include/physics/Environment.hpp b/AirLib/include/physics/Environment.hpp
index f3e7837ccf..9649ea0a8f 100644
--- a/AirLib/include/physics/Environment.hpp
+++ b/AirLib/include/physics/Environment.hpp
@@ -8,6 +8,7 @@
 #include "common/UpdatableObject.hpp"
 #include "common/CommonStructs.hpp"
 #include "common/EarthUtils.hpp"
+#include "common/GeodeticConverter.hpp"
 
 namespace msr
 {
@@ -53,12 +54,13 @@ namespace airlib
 
             setHomeGeoPoint(initial_.geo_point);
 
-            updateState(initial_, home_geo_point_);
+            updateState(initial_);
         }
 
         void setHomeGeoPoint(const GeoPoint& home_geo_point)
         {
             home_geo_point_ = HomeGeoPoint(home_geo_point);
+            geodetic_converter_.setHome(home_geo_point);
         }
 
         GeoPoint getHomeGeoPoint() const
@@ -87,7 +89,7 @@ namespace airlib
 
         virtual void update() override
         {
-            updateState(current_, home_geo_point_);
+            updateState(current_);
         }
 
     protected:
@@ -106,9 +108,9 @@ namespace airlib
         }
 
     private:
-        static void updateState(State& state, const HomeGeoPoint& home_geo_point)
+        void updateState(State& state)
         {
-            state.geo_point = EarthUtils::nedToGeodetic(state.position, home_geo_point);
+            geodetic_converter_.ned2Geodetic(state.position, state.geo_point);
 
             real_T geo_pot = EarthUtils::getGeopotential(state.geo_point.altitude / 1000.0f);
             state.temperature = EarthUtils::getStandardTemperature(geo_pot);
@@ -122,6 +124,7 @@ namespace airlib
     private:
         State initial_, current_;
         HomeGeoPoint home_geo_point_;
+        GeodeticConverter geodetic_converter_;
     };
 }
 } //namespace