From acf7e9c486725e91c4e0d569eba74124d05289db Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 12 Sep 2011 20:39:17 +0200 Subject: [PATCH 1/3] Fixed a number of small bugs and issues, WP handling makes more sense now --- src/uas/UASWaypointManager.cc | 28 ++++++++++++++++++------- src/uas/UASWaypointManager.h | 1 + src/ui/HSIDisplay.cc | 27 +++++------------------- src/ui/MainWindow.cc | 18 ++++++++++++---- src/ui/QGCToolBar.cc | 6 ++---- src/ui/linechart/LinechartPlot.cc | 43 +++++++++++++++++++++++++++++++-------- src/ui/linechart/LinechartPlot.h | 7 ++++++- 7 files changed, 83 insertions(+), 47 deletions(-) diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 8398ed9..3949cbc 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -46,7 +46,8 @@ UASWaypointManager::UASWaypointManager(UAS &_uas) current_state(WP_IDLE), current_partner_systemid(0), current_partner_compid(0), - protocol_timer(this) + protocol_timer(this), + currentWaypoint(NULL) { connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout())); connect(&uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64))); @@ -92,11 +93,11 @@ void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x, { Q_UNUSED(mav); Q_UNUSED(time); - if (waypoints.count() > 0 && (waypoints[current_wp_id]->getFrame() == MAV_FRAME_LOCAL || waypoints[current_wp_id]->getFrame() == MAV_FRAME_LOCAL_ENU)) + if (waypoints.count() > 0 && currentWaypoint && (currentWaypoint->getFrame() == MAV_FRAME_LOCAL || currentWaypoint->getFrame() == MAV_FRAME_LOCAL_ENU)) { - double xdiff = x-waypoints[current_wp_id]->getX(); - double ydiff = y-waypoints[current_wp_id]->getY(); - double zdiff = z-waypoints[current_wp_id]->getZ(); + double xdiff = x-currentWaypoint->getX(); + double ydiff = y-currentWaypoint->getY(); + double zdiff = z-currentWaypoint->getZ(); double dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff); emit waypointDistanceChanged(dist); } @@ -146,6 +147,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ //// 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; //get next waypoint current_wp_id++; @@ -164,6 +166,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ protocol_timer.stop(); emit readGlobalWPFromUAS(false); + if (currentWaypoint) emit currentWaypointChanged(currentWaypoint->getId()); emit updateStatusString("done."); // qDebug() << "got all waypoints from ID " << systemId; @@ -232,6 +235,8 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m for(int i = 0; i < waypoints.size(); i++) { if (waypoints[i]->getId() == wpc->seq) { waypoints[i]->setCurrent(true); + currentWaypoint = waypoints[i]; + } else { waypoints[i]->setCurrent(false); } @@ -267,6 +272,7 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq) for(int i = 0; i < waypoints.size(); i++) { if (waypoints[i]->getId() == seq) { waypoints[i]->setCurrent(true); + currentWaypoint = waypoints[i]; } else { waypoints[i]->setCurrent(false); } @@ -302,7 +308,11 @@ void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive) { if (wp) { wp->setId(waypoints.size()); - if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true); + if (enforceFirstActive && waypoints.size() == 0) + { + wp->setCurrent(true); + currentWaypoint = wp; + } waypoints.insert(waypoints.size(), wp); connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*))); @@ -318,7 +328,11 @@ Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive) { Waypoint* wp = new Waypoint(); wp->setId(waypoints.size()); - if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true); + if (enforceFirstActive && waypoints.size() == 0) + { + wp->setCurrent(true); + currentWaypoint = wp; + } waypoints.insert(waypoints.size(), wp); connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*))); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index b430347..90a6a0e 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -163,6 +163,7 @@ private: quint8 current_partner_compid; ///< The current protocol communication target component QVector waypoints; ///< local waypoint list (main storage) + Waypoint* currentWaypoint; ///< The currently used waypoint QVector waypoint_buffer; ///< buffer for waypoints during communication QTimer protocol_timer; ///< Timer to catch timeouts }; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 2aa8caf..e24034f 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -42,7 +42,7 @@ This file is part of the QGROUNDCONTROL project #include "UASWaypointManager.h" #include //#include "Waypoint2DIcon.h" -//#include "MAV2DIcon.h" +#include "MAV2DIcon.h" #include @@ -115,24 +115,6 @@ HSIDisplay::HSIDisplay(QWidget *parent) : xCenterPos = vwidth/2.0f; yCenterPos = vheight/2.0f + topMargin - bottomMargin; - //qDebug() << "CENTER" << xCenterPos << yCenterPos; - - // Add interaction elements - QHBoxLayout* layout = new QHBoxLayout(this); - layout->setMargin(0); - layout->setSpacing(12); - QDoubleSpinBox* spinBox = new QDoubleSpinBox(this); - spinBox->setMinimum(0.1); - spinBox->setMaximum(9999); - spinBox->setMaximumWidth(50); - spinBox->setValue(metricWidth); - spinBox->setToolTip(tr("Ground width in meters shown on instrument")); - spinBox->setStatusTip(tr("Ground width in meters shown on instrument")); - connect(spinBox, SIGNAL(valueChanged(double)), this, SLOT(setMetricWidth(double))); - connect(this, SIGNAL(metricWidthChanged(double)), spinBox, SLOT(setValue(double))); - layout->addWidget(spinBox); - layout->setAlignment(spinBox, Qt::AlignBottom | Qt::AlignRight); - this->setLayout(layout); uas = NULL; resetMAVState(); @@ -246,6 +228,7 @@ void HSIDisplay::renderOverlay() // Translate to center painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor); QColor uasColor = uas->getColor(); + MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f); //MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f); // Translate back painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor); @@ -309,8 +292,8 @@ void HSIDisplay::renderOverlay() // Draw crosstrack error to top right float crossTrackError = 0; - paintText(tr("XTRACK"), QGC::colorCyan, 2.2f, 57, 11, &painter); - paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), Qt::white, 2.2f, 70, 11, &painter); + paintText(tr("XTRACK"), QGC::colorCyan, 2.2f, 54, 11, &painter); + paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), Qt::white, 2.2f, 67, 11, &painter); // Draw position to bottom left if (localAvailable > 0 && globalAvailable == 0) { @@ -330,7 +313,7 @@ void HSIDisplay::renderOverlay() } // Draw Field of view to bottom right - paintText(tr("FOV"), QGC::colorCyan, 2.6f, 62, vheight- 5.0f, &painter); + paintText(tr("FOV %1 m").arg(metricWidth, 5, 'f', 1, ' '), QGC::colorCyan, 2.6f, 55, vheight- 5.0f, &painter); } void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter) diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 81d6c89..7062b08 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -134,7 +134,7 @@ MainWindow::MainWindow(QWidget *parent): toolBar->addPerspectiveChangeAction(ui.actionOperatorsView); toolBar->addPerspectiveChangeAction(ui.actionEngineersView); toolBar->addPerspectiveChangeAction(ui.actionPilotsView); - toolBar->addPerspectiveChangeAction(ui.actionUnconnectedView); +// toolBar->addPerspectiveChangeAction(ui.actionUnconnectedView); buildCommonWidgets(); connectCommonWidgets(); @@ -296,7 +296,6 @@ void MainWindow::setDefaultSettingsForAp() void MainWindow::resizeEvent(QResizeEvent * event) { - Q_UNUSED(event); if (height() < 800) { ui.statusBar->setVisible(false); @@ -306,6 +305,17 @@ void MainWindow::resizeEvent(QResizeEvent * event) ui.statusBar->setVisible(true); ui.statusBar->setSizeGripEnabled(true); } + + if (width() > 1200) + { + toolBar->setToolButtonStyle(Qt::ToolButtonTextBesideIcon); + } + else + { + toolBar->setToolButtonStyle(Qt::ToolButtonIconOnly); + } + + QMainWindow::resizeEvent(event); } QString MainWindow::getWindowStateKey() @@ -730,8 +740,8 @@ void MainWindow::loadCustomWidgetsFromDefaults(const QString& systemType, const { // Will only be loaded if not already a custom widget with // the same name is present - qDebug() << "Tried to load file: " << file; - loadCustomWidget(defaultsDir+"/"+file, true); + qDebug() << "Tried to load file: " << defaultsDir+file; + loadCustomWidget(defaultsDir+file, true); } } } diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 357ea8b..f8a0bae 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -41,10 +41,9 @@ QGCToolBar::QGCToolBar(QWidget *parent) : logReplayAction = new QAction(QIcon(":"), "Replay", this); logReplayAction->setCheckable(true); - addSeparator(); - addAction(toggleLoggingAction); addAction(logReplayAction); +// addSeparator(); // CREATE TOOLBAR ITEMS // Add internal actions @@ -64,11 +63,10 @@ QGCToolBar::QGCToolBar(QWidget *parent) : toolBarBatteryBar->setStyleSheet("QProgressBar:horizontal { margin: 0px 4px 0px 0px; border: 1px solid #4A4A4F; border-radius: 4px; text-align: center; padding: 2px; color: #111111; background-color: #111118; height: 10px; } QProgressBar:horizontal QLabel { font-size: 9px; color: #111111; } QProgressBar::chunk { background-color: green; }"); toolBarBatteryBar->setMinimum(0); toolBarBatteryBar->setMaximum(100); - toolBarBatteryBar->setMinimumWidth(200); + toolBarBatteryBar->setMinimumWidth(20); toolBarBatteryBar->setMaximumWidth(200); toolBarBatteryVoltageLabel = new QLabel("xx.x V"); toolBarBatteryVoltageLabel->setStyleSheet(QString("QLabel { margin: 0px 0px 0px 4px; font: 14px; color: %1; }").arg(QColor(Qt::green).name())); - //symbolButton->setIcon(":"); symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px 0px 0px 20px; background-color: none; }"); addWidget(symbolButton); addWidget(toolBarNameLabel); diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 765a7a9..833e000 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -126,10 +126,7 @@ LinechartPlot::LinechartPlot(QWidget *parent, int plotid, quint64 interval): Qwt connect(updateTimer, SIGNAL(timeout()), this, SLOT(paintRealtime())); //updateTimer->start(DEFAULT_REFRESH_RATE); - // QwtPlot::setAutoReplot(); - - // canvas()->setPaintAttribute(QwtPlotCanvas::PaintCached, false); - // canvas()->setPaintAttribute(QwtPlotCanvas::PaintPacked, false); + connect(&timeoutTimer, SIGNAL(timeout()), this, SLOT(removeTimedOutCurves())); } LinechartPlot::~LinechartPlot() @@ -206,6 +203,34 @@ void LinechartPlot::setActive(bool active) m_active = active; } +void LinechartPlot::removeTimedOutCurves() +{ + foreach(QString key, lastUpdate.keys()) + { + quint64 time = lastUpdate.value(key); + if (QGC::groundTimeMilliseconds() - time > 20000) + { + // Remove this curve + // Delete curves + QwtPlotCurve* curve = curves.take(key); + // Delete the object + delete curve; + // Set the pointer null + curve = NULL; + + // Notify connected components about the removal + emit curveRemoved(key); + + // Remove from data list + TimeSeriesData* d = data.take(key); + // Delete the object + delete d; + // Set the pointer null + d = NULL; + } + } +} + /** * @brief Set the zero (center line) value * The zero value defines the centerline of the plot. @@ -235,18 +260,18 @@ void LinechartPlot::appendData(QString dataname, quint64 ms, double value) // Add new value TimeSeriesData* dataset = data.value(dataname); - quint64 time; + quint64 time = QGC::groundTimeMilliseconds(); // Append data - if (m_groundTime) { - // Use the current (receive) time - time = QGC::groundTimeUsecs()/1000; - } else { + if (!m_groundTime) + { // Use timestamp from dataset time = ms; } dataset->append(time, value); + lastUpdate.insert(dataname, time); + // Scaling values if(ms < minTime) minTime = ms; if(ms > maxTime) maxTime = ms; diff --git a/src/ui/linechart/LinechartPlot.h b/src/ui/linechart/LinechartPlot.h index ec4128c..dd62e91 100644 --- a/src/ui/linechart/LinechartPlot.h +++ b/src/ui/linechart/LinechartPlot.h @@ -39,6 +39,7 @@ This file is part of the PIXHAWK project #include #include #include +#include #include #include #include @@ -47,7 +48,7 @@ This file is part of the PIXHAWK project #include #include #include -#include +#include "MG.h" class TimeScaleDraw: public QwtScaleDraw { @@ -266,13 +267,16 @@ public slots: /** @brief Set the number of values to average over */ void setAverageWindow(int windowSize); + void removeTimedOutCurves(); + public: QColor getColorForCurve(QString id); protected: QMap curves; QMap data; QMap scaleMaps; + QMap lastUpdate; ScrollZoomer* zoomer; QList colors; @@ -306,6 +310,7 @@ protected: int plotid; bool m_active; ///< Decides wether the plot is active or not bool m_groundTime; ///< Enforce the use of the receive timestamp instead of the data timestamp + QTimer timeoutTimer; // Methods void addCurve(QString id); From 955e3a529b3804f734edcc4b2dacacc56283a473 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Tue, 13 Sep 2011 11:02:34 +0200 Subject: [PATCH 2/3] Added global localization plotting --- src/uas/PxQuadMAV.cc | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 362de7f..3954efb 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -108,6 +108,19 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "vis. z", "m", pos.z, time); } break; + case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: { + mavlink_global_vision_position_estimate_t pos; + mavlink_msg_global_vision_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + //emit valueChanged(uasId, "vis. time", pos.usec, time); + emit valueChanged(uasId, "glob. vis. roll", "rad", pos.roll, time); + emit valueChanged(uasId, "glob. vis. pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "glob. vis. yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "glob. vis. x", "m", pos.x, time); + emit valueChanged(uasId, "glob. vis. y", "m", pos.y, time); + emit valueChanged(uasId, "glob. vis. z", "m", pos.z, time); + } + break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: { mavlink_vicon_position_estimate_t pos; mavlink_msg_vicon_position_estimate_decode(&message, &pos); From f72e9c5113a39828b443e0c912ea8d47ae6af0dd Mon Sep 17 00:00:00 2001 From: pixhawk Date: Wed, 14 Sep 2011 20:29:36 +0200 Subject: [PATCH 3/3] added custom commands 0-15 to the dropdown field in the widget designer --- src/ui/designer/QGCCommandButton.cc | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/ui/designer/QGCCommandButton.cc b/src/ui/designer/QGCCommandButton.cc index eae2934..6e262e3 100644 --- a/src/ui/designer/QGCCommandButton.cc +++ b/src/ui/designer/QGCCommandButton.cc @@ -37,6 +37,23 @@ QGCCommandButton::QGCCommandButton(QWidget *parent) : // Add commands to combo box ui->editCommandComboBox->addItem("DO: Control Video", MAV_CMD_DO_CONTROL_VIDEO); ui->editCommandComboBox->addItem("PREFLIGHT: Calibration", MAV_CMD_PREFLIGHT_CALIBRATION); + ui->editCommandComboBox->addItem("CUSTOM 0", 0); + ui->editCommandComboBox->addItem("CUSTOM 1", 1); + ui->editCommandComboBox->addItem("CUSTOM 2", 2); + ui->editCommandComboBox->addItem("CUSTOM 3", 3); + ui->editCommandComboBox->addItem("CUSTOM 4", 4); + ui->editCommandComboBox->addItem("CUSTOM 5", 5); + ui->editCommandComboBox->addItem("CUSTOM 6", 6); + ui->editCommandComboBox->addItem("CUSTOM 7", 7); + ui->editCommandComboBox->addItem("CUSTOM 8", 8); + ui->editCommandComboBox->addItem("CUSTOM 9", 9); + ui->editCommandComboBox->addItem("CUSTOM 10", 10); + ui->editCommandComboBox->addItem("CUSTOM 11", 11); + ui->editCommandComboBox->addItem("CUSTOM 12", 12); + ui->editCommandComboBox->addItem("CUSTOM 13", 13); + ui->editCommandComboBox->addItem("CUSTOM 14", 14); + ui->editCommandComboBox->addItem("CUSTOM 15", 15); + ui->editCommandComboBox->setEditable(true); } QGCCommandButton::~QGCCommandButton()