Commit 259403be authored by Lorenz Meier's avatar Lorenz Meier

Checkpoint WPs issue-free in operation

parent 4f91c4af
......@@ -139,7 +139,7 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
if (read_to_edit == true){
while(waypointsEditable.count()>0) {
Waypoint *t = waypointsEditable[0];
waypointsEditable.remove(0);
waypointsEditable.removeAt(0);
delete t;
}
emit waypointEditableListChanged();
......@@ -425,7 +425,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
}
}
waypointsEditable.remove(seq);
waypointsEditable.removeAt(seq);
delete t;
t = NULL;
......@@ -496,7 +496,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
while(waypointsEditable.count()>0) {
Waypoint *t = waypointsEditable[0];
waypointsEditable.remove(0);
waypointsEditable.removeAt(0);
delete t;
}
......@@ -549,12 +549,12 @@ void UASWaypointManager::clearWaypointList()
}
}
const QVector<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
const QList<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
{
// TODO Keep this global frame list up to date
// with complete waypoint list
// instead of filtering on each request
QVector<Waypoint*> wps;
QList<Waypoint*> wps;
foreach (Waypoint* wp, waypointsEditable)
{
if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
......@@ -565,12 +565,12 @@ const QVector<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
return wps;
}
const QVector<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointList()
const QList<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointList()
{
// TODO Keep this global frame list up to date
// with complete waypoint list
// instead of filtering on each request
QVector<Waypoint*> wps;
QList<Waypoint*> wps;
foreach (Waypoint* wp, waypointsEditable)
{
if ((wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType())
......@@ -581,12 +581,12 @@ const QVector<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointLi
return wps;
}
const QVector<Waypoint *> UASWaypointManager::getNavTypeWaypointList()
const QList<Waypoint *> UASWaypointManager::getNavTypeWaypointList()
{
// TODO Keep this global frame list up to date
// with complete waypoint list
// instead of filtering on each request
QVector<Waypoint*> wps;
QList<Waypoint*> wps;
foreach (Waypoint* wp, waypointsEditable)
{
if (wp->isNavigationType())
......@@ -775,7 +775,7 @@ void UASWaypointManager::readWaypoints(bool readToEdit)
//Clear the old view-list before receiving the new one
while(waypointsViewOnly.size()>0) {
Waypoint *t = waypointsViewOnly[0];
waypointsViewOnly.remove(0);
waypointsViewOnly.removeAt(0);
delete t;
}
emit waypointViewOnlyListChanged();
......
......@@ -33,7 +33,7 @@ This file is part of the QGROUNDCONTROL project
#define UASWAYPOINTMANAGER_H
#include <QObject>
#include <QVector>
#include <QList>
#include <QTimer>
#include "Waypoint.h"
#include "QGCMAVLink.h"
......@@ -44,7 +44,7 @@ class UASInterface;
* @brief Implementation of the MAVLINK waypoint protocol
*
* This class handles the communication with a waypoint manager on the MAV.
* All waypoints are stored in the QVector waypoints, modifications can be done with the WaypointList widget.
* All waypoints are stored in the QList waypoints, modifications can be done with the WaypointList widget.
* Notice that currently the access to the internal waypoint storage is not guarded nor thread-safe. This works as long as no other widget alters the data.
*
* See http://qgroundcontrol.org/waypoint_protocol for more information about the protocol and the states.
......@@ -91,15 +91,15 @@ public:
/** @name Waypoint list operations */
/*@{*/
const QVector<Waypoint *> &getWaypointEditableList(void) {
const QList<Waypoint *> &getWaypointEditableList(void) {
return waypointsEditable; ///< Returns a const reference to the waypoint list.
}
const QVector<Waypoint *> &getWaypointViewOnlyList(void) {
const QList<Waypoint *> &getWaypointViewOnlyList(void) {
return waypointsViewOnly; ///< Returns a const reference to the waypoint list.
}
const QVector<Waypoint *> getGlobalFrameWaypointList(); ///< Returns a global waypoint list
const QVector<Waypoint *> getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
const QVector<Waypoint *> getNavTypeWaypointList(); ///< Returns a waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
const QList<Waypoint *> getGlobalFrameWaypointList(); ///< Returns a global waypoint list
const QList<Waypoint *> getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
const QList<Waypoint *> getNavTypeWaypointList(); ///< Returns a waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
int getIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list
int getGlobalFrameIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only global waypoints
int getGlobalFrameAndNavTypeIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only global AND navigation mode waypoints
......@@ -171,10 +171,10 @@ private:
quint8 current_partner_compid; ///< The current protocol communication target component
bool read_to_edit; ///< If true, after readWaypoints() incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab.
QVector<Waypoint *> waypointsViewOnly; ///< local copy of current waypoint list on MAV
QVector<Waypoint *> waypointsEditable; ///< local editable waypoint list
QList<Waypoint *> waypointsViewOnly; ///< local copy of current waypoint list on MAV
QList<Waypoint *> waypointsEditable; ///< local editable waypoint list
Waypoint* currentWaypointEditable; ///< The currently used waypoint
QVector<mavlink_mission_item_t *> waypoint_buffer; ///< buffer for waypoints during communication
QList<mavlink_mission_item_t *> waypoint_buffer; ///< buffer for waypoints during communication
QTimer protocol_timer; ///< Timer to catch timeouts
bool standalone; ///< If standalone is set, do not write to UAS
quint16 uasid;
......
This diff is collapsed.
......@@ -634,7 +634,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
if (wpindex > 0)
{
// Get predecessor of this WP
QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
Waypoint* wp1 = wps.at(wpindex-1);
mapcontrol::WayPointItem* prevIcon = waypointsToIcons.value(wp1, NULL);
// If we got a valid graphics item, continue
......@@ -728,7 +728,7 @@ void QGCMapWidget::updateWaypointList(int uas)
// Delete first all old waypoints
// this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
foreach (Waypoint* wp, waypointsToIcons.keys())
{
if (!wps.contains(wp))
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment