diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/nav.svg b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/nav.svg new file mode 100644 index 000000000..cf98903e0 --- /dev/null +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/images/nav.svg @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h index f14c43fbc..11e778e8a 100644 --- a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file mapgraphicitem.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief The main graphicsItem used on the widget, contains the map and map logic * @see The GNU Public License (GPL) Version 3 * @defgroup OPMapWidget @@ -43,6 +44,7 @@ namespace mapcontrol { class WayPointItem; class OPMapWidget; +class HomeItem; /** * @brief The main graphicsItem used on the widget, contains the map and map logic * @@ -105,6 +107,8 @@ public: double ZoomDigi(); double ZoomTotal(); void setOverlayOpacity(qreal value); + + HomeItem *Home; protected: void mouseMoveEvent(QGraphicsSceneMouseEvent *event); void mousePressEvent(QGraphicsSceneMouseEvent *event); diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc index b7709e2cc..3e0c53a0e 100644 --- a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapresources.qrc @@ -6,6 +6,7 @@ images/airplane.svg images/home.png images/home.svg + images/nav.svg images/home2.svg images/airplanepip.png images/EasystarBlue.png diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro index 5d4e3e346..1af31f4a6 100644 --- a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/mapwidget.pro @@ -12,6 +12,7 @@ SOURCES += mapgraphicitem.cpp \ gpsitem.cpp \ trailitem.cpp \ homeitem.cpp \ + navitem.cpp \ mapripform.cpp \ mapripper.cpp \ traillineitem.cpp \ @@ -39,6 +40,7 @@ HEADERS += mapgraphicitem.h \ uavtrailtype.h \ trailitem.h \ homeitem.h \ + navitem.h \ mapripform.h \ mapripper.h \ traillineitem.h \ diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/navitem.cpp b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/navitem.cpp new file mode 100644 index 000000000..1820135c5 --- /dev/null +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/navitem.cpp @@ -0,0 +1,99 @@ +/** + ****************************************************************************** + * + * @file navitem.cpp + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * 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 "navitem.h" +#include +namespace mapcontrol { +NavItem::NavItem(MapGraphicItem *map, OPMapWidget *parent) : map(map), mapwidget(parent), + toggleRefresh(true), altitude(0) +{ + pic.load(QString::fromUtf8(":/markers/images/nav.svg")); + pic = pic.scaled(30, 30, Qt::IgnoreAspectRatio); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations, true); + this->setFlag(QGraphicsItem::ItemIsMovable, false); + this->setFlag(QGraphicsItem::ItemIsSelectable, true); + localposition = map->FromLatLngToLocal(mapwidget->CurrentPosition()); + this->setPos(localposition.X(), localposition.Y()); + this->setZValue(4); + coord = internals::PointLatLng(50, 50); + RefreshToolTip(); + setCacheMode(QGraphicsItem::DeviceCoordinateCache); + connect(map, SIGNAL(childRefreshPosition()), this, SLOT(RefreshPos())); + connect(map, SIGNAL(childSetOpacity(qreal)), this, SLOT(setOpacitySlot(qreal))); +} + +void NavItem::RefreshToolTip() +{ + QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); + + setToolTip(QString("Waypoint: Nav\nCoordinate:%1\nAltitude:%2\n").arg(coord_str).arg(QString::number(altitude))); +} + + +void NavItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + Q_UNUSED(option); + Q_UNUSED(widget); + painter->drawPixmap(-pic.width() / 2, -pic.height() / 2, pic); +} +QRectF NavItem::boundingRect() const +{ + return QRectF(-pic.width() / 2, -pic.height() / 2, pic.width(), pic.height()); +} + + +int NavItem::type() const +{ + return Type; +} + +void NavItem::RefreshPos() +{ + prepareGeometryChange(); + localposition = map->FromLatLngToLocal(coord); + this->setPos(localposition.X(), localposition.Y()); + + RefreshToolTip(); + + this->update(); + toggleRefresh = false; +} + +void NavItem::setOpacitySlot(qreal opacity) +{ + setOpacity(opacity); +} + + +// Set clickable area as smaller than the bounding rect. +QPainterPath NavItem::shape() const +{ + QPainterPath path; + + path.addEllipse(QRectF(-12, -25, 24, 50)); + return path; +} +} diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/navitem.h b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/navitem.h new file mode 100644 index 000000000..c367740e1 --- /dev/null +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/navitem.h @@ -0,0 +1,85 @@ +/** + ****************************************************************************** + * + * @file navitem.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief A graphicsItem representing a WayPoint + * @see The GNU Public License (GPL) Version 3 + * @defgroup OPMapWidget + * @{ + * + *****************************************************************************/ +/* + * 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 NAVITEM_H +#define NAVITEM_H + +#include +#include +#include +#include "../internals/pointlatlng.h" +#include +#include "opmapwidget.h" +namespace mapcontrol { +class NavItem : public QObject, public QGraphicsItem { + Q_OBJECT Q_INTERFACES(QGraphicsItem) +public: + enum { Type = UserType + 4 }; + NavItem(MapGraphicItem *map, OPMapWidget *parent); + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, + QWidget *widget); + QRectF boundingRect() const; + int type() const; + void SetToggleRefresh(bool const & value) + { + toggleRefresh = value; + } + void SetCoord(internals::PointLatLng const & value) + { + coord = value; emit navPositionChanged(value, Altitude()); + } + internals::PointLatLng Coord() const + { + return coord; + } + void SetAltitude(float const & value) + { + altitude = value; emit navPositionChanged(Coord(), Altitude()); + } + float Altitude() const + { + return altitude; + } + void RefreshToolTip(); +private: + + MapGraphicItem *map; + OPMapWidget *mapwidget; + QPixmap pic; + core::Point localposition; + internals::PointLatLng coord; + bool toggleRefresh; + float altitude; +protected: + QPainterPath shape() const; +public slots: + void RefreshPos(); + void setOpacitySlot(qreal opacity); +signals: + void navPositionChanged(internals::PointLatLng coord, float); +}; +} +#endif // NAVITEM_H diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index f38632da4..ea1301a24 100644 --- a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file opmapwidget.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief The Map Widget, this is the part exposed to the user * @see The GNU Public License (GPL) Version 3 * @defgroup OPMapWidget @@ -33,8 +34,8 @@ #include namespace mapcontrol { -OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config) : QGraphicsView(parent), configuration(config), UAV(0), GPS(0), Home(0) - , followmouse(true), compass(0), showuav(false), showhome(false), diagTimer(0), diagGraphItem(0), showDiag(false), overlayOpacity(1) +OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config) : QGraphicsView(parent), configuration(config), UAV(0), GPS(0), Home(0), Nav(0) + , followmouse(true), compass(0), showuav(false), showhome(false), diagTimer(0), diagGraphItem(0), showDiag(false), showNav(false), overlayOpacity(1) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); core = new internals::Core; @@ -42,8 +43,12 @@ OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config) : QGraphicsView mscene.addItem(map); this->setScene(&mscene); Home = new HomeItem(map, this); + map->Home = Home; Home->setParentItem(map); Home->setZValue(-1); + Nav = new NavItem(map, this); + Nav->setParentItem(map); + Nav->setZValue(-1); setStyleSheet("QToolTip {font-size:8pt; color:blue;opacity: 223; padding:2px; border-width:2px; border-style:solid; border-color: rgb(170, 170, 127);border-radius:4px }"); this->adjustSize(); connect(map, SIGNAL(zoomChanged(double, double, double)), this, SIGNAL(zoomChanged(double, double, double))); @@ -58,6 +63,7 @@ OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config) : QGraphicsView connect(map, SIGNAL(wpdoubleclicked(WayPointItem *)), this, SIGNAL(OnWayPointDoubleClicked(WayPointItem *))); connect(&mscene, SIGNAL(selectionChanged()), this, SLOT(OnSelectionChanged())); SetShowDiagnostics(showDiag); + SetShowNav(showNav); this->setMouseTracking(followmouse); SetShowCompass(true); QPixmapCache::setCacheLimit(64 * 1024); @@ -158,6 +164,10 @@ void OPMapWidget::SetShowHome(const bool &value) { Home->setVisible(value); } +void OPMapWidget::SetShowNav(const bool &value) +{ + Nav->setVisible(value); +} void OPMapWidget::resizeEvent(QResizeEvent *event) { diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h index 4f7d9d7fc..af79774c7 100644 --- a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file opmapwidget.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief The Map Widget, this is the part exposed to the user * @see The GNU Public License (GPL) Version 3 * @defgroup OPMapWidget @@ -37,6 +38,7 @@ #include "uavitem.h" #include "gpsitem.h" #include "homeitem.h" +#include "navitem.h" #include "mapripper.h" #include "waypointline.h" #include "waypointcircle.h" @@ -50,6 +52,7 @@ namespace mapcontrol { class UAVItem; class GPSItem; class HomeItem; +class NavItem; /** * @brief Collection of static functions to help dealing with various enums used * Contains functions for enumToString conversio, StringToEnum, QStringList of enum values... @@ -500,6 +503,7 @@ public: UAVItem *UAV; GPSItem *GPS; HomeItem *Home; + NavItem *Nav; void SetShowUAV(bool const & value); bool ShowUAV() const { @@ -510,6 +514,11 @@ public: { return showhome; } + void SetShowNav(bool const & value); + bool ShowNav() const + { + return showNav; + } void SetShowDiagnostics(bool const & value); void SetUavPic(QString UAVPic); WayPointLine *WPLineCreate(WayPointItem *from, WayPointItem *to, QColor color, bool dashed = false, int width = -1); @@ -540,6 +549,7 @@ private: QTimer *diagTimer; QGraphicsTextItem *diagGraphItem; bool showDiag; + bool showNav; qreal overlayOpacity; private slots: void diagRefresh(); diff --git a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp index 7a187685c..010a5c393 100644 --- a/ground/gcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp +++ b/ground/gcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file waypointitem.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief A graphicsItem representing a WayPoint * @see The GNU Public License (GPL) Version 3 * @defgroup OPMapWidget @@ -43,16 +44,7 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & alti SetShowNumber(shownumber); RefreshToolTip(); RefreshPos(); - myHome = NULL; - QList list = map->childItems(); - foreach(QGraphicsItem * obj, list) { - HomeItem *h = qgraphicsitem_cast (obj); - - if (h) { - myHome = h; - } - } - + myHome = map->Home; if (myHome) { map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); relativeCoord.altitudeRelative = Altitude() - myHome->Altitude(); @@ -87,16 +79,7 @@ WayPointItem::WayPointItem(MapGraphicItem *map, bool magicwaypoint) : reached(fa SetShowNumber(shownumber); RefreshToolTip(); RefreshPos(); - myHome = NULL; - QList list = map->childItems(); - foreach(QGraphicsItem * obj, list) { - HomeItem *h = qgraphicsitem_cast (obj); - - if (h) { - myHome = h; - } - } - + myHome = map->Home; if (myHome) { coord = map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing); SetAltitude(myHome->Altitude() + relativeCoord.altitudeRelative); @@ -121,15 +104,7 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & alti SetShowNumber(shownumber); RefreshToolTip(); RefreshPos(); - myHome = NULL; - QList list = map->childItems(); - foreach(QGraphicsItem * obj, list) { - HomeItem *h = qgraphicsitem_cast (obj); - - if (h) { - myHome = h; - } - } + myHome = map->Home; if (myHome) { map->Projection()->offSetFromLatLngs(myHome->Coord(), coord, relativeCoord.distance, relativeCoord.bearing); relativeCoord.altitudeRelative = Altitude() - myHome->Altitude(); @@ -143,15 +118,7 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord, int const & alti WayPointItem::WayPointItem(const distBearingAltitude &relativeCoordenate, const QString &description, MapGraphicItem *map) : relativeCoord(relativeCoordenate), reached(false), description(description), shownumber(true), isDragging(false), map(map) { - myHome = NULL; - QList list = map->childItems(); - foreach(QGraphicsItem * obj, list) { - HomeItem *h = qgraphicsitem_cast (obj); - - if (h) { - myHome = h; - } - } + myHome = map->Home; if (myHome) { connect(myHome, SIGNAL(homePositionChanged(internals::PointLatLng, float)), this, SLOT(onHomePositionChanged(internals::PointLatLng, float))); coord = map->Projection()->translate(myHome->Coord(), relativeCoord.distance, relativeCoord.bearing); diff --git a/ground/gcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/gcs/src/plugins/opmap/opmapgadgetwidget.cpp index 0b6994bfb..8db51bbda 100644 --- a/ground/gcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/gcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -2,7 +2,8 @@ ****************************************************************************** * * @file opmapgadgetwidget.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin @@ -50,6 +51,7 @@ #include "uavobject.h" #include "positionstate.h" +#include "pathdesired.h" #include "homelocation.h" #include "gpspositionsensor.h" #include "gyrostate.h" @@ -166,6 +168,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) m_map->SetShowHome(true); // display the HOME position on the map m_map->SetShowUAV(true); // display the UAV position on the map + m_map->SetShowNav(false); // initially don't display the NAV position on the map m_map->Home->SetSafeArea(safe_area_radius_list[0]); // set radius (meters) //SHOULDN'T THE DEFAULT BE USER DEFINED? m_map->Home->SetShowSafeArea(true); // show the safe area //SHOULDN'T THE DEFAULT BE USER DEFINED? @@ -213,6 +216,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_map, SIGNAL(OnWayPointDoubleClicked(WayPointItem *)), this, SLOT(wpDoubleClickEvent(WayPointItem *))); m_map->SetCurrentPosition(m_home_position.coord); // set the map position m_map->Home->SetCoord(m_home_position.coord); // set the HOME position + m_map->Nav->SetCoord(m_home_position.coord); // set the NAV position m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position m_map->UAV->update(); if (m_map->GPS) { @@ -287,6 +291,7 @@ OPMapGadgetWidget::~OPMapGadgetWidget() disconnect(m_map, 0, 0, 0); m_map->SetShowHome(false); // doing this appears to stop the map lib crashing on exit m_map->SetShowUAV(false); // " " + m_map->SetShowNav(false); // " " } if (m_map) { @@ -442,6 +447,8 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) contextMenu.addAction(showDiagnostics); + contextMenu.addAction(showNav); + contextMenu.addAction(showUAVInfo); // Zoom section @@ -670,6 +677,19 @@ void OPMapGadgetWidget::updatePosition() m_map->UAV->updateTextOverlay(); m_map->UAV->update(); // ************* + + // ************* + // update active waypoint position at same update rate + if (m_map->Nav) { + double latitude, longitude, altitude; + getNavPosition(latitude, longitude, altitude); + m_map->Nav->SetCoord(internals::PointLatLng(latitude, longitude)); // set the maps Nav position + m_map->Nav->SetAltitude(altitude); + m_map->Nav->RefreshPos(); + m_map->Nav->update(); + } + m_map->UAV->updateTextOverlay(); + m_map->UAV->update(); } /** @@ -1344,6 +1364,12 @@ void OPMapGadgetWidget::createActions() showDiagnostics->setChecked(false); connect(showDiagnostics, SIGNAL(toggled(bool)), this, SLOT(onShowDiagnostics_toggled(bool))); + showNav = new QAction(tr("Show Nav"), this); + showNav->setStatusTip(tr("Show/Hide pathfollower info")); + showNav->setCheckable(true); + showNav->setChecked(false); + connect(showNav, SIGNAL(toggled(bool)), this, SLOT(onShowNav_toggled(bool))); + showUAVInfo = new QAction(tr("Show UAV Info"), this); showUAVInfo->setStatusTip(tr("Show/Hide the UAV info")); showUAVInfo->setCheckable(true); @@ -1647,6 +1673,15 @@ void OPMapGadgetWidget::onShowDiagnostics_toggled(bool show) m_map->SetShowDiagnostics(show); } +void OPMapGadgetWidget::onShowNav_toggled(bool show) +{ + if (!m_widget || !m_map) { + return; + } + + m_map->SetShowNav(show); +} + void OPMapGadgetWidget::onShowUAVInfo_toggled(bool show) { if (!m_widget || !m_map) { @@ -2236,6 +2271,57 @@ bool OPMapGadgetWidget::getUAVPosition(double &latitude, double &longitude, doub return true; } +bool OPMapGadgetWidget::getNavPosition(double &latitude, double &longitude, double &altitude) +{ + double NED[3]; + double LLA[3]; + double homeLLA[3]; + + Q_ASSERT(obm != NULL); + + PathDesired *pathDesired = PathDesired::GetInstance(obm); + Q_ASSERT(pathDesired != NULL); + PathDesired::DataFields pathDesiredData = pathDesired->getData(); + HomeLocation *homeLocation = HomeLocation::GetInstance(obm); + Q_ASSERT(homeLocation != NULL); + HomeLocation::DataFields homeLocationData = homeLocation->getData(); + + homeLLA[0] = homeLocationData.Latitude / 1.0e7; + homeLLA[1] = homeLocationData.Longitude / 1.0e7; + homeLLA[2] = homeLocationData.Altitude; + + NED[0] = pathDesiredData.End[0]; + NED[1] = pathDesiredData.End[1]; + NED[2] = pathDesiredData.End[2]; + + Utils::CoordinateConversions().NED2LLA_HomeLLA(homeLLA, NED, LLA); + + latitude = LLA[0]; + longitude = LLA[1]; + altitude = LLA[2]; + + if (latitude != latitude) { + latitude = 0; // nan detection + } else if (latitude > 90) { + latitude = 90; + } else if (latitude < -90) { + latitude = -90; + } + + if (longitude != longitude) { + longitude = 0; // nan detection + } else if (longitude > 180) { + longitude = 180; + } else if (longitude < -180) { + longitude = -180; + } + + if (altitude != altitude) { + altitude = 0; // nan detection + } + return true; +} + double OPMapGadgetWidget::getUAV_Yaw() { if (!obm) { diff --git a/ground/gcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/gcs/src/plugins/opmap/opmapgadgetwidget.h index 13d597258..02aa9d6f1 100644 --- a/ground/gcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/gcs/src/plugins/opmap/opmapgadgetwidget.h @@ -2,7 +2,8 @@ ****************************************************************************** * * @file opmapgadgetwidget.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup OPMapPlugin OpenPilot Map Plugin @@ -174,6 +175,7 @@ private slots: void onCopyMouseLonToClipAct_triggered(); void onShowCompassAct_toggled(bool show); void onShowDiagnostics_toggled(bool show); + void onShowNav_toggled(bool show); void onShowUAVInfo_toggled(bool show); void onShowUAVAct_toggled(bool show); void onShowHomeAct_toggled(bool show); @@ -248,6 +250,7 @@ private: QAction *copyMouseLonToClipAct; QAction *showCompassAct; QAction *showDiagnostics; + QAction *showNav; QAction *showUAVInfo; QAction *showHomeAct; QAction *showUAVAct; @@ -308,6 +311,7 @@ private: internals::PointLatLng destPoint(internals::PointLatLng source, double bear, double dist); bool getUAVPosition(double &latitude, double &longitude, double &altitude); + bool getNavPosition(double &latitude, double &longitude, double &altitude); double getUAV_Yaw(); void setMapFollowingMode();