Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
6cd0d9f6
Commit
6cd0d9f6
authored
Sep 21, 2011
by
LM
Browse files
Fixed all known waypoint-editing related issues
parent
dc3b21fb
Changes
7
Show whitespace changes
Inline
Side-by-side
images/earth.html
View file @
6cd0d9f6
...
...
@@ -347,12 +347,12 @@ function updateWaypointListLength(id, len)
// Delete any non-needed waypoints
if
(
waypoints
.
length
>
len
)
{
for
(
var
i
=
len
;
i
<
waypoints
.
length
;
i
++
)
var
initialLength
=
waypoints
.
length
;
for
(
var
i
=
len
;
i
<
initialLength
;
i
++
)
{
var
placemark
=
waypoints
.
pop
();
ge
.
getFeatures
().
removeChild
(
placemark
);
var
line
=
waypointLines
[
id
].
pop
();
ge
.
getFeatures
().
removeChild
(
line
);
waypointLines
[
id
].
getCoordinates
().
pop
();
}
}
}
...
...
@@ -480,11 +480,6 @@ function createWaypointLine(id, color)
lineStyle
.
getColor
().
set
(
color
);
// aabbggrr format
// Add waypoint line
//waypointLines[id].setExtrude(false);
//waypointLines[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
// Create a style and set width and color of line
//waypointLinePlacemarks[id].setStyleSelector(ge.createStyle(''));
...
...
src/Waypoint.cc
View file @
6cd0d9f6
...
...
@@ -232,8 +232,8 @@ void Waypoint::setAcceptanceRadius(double radius)
void
Waypoint
::
setParam1
(
double
param1
)
{
//qDebug() << "SENDER:" << QObject::sender();
//qDebug() << "PARAM1 SET REQ:" << param1;
//
// //
qDebug() << "SENDER:" << QObject::sender();
//
// //
qDebug() << "PARAM1 SET REQ:" << param1;
if
(
this
->
param1
!=
param1
)
{
this
->
param1
=
param1
;
...
...
src/uas/UASWaypointManager.cc
View file @
6cd0d9f6
...
...
@@ -59,7 +59,7 @@ void UASWaypointManager::timeout()
protocol_timer
.
start
(
PROTOCOL_TIMEOUT_MS
);
current_retries
--
;
emit
updateStatusString
(
tr
(
"Timeout, retrying (retries left: %1)"
).
arg
(
current_retries
));
// qDebug() << "Timeout, retrying (retries left:" << current_retries << ")";
//
//
qDebug() << "Timeout, retrying (retries left:" << current_retries << ")";
if
(
current_state
==
WP_GETLIST
)
{
sendWaypointRequestList
();
}
else
if
(
current_state
==
WP_GETLIST_GETWPS
)
{
...
...
@@ -76,7 +76,7 @@ void UASWaypointManager::timeout()
}
else
{
protocol_timer
.
stop
();
// qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE";
//
//
qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE";
emit
updateStatusString
(
"Operation timed out."
);
...
...
@@ -113,7 +113,7 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
protocol_timer
.
start
(
PROTOCOL_TIMEOUT_MS
);
current_retries
=
PROTOCOL_MAX_RETRIES
;
// qDebug() << "got waypoint count (" << count << ") from ID " << systemId;
//
//
qDebug() << "got waypoint count (" << count << ") from ID " << systemId;
if
(
count
>
0
)
{
current_count
=
count
;
...
...
@@ -124,7 +124,7 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
}
else
{
protocol_timer
.
stop
();
emit
updateStatusString
(
"done."
);
// qDebug() << "No waypoints on UAS " << systemId;
//
//
qDebug() << "No waypoints on UAS " << systemId;
current_state
=
WP_IDLE
;
current_count
=
0
;
current_wp_id
=
0
;
...
...
@@ -143,7 +143,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
current_retries
=
PROTOCOL_MAX_RETRIES
;
if
(
wp
->
seq
==
current_wp_id
)
{
//// qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command;
////
//
qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command;
Waypoint
*
lwp
=
new
Waypoint
(
wp
->
seq
,
wp
->
x
,
wp
->
y
,
wp
->
z
,
wp
->
param1
,
wp
->
param2
,
wp
->
param3
,
wp
->
param4
,
wp
->
autocontinue
,
wp
->
current
,
(
MAV_FRAME
)
wp
->
frame
,
(
MAV_CMD
)
wp
->
command
);
addWaypoint
(
lwp
,
false
);
if
(
wp
->
current
==
1
)
currentWaypoint
=
lwp
;
...
...
@@ -168,7 +168,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
if
(
currentWaypoint
)
emit
currentWaypointChanged
(
currentWaypoint
->
getId
());
emit
updateStatusString
(
"done."
);
// qDebug() << "got all waypoints from ID " << systemId;
//
//
qDebug() << "got all waypoints from ID " << systemId;
}
}
else
{
emit
updateStatusString
(
tr
(
"Waypoint ID mismatch, rejecting waypoint"
));
...
...
@@ -186,12 +186,12 @@ void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavli
protocol_timer
.
stop
();
current_state
=
WP_IDLE
;
emit
updateStatusString
(
"done."
);
// qDebug() << "sent all waypoints to ID " << systemId;
//
//
qDebug() << "sent all waypoints to ID " << systemId;
}
else
if
(
current_state
==
WP_CLEARLIST
)
{
protocol_timer
.
stop
();
current_state
=
WP_IDLE
;
emit
updateStatusString
(
"done."
);
// qDebug() << "cleared waypoint list of ID " << systemId;
//
//
qDebug() << "cleared waypoint list of ID " << systemId;
}
}
}
...
...
@@ -242,18 +242,18 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
}
}
//// qDebug() << "Updated waypoints list";
////
//
qDebug() << "Updated waypoints list";
}
emit
updateStatusString
(
QString
(
"New current waypoint %1"
).
arg
(
wpc
->
seq
));
//emit update to UI widgets
emit
currentWaypointChanged
(
wpc
->
seq
);
//// qDebug() << "new current waypoint" << wpc->seq;
////
//
qDebug() << "new current waypoint" << wpc->seq;
}
}
void
UASWaypointManager
::
notifyOfChange
(
Waypoint
*
wp
)
{
// qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
//
//
qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
// If only one waypoint was changed, emit only WP signal
if
(
wp
!=
NULL
)
{
emit
waypointChanged
(
uas
.
getUASID
(),
wp
);
...
...
@@ -316,8 +316,8 @@ void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive)
waypoints
.
insert
(
waypoints
.
size
(),
wp
);
connect
(
wp
,
SIGNAL
(
changed
(
Waypoint
*
)),
this
,
SLOT
(
notifyOfChange
(
Waypoint
*
)));
emit
waypointListChanged
(
uas
.
getUASID
());
emit
waypointListChanged
();
emit
waypointListChanged
(
uas
.
getUASID
());
}
}
...
...
@@ -483,7 +483,7 @@ const QVector<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
// instead of filtering on each request
QVector
<
Waypoint
*>
wps
;
foreach
(
Waypoint
*
wp
,
waypoints
)
{
if
(
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL
)
{
if
(
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
)
{
wps
.
append
(
wp
);
}
}
...
...
@@ -497,7 +497,7 @@ const QVector<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointLi
// instead of filtering on each request
QVector
<
Waypoint
*>
wps
;
foreach
(
Waypoint
*
wp
,
waypoints
)
{
if
(
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL
&&
wp
->
isNavigationType
())
{
if
(
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
&&
wp
->
isNavigationType
())
{
wps
.
append
(
wp
);
}
}
...
...
@@ -529,8 +529,10 @@ int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
// counting only those in global frame
int
i
=
0
;
foreach
(
Waypoint
*
p
,
waypoints
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
)
{
if
(
p
==
wp
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
)
{
if
(
p
==
wp
)
{
return
i
;
}
i
++
;
...
...
@@ -546,8 +548,10 @@ int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp)
// counting only those in global frame
int
i
=
0
;
foreach
(
Waypoint
*
p
,
waypoints
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
&&
p
->
isNavigationType
())
{
if
(
p
==
wp
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
&&
p
->
isNavigationType
())
{
if
(
p
==
wp
)
{
return
i
;
}
i
++
;
...
...
@@ -580,7 +584,8 @@ int UASWaypointManager::getGlobalFrameCount()
// counting only those in global frame
int
i
=
0
;
foreach
(
Waypoint
*
p
,
waypoints
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
p
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
)
{
i
++
;
}
}
...
...
@@ -594,7 +599,8 @@ int UASWaypointManager::getGlobalFrameAndNavTypeCount()
// counting only those in global frame
int
i
=
0
;
foreach
(
Waypoint
*
p
,
waypoints
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
&&
p
->
isNavigationType
())
{
if
(
p
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
p
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
&&
p
->
isNavigationType
())
{
i
++
;
}
}
...
...
@@ -623,7 +629,7 @@ int UASWaypointManager::getLocalFrameCount()
int
i
=
0
;
foreach
(
Waypoint
*
p
,
waypoints
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_
G
LO
B
AL
)
if
(
p
->
getFrame
()
==
MAV_FRAME_
LOCAL_NED
||
p
->
getFrame
()
==
MAV_FRAME_
LO
C
AL
_ENU
)
{
i
++
;
}
...
...
@@ -658,8 +664,10 @@ int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
int
i
=
0
;
foreach
(
Waypoint
*
p
,
waypoints
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_MISSION
)
{
if
(
p
==
wp
)
{
if
(
p
->
getFrame
()
==
MAV_FRAME_MISSION
)
{
if
(
p
==
wp
)
{
return
i
;
}
i
++
;
...
...
@@ -753,7 +761,7 @@ void UASWaypointManager::writeWaypoints()
sendWaypointClearAll
();
}
else
{
//we're in another transaction, ignore command
// qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
//
//
qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
}
}
...
...
@@ -771,7 +779,7 @@ void UASWaypointManager::sendWaypointClearAll()
uas
.
sendMessage
(
message
);
MG
::
SLEEP
::
usleep
(
PROTOCOL_DELAY_MS
*
1000
);
// qDebug() << "sent waypoint clear all to ID " << wpca.target_system;
//
//
qDebug() << "sent waypoint clear all to ID " << wpca.target_system;
}
void
UASWaypointManager
::
sendWaypointSetCurrent
(
quint16
seq
)
...
...
@@ -789,7 +797,7 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
uas
.
sendMessage
(
message
);
MG
::
SLEEP
::
usleep
(
PROTOCOL_DELAY_MS
*
1000
);
// qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system;
//
//
qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system;
}
void
UASWaypointManager
::
sendWaypointCount
()
...
...
@@ -801,14 +809,14 @@ void UASWaypointManager::sendWaypointCount()
wpc
.
target_component
=
MAV_COMP_ID_MISSIONPLANNER
;
wpc
.
count
=
current_count
;
// qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
//
//
qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
emit
updateStatusString
(
QString
(
"Starting to transmit waypoints..."
));
mavlink_msg_mission_count_encode
(
uas
.
mavlink
->
getSystemId
(),
uas
.
mavlink
->
getComponentId
(),
&
message
,
&
wpc
);
uas
.
sendMessage
(
message
);
MG
::
SLEEP
::
usleep
(
PROTOCOL_DELAY_MS
*
1000
);
// qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
//
//
qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
}
void
UASWaypointManager
::
sendWaypointRequestList
()
...
...
@@ -825,7 +833,7 @@ void UASWaypointManager::sendWaypointRequestList()
uas
.
sendMessage
(
message
);
MG
::
SLEEP
::
usleep
(
PROTOCOL_DELAY_MS
*
1000
);
// qDebug() << "sent waypoint list request to ID " << wprl.target_system;
//
//
qDebug() << "sent waypoint list request to ID " << wprl.target_system;
}
...
...
@@ -845,13 +853,13 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq)
uas
.
sendMessage
(
message
);
MG
::
SLEEP
::
usleep
(
PROTOCOL_DELAY_MS
*
1000
);
// qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system;
//
//
qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system;
}
void
UASWaypointManager
::
sendWaypoint
(
quint16
seq
)
{
mavlink_message_t
message
;
// qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
//
//
qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
if
(
seq
<
waypoint_buffer
.
count
())
{
...
...
@@ -864,7 +872,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
emit
updateStatusString
(
QString
(
"Sending waypoint ID %1 of %2 total"
).
arg
(
wp
->
seq
).
arg
(
current_count
));
// qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<<waypoint_buffer.count();
//
//
qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<<waypoint_buffer.count();
mavlink_msg_mission_item_encode
(
uas
.
mavlink
->
getSystemId
(),
uas
.
mavlink
->
getComponentId
(),
&
message
,
wp
);
uas
.
sendMessage
(
message
);
...
...
@@ -886,5 +894,5 @@ void UASWaypointManager::sendWaypointAck(quint8 type)
uas
.
sendMessage
(
message
);
MG
::
SLEEP
::
usleep
(
PROTOCOL_DELAY_MS
*
1000
);
// qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system;
//
//
qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system;
}
src/uas/UASWaypointManager.h
View file @
6cd0d9f6
...
...
@@ -108,13 +108,6 @@ public:
return
this
->
uas
;
///< Returns the owning UAS
}
// /** @name Global waypoint list operations */
// /*@{*/
// const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; } ///< Returns a const reference to the global waypoint list.
// void globalAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
// int globalRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage
// /*@}*/
private:
/** @name Message send functions */
/*@{*/
...
...
src/ui/WaypointList.cc
View file @
6cd0d9f6
...
...
@@ -131,7 +131,6 @@ void WaypointList::setUAS(UASInterface* uas)
connect
(
uas
->
getWaypointManager
(),
SIGNAL
(
currentWaypointChanged
(
quint16
)),
this
,
SLOT
(
currentWaypointChanged
(
quint16
)));
//connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
//connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
}
}
...
...
src/ui/map/QGCMapWidget.cc
View file @
6cd0d9f6
...
...
@@ -18,7 +18,6 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) :
{
// Widget is inactive until shown
loadSettings
(
false
);
// Set cache mode
}
QGCMapWidget
::~
QGCMapWidget
()
...
...
@@ -167,23 +166,22 @@ void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
/**
*
* @param uas the UAS/MAV to monitor/display with the
HUD
* @param uas the UAS/MAV to monitor/display with the
map widget
*/
void
QGCMapWidget
::
addUAS
(
UASInterface
*
uas
)
{
// // qDebug() << "ADDING UAS";
connect
(
uas
,
SIGNAL
(
globalPositionChanged
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)),
this
,
SLOT
(
updateGlobalPosition
(
UASInterface
*
,
double
,
double
,
double
,
quint64
)));
//connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
connect
(
uas
,
SIGNAL
(
systemSpecsChanged
(
int
)),
this
,
SLOT
(
updateSystemSpecs
(
int
)));
}
void
QGCMapWidget
::
activeUASSet
(
UASInterface
*
uas
)
{
// Only execute if proper UAS is set
if
(
!
uas
||
!
dynamic_cast
<
UASInterface
*>
(
uas
)
)
return
;
if
(
!
uas
)
return
;
// Disconnect old MAV manager
if
(
currWPManager
)
{
if
(
currWPManager
)
{
// Disconnect the waypoint manager / data storage from the UI
disconnect
(
currWPManager
,
SIGNAL
(
waypointListChanged
(
int
)),
this
,
SLOT
(
updateWaypointList
(
int
)));
disconnect
(
currWPManager
,
SIGNAL
(
waypointChanged
(
int
,
Waypoint
*
)),
this
,
SLOT
(
updateWaypoint
(
int
,
Waypoint
*
)));
...
...
@@ -191,12 +189,10 @@ void QGCMapWidget::activeUASSet(UASInterface* uas)
disconnect
(
this
,
SIGNAL
(
waypointChanged
(
Waypoint
*
)),
currWPManager
,
SLOT
(
notifyOfChange
(
Waypoint
*
)));
}
if
(
uas
)
{
if
(
uas
)
{
currWPManager
=
uas
->
getWaypointManager
();
// Delete all waypoints and add waypoint from new system
updateWaypointList
(
uas
->
getUASID
());
// Connect the waypoint manager / data storage to the UI
connect
(
currWPManager
,
SIGNAL
(
waypointListChanged
(
int
)),
this
,
SLOT
(
updateWaypointList
(
int
)));
connect
(
currWPManager
,
SIGNAL
(
waypointChanged
(
int
,
Waypoint
*
)),
this
,
SLOT
(
updateWaypoint
(
int
,
Waypoint
*
)));
...
...
@@ -204,6 +200,9 @@ void QGCMapWidget::activeUASSet(UASInterface* uas)
connect
(
this
,
SIGNAL
(
waypointChanged
(
Waypoint
*
)),
currWPManager
,
SLOT
(
notifyOfChange
(
Waypoint
*
)));
updateSelectedSystem
(
uas
->
getUASID
());
followUAVID
=
uas
->
getUASID
();
// Delete all waypoints and add waypoint from new system
updateWaypointList
(
uas
->
getUASID
());
}
}
...
...
@@ -317,16 +316,19 @@ void QGCMapWidget::showGoToDialog()
QString
text
=
QInputDialog
::
getText
(
this
,
tr
(
"Please enter coordinates"
),
tr
(
"Coordinates (Lat,Lon):"
),
QLineEdit
::
Normal
,
QString
(
"%1,%2"
).
arg
(
CurrentPosition
().
Lat
(),
0
,
'g'
,
6
).
arg
(
CurrentPosition
().
Lng
(),
0
,
'g'
,
6
),
&
ok
);
if
(
ok
&&
!
text
.
isEmpty
())
{
if
(
ok
&&
!
text
.
isEmpty
())
{
QStringList
split
=
text
.
split
(
","
);
if
(
split
.
length
()
==
2
)
{
if
(
split
.
length
()
==
2
)
{
bool
convert
;
double
latitude
=
split
.
first
().
toDouble
(
&
convert
);
ok
&=
convert
;
double
longitude
=
split
.
last
().
toDouble
(
&
convert
);
ok
&=
convert
;
if
(
ok
)
{
if
(
ok
)
{
internals
::
PointLatLng
pos_lat_lon
=
internals
::
PointLatLng
(
latitude
,
longitude
);
SetCurrentPosition
(
pos_lat_lon
);
// set the map position
}
...
...
@@ -422,6 +424,7 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
*/
void
QGCMapWidget
::
updateWaypoint
(
int
uas
,
Waypoint
*
wp
)
{
qDebug
()
<<
"UPDATING WP FUNCTION CALLED"
;
// Source of the event was in this widget, do nothing
if
(
firingWaypointChange
==
wp
)
return
;
// Currently only accept waypoint updates from the UAS in focus
...
...
@@ -443,6 +446,8 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
// Mark this wp as currently edited
firingWaypointChange
=
wp
;
qDebug
()
<<
"UPDATING WAYPOINT"
<<
wpindex
<<
"IN 2D MAP"
;
// Check if wp exists yet in map
if
(
!
waypointsToIcons
.
contains
(
wp
))
{
...
...
@@ -531,6 +536,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
*/
void
QGCMapWidget
::
updateWaypointList
(
int
uas
)
{
qDebug
()
<<
"UPDATE WP LIST IN 2D MAP CALLED FOR UAS"
<<
uas
;
// Currently only accept waypoint updates from the UAS in focus
// this has to be changed to accept read-only updates from other systems as well.
UASInterface
*
uasInstance
=
UASManager
::
instance
()
->
getUASForId
(
uas
);
...
...
@@ -539,6 +545,8 @@ void QGCMapWidget::updateWaypointList(int uas)
// ORDER MATTERS HERE!
// TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
qDebug
()
<<
"DELETING NOW OLD WPS"
;
// Delete first all old waypoints
// this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
QVector
<
Waypoint
*
>
wps
=
currWPManager
->
getGlobalFrameAndNavTypeWaypointList
();
...
...
@@ -564,6 +572,7 @@ void QGCMapWidget::updateWaypointList(int uas)
// Update all potentially new waypoints
foreach
(
Waypoint
*
wp
,
wps
)
{
qDebug
()
<<
"UPDATING NEW WP"
<<
wp
->
getId
();
// Update / add only if new
if
(
!
waypointsToIcons
.
contains
(
wp
))
updateWaypoint
(
uas
,
wp
);
}
...
...
src/ui/map3D/QGCGoogleEarthView.cc
View file @
6cd0d9f6
...
...
@@ -217,7 +217,8 @@ void QGCGoogleEarthView::addUAS(UASInterface* uas)
void
QGCGoogleEarthView
::
setActiveUAS
(
UASInterface
*
uas
)
{
if
(
uas
)
{
if
(
uas
)
{
mav
=
uas
;
javaScript
(
QString
(
"setCurrAircraft(%1);"
).
arg
(
uas
->
getUASID
()));
updateWaypointList
(
uas
->
getUASID
());
...
...
@@ -231,7 +232,8 @@ void QGCGoogleEarthView::setActiveUAS(UASInterface* uas)
void
QGCGoogleEarthView
::
updateWaypoint
(
int
uas
,
Waypoint
*
wp
)
{
// Only accept waypoints in global coordinate frame
if
(
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL
&&
wp
->
isNavigationType
())
{
if
(
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL
||
wp
->
getFrame
()
==
MAV_FRAME_GLOBAL_RELATIVE_ALT
&&
wp
->
isNavigationType
())
{
// We're good, this is a global waypoint
// Get the index of this waypoint
...
...
@@ -239,9 +241,12 @@ void QGCGoogleEarthView::updateWaypoint(int uas, Waypoint* wp)
// as we're only handling global waypoints
int
wpindex
=
UASManager
::
instance
()
->
getUASForId
(
uas
)
->
getWaypointManager
()
->
getGlobalFrameAndNavTypeIndexOf
(
wp
);
// If not found, return (this should never happen, but helps safety)
if
(
wpindex
==
-
1
)
{
if
(
wpindex
==
-
1
)
{
return
;
}
else
{
}
else
{
javaScript
(
QString
(
"updateWaypoint(%1,%2,%3,%4,%5,%6);"
).
arg
(
uas
).
arg
(
wpindex
).
arg
(
wp
->
getLatitude
(),
0
,
'f'
,
18
).
arg
(
wp
->
getLongitude
(),
0
,
'f'
,
18
).
arg
(
wp
->
getAltitude
(),
0
,
'f'
,
18
).
arg
(
wp
->
getAction
()));
//qDebug() << QString("updateWaypoint(%1,%2,%3,%4,%5,%6);").arg(uas).arg(wpindex).arg(wp->getLatitude(), 0, 'f', 18).arg(wp->getLongitude(), 0, 'f', 18).arg(wp->getAltitude(), 0, 'f', 18).arg(wp->getAction());
}
...
...
@@ -257,7 +262,8 @@ void QGCGoogleEarthView::updateWaypointList(int uas)
{
// Get already existing waypoints
UASInterface
*
uasInstance
=
UASManager
::
instance
()
->
getUASForId
(
uas
);
if
(
uasInstance
)
{
if
(
uasInstance
)
{
// Get all waypoints, including non-global waypoints
QVector
<
Waypoint
*>
wpList
=
uasInstance
->
getWaypointManager
()
->
getGlobalFrameAndNavTypeWaypointList
();
...
...
@@ -267,7 +273,8 @@ void QGCGoogleEarthView::updateWaypointList(int uas)
qDebug
()
<<
QString
(
"updateWaypointListLength(%1,%2);"
).
arg
(
uas
).
arg
(
wpList
.
count
());
// Load all existing waypoints into map view
foreach
(
Waypoint
*
wp
,
wpList
)
{
foreach
(
Waypoint
*
wp
,
wpList
)
{
updateWaypoint
(
uas
,
wp
);
}
}
...
...
@@ -284,7 +291,8 @@ void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, dou
void
QGCGoogleEarthView
::
clearTrails
()
{
QList
<
UASInterface
*>
mavs
=
UASManager
::
instance
()
->
getUASList
();
foreach
(
UASInterface
*
currMav
,
mavs
)
{
foreach
(
UASInterface
*
currMav
,
mavs
)
{
javaScript
(
QString
(
"clearTrail(%1);"
).
arg
(
currMav
->
getUASID
()));
}
}
...
...
@@ -292,9 +300,11 @@ void QGCGoogleEarthView::clearTrails()
void
QGCGoogleEarthView
::
showTrail
(
bool
state
)
{
// Check if the current trail has to be hidden
if
(
trailEnabled
&&
!
state
)
{
if
(
trailEnabled
&&
!
state
)
{
QList
<
UASInterface
*>
mavs
=
UASManager
::
instance
()
->
getUASList
();
foreach
(
UASInterface
*
currMav
,
mavs
)
{
foreach
(
UASInterface
*
currMav
,
mavs
)
{
javaScript
(
QString
(
"hideTrail(%1);"
).
arg
(
currMav
->
getUASID
()));
}
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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