From 8c46d22aa7ca9e8f168918a1794375023af2858d Mon Sep 17 00:00:00 2001
From: sambas <sambas>
Date: Sun, 17 Oct 2010 17:37:51 +0000
Subject: [PATCH] OP-169 hitl/FlightGear fixes

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1980 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 .../src/libs/utils/coordinateconversions.cpp   | 16 ++++++++++++++++
 ground/src/libs/utils/coordinateconversions.h  |  1 +
 ground/src/plugins/hitlnew/fgsimulator.cpp     | 18 +++++++++++-------
 3 files changed, 28 insertions(+), 7 deletions(-)

diff --git a/ground/src/libs/utils/coordinateconversions.cpp b/ground/src/libs/utils/coordinateconversions.cpp
index e2f558e36..67017283f 100644
--- a/ground/src/libs/utils/coordinateconversions.cpp
+++ b/ground/src/libs/utils/coordinateconversions.cpp
@@ -151,4 +151,20 @@ int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double po
     return 0;
 }
 
+void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
+{
+        double ECEF[3];
+        float diff[3];
+
+        LLA2ECEF(LLA, ECEF);
+
+        diff[0] = (float)(ECEF[0] - BaseECEF[0]);
+        diff[1] = (float)(ECEF[1] - BaseECEF[1]);
+        diff[2] = (float)(ECEF[2] - BaseECEF[2]);
+
+        NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
+        NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
+        NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
+}
+
 }
diff --git a/ground/src/libs/utils/coordinateconversions.h b/ground/src/libs/utils/coordinateconversions.h
index e7ab161f1..0bb7a4571 100644
--- a/ground/src/libs/utils/coordinateconversions.h
+++ b/ground/src/libs/utils/coordinateconversions.h
@@ -45,6 +45,7 @@ public:
     void RneFromLLA(double LLA[3], double Rne[3][3]);
     void LLA2ECEF(double LLA[3], double ECEF[3]);
     int ECEF2LLA(double ECEF[3], double LLA[3]);
+    void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
 };
 
 }
diff --git a/ground/src/plugins/hitlnew/fgsimulator.cpp b/ground/src/plugins/hitlnew/fgsimulator.cpp
index c861af394..07b846f82 100644
--- a/ground/src/plugins/hitlnew/fgsimulator.cpp
+++ b/ground/src/plugins/hitlnew/fgsimulator.cpp
@@ -213,7 +213,7 @@ void FGSimulator::processUpdate(const QByteArray& inp)
 	float velocityActualNorth = fields[23].toFloat() * FPS2CMPS;
 
         //run once
-        HomeLocation::DataFields homeData;
+        HomeLocation::DataFields homeData = posHome->getData();
         if(!once)
         {
             memset(&homeData, 0, sizeof(HomeLocation::DataFields));
@@ -226,7 +226,11 @@ void FGSimulator::processUpdate(const QByteArray& inp)
             LLA[1]=longitude;
             LLA[2]=0;
             double ECEF[3];
-            Utils::CoordinateConversions().RneFromLLA(LLA,(double(*)[3])homeData.RNE);
+            double RNE[9];
+            Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
+            for (int t=0;t<9;t++) {
+                    homeData.RNE[t]=RNE[t];
+            }
             Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
             homeData.ECEF[0]=ECEF[0]*100;
             homeData.ECEF[1]=ECEF[1]*100;
@@ -286,15 +290,15 @@ void FGSimulator::processUpdate(const QByteArray& inp)
         gpsData.Status = GPSPosition::STATUS_FIX3D;
         gpsPos->setData(gpsData);
 
-        /*float NED[3];
+        float NED[3];
         double LLA[3] = {(double) gpsData.Latitude / 1e7, (double) gpsData.Longitude / 1e7, (double) (gpsData.GeoidSeparation + gpsData.Altitude)};
         // convert from cm back to meters
         double ECEF[3] = {(double) (homeData.ECEF[0] / 100), (double) (homeData.ECEF[1] / 100), (double) (homeData.ECEF[2] / 100)};
                 Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) homeData.RNE, NED);
 
-        positionActualData.North = NED[0]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
-        positionActualData.East = NED[1]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
-        positionActualData.Down = NED[2]; //Multiply by 100 because positionActual expects input in Centimeters.
-        posActual->setData(positionActualData);*/
+        positionActualData.North = NED[0]*100; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
+        positionActualData.East = NED[1]*100; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
+        positionActualData.Down = NED[2]*100; //Multiply by 100 because positionActual expects input in Centimeters.
+        posActual->setData(positionActualData);
 }