From 56af74d17c6027f74283e97cf7ed8737a12d1a3e Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Sat, 3 Mar 2012 12:37:01 -0600
Subject: [PATCH] Fix map to work with the new HomeLocation (no ECEF)

---
 .../src/libs/utils/coordinateconversions.cpp  | 24 +++-----
 .../src/plugins/opmap/opmapgadgetwidget.cpp   | 60 ++++++++++---------
 2 files changed, 39 insertions(+), 45 deletions(-)

diff --git a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
index ba67d8ab5..497f47412 100644
--- a/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
+++ b/ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp
@@ -129,24 +129,16 @@ int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3])
   *  @arg 0 success
   *  @arg -1 for failure
   */
-int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double position[3])
+int CoordinateConversions::GetLLA(double homeLLA[3], double NED[3], double position[3])
 {
-    int i;
-    // stored value is in cm, convert to m
-    double BaseECEFm[3] = {BaseECEFcm[0], BaseECEFcm[1], BaseECEFcm[2]};
-    double BaseLLA[3];
-    double ECEF[3];
-    double Rne [3][3];
+    float T[3];
+    T[0] = homeLLA[2]+6.378137E6f;
+    T[1] = cosf(homeLLA[0])*(homeLLA[2]+6.378137E6f);
+    T[2] = -1.0f;
 
-    // Get LLA address to compute conversion matrix
-    ECEF2LLA(BaseECEFm, BaseLLA);
-    RneFromLLA(BaseLLA, Rne);
-
-    /* P = ECEF + Rne' * NED */
-    for(i = 0; i < 3; i++)
-        ECEF[i] = BaseECEFm[i] + Rne[0][i]*NED[0] + Rne[1][i]*NED[1] + Rne[2][i]*NED[2];
-
-    ECEF2LLA(ECEF,position);
+    position[0] = homeLLA[0] + NED[0] / T[0];
+    position[1] = homeLLA[1] + NED[1] / T[1];
+    position[2] = homeLLA[2] + NED[2] / T[2];
 
     return 0;
 }
diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
index abc51de20..42ada44d3 100644
--- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
@@ -38,6 +38,9 @@
 #include <QFile>
 #include <QDateTime>
 
+#include "homelocation.h"
+#include "positionactual.h"
+
 #include <math.h>
 
 #include "utils/stylehelper.h"
@@ -2317,54 +2320,53 @@ internals::PointLatLng OPMapGadgetWidget::destPoint(internals::PointLatLng sourc
 
 bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, double &altitude)
 {
-    double BaseECEF[3];
     double NED[3];
     double LLA[3];
+    double homeLLA[3];
     UAVObject *obj;
 
-	if (!obm)
-		return false;
+    Q_ASSERT(obm != NULL);
 
-	obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("HomeLocation")));
-    if (!obj) return false;
-    BaseECEF[0] = obj->getField(QString("ECEF"))->getDouble(0) / 100;
-    BaseECEF[1] = obj->getField(QString("ECEF"))->getDouble(1) / 100;
-    BaseECEF[2] = obj->getField(QString("ECEF"))->getDouble(2) / 100;
+    HomeLocation *homeLocation = HomeLocation::GetInstance(obm);
+    Q_ASSERT(homeLocation != NULL);
+    HomeLocation::DataFields homeLocationData = homeLocation->getData();
 
-	obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("PositionActual")));
-    if (!obj) return false;
-    NED[0] = obj->getField(QString("North"))->getDouble() / 100;
-    NED[1] = obj->getField(QString("East"))->getDouble() / 100;
-    NED[2] = obj->getField(QString("Down"))->getDouble() / 100;
+    homeLLA[0] = homeLocationData.Latitude / 10e6;
+    homeLLA[1] = homeLocationData.Longitude / 10e6;
+    homeLLA[2] = homeLocationData.Altitude;
 
-//    obj = dynamic_cast<UAVDataObject*>(om->getObject(QString("PositionDesired")));
+    PositionActual *positionActual = PositionActual::GetInstance(obm);
+    Q_ASSERT(positionActual != NULL);
+    PositionActual::DataFields positionActualData = positionActual->getData();
 
-//    obj = dynamic_cast<UAVDataObject*>(objManager->getObject("VelocityActual"));      // air speed
+    NED[0] = positionActualData.North / 100;
+    NED[1] = positionActualData.East / 100;
+    NED[2] = positionActualData.Down / 100;
 
-    Utils::CoordinateConversions().GetLLA(BaseECEF, NED, LLA);
+    Utils::CoordinateConversions().GetLLA(homeLLA, NED, LLA);
 
     latitude = LLA[0];
     longitude = LLA[1];
     altitude = LLA[2];
 
     if (latitude != latitude) latitude = 0; // nan detection
-//    if (isNan(latitude)) latitude = 0; // nan detection
+    //    if (isNan(latitude)) latitude = 0; // nan detection
     else
-//    if (!isFinite(latitude)) latitude = 0;
-//    else
-    if (latitude >  90) latitude =  90;
-    else
-    if (latitude < -90) latitude = -90;
+        //    if (!isFinite(latitude)) latitude = 0;
+        //    else
+        if (latitude >  90) latitude =  90;
+        else
+            if (latitude < -90) latitude = -90;
 
     if (longitude != longitude) longitude = 0; // nan detection
     else
-//    if (longitude > std::numeric_limits<double>::max()) longitude = 0;  // +infinite
-//    else
-//    if (longitude < -std::numeric_limits<double>::max()) longitude = 0;  // -infinite
-//    else
-    if (longitude >  180) longitude =  180;
-    else
-    if (longitude < -180) longitude = -180;
+        //    if (longitude > std::numeric_limits<double>::max()) longitude = 0;  // +infinite
+        //    else
+        //    if (longitude < -std::numeric_limits<double>::max()) longitude = 0;  // -infinite
+        //    else
+        if (longitude >  180) longitude =  180;
+        else
+            if (longitude < -180) longitude = -180;
 
     if (altitude != altitude) altitude = 0; // nan detection