diff --git a/ground/src/libs/utils/coordinateconversions.cpp b/ground/src/libs/utils/coordinateconversions.cpp new file mode 100644 index 000000000..3b76a6336 --- /dev/null +++ b/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 + +#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; +} + +} diff --git a/ground/src/libs/utils/coordinateconversions.h b/ground/src/libs/utils/coordinateconversions.h new file mode 100644 index 000000000..fa3168931 --- /dev/null +++ b/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 */ diff --git a/ground/src/libs/utils/utils.pro b/ground/src/libs/utils/utils.pro index 3645cbb2a..cef33ca04 100644 --- a/ground/src/libs/utils/utils.pro +++ b/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 \ diff --git a/ground/src/plugins/opmap/opmap.pro b/ground/src/plugins/opmap/opmap.pro index a66d5352b..e0da62489 100644 --- a/ground/src/plugins/opmap/opmap.pro +++ b/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 \ diff --git a/ground/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/src/plugins/opmap/opmapgadgetwidget.cpp index 16d7a9fdf..48c40f8c6 100644 --- a/ground/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/src/plugins/opmap/opmapgadgetwidget.cpp @@ -27,7 +27,6 @@ #include "opmapgadgetwidget.h" #include "ui_opmap_widget.h" - #include #include #include @@ -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(); - 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(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(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 } } diff --git a/ground/src/plugins/opmap/opmapgadgetwidget.h b/ground/src/plugins/opmap/opmapgadgetwidget.h index b32c4ab78..5ad4f0004 100644 --- a/ground/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/src/plugins/opmap/opmapgadgetwidget.h @@ -34,11 +34,13 @@ #include #include #include +#include #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; diff --git a/ground/src/plugins/uavobjects/uavobjectfield.cpp b/ground/src/plugins/uavobjects/uavobjectfield.cpp index 85c615f56..520a17c58 100644 --- a/ground/src/plugins/uavobjects/uavobjectfield.cpp +++ b/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)