From a5f17b148b2dfe6db394a5c1242abc95e33243da Mon Sep 17 00:00:00 2001
From: pip <pip>
Date: Mon, 21 Feb 2011 13:32:22 +0000
Subject: [PATCH] Added getGPSPosition() function to the UAVObject utility

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2830 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 .../uavobjectutil/UAVObjectUtil.pluginspec    |  3 +-
 .../uavobjectutil/uavobjectutilmanager.cpp    | 37 +++++++++++++++++++
 .../uavobjectutil/uavobjectutilmanager.h      |  3 ++
 .../uavobjectutil/uavobjectutilplugin.cpp     | 10 +----
 4 files changed, 43 insertions(+), 10 deletions(-)

diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/UAVObjectUtil.pluginspec b/ground/openpilotgcs/src/plugins/uavobjectutil/UAVObjectUtil.pluginspec
index 8f2a24d0b..af65371e7 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectutil/UAVObjectUtil.pluginspec
+++ b/ground/openpilotgcs/src/plugins/uavobjectutil/UAVObjectUtil.pluginspec
@@ -6,5 +6,6 @@
     <url>http://www.openpilot.org</url>
     <dependencyList>
         <dependency name="Core" version="1.0.0"/>
-    </dependencyList>
+		<dependency name="UAVObjects" version="1.0.0"/>
+	</dependencyList>
 </plugin> 
diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
index 63e6f64dd..8c31d0309 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp
@@ -193,3 +193,40 @@ int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3], double ECEF[
 }
 
 // ******************************
+// GPS
+
+int UAVObjectUtilManager::getGPSPosition(double LLA[3])
+{
+	QMutexLocker locker(mutex);
+
+	ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+	if (!pm) return -1;
+
+	UAVObjectManager *obm = pm->getObject<UAVObjectManager>();
+	if (!obm) return -2;
+
+	UAVDataObject *obj = dynamic_cast<UAVDataObject*>(obm->getObject(QString("GPSPosition")));
+	if (!obj) return -3;
+
+	LLA[0] = obj->getField(QString("Latitude"))->getDouble() * 1E-7;
+	LLA[1] = obj->getField(QString("Longitude"))->getDouble() * 1E-7;
+	LLA[2] = obj->getField(QString("Altitude"))->getDouble();
+
+	if (LLA[0] != LLA[0]) LLA[0] = 0; // nan detection
+	else
+	if (LLA[0] >  90) LLA[0] =  90;
+	else
+	if (LLA[0] < -90) LLA[0] = -90;
+
+	if (LLA[1] != LLA[1]) LLA[1] = 0; // nan detection
+	else
+	if (LLA[1] >  180) LLA[1] =  180;
+	else
+	if (LLA[1] < -180) LLA[1] = -180;
+
+	if (LLA[2] != LLA[2]) LLA[2] = 0; // nan detection
+
+	return 0;	// OK
+}
+
+// ******************************
diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h
index abea4aa14..3dafea601 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h
+++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h
@@ -25,6 +25,7 @@
  * with this program; if not, write to the Free Software Foundation, Inc., 
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+
 #ifndef UAVOBJECTUTILMANAGER_H
 #define UAVOBJECTUTILMANAGER_H
 
@@ -46,6 +47,8 @@ public:
 	int getHomeLocation(bool &set, double LLA[3]);
 	int getHomeLocation(bool &set, double LLA[3], double ECEF[3], double RNE[9], double Be[3]);
 
+	int getGPSPosition(double LLA[3]);
+
 private:
 	QMutex *mutex;
 
diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp
index cf557644f..ad7f55ad8 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilplugin.cpp
@@ -30,17 +30,14 @@
 
 UAVObjectUtilPlugin::UAVObjectUtilPlugin()
 {
-
 }
 
 UAVObjectUtilPlugin::~UAVObjectUtilPlugin()
 {
-
 }
 
 void UAVObjectUtilPlugin::extensionsInitialized()
 {
-
 }
 
 bool UAVObjectUtilPlugin::initialize(const QStringList & arguments, QString * errorString)
@@ -48,20 +45,15 @@ bool UAVObjectUtilPlugin::initialize(const QStringList & arguments, QString * er
 	Q_UNUSED(arguments)
 	Q_UNUSED(errorString)
 
-	// Create object manager and expose object
+	// Create manager and expose object
 	UAVObjectUtilManager *objUtilMngr = new UAVObjectUtilManager();
 	addAutoReleasedObject(objUtilMngr);
 
-	// Initialize UAVObjects
-//	UAVObjectUtilInitialize(objMngr);
-
-
     return true;
 }
 
 void UAVObjectUtilPlugin::shutdown()
 {
-
 }
 
 Q_EXPORT_PLUGIN(UAVObjectUtilPlugin)