From a33541b96033165a84a4af4998b1670fa97b512a Mon Sep 17 00:00:00 2001 From: lm Date: Tue, 4 Jan 2011 10:45:54 +0100 Subject: [PATCH] Improved performance of 2D map, trails need further debugging --- src/GAudioOutput.cc | 6 +- src/ui/HUD.cc | 3 + src/ui/MainWindow.cc | 2 +- src/ui/MapWidget.cc | 199 ++++++++++++++++++++++------------------ src/ui/MapWidget.h | 11 ++- src/ui/QGCDataPlot2D.cc | 20 +--- src/ui/WaypointList.cc | 14 ++- 7 files changed, 138 insertions(+), 117 deletions(-) diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index 87c66503a..c672ef94c 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -144,9 +144,6 @@ muted(false) GAudioOutput::~GAudioOutput() { - QSettings settings; - settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", muted); - settings.sync(); #ifdef _MSC_VER2 ::CoUninitialize(); #endif @@ -155,6 +152,9 @@ GAudioOutput::~GAudioOutput() void GAudioOutput::mute(bool mute) { this->muted = mute; + QSettings settings; + settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", muted); + settings.sync(); } bool GAudioOutput::isMuted() diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index ad090a1c7..662df6355 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -117,6 +117,9 @@ HUD::HUD(int width, int height, QWidget* parent) // Set auto fill to false setAutoFillBackground(false); + // Set minimum size + setMinimumSize(80, 60); + // Fill with black background QImage fill = QImage(width, height, QImage::Format_Indexed8); fill.setNumColors(3); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 968f64055..3fba2a607 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -703,7 +703,7 @@ void MainWindow::connectCommonWidgets() if (mapWidget && waypointsDockWidget->widget()) { // clear path create on the map - connect(waypointsDockWidget->widget(), SIGNAL(clearPathclicked()), mapWidget, SLOT(clearPath())); + connect(waypointsDockWidget->widget(), SIGNAL(clearPathclicked()), mapWidget, SLOT(clearWaypoints())); // add Waypoint widget in the WaypointList widget when mouse clicked connect(mapWidget, SIGNAL(captureMapCoordinateClick(QPointF)), waypointsDockWidget->widget(), SLOT(addWaypointMouse(QPointF))); diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 385f66f9f..cda58c62d 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -30,6 +30,7 @@ MapWidget::MapWidget(QWidget *parent) : uasIcons(), uasTrails(), mav(NULL), + lastUpdate(0), m_ui(new Ui::MapWidget) { m_ui->setupUi(this); @@ -61,6 +62,10 @@ MapWidget::MapWidget(QWidget *parent) : overlay->setVisible(false); mc->addLayer(overlay); + // MAV FLIGHT TRACKS + tracks = new qmapcontrol::MapLayer("Tracking", mapadapter); + mc->addLayer(tracks); + // WAYPOINT LAYER // create a layer with the mapadapter and type GeometryLayer (for waypoints) geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter); @@ -139,12 +144,14 @@ MapWidget::MapWidget(QWidget *parent) : QPushButton* zoomin = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this); QPushButton* zoomout = new QPushButton(QIcon(":/images/actions/list-remove.svg"), "", this); createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this); + clearTracking = new QPushButton(QIcon(""), "", this); followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this); - zoomin->setMaximumWidth(50); - zoomout->setMaximumWidth(50); - createPath->setMaximumWidth(50); - followgps->setMaximumWidth(50); + zoomin->setMaximumWidth(30); + zoomout->setMaximumWidth(30); + createPath->setMaximumWidth(30); + clearTracking->setMaximumWidth(30); + followgps->setMaximumWidth(30); // Set checkable buttons // TODO: Currently checked buttons are are very difficult to distinguish when checked. @@ -160,9 +167,10 @@ MapWidget::MapWidget(QWidget *parent) : innerlayout->addWidget(zoomout, 1, 0); innerlayout->addWidget(followgps, 2, 0); innerlayout->addWidget(createPath, 3, 0); + innerlayout->addWidget(clearTracking, 4, 0); // Add spacers to compress buttons on the top left - innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 4, 0); - innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 5); + innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); + innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 6); innerlayout->setRowStretch(0, 1); innerlayout->setRowStretch(1, 100); mc->setLayout(innerlayout); @@ -206,9 +214,8 @@ MapWidget::MapWidget(QWidget *parent) : // Configure the WP Path's pen pointPen = new QPen(QColor(0, 255,0)); pointPen->setWidth(3); - - path = new qmapcontrol::LineString (wps, "UAV Path", pointPen); - mc->layer("Waypoints")->addGeometry(path); + waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen); + mc->layer("Waypoints")->addGeometry(waypointPath); //Camera Control // CAMERA INDICATOR LAYER @@ -310,8 +317,6 @@ void MapWidget::createPathButtonClicked(bool checked) { Q_UNUSED(checked); - - if (createPath->isChecked()) { // change the cursor shape @@ -329,14 +334,12 @@ void MapWidget::createPathButtonClicked(bool checked) // path->setPoints(wps); // mc->layer("Waypoints")->addGeometry(path); // wpIndex.clear(); - - - } else { + } + else + { this->setCursor(Qt::ArrowCursor); mc->setMouseMode(qmapcontrol::MapControl::Panning); - - } } @@ -351,15 +354,13 @@ void MapWidget::createPathButtonClicked(bool checked) void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate) { - - qDebug() << mc->mouseMode(); - + //qDebug() << mc->mouseMode(); if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked()) { // Create waypoint name QString str; - str = QString("%1").arg(path->numberOfPoints()); + str = QString("%1").arg(waypointPath->numberOfPoints()); // create the WP and set everything in the LineString to display the path Waypoint2DIcon* tempCirclePoint; @@ -376,27 +377,27 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str); wps.append(tempPoint); - path->addPoint(tempPoint); + waypointPath->addPoint(tempPoint); wpIndex.insert(str,tempPoint); // Refresh the screen - mc->updateRequestNew(); + mc->updateRequest(tempPoint->boundingBox().toRect()); // emit signal mouse was clicked emit captureMapCoordinateClick(coordinate); - } } void MapWidget::createWaypointGraphAtMap(const QPointF coordinate) { - if (!wpExists(coordinate)){ + if (!wpExists(coordinate)) + { // Create waypoint name QString str; - str = QString("%1").arg(path->numberOfPoints()); + str = QString("%1").arg(waypointPath->numberOfPoints()); // create the WP and set everything in the LineString to display the path //CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str); @@ -416,13 +417,13 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate) Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str); wps.append(tempPoint); - path->addPoint(tempPoint); + waypointPath->addPoint(tempPoint); wpIndex.insert(str,tempPoint); qDebug()<<"Funcion createWaypointGraphAtMap WP= "< x= "<latitude()<<" y= "<longitude(); // Refresh the screen - mc->updateRequestNew(); + mc->updateRequest(tempPoint->boundingBox().toRect()); } //// // emit signal mouse was clicked @@ -456,7 +457,7 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) waypointIsDrag = true; // Refresh the screen - mc->updateRequestNew(); + mc->updateRequest(geom->boundingBox().toRect()); int temp = 0; qmapcontrol::Point* point2Find; @@ -505,10 +506,6 @@ MapWidget::~MapWidget() */ void MapWidget::addUAS(UASInterface* uas) { - if(mav != NULL) - { - disconnect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - } connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); } @@ -517,7 +514,11 @@ void MapWidget::activeUASSet(UASInterface* uas) if (uas) { mav = uas; - path->setPen(new QPen(mav->getColor())); + QColor color = mav->getColor(); + color.setAlphaF(0.6); + QPen* pen = new QPen(color); + pen->setWidth(3.0); + // FIXME Load waypoints of this system } } @@ -534,62 +535,73 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, { Q_UNUSED(usec); Q_UNUSED(alt); // FIXME Use altitude + + // create a LineString + //QList points; + // Points with a circle + // A QPen can be used to customize the + //pointpen->setWidth(3); + //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); + + MAV2DIcon* p; + + if (!uasIcons.contains(uas->getUASID())) + { + // Get the UAS color + QColor uasColor = uas->getColor(); + + // Icon + QPen* pointpen = new QPen(uasColor); + qDebug() << uas->getUASName(); + p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, pointpen); + uasIcons.insert(uas->getUASID(), p); + tracks->addGeometry(p); + + // Line + // A QPen also can use transparency + + QList points; + points.append(new qmapcontrol::Point(lat, lon, "")); + QPen* linepen = new QPen(uasColor.darker()); + linepen->setWidth(2); + + // Create tracking line string + qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, uas->getUASName(), linepen); + uasTrails.insert(uas->getUASID(), ls); + + // Add the LineString to the layer + mc->layer("Tracking")->addGeometry(ls); + } + else + { + p = dynamic_cast(uasIcons.value(uas->getUASID())); + if (p) + { + p->setCoordinate(QPointF(lat, lon)); + p->setYaw(uas->getYaw()); + } + // Extend trail + uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, "")); + } + + //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); + + + // Limit the position update rate quint64 currTime = MG::TIME::getGroundTimeNow(); - if (currTime - lastUpdate > 90) + if (currTime - lastUpdate > 120) { lastUpdate = currTime; - // create a LineString - //QList points; - // Points with a circle - // A QPen can be used to customize the - //pointpen->setWidth(3); - //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); - - if (!uasIcons.contains(uas->getUASID())) + // Sets the view to the interesting area + if (followgps->isChecked()) { - // Get the UAS color - QColor uasColor = uas->getColor(); - - // Icon - QPen* pointpen = new QPen(uasColor); - qDebug() << uas->getUASName(); - MAV2DIcon* p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, pointpen); - uasIcons.insert(uas->getUASID(), p); - geomLayer->addGeometry(p); - - // Line - // A QPen also can use transparency - - QList points; - points.append(new qmapcontrol::Point(lat, lon, "")); - QPen* linepen = new QPen(uasColor.darker()); - linepen->setWidth(2); - // Add the Points and the QPen to a LineString - qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, uas->getUASName(), linepen); - uasTrails.insert(uas->getUASID(), ls); - - // Add the LineString to the layer - geomLayer->addGeometry(ls); + updatePosition(0, lat, lon); } else { - MAV2DIcon* p = dynamic_cast(uasIcons.value(uas->getUASID())); - if (p) - { - p->setCoordinate(QPointF(lat, lon)); - p->setYaw(uas->getYaw()); - } - // Extend trail - uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, "")); + // Refresh the screen + //mc->updateRequestNew(); } - - - // Connect click events of the layer to this object - // connect(osmLayer, SIGNAL(geometryClicked(Geometry*, QPoint)), - // this, SLOT(geometryClicked(Geometry*, QPoint))); - - // Sets the view to the interesting area - updatePosition(0, lat, lon); } } @@ -677,23 +689,36 @@ void MapWidget::changeEvent(QEvent *e) break; } } -void MapWidget::clearPath() + +void MapWidget::clearWaypoints() { // Clear the previous WP track mc->layer("Waypoints")->clearGeometries(); wps.clear(); - path->setPoints(wps); - mc->layer("Waypoints")->addGeometry(path); + waypointPath->setPoints(wps); + mc->layer("Waypoints")->addGeometry(waypointPath); wpIndex.clear(); - mc->updateRequestNew(); - + mc->updateRequest(waypointPath->boundingBox().toRect()); if(createPath->isChecked()) { createPath->click(); } +} +void MapWidget::clearPath() +{ + mc->layer("Tracking")->clearGeometries(); + foreach (qmapcontrol::LineString* ls, uasTrails) + { + QPen* linepen = ls->pen(); + delete ls; + qmapcontrol::LineString* lsNew = new qmapcontrol::LineString(QList(), "", linepen); + mc->layer("Tracking")->addGeometry(lsNew); + } + // FIXME update this with update request only for bounding box of trails + mc->updateRequest(QRect(0, 0, width(), height())); } void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon) @@ -714,7 +739,7 @@ void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, floa point2Find->setCoordinate(coordinate); // Refresh the screen - mc->updateRequestNew(); + mc->updateRequest(point2Find->boundingBox().toRect()); } diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h index f3dde1ed4..536543316 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -71,7 +71,9 @@ public slots: void updateCameraPosition(double distance, double bearing, QString dir); QPointF getPointxBearing_Range(double lat1, double lon1, double bearing, double distance); - //ROCA + /** @brief Clear the waypoints overlay layer */ + void clearWaypoints(); + /** @brief Clear the UAV tracks on the map */ void clearPath(); void changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon); void drawBorderCamAtMap(bool status); @@ -96,6 +98,7 @@ protected: QPushButton* followgps; QPushButton* createPath; + QPushButton* clearTracking; QLabel* gpsposition; QMenu* mapMenu; QPushButton* mapButton; @@ -104,6 +107,7 @@ protected: qmapcontrol::MapAdapter* mapadapter; ///< Adapter to load the map data qmapcontrol::Layer* l; ///< Current map layer (background) qmapcontrol::Layer* overlay; ///< Street overlay (foreground) + qmapcontrol::Layer* tracks; ///< Layer for UAV tracks qmapcontrol::GeometryLayer* geomLayer; ///< Layer for waypoints //only for experiment @@ -132,9 +136,6 @@ protected: void createWaypointGraphAtMap (const QPointF coordinate); void mapproviderSelected(QAction* action); - - - signals: //void movePoint(QPointF newCoord); void captureMapCoordinateClick(const QPointF coordinate); //ROCA @@ -145,8 +146,8 @@ protected: private: Ui::MapWidget *m_ui; QList wps; + qmapcontrol::LineString* waypointPath; QHash wpIndex; - qmapcontrol::LineString* path; QPen* pointPen; int wpExists(const QPointF coordinate); bool waypointIsDrag; diff --git a/src/ui/QGCDataPlot2D.cc b/src/ui/QGCDataPlot2D.cc index 5d0e01179..f4a41b378 100644 --- a/src/ui/QGCDataPlot2D.cc +++ b/src/ui/QGCDataPlot2D.cc @@ -307,7 +307,8 @@ void QGCDataPlot2D::selectFile() void QGCDataPlot2D::loadRawLog(QString file, QString xAxisName, QString yAxisFilter) { - qDebug() << "LOADING RAW LOG!"; + Q_UNUSED(xAxisName); + Q_UNUSED(yAxisFilter); if (logFile != NULL) { @@ -320,23 +321,6 @@ void QGCDataPlot2D::loadRawLog(QString file, QString xAxisName, QString yAxisFil connect(compressor, SIGNAL(logProcessingStatusChanged(QString)), MainWindow::instance(), SLOT(showStatusMessage(QString))); connect(compressor, SIGNAL(finishedFile(QString)), this, SLOT(loadFile(QString))); compressor->startCompression(); - -// // Block UI -// QProgressDialog progress("Transforming RAW log file to CSV", "Abort Transformation", 0, 1, this); -// progress.setWindowModality(Qt::WindowModal); - -// while (!compressor->isFinished()) -// { -// MG::SLEEP::usleep(100000); -//// progress.setMaximum(compressor->getDataLines()); -//// progress.setValue(compressor->getCurrentLine()); -// } -// // Enforce end -// progress.setMaximum(compressor->getDataLines()); -// progress.setValue(compressor->getDataLines()); - - // Done with preprocessing - now load csv log - //loadFile(logFile->fileName()); } /** diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 2632a1b71..b9a090b2f 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -197,20 +197,28 @@ void WaypointList::add() { const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + Waypoint *wp; + + if (waypoints.size() > 0) { + // Create waypoint with last frame Waypoint *last = waypoints.at(waypoints.size()-1); - Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), + wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime(), last->getFrame(), last->getAction()); uas->getWaypointManager().addWaypoint(wp); } else { - //isLocalWP = true; - Waypoint *wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), + // Create global frame waypoint per default + wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 2000); uas->getWaypointManager().addWaypoint(wp); } + if (wp->getFrame() == MAV_FRAME_GLOBAL) + { + emit createWaypointAtMap(QPointF(wp->getX(), wp->getY())); + } } } } -- 2.22.0