Added conversion functions

src/uas/UASManager.cc

@@ -18,6 +18,10 @@
 #include "UASManager.h"
 #include "QGC.h"
 
+#define PI 3.1415926535897932384626433832795
+#define MEAN_EARTH_DIAMETER	12756274.0
+#define UMR	0.017453292519943295769236907684886
+
 UASManager* UASManager::instance()
 {
     static UASManager* _instance = 0;
@@ -181,6 +185,22 @@ void UASManager::wgs84ToEnu(const double& lat, const double& lon, const double&
 //}
 
 
+
+void UASManager::enuToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt)
+{
+    *lat=homeLat+y/MEAN_EARTH_DIAMETER*360./PI;
+    *lon=homeLon+x/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR);
+    *alt=homeAlt+z;
+}
+
+void UASManager::nedToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt)
+{
+    *lat=homeLat+x/MEAN_EARTH_DIAMETER*360./PI;
+    *lon=homeLon+y/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR);
+    *alt=homeAlt-z;
+}
+
+
 /**
  * This function will change QGC's home position on a number of conditions only
  */

src/uas/UASManager.h

@@ -96,6 +96,10 @@ public:
     Eigen::Vector3d ecefToEnu(const Eigen::Vector3d & ecef);
     /** @brief Convert WGS84 lat/lon coordinates to carthesian coordinates with home position as origin */
     void wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up);
+    /** @brief Convert x,y,z coordinates to lat / lon / alt coordinates in east-north-up frame */
+    void enuToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt);
+    /** @brief Convert x,y,z coordinates to lat / lon / alt coordinates in north-east-down frame */
+    void nedToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt);
 
     void getLocalNEDSafetyLimits(double* x1, double* y1, double* z1, double* x2, double* y2, double* z2)
     {