Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
8c6c0741
Commit
8c6c0741
authored
Jun 27, 2011
by
pixhawk
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Added full-fledged multi-wp type interaction. WP-connecting line still missing.
parent
e313f6c4
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
326 additions
and
118 deletions
+326
-118
qgroundcontrol.pro
qgroundcontrol.pro
+2
-3
WaypointView.cc
src/ui/WaypointView.cc
+13
-12
MAV2DIcon.h
src/ui/map/MAV2DIcon.h
+0
-1
QGCMapWidget.cc
src/ui/map/QGCMapWidget.cc
+124
-21
QGCMapWidget.h
src/ui/map/QGCMapWidget.h
+11
-0
Waypoint2DIcon.cc
src/ui/map/Waypoint2DIcon.cc
+154
-52
Waypoint2DIcon.h
src/ui/map/Waypoint2DIcon.h
+22
-29
No files found.
qgroundcontrol.pro
View file @
8c6c0741
...
...
@@ -302,11 +302,10 @@ HEADERS += src/MG.h \
src
/
uas
/
QGCUASParamManager
.
h
\
src
/
ui
/
map
/
QGCMapWidget
.
h
\
src
/
ui
/
map
/
MAV2DIcon
.
h
\
src
/
ui
/
map
/
Waypoint2DIcon
.
h
\
src
/
ui
/
mavlink
/
QGCMAVLinkTextEdit
.
h
\
src
/
ui
/
map
/
QGCMapToolbar
.
h
#
src
/
ui
/
map
/
Waypoint2DIcon
.
h
\
#
Google
Earth
is
only
supported
on
Mac
OS
and
Windows
with
Visual
Studio
Compiler
macx
|
win32
-
msvc2008
::
HEADERS
+=
src
/
ui
/
map3D
/
QGCGoogleEarthView
.
h
contains
(
DEPENDENCIES_PRESENT
,
osg
)
{
...
...
@@ -431,9 +430,9 @@ SOURCES += src/main.cc \
src
/
uas
/
QGCUASParamManager
.
cc
\
src
/
ui
/
map
/
QGCMapWidget
.
cc
\
src
/
ui
/
map
/
MAV2DIcon
.
cc
\
src
/
ui
/
map
/
Waypoint2DIcon
.
cc
\
src
/
ui
/
mavlink
/
QGCMAVLinkTextEdit
.
cc
\
src
/
ui
/
map
/
QGCMapToolbar
.
cc
#
src
/
ui
/
map
/
Waypoint2DIcon
.
cc
macx
|
win32
-
msvc2008
::
SOURCES
+=
src
/
ui
/
map3D
/
QGCGoogleEarthView
.
cc
contains
(
DEPENDENCIES_PRESENT
,
osg
)
{
message
(
"Including sources for OpenSceneGraph"
)
...
...
src/ui/WaypointView.cc
View file @
8c6c0741
...
...
@@ -427,18 +427,19 @@ void WaypointView::updateValues()
}
}
}
switch
(
action
)
{
case
MAV_CMD_NAV_TAKEOFF
:
break
;
case
MAV_CMD_NAV_LAND
:
break
;
case
MAV_CMD_NAV_WAYPOINT
:
break
;
case
MAV_CMD_NAV_LOITER_UNLIM
:
break
;
default:
std
::
cerr
<<
"unknown action"
<<
std
::
endl
;
}
// Do something on actions - currently unused
// switch(action) {
// case MAV_CMD_NAV_TAKEOFF:
// break;
// case MAV_CMD_NAV_LAND:
// break;
// case MAV_CMD_NAV_WAYPOINT:
// break;
// case MAV_CMD_NAV_LOITER_UNLIM:
// break;
// default:
// std::cerr << "unknown action" << std::endl;
// }
if
(
m_ui
->
yawSpinBox
->
value
()
!=
wp
->
getYaw
())
{
if
(
!
m_ui
->
yawSpinBox
->
isVisible
())
m_ui
->
yawSpinBox
->
blockSignals
(
true
);
...
...
src/ui/map/MAV2DIcon.h
View file @
8c6c0741
...
...
@@ -77,7 +77,6 @@ protected:
bool
selected
;
///< Wether this is the system currently in focus
int
uasid
;
///< ID of tracked system
QPen
*
mypen
;
QPixmap
*
mypixmap
;
QSize
size
;
};
...
...
src/ui/map/QGCMapWidget.cc
View file @
8c6c0741
...
...
@@ -3,12 +3,14 @@
#include "UASInterface.h"
#include "UASManager.h"
#include "MAV2DIcon.h"
#include "Waypoint2DIcon.h"
#include "UASWaypointManager.h"
QGCMapWidget
::
QGCMapWidget
(
QWidget
*
parent
)
:
mapcontrol
::
OPMapWidget
(
parent
),
currWPManager
(
NULL
),
firingWaypointChange
(
NULL
)
firingWaypointChange
(
NULL
),
maxUpdateInterval
(
2
)
// 2 seconds
{
connect
(
UASManager
::
instance
(),
SIGNAL
(
UASCreated
(
UASInterface
*
)),
this
,
SLOT
(
addUAS
(
UASInterface
*
)));
connect
(
UASManager
::
instance
(),
SIGNAL
(
activeUASSet
(
UASInterface
*
)),
this
,
SLOT
(
activeUASSet
(
UASInterface
*
)));
...
...
@@ -88,6 +90,10 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) :
setFocus
();
// Start timer
connect
(
&
updateTimer
,
SIGNAL
(
timeout
()),
this
,
SLOT
(
updateGlobalPosition
()));
updateTimer
.
start
(
maxUpdateInterval
*
1000
);
}
QGCMapWidget
::~
QGCMapWidget
()
...
...
@@ -157,22 +163,53 @@ void QGCMapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lo
{
Q_UNUSED
(
usec
);
// Get reference to graphic UAV item
mapcontrol
::
UAVItem
*
uav
=
GetUAV
(
uas
->
getUASID
());
// Check if reference is valid, else create a new one
if
(
uav
==
NULL
)
// Immediate update
if
(
maxUpdateInterval
==
0
)
{
MAV2DIcon
*
newUAV
=
new
MAV2DIcon
(
map
,
this
,
uas
);
newUAV
->
setParentItem
(
map
);
UAVS
.
insert
(
uas
->
getUASID
(),
newUAV
);
uav
=
GetUAV
(
uas
->
getUASID
());
// Get reference to graphic UAV item
mapcontrol
::
UAVItem
*
uav
=
GetUAV
(
uas
->
getUASID
());
// Check if reference is valid, else create a new one
if
(
uav
==
NULL
)
{
MAV2DIcon
*
newUAV
=
new
MAV2DIcon
(
map
,
this
,
uas
);
newUAV
->
setParentItem
(
map
);
UAVS
.
insert
(
uas
->
getUASID
(),
newUAV
);
uav
=
GetUAV
(
uas
->
getUASID
());
}
// Set new lat/lon position of UAV icon
internals
::
PointLatLng
pos_lat_lon
=
internals
::
PointLatLng
(
lat
,
lon
);
uav
->
SetUAVPos
(
pos_lat_lon
,
alt
);
// Convert from radians to degrees and apply
uav
->
SetUAVHeading
((
uas
->
getYaw
()
/
M_PI
)
*
180.0
f
);
}
}
// Set new lat/lon position of UAV icon
internals
::
PointLatLng
pos_lat_lon
=
internals
::
PointLatLng
(
lat
,
lon
);
uav
->
SetUAVPos
(
pos_lat_lon
,
alt
);
// Convert from radians to degrees and apply
uav
->
SetUAVHeading
((
uas
->
getYaw
()
/
M_PI
)
*
180.0
f
);
/**
* Pulls in the positions of all UAVs from the UAS manager
*/
void
QGCMapWidget
::
updateGlobalPosition
()
{
QList
<
UASInterface
*>
systems
=
UASManager
::
instance
()
->
getUASList
();
foreach
(
UASInterface
*
system
,
systems
)
{
// Get reference to graphic UAV item
mapcontrol
::
UAVItem
*
uav
=
GetUAV
(
system
->
getUASID
());
// Check if reference is valid, else create a new one
if
(
uav
==
NULL
)
{
MAV2DIcon
*
newUAV
=
new
MAV2DIcon
(
map
,
this
,
system
);
newUAV
->
setParentItem
(
map
);
UAVS
.
insert
(
system
->
getUASID
(),
newUAV
);
uav
=
GetUAV
(
system
->
getUASID
());
}
// Set new lat/lon position of UAV icon
internals
::
PointLatLng
pos_lat_lon
=
internals
::
PointLatLng
(
system
->
getLatitude
(),
system
->
getLongitude
());
uav
->
SetUAVPos
(
pos_lat_lon
,
system
->
getAltitude
());
// Convert from radians to degrees and apply
uav
->
SetUAVHeading
((
system
->
getYaw
()
/
M_PI
)
*
180.0
f
);
}
}
...
...
@@ -239,6 +276,16 @@ void QGCMapWidget::updateHomePosition(double latitude, double longitude, double
SetShowHome
(
true
);
// display the HOME position on the map
}
/**
* Limits the update rate on the specified interval. Set to zero (0) to run at maximum
* telemetry speed. Recommended rate is 2 s.
*/
void
QGCMapWidget
::
setUpdateRateLimit
(
float
seconds
)
{
maxUpdateInterval
=
seconds
;
updateTimer
.
start
(
maxUpdateInterval
*
1000
);
}
// WAYPOINT MAP INTERACTION FUNCTIONS
...
...
@@ -288,6 +335,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
// note the call to getGlobalFrameAndNavTypeIndexOf()
// as we're only handling global waypoints
int
wpindex
=
UASManager
::
instance
()
->
getUASForId
(
uas
)
->
getWaypointManager
()
->
getGlobalFrameAndNavTypeIndexOf
(
wp
);
UASInterface
*
uasInstance
=
UASManager
::
instance
()
->
getUASForId
(
uas
);
// If not found, return (this should never happen, but helps safety)
if
(
wpindex
==
-
1
)
return
;
// Mark this wp as currently edited
...
...
@@ -296,9 +344,9 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
// Check if wp exists yet in map
if
(
!
waypointsToIcons
.
contains
(
wp
))
{
// Create icon for new WP
mapcontrol
::
WayPointItem
*
icon
=
WPCreate
(
internals
::
PointLatLng
(
wp
->
getLatitude
(),
wp
->
getLongitude
()),
wp
->
getAltitude
(),
wp
->
getDescription
()
);
icon
->
SetHeading
(
wp
->
getYaw
()
);
icon
->
SetNumber
(
wpindex
);
Waypoint2DIcon
*
icon
=
new
Waypoint2DIcon
(
map
,
this
,
wp
,
uasInstance
->
getColor
(),
wpindex
);
ConnectWP
(
icon
);
icon
->
setParentItem
(
map
);
// Update maps to allow inverse data association
waypointsToIcons
.
insert
(
wp
,
icon
);
iconsToWaypoints
.
insert
(
icon
,
wp
);
...
...
@@ -312,13 +360,25 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
// should not happen, just a precaution
this
->
blockSignals
(
true
);
// Update the WP
icon
->
SetCoord
(
internals
::
PointLatLng
(
wp
->
getLatitude
(),
wp
->
getLongitude
()));
icon
->
SetAltitude
(
wp
->
getAltitude
());
icon
->
SetHeading
(
wp
->
getYaw
());
icon
->
SetNumber
(
wpindex
);
Waypoint2DIcon
*
wpicon
=
dynamic_cast
<
Waypoint2DIcon
*>
(
icon
);
if
(
wpicon
)
{
// Let icon read out values directly from waypoint
icon
->
SetNumber
(
wpindex
);
wpicon
->
updateWaypoint
();
}
else
{
// Use safe standard interfaces for non Waypoint-class based wps
icon
->
SetCoord
(
internals
::
PointLatLng
(
wp
->
getLatitude
(),
wp
->
getLongitude
()));
icon
->
SetAltitude
(
wp
->
getAltitude
());
icon
->
SetHeading
(
wp
->
getYaw
());
icon
->
SetNumber
(
wpindex
);
}
// Re-enable signals again
this
->
blockSignals
(
false
);
}
firingWaypointChange
=
NULL
;
}
else
{
...
...
@@ -428,3 +488,46 @@ void QGCMapWidget::updateWaypointList(int uas)
// }
}
}
//// ADAPTER / HELPER FUNCTIONS
//float QGCMapWidget::metersToPixels(double meters)
//{
// return meters/map->Projection()->GetGroundResolution(map->ZoomTotal(),coord.Lat());
//}
//double QGCMapWidget::headingP1P2(internals::PointLatLng p1, internals::PointLatLng p2)
//{
// double lat1 = p1.Lat() * deg_to_rad;
// double lon1 = p2.Lng() * deg_to_rad;
// double lat2 = p2.Lat() * deg_to_rad;
// double lon2 = p2.Lng() * deg_to_rad;
// double delta_lon = lon2 - lon1;
// double y = sin(delta_lon) * cos(lat2);
// double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(delta_lon);
// double heading = atan2(y, x) * rad_to_deg;
// heading += 360;
// while (heading < 0) bear += 360;
// while (heading >= 360) bear -= 360;
// return heading;
//}
//internals::PointLatLng QGCMapWidget::targetLatLon(internals::PointLatLng source, double heading, double dist)
//{
// double lat1 = source.Lat() * deg_to_rad;
// double lon1 = source.Lng() * deg_to_rad;
// heading *= deg_to_rad;
// double ad = dist / earth_mean_radius;
// double lat2 = asin(sin(lat1) * cos(ad) + cos(lat1) * sin(ad) * cos(heading));
// double lon2 = lon1 + atan2(sin(bear) * sin(ad) * cos(lat1), cos(ad) - sin(lat1) * sin(lat2));
// return internals::PointLatLng(lat2 * rad_to_deg, lon2 * rad_to_deg);
//}
src/ui/map/QGCMapWidget.h
View file @
8c6c0741
...
...
@@ -2,6 +2,7 @@
#define QGCMAPWIDGET_H
#include <QMap>
#include <QTimer>
#include "opmapcontrol.h"
class
UASInterface
;
...
...
@@ -18,6 +19,10 @@ class QGCMapWidget : public mapcontrol::OPMapWidget
public:
explicit
QGCMapWidget
(
QWidget
*
parent
=
0
);
~
QGCMapWidget
();
// /** @brief Convert meters to pixels */
// float metersToPixels(double meters);
// double headingP1P2(internals::PointLatLng p1, internals::PointLatLng p2);
// internals::PointLatLng targetLatLon(internals::PointLatLng source, double heading, double dist);
signals:
void
homePositionChanged
(
double
latitude
,
double
longitude
,
double
altitude
);
...
...
@@ -30,6 +35,8 @@ public slots:
void
addUAS
(
UASInterface
*
uas
);
/** @brief Update the global position of a system */
void
updateGlobalPosition
(
UASInterface
*
uas
,
double
lat
,
double
lon
,
double
alt
,
quint64
usec
);
/** @brief Update the global position of all systems */
void
updateGlobalPosition
();
/** @brief Update the type, size, etc. of this system */
void
updateSystemSpecs
(
int
uas
);
/** @brief Change current system in focus / editing */
...
...
@@ -42,6 +49,8 @@ public slots:
void
updateWaypointList
(
int
uas
);
/** @brief Update the home position on the map */
void
updateHomePosition
(
double
latitude
,
double
longitude
,
double
altitude
);
/** @brief Set update rate limit */
void
setUpdateRateLimit
(
float
seconds
);
protected
slots
:
/** @brief Convert a map edit into a QGC waypoint event */
...
...
@@ -56,6 +65,8 @@ protected:
QMap
<
Waypoint
*
,
mapcontrol
::
WayPointItem
*>
waypointsToIcons
;
QMap
<
mapcontrol
::
WayPointItem
*
,
Waypoint
*>
iconsToWaypoints
;
Waypoint
*
firingWaypointChange
;
QTimer
updateTimer
;
float
maxUpdateInterval
;
// enum editMode {
// NONE,
// WAYPOINTS,
...
...
src/ui/map/Waypoint2DIcon.cc
View file @
8c6c0741
#include "Waypoint2DIcon.h"
#include <QPainter>
#include "opmapcontrol.h"
Waypoint2DIcon
::
Waypoint2DIcon
(
qreal
x
,
qreal
y
,
int
radius
,
QPen
*
pen
)
:
Point
(
x
,
y
,
name
,
alignment
),
yaw
(
0.0
f
),
radius
(
radius
)
Waypoint2DIcon
::
Waypoint2DIcon
(
mapcontrol
::
MapGraphicItem
*
map
,
mapcontrol
::
OPMapWidget
*
parent
,
qreal
latitude
,
qreal
longitude
,
qreal
altitude
,
int
listindex
,
QString
name
,
QString
description
,
int
radius
)
:
mapcontrol
::
WayPointItem
(
internals
::
PointLatLng
(
latitude
,
longitude
),
altitude
,
description
,
map
),
parent
(
parent
),
waypoint
(
NULL
),
radius
(
radius
),
showAcceptanceRadius
(
true
),
showOrbit
(
false
),
color
(
Qt
::
red
)
{
waypoint
=
NULL
;
size
=
QSize
(
radius
,
radius
);
drawIcon
(
pen
);
SetHeading
(
0
);
SetNumber
(
listindex
);
if
(
mypen
==
NULL
)
mypen
=
new
QPen
(
Qt
::
red
);
// drawIcon(mypen);
this
->
setFlag
(
QGraphicsItem
::
ItemIgnoresTransformations
,
true
);
picture
=
QPixmap
(
radius
+
1
,
radius
+
1
);
drawIcon
();
}
Waypoint2DIcon
::
Waypoint2DIcon
(
Waypoint
*
wp
,
int
listIndex
,
int
radius
,
Alignment
alignment
,
QPen
*
pen
)
:
Point
(
wp
->
getX
(),
wp
->
getY
(),
QString
(
"%1"
).
arg
(
listIndex
),
alignment
)
Waypoint2DIcon
::
Waypoint2DIcon
(
mapcontrol
::
MapGraphicItem
*
map
,
mapcontrol
::
OPMapWidget
*
parent
,
Waypoint
*
wp
,
const
QColor
&
color
,
int
listindex
,
int
radius
)
:
mapcontrol
::
WayPointItem
(
internals
::
PointLatLng
(
wp
->
getLatitude
(),
wp
->
getLongitude
()),
wp
->
getAltitude
(),
wp
->
getDescription
(),
map
),
parent
(
parent
),
waypoint
(
wp
),
radius
(
radius
),
showAcceptanceRadius
(
true
),
showOrbit
(
false
),
color
(
color
)
{
waypoint
=
wp
;
this
->
yaw
=
wp
->
getYaw
();
size
=
QSize
(
radius
,
radius
);
drawIcon
(
pen
);
}
Waypoint2DIcon
::
Waypoint2DIcon
(
qreal
x
,
qreal
y
,
QString
name
,
Alignment
alignment
,
QPen
*
pen
)
:
Point
(
x
,
y
,
name
,
alignment
)
{
waypoint
=
NULL
;
int
radius
=
20
;
size
=
QSize
(
radius
,
radius
);
drawIcon
(
pen
);
SetHeading
(
wp
->
getYaw
());
SetNumber
(
listindex
);
if
(
mypen
==
NULL
)
mypen
=
new
QPen
(
Qt
::
red
);
// drawIcon(mypen);
this
->
setFlag
(
QGraphicsItem
::
ItemIgnoresTransformations
,
true
);
picture
=
QPixmap
(
radius
+
1
,
radius
+
1
);
drawIcon
();
}
Waypoint2DIcon
::~
Waypoint2DIcon
()
{
delete
mypixmap
;
delete
&
picture
;
}
void
Waypoint2DIcon
::
setPen
(
QPen
*
pen
)
{
mypen
=
pen
;
drawIcon
(
pen
);
//
drawIcon(pen);
}
/**
* @param yaw in radians, 0 = north, positive = clockwise
*/
void
Waypoint2DIcon
::
setYaw
(
float
yaw
)
void
Waypoint2DIcon
::
SetHeading
(
float
heading
)
{
this
->
yaw
=
yaw
;
drawIcon
(
mypen
);
mapcontrol
::
WayPointItem
::
SetHeading
(
heading
)
;
drawIcon
();
}
void
Waypoint2DIcon
::
updateWaypoint
()
{
if
(
waypoint
)
{
if
(
waypoint
->
getYaw
()
!=
yaw
)
{
yaw
=
waypoint
->
getYaw
();
drawIcon
(
mypen
);
}
SetHeading
(
waypoint
->
getYaw
());
SetCoord
(
internals
::
PointLatLng
(
waypoint
->
getLatitude
(),
waypoint
->
getLongitude
()));
SetDescription
(
waypoint
->
getDescription
());
SetAltitude
(
waypoint
->
getAltitude
());
// FIXME Add SetNumber (currently needs a separate call)
drawIcon
();
this
->
update
();
qDebug
()
<<
"UPDATING WP"
;
}
}
QRectF
Waypoint2DIcon
::
boundingRect
()
const
{
int
loiter
=
0
;
int
acceptance
=
0
;
internals
::
PointLatLng
coord
=
(
internals
::
PointLatLng
)
Coord
();
if
(
waypoint
&&
showAcceptanceRadius
&&
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_WAYPOINT
))
{
acceptance
=
map
->
metersToPixels
(
waypoint
->
getAcceptanceRadius
(),
coord
);
}
if
(
waypoint
&&
((
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_UNLIM
)
||
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TIME
)
||
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TURNS
)))
{
loiter
=
map
->
metersToPixels
(
waypoint
->
getLoiterOrbit
(),
coord
);
}
int
width
=
qMax
(
picture
.
width
()
/
2
,
qMax
(
loiter
,
acceptance
));
int
height
=
qMax
(
picture
.
height
()
/
2
,
qMax
(
loiter
,
acceptance
));
return
QRectF
(
-
width
,
-
height
,
2
*
width
,
2
*
height
);
}
void
Waypoint2DIcon
::
drawIcon
(
QPen
*
pen
)
void
Waypoint2DIcon
::
drawIcon
()
{
mypixmap
=
new
QPixmap
(
radius
+
1
,
radius
+
1
);
mypixmap
->
fill
(
Qt
::
transparent
);
QPainter
painter
(
mypixmap
);
picture
.
fill
(
Qt
::
transparent
);
QPainter
painter
(
&
picture
);
// DRAW WAYPOINT
QPointF
p
(
radius
/
2
,
radius
/
2
);
QPointF
p
(
picture
.
width
()
/
2
,
picture
.
height
()
/
2
);
float
waypointSize
=
radius
;
QPolygonF
poly
(
4
);
// Top point
poly
.
replace
(
0
,
QPointF
(
p
.
x
(),
p
.
y
()
-
waypointSize
/
2.0
f
));
poly
.
replace
(
0
,
QPointF
(
p
.
x
(),
p
.
y
()
-
picture
.
height
()
/
2.0
f
));
// Right point
poly
.
replace
(
1
,
QPointF
(
p
.
x
()
+
waypointSize
/
2.0
f
,
p
.
y
()));
poly
.
replace
(
1
,
QPointF
(
p
.
x
()
+
picture
.
width
()
/
2.0
f
,
p
.
y
()));
// Bottom point
poly
.
replace
(
2
,
QPointF
(
p
.
x
(),
p
.
y
()
+
waypointSize
/
2.0
f
));
poly
.
replace
(
3
,
QPointF
(
p
.
x
()
-
waypointSize
/
2.0
f
,
p
.
y
()));
poly
.
replace
(
2
,
QPointF
(
p
.
x
(),
p
.
y
()
+
picture
.
height
()
/
2.0
f
));
poly
.
replace
(
3
,
QPointF
(
p
.
x
()
-
picture
.
width
()
/
2.0
f
,
p
.
y
()));
// // Select color based on if this is the current waypoint
// if (list.at(i)->getCurrent())
...
...
@@ -91,17 +119,91 @@ void Waypoint2DIcon::drawIcon(QPen* pen)
// }
//pen.setColor(color);
if
(
pen
)
{
pen
->
setWidthF
(
2
);
painter
.
setPen
(
*
pen
);
}
else
{
QPen
pen2
(
Qt
::
red
);
//
if (pen) {
//
pen->setWidthF(2);
//
painter.setPen(*pen);
//
} else {
QPen
pen2
(
color
);
pen2
.
setWidth
(
2
);
painter
.
setPen
(
pen2
);
}
//
}
painter
.
setBrush
(
Qt
::
NoBrush
);
int
waypointSize
=
qMin
(
picture
.
width
(),
picture
.
height
());
float
rad
=
(
waypointSize
/
2.0
f
)
*
0.8
*
(
1
/
sqrt
(
2.0
f
));
painter
.
drawLine
(
p
.
x
(),
p
.
y
(),
p
.
x
()
+
sin
(
yaw
)
*
radius
,
p
.
y
()
-
cos
(
yaw
)
*
rad
);
painter
.
drawPolygon
(
poly
);
// If this is not a waypoint (only the default representation)
// or it is a waypoint, but not one where direction has no meaning
// then draw the heading indicator
if
(
!
waypoint
||
(
waypoint
&&
(
(
waypoint
->
getAction
()
!=
(
int
)
MAV_CMD_NAV_TAKEOFF
)
&&
(
waypoint
->
getAction
()
!=
(
int
)
MAV_CMD_NAV_LAND
)
&&
(
waypoint
->
getAction
()
!=
(
int
)
MAV_CMD_NAV_LOITER_UNLIM
)
&&
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TIME
)
&&
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TURNS
)
)))
{
painter
.
drawLine
(
p
.
x
(),
p
.
y
(),
p
.
x
()
+
sin
(
Heading
())
*
rad
,
p
.
y
()
-
cos
(
Heading
())
*
rad
);
}
if
((
waypoint
!=
NULL
)
&&
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_TAKEOFF
))
{
// Takeoff waypoint
int
width
=
(
picture
.
width
()
-
1
);
int
height
=
(
picture
.
height
()
-
1
);
painter
.
drawRect
(
0
,
0
,
2
*
width
,
2
*
height
);
painter
.
drawRect
(
width
*
0.2
,
height
*
0.2
f
,
2
*
width
*
0.6
f
,
2
*
height
*
0.6
f
);
}
else
if
((
waypoint
!=
NULL
)
&&
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LAND
))
{
// Landing waypoint
int
width
=
(
picture
.
width
()
-
1
)
/
2
;
int
height
=
(
picture
.
height
()
-
1
)
/
2
;
painter
.
drawEllipse
(
p
,
width
,
height
);
painter
.
drawEllipse
(
p
,
width
*
0.6
f
,
height
*
0.6
f
);
painter
.
drawLine
(
0
,
0
,
picture
.
width
(),
picture
.
height
());
painter
.
drawLine
(
picture
.
width
(),
0
,
0
,
picture
.
height
());
}
else
if
((
waypoint
!=
NULL
)
&&
((
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_UNLIM
)
||
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TIME
)
||
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TURNS
)))
{
// Loiter waypoint
int
width
=
(
picture
.
width
()
-
1
)
/
2
;
int
height
=
(
picture
.
height
()
-
1
)
/
2
;
painter
.
drawEllipse
(
p
,
width
,
height
);
}
else
{
// Navigation waypoint
painter
.
drawPolygon
(
poly
);
}
}
void
Waypoint2DIcon
::
paint
(
QPainter
*
painter
,
const
QStyleOptionGraphicsItem
*
option
,
QWidget
*
widget
)
{
Q_UNUSED
(
option
);
Q_UNUSED
(
widget
);
painter
->
drawPixmap
(
-
picture
.
width
()
/
2
,
-
picture
.
height
()
/
2
,
picture
);
if
(
this
->
isSelected
())
{
painter
->
drawRect
(
QRectF
(
-
picture
.
width
()
/
2
,
-
picture
.
height
()
/
2
,
picture
.
width
()
-
1
,
picture
.
height
()
-
1
));
}
QPen
pen
=
painter
->
pen
();
pen
.
setColor
(
color
);
if
((
waypoint
)
&&
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_WAYPOINT
)
&&
showAcceptanceRadius
)
{
QPen
redPen
=
QPen
(
pen
);
redPen
.
setColor
(
Qt
::
red
);
painter
->
setPen
(
redPen
);
const
int
acceptance
=
map
->
metersToPixels
(
waypoint
->
getAcceptanceRadius
(),
Coord
());
if
(
waypoint
)
painter
->
drawEllipse
(
QPointF
(
0
,
0
),
acceptance
,
acceptance
);
}
if
((
waypoint
)
&&
((
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_UNLIM
)
||
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TIME
)
||
(
waypoint
->
getAction
()
==
(
int
)
MAV_CMD_NAV_LOITER_TURNS
)))
{
painter
->
setPen
(
pen
);
pen
.
setWidth
(
2
);
const
int
loiter
=
map
->
metersToPixels
(
waypoint
->
getLoiterOrbit
(),
Coord
());
if
(
waypoint
)
painter
->
drawEllipse
(
QPointF
(
0
,
0
),
loiter
,
loiter
);
}
}
src/ui/map/Waypoint2DIcon.h
View file @
8c6c0741
...
...
@@ -4,42 +4,25 @@
#include <QGraphicsItem>
#include "Waypoint.h"
#include "opmapcontrol.h"
class
Waypoint2DIcon
:
public
QGraphics
Item
class
Waypoint2DIcon
:
public
mapcontrol
::
WayPoint
Item
{
public:
/*!
*
* @param x longitude
* @param y latitude
* @param name name of the circle point
* @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing
*/
Waypoint2DIcon
(
qreal
x
,
qreal
y
,
QString
name
=
QString
(),
Alignment
alignment
=
Middle
,
QPen
*
pen
=
0
);
//!
/*!
/**
*
* @param x longitude
* @param y latitude
* @param radius the radius of the circle
* @param latitude
* @param longitude
* @param name name of the circle point
* @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing
*/
Waypoint2DIcon
(
qreal
x
,
qreal
y
,
int
radius
=
20
,
QString
name
=
QString
(),
Alignment
alignment
=
Middle
,
QPen
*
pen
=
0
);
Waypoint2DIcon
(
mapcontrol
::
MapGraphicItem
*
map
,
mapcontrol
::
OPMapWidget
*
parent
,
qreal
latitude
,
qreal
longitude
,
qreal
altitude
,
int
listindex
,
QString
name
=
QString
(),
QString
description
=
QString
(),
int
radius
=
24
);
/**
*
* @param wp Waypoint
* @param listIndex Index in the waypoint list
* @param radius the radius of the circle
* @param name name of the circle point
* @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing
*/
Waypoint2DIcon
(
Waypoint
*
wp
,
int
listIndex
,
int
radius
=
20
,
Alignment
alignment
=
Middle
,
QPen
*
pen
=
0
);
Waypoint2DIcon
(
mapcontrol
::
MapGraphicItem
*
map
,
mapcontrol
::
OPMapWidget
*
parent
,
Waypoint
*
wp
,
const
QColor
&
color
,
int
listindex
,
int
radius
=
24
);
virtual
~
Waypoint2DIcon
();
...
...
@@ -51,17 +34,27 @@ public:
*/
virtual
void
setPen
(
QPen
*
pen
);
void
setYaw
(
float
yaw
);
void
SetHeading
(
float
heading
);
void
drawIcon
(
QPen
*
pen
);
/** @brief Rectangle to be updated on changes */
QRectF
boundingRect
()
const
;
/** @brief Draw the icon in a double buffer */
void
drawIcon
();
/** @brief Draw the icon on a QPainter device (map) */
void
paint
(
QPainter
*
painter
,
const
QStyleOptionGraphicsItem
*
option
,
QWidget
*
widget
);
public
slots
:
public:
void
updateWaypoint
();
protected:
float
yaw
;
///< Yaw angle of the MAV
int
radius
;
///< Radius / diameter of the icon in pixels
mapcontrol
::
OPMapWidget
*
parent
;
///< Parent widget
int
radius
;
///< Radius / diameter of the icon in pixels
Waypoint
*
waypoint
;
///< Waypoint data container this icon represents
QPen
*
mypen
;
QColor
color
;
bool
showAcceptanceRadius
;
bool
showOrbit
;
// QSize size;
};
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment