Ground/OPMap + Position Information: Initial version of conversion from NED to LLA and integration into OPMap. I don't particular love how this is done. The main issue is the Utils didn't like having any UAVObject stuff linked to them (in my hands). That means in OPMap gadget I had to write the code that will pull the HomeLocation and PositionActual together, but this code will likely need to be reused. I will update the Altitude getting code later too, I need to look how the whole internals::position is working on OPMap.

To comment please post to http://forums.openpilot.org/topic/1578-changes-to-gps-objects/page__pid__4705#entry4705

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1477 ebee16cc-31ac-478f-84a7-5cbb03baadba

ground/src/libs/utils/coordinateconversions.cpp

@@ -0,0 +1,138 @@
+/**
+ ******************************************************************************
+ *
+ * @file       coordinateconversions.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      General conversions with different coordinate systems.
+ *             - all angles in deg
+ *             - distances in meters
+ *             - altitude above WGS-84 elipsoid
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "coordinateconversions.h"
+#include <QDebug>
+
+#define RAD2DEG (180.0/3.14)
+#define DEG2RAD (3.14/180.0)
+
+namespace Utils {
+
+CoordinateConversions::CoordinateConversions()
+{
+
+}
+
+/**
+  * Convert from LLA coordinates to ECEF coordinates
+  * @param[in] LLA[3] latitude longitude alititude coordinates in
+  * @param[out] ECEF[3] location in ECEF coordinates
+  */
+void CoordinateConversions::LLA2ECEF(double LLA[3], double ECEF[3]){
+  const double a = 6378137.0;           // Equatorial Radius
+  const double e = 8.1819190842622e-2;  // Eccentricity
+  double sinLat, sinLon, cosLat, cosLon;
+  double N;
+
+        sinLat=sin(DEG2RAD*LLA[0]);
+        sinLon=sin(DEG2RAD*LLA[1]);
+        cosLat=cos(DEG2RAD*LLA[0]);
+        cosLon=cos(DEG2RAD*LLA[1]);
+
+        N = a / sqrt(1.0 - e*e*sinLat*sinLat);  //prime vertical radius of curvature
+
+        ECEF[0] = (N+LLA[2])*cosLat*cosLon;
+        ECEF[1] = (N+LLA[2])*cosLat*sinLon;
+        ECEF[2] = ((1-e*e)*N + LLA[2]) * sinLat;
+}
+
+/**
+  * Convert from ECEF coordinates to LLA coordinates
+  * @param[in] ECEF[3] location in ECEF coordinates
+  * @param[out] LLA[3] latitude longitude alititude coordinates
+  */
+int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3])
+{
+    const double a = 6378137.0;           // Equatorial Radius
+    const double e = 8.1819190842622e-2;  // Eccentricity
+    double x=ECEF[0], y=ECEF[1], z=ECEF[2];
+    double Lat, N, NplusH, delta, esLat;
+    uint16_t iter;
+
+    LLA[1] = RAD2DEG*atan2(y,x);
+    N = a;
+    NplusH = N;
+    delta = 1;
+    Lat = 1;
+    iter=0;
+
+    while (((delta > 1.0e-14)||(delta < -1.0e-14)) && (iter < 100))
+    {
+        delta = Lat - atan(z / (sqrt(x*x + y*y)*(1-(N*e*e/NplusH))));
+        Lat = Lat-delta;
+        esLat = e*sin(Lat);
+        N = a / sqrt(1 - esLat*esLat);
+        NplusH = sqrt(x*x + y*y)/cos(Lat);
+        iter += 1;
+    }
+
+    LLA[0] = RAD2DEG*Lat;
+    LLA[2] = NplusH - N;
+
+    if (iter==500) return (0);
+    else return (1);
+}
+
+/**
+  * Get the current location in Longitude, Latitude Altitude (above WSG-48 ellipsoid)
+  * @param[in] BaseECEF the ECEF of the home location (in cm)
+  * @param[in] NED the offset from the home location (in m)
+  * @param[out] position three element double for position in degrees and meters
+  * @returns
+  *  @arg 0 success
+  *  @arg -1 for failure
+  */
+int CoordinateConversions::GetLLA(double BaseECEFcm[3], double NED[3], double position[3])
+{
+    int i;
+    // stored value is in cm, convert to m
+    double BaseECEFm[3] = {BaseECEFcm[0] / 100, BaseECEFcm[1] / 100, BaseECEFcm[2] / 100};
+    double BaseLLA[3];
+    double ECEF[3];
+
+    // Get LLA address to compute conversion matrix
+    ECEF2LLA(BaseECEFm, BaseLLA);
+
+    // TODO: Find/derive correct inverse of Rne using LLA
+    double RnePrime [3][3] =
+    {{1,0,0},
+     {0,1,0},
+     {0,0,1}};
+
+    /* P = ECEF + CM * NED */
+    for(i = 0; i < 3; i++)
+        ECEF[i] = BaseECEFm[i] + RnePrime[i][0]*NED[0] + RnePrime[i][1]*NED[1] + RnePrime[i][2]*NED[2];
+
+    ECEF2LLA(ECEF,position);
+
+    return 0;
+}
+
+}

ground/src/libs/utils/coordinateconversions.h

@@ -0,0 +1,53 @@
+/**
+ ******************************************************************************
+ *
+ * @file       coordinateconversions.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ *             Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009.
+ * @brief
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef COORDINATECONVERSIONS_H
+#define COORDINATECONVERSIONS_H
+
+#include "utils_global.h"
+#include "../extensionsystem/pluginmanager.h"
+#include "../../plugins/uavobjects/uavobjectmanager.h"
+#include "../../plugins/uavobjects/uavobject.h"
+#include "math.h"
+
+namespace Utils {
+
+class QTCREATOR_UTILS_EXPORT CoordinateConversions
+{
+public:
+    CoordinateConversions();
+    int GetLLA(double LLA[3], double NED[3], double position[3]);
+
+protected:
+    void LLA2ECEF(double LLA[3], double ECEF[3]);
+    int ECEF2LLA(double ECEF[3], double LLA[3]);
+};
+
+}
+
+#endif /* COORDINATECONVERSIONS_H */

ground/src/libs/utils/utils.pro

@@ -36,8 +36,9 @@ SOURCES += reloadpromptutils.cpp \
     iwelcomepage.cpp \
     fancymainwindow.cpp \
     detailsbutton.cpp \
-    detailswidget.cpp
-win32 { 
+    detailswidget.cpp \
+    coordinateconversions.cpp
+win32 {
     SOURCES += abstractprocess_win.cpp \
         consoleprocess_win.cpp \
         winutils.cpp
@@ -80,7 +81,8 @@ HEADERS += utils_global.h \
     iwelcomepage.h \
     fancymainwindow.h \
     detailsbutton.h \
-    detailswidget.h
+    detailswidget.h \
+    coordinateconversions.h
 FORMS += filewizardpage.ui \
     projectintropage.ui \
     newclasswidget.ui \

ground/src/plugins/opmap/opmap.pro

@@ -4,6 +4,7 @@ include(../../openpilotgcsplugin.pri)
 include(../../plugins/coreplugin/coreplugin.pri)
 include(../../libs/opmapcontrol/opmapcontrol.pri)
 include(../../plugins/uavobjects/uavobjects.pri)
+include(../../libs/utils/utils.pri)
 HEADERS += opmapplugin.h \
     opmapgadgetoptionspage.h \
     opmapgadgetfactory.h \

ground/src/plugins/opmap/opmapgadgetwidget.cpp

@@ -27,7 +27,6 @@
 
 #include "opmapgadgetwidget.h"
 #include "ui_opmap_widget.h"
-
 #include <QtGui/QApplication>
 #include <QtGui/QHBoxLayout>
 #include <QtGui/QVBoxLayout>
@@ -57,7 +56,6 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
 
     m_plugin_manager = NULL;
     m_objManager = NULL;
-    m_positionActual = NULL;
 
     m_mouse_waypoint = NULL;
 
@@ -68,10 +66,6 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
 
     m_plugin_manager = ExtensionSystem::PluginManager::instance();
     m_objManager = m_plugin_manager->getObject<UAVObjectManager>();
-    m_positionActual = PositionActual::GetInstance(m_objManager);
-
-    // get current UAV data
-    PositionActual::DataFields data = m_positionActual->getData();
 
     // **************
     // create the widget that holds the user controls and the map
@@ -278,7 +272,8 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
     connect(m_map, SIGNAL(WPInserted(int const&, WayPointItem*)), this, SLOT(WPInserted(int const&, WayPointItem*)));
     connect(m_map, SIGNAL(WPDeleted(int const&)), this, SLOT(WPDeleted(int const&)));
 
-    m_map->SetCurrentPosition(internals::PointLatLng(data.Latitude, data.Longitude));	    // set the default map position
+    QPointF pos = getLatLon();
+    m_map->SetCurrentPosition(internals::PointLatLng(pos.x(), pos.y()));	    // set the default map position
 
     // **************
     // create various context menu (mouse right click menu) actions
@@ -517,12 +512,12 @@ void OPMapGadgetWidget::keyPressEvent(QKeyEvent* event)
 
 void OPMapGadgetWidget::updatePosition()
 {
-    PositionActual::DataFields data = m_positionActual->getData();				// get current UAV data
+    QPointF pos = getLatLon();
 
-    internals::PointLatLng uav_pos = internals::PointLatLng(data.Latitude, data.Longitude);	// current UAV position
-    float uav_heading = data.Heading;								// current UAV heading
-    float uav_altitude_meters = data.Altitude;							// current UAV height
-    float uav_ground_speed_meters_per_second = data.Groundspeed;				// current UAV ground speed
+    internals::PointLatLng uav_pos = internals::PointLatLng(pos.x(), pos.y());	// current UAV position
+    float uav_heading = 0; //data.Heading;								// current UAV heading
+    float uav_altitude_meters = 0; //data.Altitude;							// current UAV height
+    float uav_ground_speed_meters_per_second = 0; //data.Groundspeed;				// current UAV ground speed
 
     // display the UAV lat/lon position
     QString str =   " lat: " + QString::number(uav_pos.Lat(), 'f', 7) +
@@ -1029,6 +1024,27 @@ void OPMapGadgetWidget::createActions()
     // ***********************
 }
 
+QPointF OPMapGadgetWidget::getLatLon()
+{
+    double BaseECEF[3];
+    double NED[3];
+    double LLA[3];
+    UAVObject *obj;
+
+    obj = dynamic_cast<UAVDataObject*>(m_objManager->getObject(QString("HomeLocation")));
+    BaseECEF[0] = obj->getField(QString("ECEF"))->getDouble(0);
+    BaseECEF[1] = obj->getField(QString("ECEF"))->getDouble(1);
+    BaseECEF[2] = obj->getField(QString("ECEF"))->getDouble(2);
+
+    obj = dynamic_cast<UAVDataObject*>(m_objManager->getObject(QString("PositionActual")));
+    NED[0] = obj->getField(QString("NED"))->getDouble(0);
+    NED[1] = obj->getField(QString("NED"))->getDouble(1);
+    NED[2] = obj->getField(QString("NED"))->getDouble(2);
+
+    Utils::CoordinateConversions().GetLLA(BaseECEF, NED, LLA);
+    return QPointF(LLA[0],LLA[1]);
+}
+
 void OPMapGadgetWidget::onReloadAct_triggered()
 {
     if (m_map)
@@ -1135,11 +1151,11 @@ void OPMapGadgetWidget::onGoHomeAct_triggered()
 
 void OPMapGadgetWidget::onGoUAVAct_triggered()
 {
-    PositionActual::DataFields data = m_positionActual->getData();				// get current UAV data
+    QPointF pos = getLatLon();
 
     if (m_map)
     {
-	internals::PointLatLng uav_pos = internals::PointLatLng(data.Latitude, data.Longitude);	// current UAV position
+        internals::PointLatLng uav_pos = internals::PointLatLng(pos.x(), pos.y());	// current UAV position
 	internals::PointLatLng map_pos = m_map->CurrentPosition();				// current MAP position
 	if (map_pos != uav_pos) m_map->SetCurrentPosition(uav_pos);				// center the map onto the UAV
     }
@@ -1147,13 +1163,13 @@ void OPMapGadgetWidget::onGoUAVAct_triggered()
 
 void OPMapGadgetWidget::onSetHomePosAct_triggered()
 {
-    PositionActual::DataFields data = m_positionActual->getData();				// get current UAV data
+    QPointF pos = getLatLon();
 
     if (m_map)
     {
-	internals::PointLatLng uav_pos = internals::PointLatLng(data.Latitude, data.Longitude);	// current UAV position
+        internals::PointLatLng uav_pos = internals::PointLatLng(pos.x(), pos.y());	// current UAV position
 	m_map->Home->SetCoord(uav_pos);								// move the HOME location to match the current UAV location
-	m_map->Home->SetAltitude(data.Altitude);
+        m_map->Home->SetAltitude(0);  // TODO: Update
     }
 }
 

ground/src/plugins/opmap/opmapgadgetwidget.h

@@ -34,11 +34,13 @@
 #include <QStandardItemModel>
 #include <QList>
 #include <QMutex>
+#include <QPointF>
 
 #include "opmapcontrol/opmapcontrol.h"
 
 #include "uavobjects/uavobjectmanager.h"
 #include "uavobjects/positionactual.h"
+#include "uavobjects/homelocation.h"
 #include "extensionsystem/pluginmanager.h"
 
 #include "opmap_overlay_widget.h"
@@ -47,6 +49,8 @@
 #include "opmap_waypointeditor_dialog.h"
 #include "opmap_edit_waypoint_dialog.h"
 
+#include "utils/coordinateconversions.h"
+
 namespace Ui
 {
     class OPMap_Widget;
@@ -178,6 +182,8 @@ private slots:
     void onZoomActGroup_triggered(QAction *action);
 
 private:
+    QPointF getLatLon();
+
     double m_heading;	// uav heading
 
     internals::PointLatLng mouse_lat_lon;
@@ -192,7 +198,6 @@ private:
 
     ExtensionSystem::PluginManager *m_plugin_manager;
     UAVObjectManager *m_objManager;
-    PositionActual *m_positionActual;
 
     Ui::OPMap_Widget *m_widget;
 

ground/src/plugins/uavobjects/uavobjectfield.cpp

@@ -540,7 +540,7 @@ void UAVObjectField::setValue(const QVariant& value, quint32 index)
 
 double UAVObjectField::getDouble(quint32 index)
 {
-    return getValue().toDouble();
+    return getValue(index).toDouble();
 }
 
 void UAVObjectField::setDouble(double value, quint32 index)