diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index f4a0b6d..253720c 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -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 UASWaypointManager::getGlobalFrameWaypointList() +const QList UASWaypointManager::getGlobalFrameWaypointList() { // TODO Keep this global frame list up to date // with complete waypoint list // instead of filtering on each request - QVector wps; + QList wps; foreach (Waypoint* wp, waypointsEditable) { if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) @@ -565,12 +565,12 @@ const QVector UASWaypointManager::getGlobalFrameWaypointList() return wps; } -const QVector UASWaypointManager::getGlobalFrameAndNavTypeWaypointList() +const QList UASWaypointManager::getGlobalFrameAndNavTypeWaypointList() { // TODO Keep this global frame list up to date // with complete waypoint list // instead of filtering on each request - QVector wps; + QList 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 UASWaypointManager::getGlobalFrameAndNavTypeWaypointLi return wps; } -const QVector UASWaypointManager::getNavTypeWaypointList() +const QList UASWaypointManager::getNavTypeWaypointList() { // TODO Keep this global frame list up to date // with complete waypoint list // instead of filtering on each request - QVector wps; + QList 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(); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index ad0fbbb..f0a5566 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -33,7 +33,7 @@ This file is part of the QGROUNDCONTROL project #define UASWAYPOINTMANAGER_H #include -#include +#include #include #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 &getWaypointEditableList(void) { + const QList &getWaypointEditableList(void) { return waypointsEditable; ///< Returns a const reference to the waypoint list. } - const QVector &getWaypointViewOnlyList(void) { + const QList &getWaypointViewOnlyList(void) { return waypointsViewOnly; ///< Returns a const reference to the waypoint list. } - const QVector getGlobalFrameWaypointList(); ///< Returns a global waypoint list - const QVector getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out. - const QVector getNavTypeWaypointList(); ///< Returns a waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out. + const QList getGlobalFrameWaypointList(); ///< Returns a global waypoint list + const QList getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out. + const QList 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 waypointsViewOnly; ///< local copy of current waypoint list on MAV - QVector waypointsEditable; ///< local editable waypoint list + QList waypointsViewOnly; ///< local copy of current waypoint list on MAV + QList waypointsEditable; ///< local editable waypoint list Waypoint* currentWaypointEditable; ///< The currently used waypoint - QVector waypoint_buffer; ///< buffer for waypoints during communication + QList 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; diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 81eef7f..f4473a0 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -254,15 +254,15 @@ void WaypointList::addEditable() void WaypointList::addEditable(bool onCurrentPosition) { - const QVector &waypoints = WPM->getWaypointEditableList(); + const QList &waypoints = WPM->getWaypointEditableList(); Waypoint *wp; - if (waypoints.size() > 0 && + if (waypoints.count() > 0 && !(onCurrentPosition && uas && (uas->localPositionKnown() || uas->globalPositionKnown()))) { // Create waypoint with last frame - Waypoint *last = waypoints.at(waypoints.size()-1); + Waypoint *last = waypoints.last(); wp = WPM->createWaypoint(); - wp->blockSignals(true); +// wp->blockSignals(true); MAV_FRAME frame = (MAV_FRAME)last->getFrame(); wp->setFrame(frame); if (frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) @@ -280,8 +280,7 @@ void WaypointList::addEditable(bool onCurrentPosition) wp->setParam1(last->getParam3()); wp->setParam1(last->getParam4()); wp->setAutocontinue(last->getAutoContinue()); - - wp->blockSignals(false); +// wp->blockSignals(false); wp->setAction(last->getAction()); // WPM->addWaypointEditable(wp); } @@ -367,61 +366,6 @@ void WaypointList::addCurrentPositionWaypoint() { addEditable(true); } - -//int WaypointList::addCurrentPositionWaypoint() -//{ -// if (uas) -// { -// const QVector &waypoints = WPM->getWaypointEditableList(); -// Waypoint *wp; -// Waypoint *last = 0; -// if (waypoints.size() > 0) -// { -// last = waypoints.at(waypoints.size()-1); -// } - -// if (uas->globalPositionKnown()) -// { -// float acceptanceRadiusGlobal = 10.0f; -// float holdTime = 0.0f; -// float yawGlobal = 0.0f; -// if (last) -// { -// acceptanceRadiusGlobal = last->getAcceptanceRadius(); -// holdTime = last->getHoldTime(); -// yawGlobal = last->getYaw(); -// } -// // Create global frame waypoint per default -// wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); -// WPM->addWaypointEditable(wp); -// updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint")); -// return 0; -// } -// else if (uas->localPositionKnown()) -// { -// float acceptanceRadiusLocal = 0.2f; -// float holdTime = 0.5f; -// if (last) -// { -// acceptanceRadiusLocal = last->getAcceptanceRadius(); -// holdTime = last->getHoldTime(); -// } -// // Create local frame waypoint as second option -// wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, false, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); -// WPM->addWaypointEditable(wp); -// updateStatusLabel(tr("Added LOCAL (NED) waypoint")); -// return 0; -// } -// else -// { -// // Do nothing -// updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet.")); -// return 1; -// } -// } -// return 1; -//} - void WaypointList::updateStatusLabel(const QString &string) { // Status label in write widget @@ -443,11 +387,11 @@ void WaypointList::changeCurrentWaypoint(quint16 seq) void WaypointList::currentWaypointEditableChanged(quint16 seq) { WPM->setCurrentEditable(seq); - const QVector &waypoints = WPM->getWaypointEditableList(); + const QList &waypoints = WPM->getWaypointEditableList(); - if (seq < waypoints.size()) + if (seq < waypoints.count()) { - for(int i = 0; i < waypoints.size(); i++) + for(int i = 0; i < waypoints.count(); i++) { WaypointEditableView* widget = wpEditableViews.find(waypoints[i]).value(); @@ -466,11 +410,11 @@ void WaypointList::currentWaypointEditableChanged(quint16 seq) // Update waypointViews to correctly indicate the new current waypoint void WaypointList::currentWaypointViewOnlyChanged(quint16 seq) { - const QVector &waypoints = WPM->getWaypointViewOnlyList(); + const QList &waypoints = WPM->getWaypointViewOnlyList(); - if (seq < waypoints.size()) + if (seq < waypoints.count()) { - for(int i = 0; i < waypoints.size(); i++) + for(int i = 0; i < waypoints.count(); i++) { WaypointViewOnlyView* widget = wpViewOnlyViews.find(waypoints[i]).value(); @@ -504,7 +448,7 @@ void WaypointList::waypointViewOnlyListChanged() { // Prevent updates to prevent visual flicker this->setUpdatesEnabled(false); - const QVector &waypoints = WPM->getWaypointViewOnlyList(); + const QList &waypoints = WPM->getWaypointViewOnlyList(); if (!wpViewOnlyViews.empty()) { QMapIterator viewIt(wpViewOnlyViews); @@ -513,12 +457,12 @@ void WaypointList::waypointViewOnlyListChanged() viewIt.next(); Waypoint *cur = viewIt.key(); int i; - for (i = 0; i < waypoints.size(); i++) { + for (i = 0; i < waypoints.count(); i++) { if (waypoints[i] == cur) { break; } } - if (i == waypoints.size()) { + if (i == waypoints.count()) { WaypointViewOnlyView* widget = wpViewOnlyViews.find(cur).value(); widget->hide(); viewOnlyListLayout->removeWidget(widget); @@ -528,7 +472,7 @@ void WaypointList::waypointViewOnlyListChanged() } // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) { + for(int i = 0; i < waypoints.count(); i++) { Waypoint *wp = waypoints[i]; if (!wpViewOnlyViews.contains(wp)) { WaypointViewOnlyView* wpview = new WaypointViewOnlyView(wp, this); @@ -556,7 +500,7 @@ void WaypointList::waypointEditableListChanged() { // Prevent updates to prevent visual flicker this->setUpdatesEnabled(false); - const QVector &waypoints = WPM->getWaypointEditableList(); + const QList &waypoints = WPM->getWaypointEditableList(); if (!wpEditableViews.empty()) { QMapIterator viewIt(wpEditableViews); @@ -565,12 +509,12 @@ void WaypointList::waypointEditableListChanged() viewIt.next(); Waypoint *cur = viewIt.key(); int i; - for (i = 0; i < waypoints.size(); i++) { + for (i = 0; i < waypoints.count(); i++) { if (waypoints[i] == cur) { break; } } - if (i == waypoints.size()) { + if (i == waypoints.count()) { WaypointEditableView* widget = wpEditableViews.find(cur).value(); widget->hide(); editableListLayout->removeWidget(widget); @@ -580,7 +524,7 @@ void WaypointList::waypointEditableListChanged() } // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) { + for(int i = 0; i < waypoints.count(); i++) { Waypoint *wp = waypoints[i]; if (!wpEditableViews.contains(wp)) { WaypointEditableView* wpview = new WaypointEditableView(wp, this); @@ -610,34 +554,34 @@ void WaypointList::waypointEditableListChanged() void WaypointList::moveUp(Waypoint* wp) { - const QVector &waypoints = WPM->getWaypointEditableList(); + const QList &waypoints = WPM->getWaypointEditableList(); //get the current position of wp in the local storage int i; - for (i = 0; i < waypoints.size(); i++) { + for (i = 0; i < waypoints.count(); i++) { if (waypoints[i] == wp) break; } // if wp was found and its not the first entry, move it - if (i < waypoints.size() && i > 0) { + if (i < waypoints.count() && i > 0) { WPM->moveWaypoint(i, i-1); } } void WaypointList::moveDown(Waypoint* wp) { - const QVector &waypoints = WPM->getWaypointEditableList(); + const QList &waypoints = WPM->getWaypointEditableList(); //get the current position of wp in the local storage int i; - for (i = 0; i < waypoints.size(); i++) { + for (i = 0; i < waypoints.count(); i++) { if (waypoints[i] == wp) break; } // if wp was found and its not the last entry, move it - if (i < waypoints.size()-1) { + if (i < waypoints.count()-1) { WPM->moveWaypoint(i, i+1); } } @@ -664,72 +608,25 @@ void WaypointList::on_clearWPListButton_clicked() { if (uas) { emit clearPathclicked(); - const QVector &waypoints = WPM->getWaypointEditableList(); - while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) + const QList &waypoints = WPM->getWaypointEditableList(); + while(!waypoints.isEmpty()) { WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); widget->remove(); } } } -///** @brief The MapWidget informs that a waypoint global was changed on the map */ - -//void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) -//{ -// if (uas) -// { -// const QVector &waypoints = WPM->getWaypointEditableList(); -// if (waypoints.size() > 0) -// { -// Waypoint *temp = waypoints.at(indexWP); - -// temp->setX(coordinate.x()); -// temp->setY(coordinate.y()); - -// //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); -// //widget->updateValues(); -// } -// } - - -//} - -///** @brief The MapWidget informs that a waypoint global was changed on the map */ - -//void WaypointList::waypointGlobalPositionChanged(Waypoint* wp) -//{ -// QPointF coordinate; -// coordinate.setX(wp->getX()); -// coordinate.setY(wp->getY()); - -// emit ChangeWaypointGlobalPosition(wp->getId(), coordinate); - - -//} - void WaypointList::clearWPWidget() { // Get list - const QVector &waypoints = WPM->getWaypointEditableList(); + const QList &waypoints = WPM->getWaypointEditableList(); // XXX delete wps as well // Clear UI elements - while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) + while(!waypoints.isEmpty()) { WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); widget->remove(); } } - -//void WaypointList::setIsLoadFileWP() -//{ -// loadFileGlobalWP = true; -//} - -//void WaypointList::setIsReadGlobalWP(bool value) -//{ -// // FIXME James Check this -// Q_UNUSED(value); -// // readGlobalWP = value; -//} diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc index 6b3e8aa..e9df9cc 100644 --- a/src/ui/map/QGCMapWidget.cc +++ b/src/ui/map/QGCMapWidget.cc @@ -634,7 +634,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp) if (wpindex > 0) { // Get predecessor of this WP - QVector wps = currWPManager->getGlobalFrameAndNavTypeWaypointList(); + QList 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 wps = currWPManager->getGlobalFrameAndNavTypeWaypointList(); + QList wps = currWPManager->getGlobalFrameAndNavTypeWaypointList(); foreach (Waypoint* wp, waypointsToIcons.keys()) { if (!wps.contains(wp))