Merge pull request microsoft#3839 from rajat2004/use-geodetic-converter

Use Geodetic Converter for NED<->GPS conversions
kamanazan · Apr 4, 2022 · 0a3263b · 0a3263b
2 parents a8a0e2f + 4accf0a
commit 0a3263b
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 12 deletions.
diff --git 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)

diff --git 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