rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

Add a coordinate conversion method to go from LLA to NED

Browse Source
This commit is contained in:
James Cotton 2012-06-02 14:28:52 -05:00
parent f7e0fc1065
commit 4e1044589f
2 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
View File

@ -143,6 +143,29 @@ int CoordinateConversions::GetLLA(double homeLLA[3], double NED[3], double posit
return 0; return 0;
} }
/**
* Get the current location in Longitude, Latitude Altitude (above WSG-48 ellipsoid)
* @param[in] BaseECEF the ECEF of the home location (in cm)
* @param[in] position three element double for position in degrees and meters
* @param[out] NED the offset from the home location (in m)
* @returns
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::GetNED(double homeLLA[3], double position[3], double NED[3])
{
double T[3];
T[0] = homeLLA[2]+6.378137E6f * M_PI / 180.0;
T[1] = cosf(homeLLA[0] * M_PI / 180.0)*(homeLLA[2]+6.378137E6f) * M_PI / 180.0;
T[2] = -1.0f;
NED[0] = (position[0] - homeLLA[0]) * T[0];
NED[1] = (position[1] - homeLLA[1]) * T[1];
NED[2] = (position[2] - homeLLA[2]) * T[2];
return 0;
}
void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]) void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{ {
double ECEF[3]; double ECEF[3];

1
ground/openpilotgcs/src/libs/utils/coordinateconversions.h
View File

@ -42,6 +42,7 @@ class QTCREATOR_UTILS_EXPORT CoordinateConversions
public: public:
CoordinateConversions(); CoordinateConversions();
int GetLLA(double LLA[3], double NED[3], double position[3]); int GetLLA(double LLA[3], double NED[3], double position[3]);
int GetNED(double homeLLA[3], double position[3], double NED[3]);
void RneFromLLA(double LLA[3], double Rne[3][3]); void RneFromLLA(double LLA[3], double Rne[3][3]);
void LLA2ECEF(double LLA[3], double ECEF[3]); void LLA2ECEF(double LLA[3], double ECEF[3]);
int ECEF2LLA(double ECEF[3], double LLA[3]); int ECEF2LLA(double ECEF[3], double LLA[3]);