From bcabd2cee3b0e076d8b43e3da2c8d5831f731675 Mon Sep 17 00:00:00 2001
From: LM <lm@student.ethz.ch>
Date: Fri, 21 Oct 2011 20:08:22 +0200
Subject: [PATCH] Added conversion functions

---
 src/uas/UASManager.cc | 20 ++++++++++++++++++++
 src/uas/UASManager.h  |  4 ++++
 2 files changed, 24 insertions(+)

diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc
index ad8d9fdce..7c247f538 100644
--- a/src/uas/UASManager.cc
+++ b/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
  */
diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h
index fc6bb0d51..750096c80 100644
--- a/src/uas/UASManager.h
+++ b/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)
     {
-- 
2.22.0