From 50f933d3cb5afc8c1e2df2f1182990524b8d0a46 Mon Sep 17 00:00:00 2001 From: lm Date: Sun, 13 Feb 2011 16:12:11 +0100 Subject: [PATCH] Fixed waypoint reading --- src/comm/MAVLinkSimulationWaypointPlanner.cc | 2 +- src/ui/WaypointList.cc | 147 ++++++++++++++----- src/ui/WaypointView.cc | 8 +- 3 files changed, 112 insertions(+), 45 deletions(-) diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index 7ac54d387..d86eaa8a8 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -575,7 +575,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* float x = static_cast(pos.lat)/1E7; float y = static_cast(pos.lon)/1E7; - float z = static_cast(pos.alt)/1000; + //float z = static_cast(pos.alt)/1000; //qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z; diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index fe81e324c..ee69e6b37 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -276,49 +276,113 @@ void WaypointList::waypointListChanged() { // Prevent updates to prevent visual flicker this->setUpdatesEnabled(false); - // Get all waypoints const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - // Store the current state, then check which widgets to update - // and which ones to delete - QList oldWaypoints = wpViews.keys(); - - foreach (Waypoint* wp, waypoints) - { - // Create any new waypoint - if (!wpViews.contains(wp)) + if (!wpViews.empty()) { - WaypointView* wpview = new WaypointView(wp, this); - wpViews.insert(wp, wpview); - connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + QMapIterator viewIt(wpViews); + viewIt.toFront(); + while(viewIt.hasNext()) + { + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) + { + if (waypoints[i] == cur) + { + break; + } + } + if (i == waypoints.size()) + { + WaypointView* widget = wpViews.find(cur).value(); + widget->hide(); + listLayout->removeWidget(widget); + wpViews.remove(cur); + } + } } - else + + // then add/update the views for each waypoint in the list + for(int i = 0; i < waypoints.size(); i++) { - // Update existing waypoints + Waypoint *wp = waypoints[i]; + if (!wpViews.contains(wp)) + { + WaypointView* wpview = new WaypointView(wp, this); + wpViews.insert(wp, wpview); + connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); + connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); + connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + } + WaypointView *wpv = wpViews.value(wp); + wpv->updateValues(); // update the values of the ui elements in the view + listLayout->addWidget(wpv); - // Mark as updated by removing from old list - oldWaypoints.removeAt(oldWaypoints.indexOf(wp)); } - WaypointView *wpv = wpViews.value(wp); - wpv->updateValues(); // update the values of the ui elements in the view - listLayout->addWidget(wpv); + this->setUpdatesEnabled(true); } - // The old list now contains all wps to be deleted - foreach (Waypoint* wp, oldWaypoints) - { - // Delete waypoint view and entry in list - WaypointView* wpv = wpViews.value(wp); - if (wpv) - { - wpv->deleteLater(); - } - wpViews.remove(wp); - } + + loadFileGlobalWP = false; +} + +//void WaypointList::waypointListChanged() +//{ +// if (uas) +// { +// // Prevent updates to prevent visual flicker +// this->setUpdatesEnabled(false); +// // Get all waypoints +// const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + +//// // Store the current state, then check which widgets to update +//// // and which ones to delete +//// QList oldWaypoints = wpViews.keys(); + +//// foreach (Waypoint* wp, waypoints) +//// { +//// WaypointView* wpview; +//// // Create any new waypoint +//// if (!wpViews.contains(wp)) +//// { +//// wpview = new WaypointView(wp, this); +//// wpViews.insert(wp, wpview); +//// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); +//// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); +//// connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); +//// connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); +//// connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); +//// listLayout->addWidget(wpview); +//// } +//// else +//// { +//// // Update existing waypoints +//// wpview = wpViews.value(wp); + +//// } +//// // Mark as updated by removing from old list +//// oldWaypoints.removeAt(oldWaypoints.indexOf(wp)); + +//// wpview->updateValues(); // update the values of the ui elements in the view + +//// } + +//// // The old list now contains all wps to be deleted +//// foreach (Waypoint* wp, oldWaypoints) +//// { +//// // Delete waypoint view and entry in list +//// WaypointView* wpv = wpViews.value(wp); +//// if (wpv) +//// { +//// listLayout->removeWidget(wpv); +//// delete wpv; +//// } +//// wpViews.remove(wp); +//// } // if (!wpViews.empty()) // { @@ -339,8 +403,11 @@ void WaypointList::waypointListChanged() // if (i == waypoints.size()) // { // WaypointView* widget = wpViews.find(cur).value(); -// widget->hide(); -// listLayout->removeWidget(widget); +// if (widget) +// { +// widget->hide(); +// listLayout->removeWidget(widget); +// } // wpViews.remove(cur); // } // } @@ -365,10 +432,10 @@ void WaypointList::waypointListChanged() // listLayout->addWidget(wpv); // } - this->setUpdatesEnabled(true); - } - loadFileGlobalWP = false; -} +// this->setUpdatesEnabled(true); +// } +//// loadFileGlobalWP = false; +//} void WaypointList::moveUp(Waypoint* wp) { diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 9ab17d265..79bf475fd 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -334,10 +334,10 @@ void WaypointView::updateFrameView(int frame) void WaypointView::deleted(QObject* waypoint) { - if (waypoint == this->wp) - { - deleteLater(); - } +// if (waypoint == this->wp) +// { +// deleteLater(); +// } } void WaypointView::changedFrame(int index) -- 2.22.0