1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

LP-498 Optionally display currently active PathDesired in map

This commit is contained in:
Corvus Corax 2017-03-21 08:25:35 +01:00
parent 556c2e9e99
commit 8c3da1139b
9 changed files with 420 additions and 2 deletions

View File

@ -0,0 +1,126 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="1111.3383"
height="1168.5715"
id="svg2"
sodipodi:version="0.32"
inkscape:version="0.46"
version="1.0"
sodipodi:docname="antenna_and_radio_waves.svg"
inkscape:output_extension="org.inkscape.output.svg.inkscape">
<defs
id="defs4">
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 526.18109 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="744.09448 : 526.18109 : 1"
inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
id="perspective10" />
</defs>
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
gridtolerance="10000"
guidetolerance="10"
objecttolerance="10"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.0875"
inkscape:cx="3193.6065"
inkscape:cy="2405.053"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
showborder="false"
inkscape:window-width="1355"
inkscape:window-height="722"
inkscape:window-x="4"
inkscape:window-y="24" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(2201.53,2081.9236)">
<g
id="g2386"
transform="matrix(0.2535648,0,0,0.2535648,-1643.2995,-1554.0211)">
<path
transform="matrix(4.3333332,0,0,4.3333332,4533.7078,193.07844)"
d="M -582.85712,3.7907546 A 465.71429,465.71429 0 1 1 -1514.2857,3.7907546 A 465.71429,465.71429 0 1 1 -582.85712,3.7907546 z"
sodipodi:ry="465.71429"
sodipodi:rx="465.71429"
sodipodi:cy="3.7907546"
sodipodi:cx="-1048.5714"
id="path3187"
style="opacity:1;fill:#ffffff;fill-opacity:1;stroke:#000000;stroke-width:80;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
sodipodi:type="arc" />
<path
transform="matrix(2.9548022,0,0,2.9548022,3088.2196,198.30411)"
d="M -582.85712,3.7907546 A 465.71429,465.71429 0 1 1 -1514.2857,3.7907546 A 465.71429,465.71429 0 1 1 -582.85712,3.7907546 z"
sodipodi:ry="465.71429"
sodipodi:rx="465.71429"
sodipodi:cy="3.7907546"
sodipodi:cx="-1048.5714"
id="path3185"
style="opacity:1;fill:#ffffff;fill-opacity:1;stroke:#000000;stroke-width:80;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
sodipodi:type="arc" />
<path
transform="matrix(1.8248587,0,0,1.8248587,1903.3932,202.58745)"
d="M -582.85712,3.7907546 A 465.71429,465.71429 0 1 1 -1514.2857,3.7907546 A 465.71429,465.71429 0 1 1 -582.85712,3.7907546 z"
sodipodi:ry="465.71429"
sodipodi:rx="465.71429"
sodipodi:cy="3.7907546"
sodipodi:cx="-1048.5714"
id="path3183"
style="opacity:1;fill:#ffffff;fill-opacity:1;stroke:#000000;stroke-width:80;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
sodipodi:type="arc" />
<path
transform="translate(1038.4699,205.71429)"
d="M -582.85712,3.7907546 A 465.71429,465.71429 0 1 1 -1514.2857,3.7907546 A 465.71429,465.71429 0 1 1 -582.85712,3.7907546 z"
sodipodi:ry="465.71429"
sodipodi:rx="465.71429"
sodipodi:cy="3.7907546"
sodipodi:cx="-1048.5714"
id="path3181"
style="opacity:1;fill:#ffffff;fill-opacity:1;stroke:#000000;stroke-width:80;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
sodipodi:type="arc" />
<path
sodipodi:nodetypes="cccc"
id="path3192"
d="M -7.2443685,220.93362 L 1504.1842,-2081.9236 L -1524.3873,-2081.9236 L -7.2443685,220.93362 z"
style="fill:#ffffff;fill-opacity:1;stroke:none;stroke-width:80;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1" />
<path
sodipodi:nodetypes="cccc"
id="rect3189"
d="M -7.2442174,223.79075 L 1504.1842,2526.6479 L -1524.3873,2526.6479 L -7.2442174,223.79075 z"
style="fill:#ffffff;fill-opacity:1;stroke:none;stroke-width:80;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1" />
<path
sodipodi:nodetypes="ccccccsc"
id="path2385"
d="M 131.32707,222.36219 C 129.85733,280.3008 96.457805,328.45177 44.931875,352.67094 C 36.472685,356.25085 126.39529,2164.396 129.8183,2222.3445 C -5.6975961,2222.8383 -8.9361161,2222.5375 -151.03146,2223.1095 C -148.6186,2165.161 -56.675715,356.25085 -65.13491,352.67094 C -118.03418,328.65681 -149.22461,279.34006 -151.53008,222.36219 C -151.53008,144.29362 -88.17009,80.933613 -10.101515,80.933613 C 67.967055,80.933613 131.32707,144.29362 131.32707,222.36219 z"
style="fill:#000000;fill-opacity:1;stroke:none;stroke-width:3;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1" />
</g>
</g>
</svg>

After

Width:  |  Height:  |  Size: 5.7 KiB

View File

@ -6,6 +6,7 @@
<file>images/airplane.svg</file>
<file>images/home.png</file>
<file>images/home.svg</file>
<file>images/nav.svg</file>
<file>images/home2.svg</file>
<file>images/airplanepip.png</file>
<file>images/EasystarBlue.png</file>

View File

@ -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 \

View File

@ -0,0 +1,99 @@
/**
******************************************************************************
*
* @file navitem.cpp
* @author The LibrePilot Team, http://www.openpilot.org Copyright (C) 2017.
* @brief A graphicsItem representing a trail point
* @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 <QGraphicsSceneMouseEvent>
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;
}
}

View File

@ -0,0 +1,85 @@
/**
******************************************************************************
*
* @file navitem.h
* @author The LibrePilot Team, 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 <QGraphicsItem>
#include <QPainter>
#include <QLabel>
#include "../internals/pointlatlng.h"
#include <QObject>
#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

View File

@ -33,8 +33,8 @@
#include <QOpenGLWidget>
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;
@ -44,6 +44,9 @@ OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config) : QGraphicsView
Home = new HomeItem(map, this);
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 +61,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 +162,10 @@ void OPMapWidget::SetShowHome(const bool &value)
{
Home->setVisible(value);
}
void OPMapWidget::SetShowNav(const bool &value)
{
Nav->setVisible(value);
}
void OPMapWidget::resizeEvent(QResizeEvent *event)
{

View File

@ -37,6 +37,7 @@
#include "uavitem.h"
#include "gpsitem.h"
#include "homeitem.h"
#include "navitem.h"
#include "mapripper.h"
#include "waypointline.h"
#include "waypointcircle.h"
@ -50,6 +51,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 +502,7 @@ public:
UAVItem *UAV;
GPSItem *GPS;
HomeItem *Home;
NavItem *Nav;
void SetShowUAV(bool const & value);
bool ShowUAV() const
{
@ -510,6 +513,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 +548,7 @@ private:
QTimer *diagTimer;
QGraphicsTextItem *diagGraphItem;
bool showDiag;
bool showNav;
qreal overlayOpacity;
private slots:
void diagRefresh();

View File

@ -50,6 +50,7 @@
#include "uavobject.h"
#include "positionstate.h"
#include "pathdesired.h"
#include "homelocation.h"
#include "gpspositionsensor.h"
#include "gyrostate.h"
@ -166,6 +167,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 +215,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 +290,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 +446,8 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event)
contextMenu.addAction(showDiagnostics);
contextMenu.addAction(showNav);
contextMenu.addAction(showUAVInfo);
// Zoom section
@ -670,6 +676,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 +1363,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 +1672,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 +2270,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) {

View File

@ -174,6 +174,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 +249,7 @@ private:
QAction *copyMouseLonToClipAct;
QAction *showCompassAct;
QAction *showDiagnostics;
QAction *showNav;
QAction *showUAVInfo;
QAction *showHomeAct;
QAction *showUAVAct;
@ -308,6 +310,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();