Skip to content

Use Geodetic Converter for NED<->GPS conversions #3839

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
zimmy87 merged 3 commits into from
Apr 4, 2022
Merged

Use Geodetic Converter for NED<->GPS conversions #3839

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5c5929d
Add small helper methods for GeodeticConverter
rajat2004 Jun 29, 2021
06d42b1
Fix some old bugs in GeodeticConverter
rajat2004 Jun 29, 2021
4accf0a
Use GeodeticConverter for NED to GPS conversion
rajat2004 Jun 29, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 15 additions & 6 deletions AirLib/include/common/GeodeticConverter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -34,10 +39,13 @@ 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(home_latitude_rad_, home_longitude_rad_);
ned_to_ecef_matrix_ = ecef_to_ned_matrix_.inverse();
}

ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
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)
Expand Down Expand Up @@ -133,10 +141,11 @@ 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)
{
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,
Expand Down
11 changes: 7 additions & 4 deletions AirLib/include/physics/Environment.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include "common/UpdatableObject.hpp"
#include "common/CommonStructs.hpp"
#include "common/EarthUtils.hpp"
#include "common/GeodeticConverter.hpp"

namespace msr
{
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -87,7 +89,7 @@ namespace airlib

virtual void update() override
{
updateState(current_, home_geo_point_);
updateState(current_);
}

protected:
Expand All @@ -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);
Expand All @@ -122,6 +124,7 @@ namespace airlib
private:
State initial_, current_;
HomeGeoPoint home_geo_point_;
GeodeticConverter geodetic_converter_;
};
}
} //namespace
Expand Down