diff --git a/images/earth.html b/images/earth.html
index 44d3fb2cb15e32b6d400778790c307c05d96be37..602799f14bdec5a7afe2db24ced99afcc85e5e87 100644
--- a/images/earth.html
+++ b/images/earth.html
@@ -192,40 +192,38 @@ function setViewRange(dist)
function addTrailPosition(id, lat, lon, alt)
{
trails[id].setExtrude(false);
-trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
+ trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
-// Add LineString points
+ // Add LineString points
trails[id].getCoordinates().pushLatLngAlt(lat, lon, alt);
-// Create a style and set width and color of line
-trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
-lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
-lineStyle.setWidth(5);
-lineStyle.getColor().set(trailColors[id]); // aabbggrr format
-//lineStyle.getColor().set(color); // aabbggrr format
+ // Create a style and set width and color of line
+ trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
+ lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
+ lineStyle.setWidth(5);
+ lineStyle.getColor().set(trailColors[id]); // aabbggrr format
+ //lineStyle.getColor().set(color); // aabbggrr format
-// Add the feature to Earth
-if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
+ // Add the feature to Earth
+ if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]);
}
function initCallback(object)
{
- ge = object;
- ge.getWindow().setVisibility(true);
- ge.getOptions().setStatusBarVisibility(true);
- ge.getOptions().setScaleLegendVisibility(true);
- ge.getOptions().setFlyToSpeed(5.0);
- //ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT);
- ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
+ ge = object;
+ ge.getWindow().setVisibility(true);
+ ge.getOptions().setStatusBarVisibility(true);
+ ge.getOptions().setScaleLegendVisibility(true);
+ ge.getOptions().setFlyToSpeed(5.0);
+ //ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT);
+ ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
- ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true);
- ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true);
- ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true);
- ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
+ ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true);
+ ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true);
+ ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true);
+ ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
- initialized = true;
-
-
+ initialized = true;
}
function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
@@ -247,13 +245,12 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
planeOrient.setRoll(((roll/M_PI)+1.0)*360.0);
planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0);
planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0);
- }
-
-
+
planeLoc.setLatitude(lat);
planeLoc.setLongitude(lon);
planeLoc.setAltitude(alt);
-
+ planeModel.setLocation(planeLoc);
+ }
}
function enableDaylight(enabled)
diff --git a/src/configuration.h b/src/configuration.h
index c91fae7efbcffd00cab6f9ef7ecefedc5c0fa230..e06ad87f0adc6ca998d39875662470d3fa29bdc3 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -17,14 +17,14 @@
#define WITH_TEXT_TO_SPEECH 1
#define QGC_APPLICATION_NAME "QGroundControl"
-#define QGC_APPLICATION_VERSION "v. 0.8.1 (Alpha)"
+#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha)"
namespace QGC
{
const QString APPNAME = "QGROUNDCONTROL";
const QString COMPANYNAME = "OPENMAV";
- const int APPLICATIONVERSION = 80; // 0.8.0
+ const int APPLICATIONVERSION = 83; // 0.8.0
}
#endif // CONFIGURATION_H
diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc
index f23b0d16fbc2088c3b1b9ef6df8d6efbd85fff79..841d5655a3f0602ce85bf120e7a2065fdb3c3272 100644
--- a/src/uas/PxQuadMAV.cc
+++ b/src/uas/PxQuadMAV.cc
@@ -64,6 +64,15 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "Temperature", "raw", raw.temp, time);
}
break;
+ case MAVLINK_MSG_ID_IMAGE_TRIGGERED:
+ {
+ // FIXME Kind of a hack to load data from disk
+ mavlink_image_triggered_t img;
+ mavlink_msg_image_triggered_decode(&message, &img);
+ qDebug() << "IMAGE AVAILABLE:" << img.timestamp;
+ emit imageStarted(img.timestamp);
+ }
+ break;
case MAVLINK_MSG_ID_PATTERN_DETECTED:
{
mavlink_pattern_detected_t detected;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 6aa9b81b0ae28847abffc472ef069b31eabea702..873a6d98e09165a926ef8a3d6d0bacd96201108e 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -464,6 +464,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "latitude", "deg", pos.lat, time);
emit valueChanged(uasId, "longitude", "deg", pos.lon, time);
+ // FIXME REMOVE
+ longitude = pos.lon;
+ latitude = pos.lat;
+ altitude = pos.alt;
+ emit globalPositionChanged(this, longitude, latitude, altitude, time);
+
if (pos.fix_type > 0)
{
emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index e260644c5e5831713758b77540db36668a05e477..dfd9d11d4693a7e50fb41fbd2435dde1527e8725 100644
--- a/src/uas/UAS.h
+++ b/src/uas/UAS.h
@@ -169,7 +169,7 @@ public:
bool isAuto();
public:
- UASWaypointManager &getWaypointManager(void) { return waypointManager; }
+ UASWaypointManager* getWaypointManager() { return &waypointManager; }
int getSystemType();
int getAutopilotType() {return autopilot;}
@@ -303,6 +303,7 @@ signals:
void loadChanged(UASInterface* uas, double load);
/** @brief Propagate a heartbeat received from the system */
void heartbeat(UASInterface* uas);
+ void imageStarted(quint64 timestamp);
protected:
/** @brief Get the UNIX timestamp in microseconds */
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 2d7df002bb2ceb10ded4ed4a19f45872b1a89ef6..514d596a24d7cdd6ced9da27aede6ed239186a55 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -79,7 +79,7 @@ public:
virtual double getYaw() const = 0;
/** @brief Get reference to the waypoint manager **/
- virtual UASWaypointManager &getWaypointManager(void) = 0;
+ virtual UASWaypointManager* getWaypointManager(void) = 0;
/* COMMUNICATION FLAGS */
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 91b56f80c17db618573e86838400ab08111f8a93..eb0317b8344f949247dbd42407f4f511fc54f1be 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -265,6 +265,13 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
}
}
+void UASWaypointManager::notifyOfChange(Waypoint* wp)
+{
+ Q_UNUSED(wp);
+ emit waypointListChanged();
+ emit waypointListChanged(uas.getUASID());
+}
+
int UASWaypointManager::setCurrentWaypoint(quint16 seq)
{
if (seq < waypoints.size())
@@ -313,6 +320,9 @@ void UASWaypointManager::addWaypoint(Waypoint *wp)
waypoints.insert(waypoints.size(), wp);
emit waypointListChanged();
+ emit waypointListChanged(uas.getUASID());
+
+ qDebug() << "ADDED WAYPOINT WITH ID:" << wp->getId();
}
}
@@ -329,6 +339,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
waypoints[i]->setId(i);
}
emit waypointListChanged();
+ emit waypointListChanged(uas.getUASID());
return 0;
}
return -1;
@@ -359,6 +370,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
//waypoints[new_seq]->setId(new_seq);
emit waypointListChanged();
+ emit waypointListChanged(uas.getUASID());
}
}
@@ -425,21 +437,28 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
emit loadWPFile();
emit waypointListChanged();
+ emit waypointListChanged(uas.getUASID());
}
-void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
-{
- // FIXME Will be removed
- Q_UNUSED(wp);
-}
+//void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
+//{
+// // FIXME Will be removed
+// Q_UNUSED(wp);
+//}
-int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
-{
- // FIXME Will be removed
- Q_UNUSED(seq);
- return 0;
-}
+//int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
+//{
+// // FIXME Will be removed
+// Q_UNUSED(seq);
+// return 0;
+//}
+
+//void UASWaypointManager::waypointUpdated(Waypoint*)
+//{
+// // FIXME Add rate limiter
+// emit waypointListChanged(uas.getUASID());
+//}
void UASWaypointManager::clearWaypointList()
{
@@ -457,6 +476,11 @@ void UASWaypointManager::clearWaypointList()
}
}
+int UASWaypointManager::getIndexOf(Waypoint* wp)
+{
+ return waypoints.indexOf(wp);
+}
+
void UASWaypointManager::readWaypoints()
{
emit readGlobalWPFromUAS(true);
diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h
index 39c13194c6b7cea7bc3a2ac3fb44b9a592f3749e..aa43dd564b2dd94e0aee62b9f51b302badcf5887 100644
--- a/src/uas/UASWaypointManager.h
+++ b/src/uas/UASWaypointManager.h
@@ -86,25 +86,27 @@ public:
/** @name Waypoint list operations */
/*@{*/
const QVector &getWaypointList(void) { return waypoints; } ///< Returns a const reference to the waypoint list.
- void addWaypoint(Waypoint *wp); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
int removeWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage
void moveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq
void saveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile
void loadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile
+ void notifyOfChange(Waypoint* wp); ///< Notifies manager to changes to a waypoint
void localAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
int localRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage
void localMoveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq
void localSaveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile
void localLoadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile
-
+ int getIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list
/*@}*/
- /** @name Global waypoint list operations */
- /*@{*/
- const QVector &getGlobalWaypointList(void) { return waypoints; } ///< Returns a const reference to the global waypoint list.
- void globalAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
- int globalRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage
- /*@}*/
+ UAS& getUAS() { return this->uas; } ///< Returns the owning UAS
+
+// /** @name Global waypoint list operations */
+// /*@{*/
+// const QVector &getGlobalWaypointList(void) { return waypoints; } ///< Returns a const reference to the global waypoint list.
+// void globalAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
+// int globalRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage
+// /*@}*/
private:
/** @name Message send functions */
@@ -120,14 +122,17 @@ private:
public slots:
void timeout(); ///< Called by the timer if a response times out. Handles send retries.
+ void addWaypoint(Waypoint *wp); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
signals:
- void waypointListChanged(void); ///< emits signal that the local waypoint list has been changed
+ void waypointListChanged(void); ///< emits signal that the waypoint list has been changed
+ void waypointListChanged(int uasid); ///< Emits signal that list has been changed
+ void waypointChanged(int uasid, Waypoint* wp); ///< emits signal that waypoint has been changed
void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number
void updateStatusString(const QString &); ///< emits the current status string
- void loadWPFile(); ///< emits signal that a file wp has been load
- void readGlobalWPFromUAS(bool value); ///< emits signal when finish to read Global WP from UAS
+ void loadWPFile(); ///< emits signal that a file wp has been load
+ void readGlobalWPFromUAS(bool value); ///< emits signal when finish to read Global WP from UAS
private:
UAS &uas; ///< Reference to the corresponding UAS
diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc
index fc365fd162492dd77f27be60241478f02cfe8057..d598f458fdafa244affd911148dabac7130a8d34 100644
--- a/src/ui/HDDisplay.cc
+++ b/src/ui/HDDisplay.cc
@@ -152,7 +152,7 @@ HDDisplay::~HDDisplay()
QSize HDDisplay::sizeHint() const
{
- return QSize(400, 400.0f*(vwidth/vheight)*1.1f);
+ return QSize(400, 400.0f*(vwidth/vheight)*1.2f);
}
void HDDisplay::enableGLRendering(bool enable)
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index d7a203ccc0386e7cfb2d2c5bfd3ca77b309d10f9..2b7da31cd97b867ae1e1d3c4aaba84701102f6f6 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -678,7 +678,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
{
if (uas)
{
- const QVector& list = uas->getWaypointManager().getWaypointList();
+ const QVector& list = uas->getWaypointManager()->getWaypointList();
// for (int i = 0; i < list.size(); i++)
// {
// QPointF in(list.at(i)->getX(), list.at(i)->getY());
diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc
index a55f73a089460d0ff565b31c57647aa82dfdced3..6929225397a081789bf8e1b1ea4e92d9675d31bb 100644
--- a/src/ui/HUD.cc
+++ b/src/ui/HUD.cc
@@ -30,6 +30,10 @@ This file is part of the QGROUNDCONTROL project
*/
#include
+#include
+#include
+#include
+#include
#include
#include
@@ -39,6 +43,7 @@ This file is part of the QGROUNDCONTROL project
#include
#include "UASManager.h"
+#include "UAS.h"
#include "HUD.h"
#include "MG.h"
#include "QGC.h"
@@ -124,7 +129,13 @@ HUD::HUD(int width, int height, QWidget* parent)
lat(0.0),
lon(0.0),
alt(0.0),
- load(0.0f)
+ load(0.0f),
+ offlineDirectory(""),
+ nextOfflineImage(""),
+ hudInstrumentsEnabled(true),
+ videoEnabled(false),
+ xImageFactor(1.0),
+ yImageFactor(1.0)
{
// Set auto fill to false
setAutoFillBackground(false);
@@ -183,6 +194,10 @@ HUD::HUD(int width, int height, QWidget* parent)
// Connect with UAS
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
+ createActions();
+
+ if (UASManager::instance()->getActiveUAS() != NULL) setActiveUAS(UASManager::instance()->getActiveUAS());
+
setVisible(false);
}
@@ -212,6 +227,40 @@ void HUD::hideEvent(QHideEvent* event)
refreshTimer->stop();
}
+void HUD::contextMenuEvent (QContextMenuEvent* event)
+{
+ QMenu menu(this);
+ // Update actions
+ enableHUDAction->setChecked(hudInstrumentsEnabled);
+ enableVideoAction->setChecked(videoEnabled);
+
+ menu.addAction(enableHUDAction);
+ //menu.addAction(selectHUDColorAction);
+ menu.addAction(enableVideoAction);
+ menu.addAction(selectOfflineDirectoryAction);
+ //menu.addAction(selectVideoChannelAction);
+ menu.exec(event->globalPos());
+}
+
+void HUD::createActions()
+{
+ enableHUDAction = new QAction(tr("Enable HUD"), this);
+ enableHUDAction->setStatusTip(tr("Show the HUD instruments in this window"));
+ enableHUDAction->setCheckable(true);
+ enableHUDAction->setChecked(hudInstrumentsEnabled);
+ connect(enableHUDAction, SIGNAL(triggered(bool)), this, SLOT(enableHUDInstruments(bool)));
+
+ enableVideoAction = new QAction(tr("Enable Video Live feed"), this);
+ enableVideoAction->setStatusTip(tr("Show the video live feed"));
+ enableVideoAction->setCheckable(true);
+ enableVideoAction->setChecked(videoEnabled);
+ connect(enableVideoAction, SIGNAL(triggered(bool)), this, SLOT(enableVideo(bool)));
+
+ selectOfflineDirectoryAction = new QAction(tr("Select image log"), this);
+ selectOfflineDirectoryAction->setStatusTip(tr("Load previously logged images into simulation / replay"));
+ connect(selectOfflineDirectoryAction, SIGNAL(triggered()), this, SLOT(selectOfflineDirectory()));
+}
+
/**
*
* @param uas the UAS/MAV to monitor/display with the HUD
@@ -230,22 +279,39 @@ void HUD::setActiveUAS(UASInterface* uas)
disconnect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
disconnect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
disconnect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
- }
- // Now connect the new UAS
- // Setup communication
- connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
- connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
- connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
- connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
- connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
+ // Try to disconnect the image link
+ UAS* u = dynamic_cast(uas);
+ if (u)
+ {
+ disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
+ }
+ }
- connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
- connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
- connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
+ if (uas)
+ {
+ // Now connect the new UAS
+ // Setup communication
+ connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
+ connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
+ connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
+ connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
+ connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
+
+ connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
+ connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
+ connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
+
+ // Try to connect the image link
+ UAS* u = dynamic_cast(uas);
+ if (u)
+ {
+ connect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
+ }
- // Set new UAS
- this->uas = uas;
+ // Set new UAS
+ this->uas = uas;
+ }
}
//void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec)
@@ -609,174 +675,183 @@ void HUD::paintHUD()
glPushMatrix();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
- // Blue / Brown background
- if (noCamera)
- {
- paintCenterBackground(roll, pitch, yawTrans);
- }
- // else {
// Fill with black background
- QString imagePath = "/Users/user/Desktop/frame0000.png";
- if (QFileInfo(imagePath).exists())
+ if (videoEnabled)
{
- //qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
- QImage fill = QImage(imagePath);
+ if (nextOfflineImage != "" && QFileInfo(nextOfflineImage).exists())
+ {
+ qDebug() << __FILE__ << __LINE__ << "template image:" << nextOfflineImage;
+ QImage fill = QImage(nextOfflineImage);
- glImage = QGLWidget::convertToGLFormat(fill);
+ xImageFactor = width() / (float)fill.width();
+ yImageFactor = height() / (float)fill.height();
- float xFactor;
- float yFactor;
+ glImage = QGLWidget::convertToGLFormat(fill);
- xFactor = width() / (float)fill.width();
- yFactor = height() / (float)fill.height();
+ // Reset to save load efforts
+ nextOfflineImage = "";
+ }
- glPixelZoom(xFactor, yFactor);
+ glPixelZoom(xImageFactor, yImageFactor);
// Resize to correct size and fill with image
glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
- // FIXME Adjust viewport to scale image into view
+ }
+ else
+ {
+ // Blue / brown background
+ paintCenterBackground(roll, pitch, yawTrans);
}
- // }
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
// END OF OPENGL PAINTING
+ if (hudInstrumentsEnabled)
+ {
+ //glEnable(GL_MULTISAMPLE);
+
+ // QT PAINTING
+ //makeCurrent();
+ QPainter painter;
+ painter.begin(this);
+ painter.setRenderHint(QPainter::Antialiasing, true);
+ painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
+ painter.translate((this->vwidth/2.0+xCenterOffset)*scalingFactor, (this->vheight/2.0+yCenterOffset)*scalingFactor);
+
+
+
+ // COORDINATE FRAME IS NOW (0,0) at CENTER OF WIDGET
+
+
+ // Draw all fixed indicators
+ // MODE
+ paintText(mode, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 10, &painter);
+ // STATE
+ paintText(state, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 15, &painter);
+ // BATTERY
+ paintText(fuelStatus, fuelColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 20, &painter);
+ // Waypoint
+ paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
- //glEnable(GL_MULTISAMPLE);
-
- // QT PAINTING
- //makeCurrent();
- QPainter painter;
- painter.begin(this);
- painter.setRenderHint(QPainter::Antialiasing, true);
- painter.setRenderHint(QPainter::HighQualityAntialiasing, true);
- painter.translate((this->vwidth/2.0+xCenterOffset)*scalingFactor, (this->vheight/2.0+yCenterOffset)*scalingFactor);
-
-
-
- // COORDINATE FRAME IS NOW (0,0) at CENTER OF WIDGET
-
-
- // Draw all fixed indicators
- // MODE
- paintText(mode, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 10, &painter);
- // STATE
- paintText(state, infoColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 15, &painter);
- // BATTERY
- paintText(fuelStatus, fuelColor, 2.0f, (-vwidth/2.0) + 10, -vheight/2.0 + 20, &painter);
- // Waypoint
- paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
-
- // YAW INDICATOR
- //
- // .
- // . .
- // .......
- //
- const float yawIndicatorWidth = 4.0f;
- const float yawIndicatorY = vheight/2.0f - 10.0f;
- QPolygon yawIndicator(4);
- yawIndicator.setPoint(0, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
- yawIndicator.setPoint(1, QPoint(refToScreenX(yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
- yawIndicator.setPoint(2, QPoint(refToScreenX(-yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
- yawIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
- painter.setPen(defaultColor);
- painter.drawPolyline(yawIndicator);
+ // YAW INDICATOR
+ //
+ // .
+ // . .
+ // .......
+ //
+ const float yawIndicatorWidth = 4.0f;
+ const float yawIndicatorY = vheight/2.0f - 10.0f;
+ QPolygon yawIndicator(4);
+ yawIndicator.setPoint(0, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
+ yawIndicator.setPoint(1, QPoint(refToScreenX(yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
+ yawIndicator.setPoint(2, QPoint(refToScreenX(-yawIndicatorWidth/2.0f), refToScreenY(yawIndicatorY+yawIndicatorWidth)));
+ yawIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(yawIndicatorY)));
+ painter.setPen(defaultColor);
+ painter.drawPolyline(yawIndicator);
- // CENTER
+ // CENTER
- // HEADING INDICATOR
- //
- // __ __
- // \/\/
- //
- const float hIndicatorWidth = 7.0f;
- const float hIndicatorY = -25.0f;
- const float hIndicatorYLow = hIndicatorY + hIndicatorWidth / 6.0f;
- const float hIndicatorSegmentWidth = hIndicatorWidth / 7.0f;
- QPolygon hIndicator(7);
- hIndicator.setPoint(0, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
- hIndicator.setPoint(1, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f+hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
- hIndicator.setPoint(2, QPoint(refToScreenX(0.0f-hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
- hIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(hIndicatorY)));
- hIndicator.setPoint(4, QPoint(refToScreenX(0.0f+hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
- hIndicator.setPoint(5, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f-hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
- hIndicator.setPoint(6, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
- painter.setPen(defaultColor);
- painter.drawPolyline(hIndicator);
+ // HEADING INDICATOR
+ //
+ // __ __
+ // \/\/
+ //
+ const float hIndicatorWidth = 7.0f;
+ const float hIndicatorY = -25.0f;
+ const float hIndicatorYLow = hIndicatorY + hIndicatorWidth / 6.0f;
+ const float hIndicatorSegmentWidth = hIndicatorWidth / 7.0f;
+ QPolygon hIndicator(7);
+ hIndicator.setPoint(0, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
+ hIndicator.setPoint(1, QPoint(refToScreenX(0.0f-hIndicatorWidth/2.0f+hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
+ hIndicator.setPoint(2, QPoint(refToScreenX(0.0f-hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
+ hIndicator.setPoint(3, QPoint(refToScreenX(0.0f), refToScreenY(hIndicatorY)));
+ hIndicator.setPoint(4, QPoint(refToScreenX(0.0f+hIndicatorSegmentWidth*1.0f), refToScreenY(hIndicatorYLow)));
+ hIndicator.setPoint(5, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f-hIndicatorSegmentWidth*1.75f), refToScreenY(hIndicatorY)));
+ hIndicator.setPoint(6, QPoint(refToScreenX(0.0f+hIndicatorWidth/2.0f), refToScreenY(hIndicatorY)));
+ painter.setPen(defaultColor);
+ painter.drawPolyline(hIndicator);
- // SETPOINT
- const float centerWidth = 4.0f;
- painter.setPen(defaultColor);
- painter.setBrush(Qt::NoBrush);
- // TODO
- //painter.drawEllipse(QPointF(refToScreenX(qMin(10.0f, values.value("roll desired", 0.0f) * 10.0f)), refToScreenY(qMin(10.0f, values.value("pitch desired", 0.0f) * 10.0f))), refToScreenX(centerWidth/2.0f), refToScreenX(centerWidth/2.0f));
+ // SETPOINT
+ const float centerWidth = 4.0f;
+ painter.setPen(defaultColor);
+ painter.setBrush(Qt::NoBrush);
+ // TODO
+ //painter.drawEllipse(QPointF(refToScreenX(qMin(10.0f, values.value("roll desired", 0.0f) * 10.0f)), refToScreenY(qMin(10.0f, values.value("pitch desired", 0.0f) * 10.0f))), refToScreenX(centerWidth/2.0f), refToScreenX(centerWidth/2.0f));
- const float centerCrossWidth = 10.0f;
- // left
- painter.drawLine(QPointF(refToScreenX(-centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(-centerCrossWidth / 2.0f), refToScreenY(0.0f)));
- // right
- painter.drawLine(QPointF(refToScreenX(centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(centerCrossWidth / 2.0f), refToScreenY(0.0f)));
- // top
- painter.drawLine(QPointF(refToScreenX(0.0f), refToScreenY(-centerWidth / 2.0f)), QPointF(refToScreenX(0.0f), refToScreenY(-centerCrossWidth / 2.0f)));
+ const float centerCrossWidth = 10.0f;
+ // left
+ painter.drawLine(QPointF(refToScreenX(-centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(-centerCrossWidth / 2.0f), refToScreenY(0.0f)));
+ // right
+ painter.drawLine(QPointF(refToScreenX(centerWidth / 2.0f), refToScreenY(0.0f)), QPointF(refToScreenX(centerCrossWidth / 2.0f), refToScreenY(0.0f)));
+ // top
+ painter.drawLine(QPointF(refToScreenX(0.0f), refToScreenY(-centerWidth / 2.0f)), QPointF(refToScreenX(0.0f), refToScreenY(-centerCrossWidth / 2.0f)));
- // COMPASS
- const float compassY = -vheight/2.0f + 10.0f;
- QRectF compassRect(QPointF(refToScreenX(-5.0f), refToScreenY(compassY)), QSizeF(refToScreenX(10.0f), refToScreenY(5.0f)));
- painter.setBrush(Qt::NoBrush);
- painter.setPen(Qt::SolidLine);
- painter.setPen(defaultColor);
- painter.drawRoundedRect(compassRect, 2, 2);
- QString yawAngle;
+ // COMPASS
+ const float compassY = -vheight/2.0f + 10.0f;
+ QRectF compassRect(QPointF(refToScreenX(-5.0f), refToScreenY(compassY)), QSizeF(refToScreenX(10.0f), refToScreenY(5.0f)));
+ painter.setBrush(Qt::NoBrush);
+ painter.setPen(Qt::SolidLine);
+ painter.setPen(defaultColor);
+ painter.drawRoundedRect(compassRect, 2, 2);
+ QString yawAngle;
- // const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f;
+ // const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f;
- // YAW is in compass-human readable format, so 0 - 360deg. This is normal in aviation, not -180 - +180.
- const float yawDeg = ((yawLP/M_PI)*180.0f)+180.0f;
- yawAngle.sprintf("%03d", (int)yawDeg);
- paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter);
+ // YAW is in compass-human readable format, so 0 - 360deg. This is normal in aviation, not -180 - +180.
+ const float yawDeg = ((yawLP/M_PI)*180.0f)+180.0f;
+ yawAngle.sprintf("%03d", (int)yawDeg);
+ paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter);
- // CHANGE RATE STRIPS
- drawChangeRateStrip(-51.0f, -50.0f, 15.0f, -1.0f, 1.0f, -zSpeed, &painter);
+ // CHANGE RATE STRIPS
+ drawChangeRateStrip(-51.0f, -50.0f, 15.0f, -1.0f, 1.0f, -zSpeed, &painter);
- // CHANGE RATE STRIPS
- drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, xSpeed, &painter);
-
- // GAUGES
+ // CHANGE RATE STRIPS
+ drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, xSpeed, &painter);
+
+ // GAUGES
- // Left altitude gauge
- drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -zPos, defaultColor, &painter, false);
+ // Left altitude gauge
+ drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -zPos, defaultColor, &painter, false);
- // Right speed gauge
- drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, xSpeed, defaultColor, &painter, false);
+ // Right speed gauge
+ drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, xSpeed, defaultColor, &painter, false);
- // Waypoint name
- if (waypointName != "") paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
-
- // MOVING PARTS
-
-
- painter.translate(refToScreenX(yawTrans), 0);
-
- // Rotate view and draw all roll-dependent indicators
- painter.rotate((rollLP/M_PI)* -180.0f);
-
- painter.translate(0, (-pitchLP/M_PI)* -180.0f * refToScreenY(1.8));
-
- //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f);
-
- // PITCH
+ // Waypoint name
+ if (waypointName != "") paintText(waypointName, defaultColor, 2.0f, (-vwidth/3.0) + 10, +vheight/3.0 + 15, &painter);
+
+ // MOVING PARTS
+
+
+ painter.translate(refToScreenX(yawTrans), 0);
+
+ // Rotate view and draw all roll-dependent indicators
+ painter.rotate((rollLP/M_PI)* -180.0f);
+
+ painter.translate(0, (-pitchLP/M_PI)* -180.0f * refToScreenY(1.8));
+
+ //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f);
+
+ // PITCH
- paintPitchLines(pitchLP, &painter);
- painter.end();
+ paintPitchLines(pitchLP, &painter);
+ painter.end();
+ }
+ else
+ {
+ QPainter painter;
+ painter.begin(this);
+ painter.end();
+ }
//glDisable(GL_MULTISAMPLE);
+
+
//glFlush();
}
}
@@ -1517,6 +1592,34 @@ void HUD::saveImage()
saveImage(fileName);
}
+void HUD::startImage(quint64 timestamp)
+{
+ if (videoEnabled && offlineDirectory != "")
+ {
+ // Load and diplay image file
+ nextOfflineImage = QString(offlineDirectory + "/%1.bmp").arg(timestamp);
+ }
+}
+
+void HUD::selectOfflineDirectory()
+{
+ QString fileName = QFileDialog::getExistingDirectory(this, tr("Select image directory"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation));
+ if (fileName != "")
+ {
+ offlineDirectory = fileName;
+ }
+}
+
+void HUD::enableHUDInstruments(bool enabled)
+{
+ hudInstrumentsEnabled = enabled;
+}
+
+void HUD::enableVideo(bool enabled)
+{
+ videoEnabled = enabled;
+}
+
void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int startIndex)
{
Q_UNUSED(imgid);
diff --git a/src/ui/HUD.h b/src/ui/HUD.h
index d9c89f73999d38e43e2de0d34051a9ed5f521e8d..8866f784abd4ee73692e34aefa922999831cb7c0 100644
--- a/src/ui/HUD.h
+++ b/src/ui/HUD.h
@@ -76,11 +76,19 @@ public slots:
void updateLoad(UASInterface*, double);
void selectWaypoint(int uasId, int id);
+ void startImage(quint64 timestamp);
void startImage(int imgid, int width, int height, int depth, int channels);
void setPixels(int imgid, const unsigned char* imageData, int length, int startIndex);
void finishImage();
void saveImage();
void saveImage(QString fileName);
+ /** @brief Select directory where to load the offline files from */
+ void selectOfflineDirectory();
+ /** @brief Enable the HUD instruments */
+ void enableHUDInstruments(bool enabled);
+ /** @brief Enable Video */
+ void enableVideo(bool enabled);
+
protected slots:
void paintCenterBackground(float roll, float pitch, float yaw);
@@ -119,6 +127,8 @@ protected:
void showEvent(QShowEvent* event);
/** @brief Stop updating widget */
void hideEvent(QHideEvent* event);
+ void contextMenuEvent (QContextMenuEvent* event);
+ void createActions();
static const int updateInterval = 50;
@@ -192,6 +202,16 @@ protected:
double lon;
double alt;
float load;
+ QString offlineDirectory;
+ QString nextOfflineImage;
+ bool hudInstrumentsEnabled;
+ bool videoEnabled;
+ float xImageFactor;
+ float yImageFactor;
+ QAction* enableHUDAction;
+ QAction* enableVideoAction;
+ QAction* selectOfflineDirectoryAction;
+ QAction* selectVideoChannelAction;
void paintEvent(QPaintEvent *event);
};
diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc
index 5e9f32a0cf07838bfdda68ca6c247f9ab9274113..08c17ce5ebbdfa7771f5f2e9defcc296bef77bc5 100644
--- a/src/ui/MainWindow.cc
+++ b/src/ui/MainWindow.cc
@@ -64,7 +64,7 @@ MainWindow* MainWindow::instance()
MainWindow::MainWindow(QWidget *parent):
QMainWindow(parent),
toolsMenuActions(),
- currentView(VIEW_ENGINEER),
+ currentView(VIEW_UNCONNECTED),
aboutToCloseFlag(false),
changingViewsFlag(false)
{
@@ -82,47 +82,17 @@ MainWindow::MainWindow(QWidget *parent):
}
else
{
- if (settings.value("CURRENT_VIEW", VIEW_PILOT) != VIEW_PILOT)
+ // LOAD THE LAST VIEW
+ VIEW_SECTIONS currentViewCandidate = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt();
+ if (currentViewCandidate != VIEW_ENGINEER &&
+ currentViewCandidate != VIEW_OPERATOR &&
+ currentViewCandidate != VIEW_PILOT)
{
- currentView = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt();
+ currentView = currentViewCandidate;
}
}
- // Check if the settings exist, instantiate defaults if necessary
-
- // OPERATOR VIEW DEFAULT
- QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR);
- if (!settings.contains(centralKey))
- {
- settings.setValue(centralKey,true);
- }
-
- // ENGINEER VIEW DEFAULT
- centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER);
- if (!settings.contains(centralKey))
- {
- settings.setValue(centralKey,true);
- }
-
- // MAVLINK VIEW DEFAULT
- centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK);
- if (!settings.contains(centralKey))
- {
- settings.setValue(centralKey,true);
- }
-
- // PILOT VIEW DEFAULT
- centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT);
- if (!settings.contains(centralKey))
- {
- settings.setValue(centralKey,true);
- }
-
- QString listKey = buildMenuKey(SUB_SECTION_CHECKED, MENU_UAS_LIST, currentView);
- if (!settings.contains(listKey))
- {
- settings.setValue(listKey, true);
- }
+ setDefaultSettingsForAp();
settings.sync();
@@ -138,6 +108,7 @@ MainWindow::MainWindow(QWidget *parent):
perspectives->addAction(ui.actionMavlinkView);
perspectives->addAction(ui.actionPilotsView);
perspectives->addAction(ui.actionOperatorsView);
+ perspectives->addAction(ui.actionUnconnectedView);
perspectives->setExclusive(true);
// Mark the right one as selected
@@ -145,9 +116,13 @@ MainWindow::MainWindow(QWidget *parent):
if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true);
if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true);
if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true);
+ if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true);
- // The pilot view is not available on startup
+ // The pilot, engineer and operator view are not available on startup
+ // since they only make sense with a system connected.
ui.actionPilotsView->setEnabled(false);
+ ui.actionOperatorsView->setEnabled(false);
+ ui.actionEngineersView->setEnabled(false);
buildCommonWidgets();
@@ -217,6 +192,54 @@ MainWindow::~MainWindow()
}
+/**
+ * Set default settings for this AP type.
+ */
+void MainWindow::setDefaultSettingsForAp()
+{
+ // Check if the settings exist, instantiate defaults if necessary
+
+ // UNCONNECTED VIEW DEFAULT
+ QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_UNCONNECTED);
+ if (!settings.contains(centralKey))
+ {
+ settings.setValue(centralKey,true);
+ }
+
+ // OPERATOR VIEW DEFAULT
+ centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR);
+ if (!settings.contains(centralKey))
+ {
+ settings.setValue(centralKey,true);
+
+ // ENABLE UAS LIST
+ settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,VIEW_OPERATOR), true);
+ // ENABLE HUD TOOL WIDGET
+ settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HUD,VIEW_OPERATOR), true);
+ }
+
+ // ENGINEER VIEW DEFAULT
+ centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER);
+ if (!settings.contains(centralKey))
+ {
+ settings.setValue(centralKey,true);
+ }
+
+ // MAVLINK VIEW DEFAULT
+ centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK);
+ if (!settings.contains(centralKey))
+ {
+ settings.setValue(centralKey,true);
+ }
+
+ // PILOT VIEW DEFAULT
+ centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT);
+ if (!settings.contains(centralKey))
+ {
+ settings.setValue(centralKey,true);
+ }
+}
+
void MainWindow::resizeEvent(QResizeEvent * event)
{
Q_UNUSED(event);
@@ -289,7 +312,7 @@ void MainWindow::buildCommonWidgets()
controlDockWidget = new QDockWidget(tr("Control"), this);
controlDockWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_DOCKWIDGET");
controlDockWidget->setWidget( new UASControlWidget(this) );
- addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget()), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea);
+ addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea);
}
if (!listDockWidget)
@@ -297,7 +320,7 @@ void MainWindow::buildCommonWidgets()
listDockWidget = new QDockWidget(tr("Unmanned Systems"), this);
listDockWidget->setWidget( new UASListWidget(this) );
listDockWidget->setObjectName("UNMANNED_SYSTEMS_LIST_DOCKWIDGET");
- addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget()), MENU_UAS_LIST, Qt::RightDockWidgetArea);
+ addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget(bool)), MENU_UAS_LIST, Qt::RightDockWidgetArea);
}
if (!waypointsDockWidget)
@@ -305,7 +328,7 @@ void MainWindow::buildCommonWidgets()
waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this);
waypointsDockWidget->setWidget( new WaypointList(this, NULL) );
waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET");
- addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget()), MENU_WAYPOINTS, Qt::BottomDockWidgetArea);
+ addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea);
}
if (!infoDockWidget)
@@ -313,7 +336,7 @@ void MainWindow::buildCommonWidgets()
infoDockWidget = new QDockWidget(tr("Status Details"), this);
infoDockWidget->setWidget( new UASInfoWidget(this) );
infoDockWidget->setObjectName("UAS_STATUS_DETAILS_DOCKWIDGET");
- addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget()), MENU_STATUS, Qt::RightDockWidgetArea);
+ addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget(bool)), MENU_STATUS, Qt::RightDockWidgetArea);
}
if (!debugConsoleDockWidget)
@@ -321,7 +344,7 @@ void MainWindow::buildCommonWidgets()
debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this);
debugConsoleDockWidget->setWidget( new DebugConsole(this) );
debugConsoleDockWidget->setObjectName("COMMUNICATION_DEBUG_CONSOLE_DOCKWIDGET");
- addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget()), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea);
+ addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget(bool)), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea);
}
if (!logPlayerDockWidget)
@@ -329,13 +352,13 @@ void MainWindow::buildCommonWidgets()
logPlayerDockWidget = new QDockWidget(tr("MAVLink Log Player"), this);
logPlayerDockWidget->setWidget( new QGCMAVLinkLogPlayer(mavlink, this) );
logPlayerDockWidget->setObjectName("MAVLINK_LOG_PLAYER_DOCKWIDGET");
- addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget()), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea);
+ addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget(bool)), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea);
}
// Center widgets
if (!mapWidget)
{
- mapWidget = new MapWidget(this);
+ mapWidget = new MapWidget(this);
addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP);
}
@@ -393,17 +416,17 @@ void MainWindow::buildPxWidgets()
#ifdef QGC_OSG_ENABLED
if (!_3DWidget)
{
-// _3DWidget = Q3DWidgetFactory::get("PIXHAWK");
-// addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
+ _3DWidget = Q3DWidgetFactory::get("PIXHAWK");
+ addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
}
#endif
#ifdef QGC_OSGEARTH_ENABLED
-// if (!_3DMapWidget)
-// {
-// _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
-// addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
-// }
+ if (!_3DMapWidget)
+ {
+ _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
+ addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
+ }
#endif
#if (defined _MSC_VER) | (defined Q_OS_MAC)
@@ -422,7 +445,7 @@ void MainWindow::buildPxWidgets()
detectionDockWidget = new QDockWidget(tr("Object Recognition"), this);
detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) );
detectionDockWidget->setObjectName("OBJECT_DETECTION_DOCK_WIDGET");
- addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget()), MENU_DETECTION, Qt::RightDockWidgetArea);
+ addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget(bool)), MENU_DETECTION, Qt::RightDockWidgetArea);
}
if (!parametersDockWidget)
@@ -430,7 +453,7 @@ void MainWindow::buildPxWidgets()
parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this);
parametersDockWidget->setWidget( new ParameterInterface(this) );
parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
- addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea);
+ addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea);
}
if (!watchdogControlDockWidget)
@@ -438,7 +461,7 @@ void MainWindow::buildPxWidgets()
watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this);
watchdogControlDockWidget->setWidget( new WatchdogControl(this) );
watchdogControlDockWidget->setObjectName("WATCHDOG_CONTROL_DOCKWIDGET");
- addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget()), MENU_WATCHDOG, Qt::BottomDockWidgetArea);
+ addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget(bool)), MENU_WATCHDOG, Qt::BottomDockWidgetArea);
}
if (!hsiDockWidget)
@@ -446,7 +469,7 @@ void MainWindow::buildPxWidgets()
hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this);
hsiDockWidget->setWidget( new HSIDisplay(this) );
hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET");
- addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget()), MENU_HSI, Qt::BottomDockWidgetArea);
+ addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea);
}
if (!headDown1DockWidget)
@@ -454,7 +477,7 @@ void MainWindow::buildPxWidgets()
headDown1DockWidget = new QDockWidget(tr("Flight Display"), this);
headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) );
headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET");
- addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea);
+ addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget(bool)), MENU_HDD_1, Qt::RightDockWidgetArea);
}
if (!headDown2DockWidget)
@@ -462,7 +485,7 @@ void MainWindow::buildPxWidgets()
headDown2DockWidget = new QDockWidget(tr("Payload Status"), this);
headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) );
headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET");
- addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea);
+ addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea);
}
if (!rcViewDockWidget)
@@ -470,7 +493,7 @@ void MainWindow::buildPxWidgets()
rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET");
- addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
+ addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
}
if (!headUpDockWidget)
@@ -478,7 +501,31 @@ void MainWindow::buildPxWidgets()
headUpDockWidget = new QDockWidget(tr("HUD"), this);
headUpDockWidget->setWidget( new HUD(320, 240, this));
headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET");
- addToToolsMenu (headUpDockWidget, tr("Control Indicator"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea);
+ addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea);
+ }
+
+ if (!video1DockWidget)
+ {
+ video1DockWidget = new QDockWidget(tr("Video Stream 1"), this);
+ HUD* video1 = new HUD(160, 120, this);
+ video1->enableHUDInstruments(false);
+ video1->enableVideo(true);
+ // FIXME select video stream as well
+ video1DockWidget->setWidget(video1);
+ video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET");
+ addToToolsMenu (video1DockWidget, tr("Video Stream 1"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_1, Qt::LeftDockWidgetArea);
+ }
+
+ if (!video2DockWidget)
+ {
+ video2DockWidget = new QDockWidget(tr("Video Stream 2"), this);
+ HUD* video2 = new HUD(160, 120, this);
+ video2->enableHUDInstruments(false);
+ video2->enableVideo(true);
+ // FIXME select video stream as well
+ video2DockWidget->setWidget(video2);
+ video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET");
+ addToToolsMenu (video2DockWidget, tr("Video Stream 2"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_2, Qt::LeftDockWidgetArea);
}
// Dialogue widgets
@@ -499,7 +546,7 @@ void MainWindow::buildSlugsWidgets()
headUpDockWidget = new QDockWidget(tr("Control Indicator"), this);
headUpDockWidget->setWidget( new HUD(320, 240, this));
headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET");
- addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea);
+ addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea);
}
if (!rcViewDockWidget)
@@ -507,7 +554,7 @@ void MainWindow::buildSlugsWidgets()
rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET");
- addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
+ addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
}
// if (!slugsDataWidget)
@@ -553,15 +600,17 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget,
QAction* tempAction;
- // Add the separator that will separate tools from central Widgets
- if (!toolsMenuActions[CENTRAL_SEPARATOR])
- {
- tempAction = ui.menuTools->addSeparator();
- toolsMenuActions[CENTRAL_SEPARATOR] = tempAction;
- tempAction->setData(CENTRAL_SEPARATOR);
- }
+// Not needed any more - separate menu now available
+
+// // Add the separator that will separate tools from central Widgets
+// if (!toolsMenuActions[CENTRAL_SEPARATOR])
+// {
+// tempAction = ui.menuTools->addSeparator();
+// toolsMenuActions[CENTRAL_SEPARATOR] = tempAction;
+// tempAction->setData(CENTRAL_SEPARATOR);
+// }
- tempAction = ui.menuTools->addAction(title);
+ tempAction = ui.menuMain->addAction(title);
tempAction->setCheckable(true);
tempAction->setData(centralWidget);
@@ -583,13 +632,17 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget,
}
// connect the action
- connect(tempAction,SIGNAL(triggered()),this, slotName);
+ connect(tempAction,SIGNAL(triggered(bool)),this, slotName);
}
void MainWindow::showCentralWidget()
{
QAction* senderAction = qobject_cast(sender());
+
+ // Block sender action while manipulating state
+ senderAction->blockSignals(true);
+
int tool = senderAction->data().toInt();
QString chKey;
@@ -597,7 +650,6 @@ void MainWindow::showCentralWidget()
if (senderAction && dockWidgets[tool])
{
-
// uncheck all central widget actions
QHashIterator i(toolsMenuActions);
while (i.hasNext())
@@ -606,7 +658,11 @@ void MainWindow::showCentralWidget()
//qDebug() << "shCW" << i.key() << "read";
if (i.value() && i.value()->data().toInt() > 255)
{
+ // Block signals and uncheck action
+ // firing would be unneccesary
+ i.value()->blockSignals(true);
i.value()->setChecked(false);
+ i.value()->blockSignals(false);
// update the settings
chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView);
@@ -625,6 +681,9 @@ void MainWindow::showCentralWidget()
chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView);
settings.setValue(chKey,true);
+ // Unblock sender action
+ senderAction->blockSignals(false);
+
presentView();
}
}
@@ -684,39 +743,97 @@ void MainWindow::addToToolsMenu ( QWidget* widget,
}
else
{
- tempAction->setChecked(settings.value(chKey).toBool());
+ tempAction->setChecked(settings.value(chKey, false).toBool());
widget->setVisible(settings.value(chKey, false).toBool());
}
// connect the action
- connect(tempAction,SIGNAL(triggered()),this, slotName);
+ connect(tempAction,SIGNAL(toggled(bool)),this, slotName);
+
+ connect(qobject_cast (dockWidgets[tool]),
+ SIGNAL(visibilityChanged(bool)), this, SLOT(showToolWidget(bool)));
-// connect(qobject_cast (dockWidgets[tool]),
-// SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool)));
+ // connect(qobject_cast (dockWidgets[tool]),
+ // SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool)));
connect(qobject_cast (dockWidgets[tool]),
SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea)));
-
}
-void MainWindow::showToolWidget()
+void MainWindow::showToolWidget(bool visible)
{
- QAction* temp = qobject_cast(sender());
- int tool = temp->data().toInt();
-
- if (temp && dockWidgets[tool])
+ if (!aboutToCloseFlag && !changingViewsFlag)
{
- if (temp->isChecked())
+ QAction* action = qobject_cast(sender());
+
+ // Prevent this to fire if undocked
+ if (action)
{
- addDockWidget(dockWidgetLocations[tool], qobject_cast (dockWidgets[tool]));
- qobject_cast(dockWidgets[tool])->show();
+ int tool = action->data().toInt();
+
+ QDockWidget* dockWidget = qobject_cast (dockWidgets[tool]);
+
+ qDebug() << "DATA:" << tool << "FLOATING" << dockWidget->isFloating() << "checked" << action->isChecked() << "visible" << dockWidget->isVisible() << "action vis:" << action->isVisible();
+ if (dockWidget && dockWidget->isVisible() != visible)
+ {
+ if (visible)
+ {
+ qDebug() << "DOCK WIDGET ADDED";
+ addDockWidget(dockWidgetLocations[tool], dockWidget);
+ dockWidget->show();
+ }
+ else
+ {
+ qDebug() << "DOCK WIDGET REMOVED";
+ removeDockWidget(dockWidget);
+ //dockWidget->hide();
+ }
+
+ QHashIterator i(dockWidgets);
+ while (i.hasNext())
+ {
+ i.next();
+ if ((static_cast (dockWidgets[i.key()])) == dockWidget)
+ {
+ QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView);
+ settings.setValue(chKey,visible);
+ qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible;
+ break;
+ }
+ }
+ }
}
- else
+
+ QDockWidget* dockWidget = qobject_cast(QObject::sender());
+
+ qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible;
+
+ if (dockWidget)
{
- removeDockWidget(qobject_cast(dockWidgets[tool]));
+ // Get action
+ int tool = dockWidgets.key(dockWidget);
+
+ qDebug() << "Updating widget setting" << tool << "to" << visible;
+
+ QAction* action = toolsMenuActions[tool];
+ action->blockSignals(true);
+ action->setChecked(visible);
+ action->blockSignals(false);
+
+ QHashIterator i(dockWidgets);
+ while (i.hasNext())
+ {
+ i.next();
+ if ((static_cast (dockWidgets[i.key()])) == dockWidget)
+ {
+ QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView);
+ settings.setValue(chKey,visible);
+ qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible;
+ break;
+ }
+ }
}
}
-
}
@@ -728,6 +845,8 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view)
tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool();
+ qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible;
+
if (tempWidget)
{
toolsMenuActions[widget]->setChecked(tempVisible);
@@ -746,12 +865,14 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view)
// }
// }
- if ((tempWidget != NULL) && tempVisible)
+ if (tempWidget != NULL)
{
- addDockWidget(tempLocation, tempWidget);
- tempWidget->show();
+ if (tempVisible)
+ {
+ addDockWidget(tempLocation, tempWidget);
+ tempWidget->show();
+ }
}
-
}
QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view)
@@ -763,11 +884,11 @@ QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES t
UASManager::instance()->getActiveUAS()->getAutopilotType():
-1;
- return (QString::number(apType) + "/" +
- QString::number(SECTION_MENU) + "/" +
- QString::number(view) + "/" +
- QString::number(tool) + "/" +
- QString::number(section) + "/" );
+ return (QString::number(apType) + "_" +
+ QString::number(SECTION_MENU) + "_" +
+ QString::number(view) + "_" +
+ QString::number(tool) + "_" +
+ QString::number(section) + "_" );
}
void MainWindow::closeEvent(QCloseEvent *event)
@@ -785,6 +906,29 @@ void MainWindow::closeEvent(QCloseEvent *event)
QMainWindow::closeEvent(event);
}
+void MainWindow::showDockWidget (bool vis)
+{
+ if (!aboutToCloseFlag && !changingViewsFlag)
+ {
+ QDockWidget* temp = qobject_cast(sender());
+
+ if (temp)
+ {
+ QHashIterator i(dockWidgets);
+ while (i.hasNext())
+ {
+ i.next();
+ if ((static_cast (dockWidgets[i.key()])) == temp)
+ {
+ QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView);
+ settings.setValue(chKey,vis);
+ toolsMenuActions[i.key()]->setChecked(vis);
+ break;
+ }
+ }
+ }
+ }
+}
void MainWindow::updateVisibilitySettings (bool vis)
{
@@ -795,9 +939,11 @@ void MainWindow::updateVisibilitySettings (bool vis)
if (temp)
{
QHashIterator i(dockWidgets);
- while (i.hasNext()) {
+ while (i.hasNext())
+ {
i.next();
- if ((static_cast (dockWidgets[i.key()])) == temp) {
+ if ((static_cast (dockWidgets[i.key()])) == temp)
+ {
QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView);
settings.setValue(chKey,vis);
toolsMenuActions[i.key()]->setChecked(vis);
@@ -838,13 +984,7 @@ void MainWindow::connectCommonWidgets()
if (mapWidget && waypointsDockWidget->widget())
{
- // clear path create on the map
- 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)));
- // it notifies that a waypoint global goes to do create and a map graphic too
- connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
}
//TODO temporaly debug
@@ -1130,6 +1270,7 @@ void MainWindow::connectCommonActions()
connect(ui.actionPilotsView, SIGNAL(triggered()), this, SLOT(loadPilotView()));
connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView()));
connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView()));
+ connect(ui.actionUnconnectedView, SIGNAL(triggered()), this, SLOT(loadUnconnectedView()));
connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView()));
connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet()));
@@ -1276,8 +1417,13 @@ void MainWindow::UASCreated(UASInterface* uas)
if (uas != NULL)
{
- // The pilot view was not available on startup, enable it now
+ // Set default settings
+ setDefaultSettingsForAp();
+
+ // The pilot, operator and engineer views were not available on startup, enable them now
ui.actionPilotsView->setEnabled(true);
+ ui.actionOperatorsView->setEnabled(true);
+ ui.actionEngineersView->setEnabled(true);
QIcon icon;
// Set matching icon
@@ -1371,6 +1517,7 @@ void MainWindow::UASCreated(UASInterface* uas)
// }
// }
// }
+
}
break;
default:
@@ -1391,8 +1538,6 @@ void MainWindow::UASCreated(UASInterface* uas)
connectPxActions();
}
break;
-
- loadOperatorView();
}
// Change the view only if this is the first UAS
@@ -1406,19 +1551,31 @@ void MainWindow::UASCreated(UASInterface* uas)
// Load last view if setting is present
if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED"))
{
+ clearView();
int view = settings.value("CURRENT_VIEW_WITH_UAS_CONNECTED").toInt();
- currentView = (VIEW_SECTIONS) view;
- presentView();
-
- // Restore the widget positions and size
- if (settings.contains(getWindowStateKey()))
+ switch (view)
{
- restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
+ case VIEW_ENGINEER:
+ loadEngineerView();
+ break;
+ case VIEW_MAVLINK:
+ loadMAVLinkView();
+ break;
+ case VIEW_PILOT:
+ loadPilotView();
+ break;
+ case VIEW_UNCONNECTED:
+ loadUnconnectedView();
+ break;
+ case VIEW_OPERATOR:
+ default:
+ loadOperatorView();
+ break;
}
}
else
{
- loadEngineerView();
+ loadOperatorView();
}
}
@@ -1436,7 +1593,7 @@ void MainWindow::UASCreated(UASInterface* uas)
void MainWindow::clearView()
{
// Save current state
- if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
+ if (UASManager::instance()->getUASList().count() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion()));
settings.setValue(getWindowGeometryKey(), saveGeometry());
QAction* temp;
@@ -1449,12 +1606,12 @@ void MainWindow::clearView()
if (temp)
{
- //qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked();
+ qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked();
settings.setValue(chKey,temp->isChecked());
}
else
{
- //qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED";
+ qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED";
settings.setValue(chKey,false);
}
}
@@ -1471,11 +1628,8 @@ void MainWindow::clearView()
if (dockWidget)
{
// Remove dock widget from main window
- //this->removeDockWidget(dockWidget);
- dockWidget->setVisible(false);
- // Deletion of dockWidget would also delete all child
- // widgets of dockWidget
- // Is there a way to unset a widget from QDockWidget?
+ removeDockWidget(dockWidget);
+ //dockWidget->setVisible(false);
}
}
changingViewsFlag = false;
@@ -1485,11 +1639,10 @@ void MainWindow::loadEngineerView()
{
if (currentView != VIEW_ENGINEER)
{
- clearView();
-
- currentView = VIEW_ENGINEER;
-
- presentView();
+ clearView();
+ currentView = VIEW_ENGINEER;
+ ui.actionEngineersView->setChecked(true);
+ presentView();
}
}
@@ -1497,11 +1650,21 @@ void MainWindow::loadOperatorView()
{
if (currentView != VIEW_OPERATOR)
{
- clearView();
-
- currentView = VIEW_OPERATOR;
+ clearView();
+ currentView = VIEW_OPERATOR;
+ ui.actionOperatorsView->setChecked(true);
+ presentView();
+ }
+}
- presentView();
+void MainWindow::loadUnconnectedView()
+{
+ if (currentView != VIEW_UNCONNECTED)
+ {
+ clearView();
+ currentView = VIEW_UNCONNECTED;
+ ui.actionUnconnectedView->setChecked(true);
+ presentView();
}
}
@@ -1509,11 +1672,10 @@ void MainWindow::loadPilotView()
{
if (currentView != VIEW_PILOT)
{
- clearView();
-
- currentView = VIEW_PILOT;
-
- presentView();
+ clearView();
+ currentView = VIEW_PILOT;
+ ui.actionPilotsView->setChecked(true);
+ presentView();
}
}
@@ -1521,11 +1683,10 @@ void MainWindow::loadMAVLinkView()
{
if (currentView != VIEW_MAVLINK)
{
- clearView();
-
- currentView = VIEW_MAVLINK;
-
- presentView();
+ clearView();
+ currentView = VIEW_MAVLINK;
+ ui.actionMavlinkView->setChecked(true);
+ presentView();
}
}
@@ -1556,7 +1717,6 @@ void MainWindow::presentView()
showTheCentralWidget(CENTRAL_DATA_PLOT, currentView);
-
// Show docked widgets based on current view and autopilot type
// UAS CONTROL
@@ -1631,8 +1791,29 @@ void MainWindow::presentView()
// MAVLINK LOG PLAYER
showTheWidget(MENU_MAVLINK_LOG_PLAYER, currentView);
+ // VIDEO 1
+ showTheWidget(MENU_VIDEO_STREAM_1, currentView);
+
+ // VIDEO 2
+ showTheWidget(MENU_VIDEO_STREAM_2, currentView);
+
this->show();
+ // Restore window state
+ if (UASManager::instance()->getUASList().count() > 0)
+ {
+ // Restore the mainwindow size
+ if (settings.contains(getWindowGeometryKey()))
+ {
+ restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray());
+ }
+
+ // Restore the widget positions and size
+ if (settings.contains(getWindowStateKey()))
+ {
+ restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
+ }
+ }
}
void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view)
@@ -1640,15 +1821,17 @@ void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SE
bool tempVisible;
QWidget* tempWidget = dockWidgets[centralWidget];
- tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view), false).toBool();
- //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible;
+ tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,centralWidget,view), false).toBool();
+ qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible;
if (toolsMenuActions[centralWidget])
{
+ qDebug() << "SETTING TO:" << tempVisible;
toolsMenuActions[centralWidget]->setChecked(tempVisible);
}
if (centerStack && tempWidget && tempVisible)
{
+ qDebug() << "ACTIVATING MAIN WIDGET";
centerStack->setCurrentWidget(tempWidget);
}
}
diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h
index df1cfb0a5a7fe579b857b3e67369aae9735aae3a..25504031af09bbb1e7c99870d1d1963c9bf8e4f3 100644
--- a/src/ui/MainWindow.h
+++ b/src/ui/MainWindow.h
@@ -112,6 +112,8 @@ public slots:
void stopVideoCapture();
void saveScreen();
+ /** @brief Load default view when no MAV is connected */
+ void loadUnconnectedView();
/** @brief Load view for pilot */
void loadPilotView();
/** @brief Load view for engineer */
@@ -166,7 +168,7 @@ public slots:
* It shows the QDockedWidget based on the action sender
*
*/
- void showToolWidget();
+ void showToolWidget(bool visible);
/**
* @brief Shows a Widget from the center stack based on the action sender
@@ -177,8 +179,10 @@ public slots:
*/
void showCentralWidget();
+ /** @brief Change actively a QDockWidgets visibility by an action */
+ void showDockWidget(bool vis);
/** @brief Updates a QDockWidget's checked status based on its visibility */
- void updateVisibilitySettings (bool vis);
+ void updateVisibilitySettings(bool vis);
/** @brief Updates a QDockWidget's location */
void updateLocationSettings (Qt::DockWidgetArea location);
@@ -187,6 +191,9 @@ protected:
MainWindow(QWidget *parent = 0);
+ /** @brief Set default window settings for the current autopilot type */
+ void setDefaultSettingsForAp();
+
// These defines are used to save the settings when selecting with
// which widgets populate the views
// FIXME: DO NOT PUT CUSTOM VALUES IN THIS ENUM since it is iterated over
@@ -212,6 +219,8 @@ protected:
MENU_SLUGS_HIL,
MENU_SLUGS_CAMERA,
MENU_MAVLINK_LOG_PLAYER,
+ MENU_VIDEO_STREAM_1,
+ MENU_VIDEO_STREAM_2,
CENTRAL_SEPARATOR= 255, // do not change
CENTRAL_LINECHART,
CENTRAL_PROTOCOL,
@@ -238,6 +247,7 @@ protected:
VIEW_OPERATOR,
VIEW_PILOT,
VIEW_MAVLINK,
+ VIEW_UNCONNECTED, ///< View in unconnected mode, when no UAS is available
} VIEW_SECTIONS;
@@ -368,6 +378,8 @@ protected:
QPointer watchdogControlDockWidget;
QPointer headUpDockWidget;
+ QPointer video1DockWidget;
+ QPointer video2DockWidget;
QPointer logPlayerDockWidget;
QPointer hsiDockWidget;
diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui
index 5808be0b0053bb85beee4a4b984c4d7dceda6936..85e8412f13c36da33604b7e93a6a358969609269 100644
--- a/src/ui/MainWindow.ui
+++ b/src/ui/MainWindow.ui
@@ -100,7 +100,7 @@
@@ -121,11 +121,18 @@
+
+
+
+
@@ -296,6 +303,9 @@
Operator
+
+ Meta+O
+
@@ -308,6 +318,9 @@
Engineer
+
+ Meta+E
+
@@ -320,6 +333,9 @@
Mavlink
+
+ Meta+M
+
@@ -341,6 +357,9 @@
Pilot
+
+ Meta+P
+
@@ -379,6 +398,21 @@
QGroundControl global settings
+
+
+ true
+
+
+
+ :/images/devices/network-wireless.svg:/images/devices/network-wireless.svg
+
+
+ Unconnected
+
+
+ Meta+U
+
+
diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc
index fca34aa29264069d612f5b58cbdf888635afbccb..ff7bff7046867f20b38e1ce9dce33bc8ca5b8d3c 100644
--- a/src/ui/MapWidget.cc
+++ b/src/ui/MapWidget.cc
@@ -21,6 +21,7 @@
#include "UASManager.h"
#include "MAV2DIcon.h"
#include "Waypoint2DIcon.h"
+#include "UASWaypointManager.h"
#include "MG.h"
@@ -38,7 +39,7 @@ MapWidget::MapWidget(QWidget *parent) :
mc = new qmapcontrol::MapControl(QSize(320, 240));
// VISUAL MAP STYLE
- QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)}");
+ QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }");
mc->setPen(QGC::colorCyan.darker(400));
@@ -93,7 +94,7 @@ MapWidget::MapWidget(QWidget *parent) :
// SET INITIAL POSITION AND ZOOM
// Set default zoom level
- mc->setZoom(16);
+ mc->setZoom(17);
// Zurich, ETH
mc->setView(QPointF(8.548056,47.376389));
@@ -371,7 +372,7 @@ void MapWidget::createPathButtonClicked(bool checked)
// emit signal start to create a Waypoint global
- emit createGlobalWP(true, mc->currentCoordinate());
+ //emit createGlobalWP(true, mc->currentCoordinate());
// // Clear the previous WP track
// // TODO: Move this to an actual clear track button and add a warning dialog
@@ -400,50 +401,104 @@ void MapWidget::createPathButtonClicked(bool checked)
void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordinate)
{
- //qDebug() << mc->mouseMode();
if (QEvent::MouseButtonRelease == event->type() && createPath->isChecked())
{
// Create waypoint name
QString str;
- str = QString("%1").arg(waypointPath->numberOfPoints());
-
// create the WP and set everything in the LineString to display the path
Waypoint2DIcon* tempCirclePoint;
if (mav)
{
- tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, new QPen(mav->getColor()));
+ mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y()));
}
else
{
+ str = QString("%1").arg(waypointPath->numberOfPoints());
tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
- }
- mc->layer("Waypoints")->addGeometry(tempCirclePoint);
- qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
- wps.append(tempPoint);
- waypointPath->addPoint(tempPoint);
+ mc->layer("Waypoints")->addGeometry(tempCirclePoint);
- wpIndex.insert(str,tempPoint);
+ qmapcontrol::Point* tempPoint = new qmapcontrol::Point(coordinate.x(), coordinate.y(),str);
+ wps.append(tempPoint);
+ waypointPath->addPoint(tempPoint);
- // Refresh the screen
- mc->updateRequest(tempPoint->boundingBox().toRect());
+ // Refresh the screen
+ mc->updateRequest(tempPoint->boundingBox().toRect());
+ }
// emit signal mouse was clicked
- emit captureMapCoordinateClick(coordinate);
+ //emit captureMapCoordinateClick(coordinate);
}
}
-void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
+void MapWidget::updateWaypoint(int uas, Waypoint* wp)
{
- if (!wpExists(coordinate))
+ //qDebug() << "UPDATING WP" << wp->getId() << __FILE__ << __LINE__;
+ if (uas == this->mav->getUASID())
{
+ int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
// Create waypoint name
- QString str;
+ QString str = QString("%1").arg(wpindex);
+ // Check if wp exists yet
+ if (!(wps.count() > wpindex))
+ {
+ QPointF coordinate;
+ coordinate.setX(wp->getX());
+ coordinate.setY(wp->getY());
+ createWaypointGraphAtMap(wpindex, coordinate);
+ qDebug() << "Waypoint Index did not contain" << str;
+ }
+ else
+ {
+ // Waypoint exists, update it
+ if(!waypointIsDrag)
+ {
+ qDebug() <<"indice WP= "<< wpindex <<"\n";
+
+ QPointF coordinate;
+ coordinate.setX(wp->getX());
+ coordinate.setY(wp->getY());
+
+ Point* waypoint;
+ waypoint = wps.at(wpindex);//wpIndex[str];
+ if (waypoint)
+ {
+ // First set waypoint coordinate
+ waypoint->setCoordinate(coordinate);
+ // Now update icon position
+ //mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex));
+ wpIcons.at(wpindex)->setCoordinate(coordinate);
+ //mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex));
+ // Then waypoint line coordinate
+ Point* linesegment = NULL;
+ if (waypointPath->points().size() > wpindex)
+ {
+ linesegment = waypointPath->points().at(wpindex);
+ }
+
+ if (linesegment)
+ {
+ linesegment->setCoordinate(coordinate);
+ }
+
+ //point2Find = dynamic_cast (mc->layer("Waypoints")->get_Geometry(wpindex));
+ //point2Find->setCoordinate(coordinate);
+ mc->updateRequest(waypoint->boundingBox().toRect());
+ }
+ }
+ }
+ }
+}
- str = QString("%1").arg(waypointPath->numberOfPoints());
+void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
+{
+ if (!wpExists(coordinate))
+ {
+ // Create waypoint name
+ QString str;
// create the WP and set everything in the LineString to display the path
//CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str);
@@ -451,35 +506,42 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate)
if (mav)
{
- tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, new QPen(mav->getColor()));
+ int uas = mav->getUASID();
+ str = QString("%1").arg(id);
+ qDebug() << "Waypoint list count:" << str;
+ tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle, mavPens.value(uas));
}
else
{
+ str = QString("%1").arg(id);
tempCirclePoint = new Waypoint2DIcon(coordinate.x(), coordinate.y(), 20, str, qmapcontrol::Point::Middle);
}
mc->layer("Waypoints")->addGeometry(tempCirclePoint);
+ wpIcons.append(tempCirclePoint);
Point* tempPoint = new Point(coordinate.x(), coordinate.y(),str);
wps.append(tempPoint);
waypointPath->addPoint(tempPoint);
- wpIndex.insert(str,tempPoint);
+ //wpIndex.insert(str,tempPoint);
qDebug()<<"Funcion createWaypointGraphAtMap WP= "< x= "<latitude()<<" y= "<longitude();
// Refresh the screen
- mc->updateRequestNew();//(tempPoint->boundingBox().toRect());
+ mc->updateRequest(tempPoint->boundingBox().toRect());
}
//// // emit signal mouse was clicked
// emit captureMapCoordinateClick(coordinate);
}
-int MapWidget::wpExists(const QPointF coordinate){
+int MapWidget::wpExists(const QPointF coordinate)
+{
for (int i = 0; i < wps.size(); i++){
if (wps.at(i)->latitude() == coordinate.y() &&
- wps.at(i)->longitude()== coordinate.x()){
+ wps.at(i)->longitude()== coordinate.x())
+ {
return 1;
}
}
@@ -493,21 +555,23 @@ void MapWidget::captureGeometryClick(Geometry* geom, QPoint point)
Q_UNUSED(point);
mc->setMouseMode(qmapcontrol::MapControl::None);
-
}
void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
{
-
-
waypointIsDrag = true;
// Refresh the screen
mc->updateRequest(geom->boundingBox().toRect());
int temp = 0;
+
+ // Get waypoint index in list
+ bool wpIndexOk;
+ int index = geom->name().toInt(&wpIndexOk);
+
qmapcontrol::Point* point2Find;
- point2Find = wpIndex[geom->name()];
+ point2Find = wps.at(geom->name().toInt());
if (point2Find)
{
@@ -518,6 +582,21 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
{
point2Find->setCoordinate(coordinate);
+ if (wpIndexOk)
+ {
+ mc->updateRequest(point2Find->boundingBox().toRect());
+ if (mav)
+ {
+ QVector wps = mav->getWaypointManager()->getWaypointList();
+
+ if (wps.size() > index)
+ {
+ wps.at(index)->setX(coordinate.x());
+ wps.at(index)->setY(coordinate.y());
+ mav->getWaypointManager()->notifyOfChange(wps.at(index));
+ }
+ }
+ }
// qDebug() << geom->name();
temp = geom->get_myIndex();
//qDebug() << temp;
@@ -553,18 +632,102 @@ MapWidget::~MapWidget()
void MapWidget::addUAS(UASInterface* uas)
{
connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
+ //connect(uas->getWaypointManager(), SIGNAL(waypointListChanged()), this, SLOT(redoWaypoints()));
+}
+
+void MapWidget::updateWaypointList(int uas)
+{
+ // Get already existing waypoints
+ UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
+ if (uasInstance)
+ {
+ QVector wpList = uasInstance->getWaypointManager()->getWaypointList();
+
+ // Clear if necessary
+ if (wpList.count() == 0)
+ {
+ clearWaypoints(uas);
+ return;
+ }
+
+ // Load all existing waypoints into map view
+ foreach (Waypoint* wp, wpList)
+ {
+ updateWaypoint(mav->getUASID(), wp);
+ }
+
+ // Delete now unused wps
+ if (wps.count() > wpList.count())
+ {
+ mc->layer("Waypoints")->removeGeometry(waypointPath);
+ for (int i = wpList.count(); i < wps.count(); ++i)
+ {
+ QRect updateRect = wps.at(i)->boundingBox().toRect();
+ wps.removeAt(i);
+ mc->layer("Waypoints")->removeGeometry(wpIcons.at(i));
+ waypointPath->points().removeAt(i);
+ //Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp));
+
+ mc->updateRequest(updateRect);
+ }
+ mc->layer("Waypoints")->addGeometry(waypointPath);
+ }
+
+ // Clear and rebuild linestring
+
+ }
+}
+
+void MapWidget::redoWaypoints(int uas)
+{
+ // QObject* sender = QObject::sender();
+ // UASWaypointManager* manager = dynamic_cast(sender);
+ // if (sender)
+ // {
+ // Get waypoint list for this MAV
+
+ // Clear all waypoints
+ clearWaypoints();
+ // Re-add the updated waypoints
+
+ // }
+
+ updateWaypointList(uas);
}
void MapWidget::activeUASSet(UASInterface* uas)
{
+ // Disconnect old MAV
+ if (mav)
+ {
+ // clear path create on the map
+ disconnect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
+ // add Waypoint widget in the WaypointList widget when mouse clicked
+ disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
+
+
+ // it notifies that a waypoint global goes to do create and a map graphic too
+ //connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF)));
+ }
+
if (uas)
{
mav = uas;
- QColor color = mav->getColor();
+ QColor color = mav->getColor().lighter(100);
color.setAlphaF(0.6);
QPen* pen = new QPen(color);
- pen->setWidth(3.0);
- // FIXME Load waypoints of this system
+ pen->setWidth(2.0);
+ mavPens.insert(mav->getUASID(), pen);
+ // FIXME Remove after refactoring
+ waypointPath->setPen(pen);
+
+ // Delete all waypoints and start fresh
+ redoWaypoints();
+
+ // clear path create on the map
+ connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
+ // add Waypoint widget in the WaypointList widget when mouse clicked
+ connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
}
}
@@ -589,7 +752,10 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
//pointpen->setWidth(3);
//points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen));
- MAV2DIcon* p;
+ qmapcontrol::Point* p;
+ QPointF coordinate;
+ coordinate.setX(lat);
+ coordinate.setY(lon);
if (!uasIcons.contains(uas->getUASID()))
{
@@ -597,40 +763,42 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
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);
+ //QPen* pointpen = new QPen(uasColor);
+ qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
+ //p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, mavPens.value(uas->getUASID()));
+ p = new Waypoint2DIcon(lat, lon, 20, QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
uasIcons.insert(uas->getUASID(), p);
- tracks->addGeometry(p);
+ mc->layer("Waypoints")->addGeometry(p);
// Line
// A QPen also can use transparency
QList points;
- points.append(new qmapcontrol::Point(lat, lon, ""));
+ points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
QPen* linepen = new QPen(uasColor.darker());
linepen->setWidth(2);
// Create tracking line string
- qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, uas->getUASName(), linepen);
+ qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
uasTrails.insert(uas->getUASID(), ls);
// Add the LineString to the layer
- mc->layer("Tracking")->addGeometry(ls);
+ mc->layer("Waypoints")->addGeometry(ls);
}
else
{
- p = dynamic_cast(uasIcons.value(uas->getUASID()));
- if (p)
- {
+// p = dynamic_cast(uasIcons.value(uas->getUASID()));
+// if (p)
+// {
+ p = uasIcons.value(uas->getUASID());
p->setCoordinate(QPointF(lat, lon));
- p->setYaw(uas->getYaw());
- }
+ //p->setYaw(uas->getYaw());
+// }
// Extend trail
- uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, ""));
+ uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
}
- mc->updateRequest(p->boundingBox().toRect());
+mc->updateRequest(p->boundingBox().toRect());
//mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
@@ -738,24 +906,28 @@ void MapWidget::changeEvent(QEvent *e)
}
}
-void MapWidget::clearWaypoints()
+void MapWidget::clearWaypoints(int uas)
{
// Clear the previous WP track
- mc->layer("Waypoints")->clearGeometries();
+ //mc->layer("Waypoints")->clearGeometries();
wps.clear();
- waypointPath->setPoints(wps);
+ waypointPath->points().clear();
+ //delete waypointPath;
+ //waypointPath = new
mc->layer("Waypoints")->addGeometry(waypointPath);
- wpIndex.clear();
+ //wpIndex.clear();
mc->updateRequestNew();//(waypointPath->boundingBox().toRect());
if(createPath->isChecked())
{
createPath->click();
}
+
+ qDebug() << "CLEARING WAYPOINTS";
}
-void MapWidget::clearPath()
+void MapWidget::clearPath(int uas)
{
mc->layer("Tracking")->clearGeometries();
foreach (qmapcontrol::LineString* ls, uasTrails)
@@ -769,30 +941,6 @@ void MapWidget::clearPath()
mc->updateRequestNew();//(QRect(0, 0, width(), height()));
}
-void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon)
-{
- if(!waypointIsDrag)
- {
- qDebug() <<"indice WP= "<setCoordinate(coordinate);
-
- point2Find = dynamic_cast (mc->layer("Waypoints")->get_Geometry(index));
- point2Find->setCoordinate(coordinate);
-
- // Refresh the screen
- mc->updateRequestNew();//(point2Find->boundingBox().toRect());
- }
-
-
-}
-
void MapWidget::updateCameraPosition(double radio, double bearing, QString dir)
{
// FIXME Mariano
diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h
index d8474cbf5c8bfebed1ee3f9da3ce2767519837c2..91d2b6d2b2fd66a121892d31f884832626b26223 100644
--- a/src/ui/MapWidget.h
+++ b/src/ui/MapWidget.h
@@ -44,6 +44,8 @@ This file is part of the QGROUNDCONTROL project
class QMenu;
+class Waypoint;
+class Waypoint2DIcon;
namespace Ui {
class MapWidget;
@@ -72,10 +74,18 @@ public slots:
QPointF getPointxBearing_Range(double lat1, double lon1, double bearing, double distance);
/** @brief Clear the waypoints overlay layer */
- void clearWaypoints();
+ void clearWaypoints(int uas=0);
/** @brief Clear the UAV tracks on the map */
- void clearPath();
- void changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon);
+ void clearPath(int uas=0);
+
+ /** @brief Update waypoint list */
+ void updateWaypointList(int uas);
+ /** @brief Clear all waypoints and re-add them from updated list */
+ void redoWaypoints(int uas=0);
+
+ /** @brief Update waypoint */
+ void updateWaypoint(int uas, Waypoint* wp);
+
void drawBorderCamAtMap(bool status);
/** @brief Bring up dialog to go to a specific location */
void goTo();
@@ -105,12 +115,12 @@ protected:
QMenu* mapMenu;
QPushButton* mapButton;
- qmapcontrol::MapControl* mc; ///< QMapControl widget
- 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
+ qmapcontrol::MapControl* mc; ///< QMapControl widget
+ 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
qmapcontrol::GeometryLayer* camLayer; ///< Layer for camera indicator
@@ -124,6 +134,9 @@ protected:
QMap uasIcons;
QMap uasTrails;
+ QMap mavPens;
+ //QMap > mavWps;
+ //QMap waypointPaths;
UASInterface* mav;
quint64 lastUpdate;
@@ -135,21 +148,24 @@ protected:
void createPathButtonClicked(bool checked);
- void createWaypointGraphAtMap (const QPointF coordinate);
+ /** @brief Create the graphic representation of the waypoint */
+ void createWaypointGraphAtMap(int id, const QPointF coordinate);
void mapproviderSelected(QAction* action);
signals:
//void movePoint(QPointF newCoord);
- void captureMapCoordinateClick(const QPointF coordinate); //ROCA
- void createGlobalWP(bool value, QPointF centerCoordinate);
+ //void captureMapCoordinateClick(const QPointF coordinate); //ROCA
+ //void createGlobalWP(bool value, QPointF centerCoordinate);
+ void waypointCreated(Waypoint* wp);
void sendGeometryEndDrag(const QPointF coordinate, const int index);
private:
Ui::MapWidget *m_ui;
QList wps;
+ QListwpIcons;
qmapcontrol::LineString* waypointPath;
- QHash wpIndex;
+ //QHash wpIndex;
QPen* pointPen;
int wpExists(const QPointF coordinate);
bool waypointIsDrag;
diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc
index e27f851f32a8deae48eee99600f669c5978ee90f..7c4fc341b2c159f9d1c9d6b8ab4e100f35da0ec9 100644
--- a/src/ui/QGCMAVLinkLogPlayer.cc
+++ b/src/ui/QGCMAVLinkLogPlayer.cc
@@ -319,7 +319,7 @@ void QGCMAVLinkLogPlayer::logLoop()
//qDebug() << "nextExecutionTime:" << nextExecutionTime << "QGC START TIME:" << currentStartTime << "LOG START TIME:" << startTime;
- if (nextExecutionTime < 5)
+ if (nextExecutionTime < 2)
{
logLoop();
}
diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc
index f2de5769ed28b86be5159a747b385c4a2dc74db3..12b7e734cd99b2783f8667c46bc71a05fd40a85a 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -127,11 +127,11 @@ void WaypointList::setUAS(UASInterface* uas)
connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updatePosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
- connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
- connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void)));
- connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
- connect(&uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
- connect(&uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
+ connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
+ connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void)));
+ connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
+ connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP()));
+ connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool)));
}
}
@@ -141,7 +141,7 @@ void WaypointList::saveWaypoints()
if (uas)
{
QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
- uas->getWaypointManager().saveWaypoints(fileName);
+ uas->getWaypointManager()->saveWaypoints(fileName);
}
}
@@ -152,7 +152,7 @@ void WaypointList::loadWaypoints()
QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
- uas->getWaypointManager().loadWaypoints(fileName);
+ uas->getWaypointManager()->loadWaypoints(fileName);
}
}
@@ -160,7 +160,7 @@ void WaypointList::transmit()
{
if (uas)
{
- uas->getWaypointManager().writeWaypoints();
+ uas->getWaypointManager()->writeWaypoints();
}
}
@@ -168,7 +168,7 @@ void WaypointList::read()
{
if (uas)
{
- uas->getWaypointManager().readWaypoints();
+ uas->getWaypointManager()->readWaypoints();
}
}
@@ -178,17 +178,17 @@ void WaypointList::add()
{
// if(isGlobalWP)
// {
-// const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+// const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
// if (waypoints.size() > 0)
// {
// Waypoint *last = waypoints.at(waypoints.size()-1);
// Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
-// uas->getWaypointManager().addWaypoint(wp);
+// uas->getWaypointManager()->addWaypoint(wp);
// }
// else
// {
// Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
-// uas->getWaypointManager().addWaypoint(wp);
+// uas->getWaypointManager()->addWaypoint(wp);
// }
//
// emit createWaypointAtMap(centerMapCoordinate);
@@ -196,7 +196,7 @@ void WaypointList::add()
// else
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
Waypoint *wp;
@@ -206,13 +206,13 @@ void WaypointList::add()
Waypoint *last = waypoints.at(waypoints.size()-1);
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);
+ uas->getWaypointManager()->addWaypoint(wp);
}
else
{
// Create global frame waypoint per default
wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE);
- uas->getWaypointManager().addWaypoint(wp);
+ uas->getWaypointManager()->addWaypoint(wp);
}
if (wp->getFrame() == MAV_FRAME_GLOBAL)
{
@@ -234,17 +234,17 @@ void WaypointList::addCurrentPositonWaypoint()
//}
//else
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
if (waypoints.size() > 0)
{
Waypoint *last = waypoints.at(waypoints.size()-1);
Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
- uas->getWaypointManager().addWaypoint(wp);
+ uas->getWaypointManager()->addWaypoint(wp);
}
else
{
Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000);
- uas->getWaypointManager().addWaypoint(wp);
+ uas->getWaypointManager()->addWaypoint(wp);
}
//isLocalWP = true;
@@ -264,7 +264,7 @@ void WaypointList::changeCurrentWaypoint(quint16 seq)
{
if (uas)
{
- uas->getWaypointManager().setCurrentWaypoint(seq);
+ uas->getWaypointManager()->setCurrentWaypoint(seq);
}
}
@@ -272,7 +272,7 @@ void WaypointList::currentWaypointChanged(quint16 seq)
{
if (uas)
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
if (seq < waypoints.size())
{
@@ -297,124 +297,125 @@ void WaypointList::waypointListChanged()
{
if (uas)
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
-
- // For Global Waypoints
- //if(isGlobalWP)
- //{
- //isLocalWP = false;
- //// first remove all views of non existing waypoints
- //if (!wpGlobalViews.empty())
- //{
- //QMapIterator viewIt(wpGlobalViews);
- //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())
- //{
- //WaypointGlobalView* widget = wpGlobalViews.find(cur).value();
- //widget->hide();
- //listLayout->removeWidget(widget);
- //wpGlobalViews.remove(cur);
- //}
- //}
- //}
-
- //// then add/update the views for each waypoint in the list
- //for(int i = 0; i < waypoints.size(); i++)
- //{
- //Waypoint *wp = waypoints[i];
- //if (!wpGlobalViews.contains(wp))
- //{
- //WaypointGlobalView* wpview = new WaypointGlobalView(wp, this);
- //wpGlobalViews.insert(wp, wpview);
- //connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*)));
- //connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*)));
-//// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*)));
-//// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*)));
- //// connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(Waypoint*)));
-//// connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
-//// connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
- //}
- //WaypointGlobalView *wpgv = wpGlobalViews.value(wp);
- //wpgv->updateValues();
- //listLayout->addWidget(wpgv);
- //}
-
- //}
- //else
+ // Prevent updates to prevent visual flicker
+ this->setUpdatesEnabled(false);
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
+
+ // For Global Waypoints
+ //if(isGlobalWP)
+ //{
+ //isLocalWP = false;
+ //// first remove all views of non existing waypoints
+ //if (!wpGlobalViews.empty())
+ //{
+ //QMapIterator viewIt(wpGlobalViews);
+ //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())
+ //{
+ //WaypointGlobalView* widget = wpGlobalViews.find(cur).value();
+ //widget->hide();
+ //listLayout->removeWidget(widget);
+ //wpGlobalViews.remove(cur);
+ //}
+ //}
+ //}
+
+ //// then add/update the views for each waypoint in the list
+ //for(int i = 0; i < waypoints.size(); i++)
+ //{
+ //Waypoint *wp = waypoints[i];
+ //if (!wpGlobalViews.contains(wp))
+ //{
+ //WaypointGlobalView* wpview = new WaypointGlobalView(wp, this);
+ //wpGlobalViews.insert(wp, wpview);
+ //connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*)));
+ //connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*)));
+ //// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*)));
+ //// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*)));
+ //// connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(Waypoint*)));
+ //// connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
+ //// connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
+ //}
+ //WaypointGlobalView *wpgv = wpGlobalViews.value(wp);
+ //wpgv->updateValues();
+ //listLayout->addWidget(wpgv);
+ //}
+
+ //}
+ //else
+ //{
+ // for local Waypoints
+ // first remove all views of non existing waypoints
+ if (!wpViews.empty())
{
- // for local Waypoints
- // first remove all views of non existing waypoints
- if (!wpViews.empty())
+ QMapIterator viewIt(wpViews);
+ viewIt.toFront();
+ while(viewIt.hasNext())
{
- QMapIterator viewIt(wpViews);
- viewIt.toFront();
- while(viewIt.hasNext())
+ viewIt.next();
+ Waypoint *cur = viewIt.key();
+ int i;
+ for (i = 0; i < waypoints.size(); i++)
{
- viewIt.next();
- Waypoint *cur = viewIt.key();
- int i;
- for (i = 0; i < waypoints.size(); i++)
+ if (waypoints[i] == cur)
{
- if (waypoints[i] == cur)
- {
- break;
- }
- }
- if (i == waypoints.size())
- {
- WaypointView* widget = wpViews.find(cur).value();
- widget->hide();
- listLayout->removeWidget(widget);
- wpViews.remove(cur);
+ break;
}
}
+ if (i == waypoints.size())
+ {
+ WaypointView* widget = wpViews.find(cur).value();
+ widget->hide();
+ listLayout->removeWidget(widget);
+ wpViews.remove(cur);
+ }
}
+ }
- // then add/update the views for each waypoint in the list
- for(int i = 0; i < waypoints.size(); i++)
+ // then add/update the views for each waypoint in the list
+ for(int i = 0; i < waypoints.size(); i++)
+ {
+ Waypoint *wp = waypoints[i];
+ if (!wpViews.contains(wp))
{
- 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* 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);
}
- }
-
-
- loadFileGlobalWP = false;
+ this->setUpdatesEnabled(true);
}
+ loadFileGlobalWP = false;
+ //}
}
void WaypointList::moveUp(Waypoint* wp)
{
if (uas)
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
//get the current position of wp in the local storage
int i;
@@ -427,7 +428,7 @@ void WaypointList::moveUp(Waypoint* wp)
// if wp was found and its not the first entry, move it
if (i < waypoints.size() && i > 0)
{
- uas->getWaypointManager().moveWaypoint(i, i-1);
+ uas->getWaypointManager()->moveWaypoint(i, i-1);
}
}
}
@@ -436,7 +437,7 @@ void WaypointList::moveDown(Waypoint* wp)
{
if (uas)
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
//get the current position of wp in the local storage
int i;
@@ -449,7 +450,7 @@ void WaypointList::moveDown(Waypoint* wp)
// if wp was found and its not the last entry, move it
if (i < waypoints.size()-1)
{
- uas->getWaypointManager().moveWaypoint(i, i+1);
+ uas->getWaypointManager()->moveWaypoint(i, i+1);
}
}
}
@@ -458,7 +459,7 @@ void WaypointList::removeWaypoint(Waypoint* wp)
{
if (uas)
{
- uas->getWaypointManager().removeWaypoint(wp->getId());
+ uas->getWaypointManager()->removeWaypoint(wp->getId());
}
}
@@ -482,7 +483,7 @@ void WaypointList::on_clearWPListButton_clicked()
if (uas)
{
emit clearPathclicked();
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
{
WaypointView* widget = wpViews.find(waypoints[0]).value();
@@ -503,17 +504,17 @@ void WaypointList::addWaypointMouse(QPointF coordinate)
{
if (uas)
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
if (waypoints.size() > 0)
{
Waypoint *last = waypoints.at(waypoints.size()-1);
Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
- uas->getWaypointManager().addWaypoint(wp);
+ uas->getWaypointManager()->addWaypoint(wp);
}
else
{
Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
- uas->getWaypointManager().addWaypoint(wp);
+ uas->getWaypointManager()->addWaypoint(wp);
}
@@ -527,7 +528,7 @@ void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)
{
if (uas)
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
if (waypoints.size() > 0)
{
Waypoint *temp = waypoints.at(indexWP);
@@ -560,7 +561,7 @@ void WaypointList::clearWPWidget()
{
if (uas)
{
- const QVector &waypoints = uas->getWaypointManager().getWaypointList();
+ const QVector &waypoints = uas->getWaypointManager()->getWaypointList();
while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
{
WaypointView* widget = wpViews.find(waypoints[0]).value();
diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui
index 6918d631120a982e2c4dca01ec4b8f369ab2ba59..7e8610374231f8d08b192da014bf53ae912c2517 100644
--- a/src/ui/WaypointView.ui
+++ b/src/ui/WaypointView.ui
@@ -7,7 +7,7 @@
0
0
1389
- 33
+ 39
@@ -375,7 +375,7 @@ QProgressBar::chunk#thrustBar {
m
- 7
+ 2
-100000.000000000000000
diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc
index 94790b69eb8f506062768bdcf68c4efe3acb1435..21ac666aa44bcfef1840b95abd698fc1ea9bacf4 100644
--- a/src/ui/linechart/LinechartWidget.cc
+++ b/src/ui/linechart/LinechartWidget.cc
@@ -483,7 +483,7 @@ void LinechartWidget::startLogging()
fileName.append(".csv");
}
- while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort)
+ while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort && fileName != "")
{
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Critical);
diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc
index dc0e187cf59d4248ddef73471306ee3349251214..8920f76d996e626505b450d55620cd144e1f3630 100644
--- a/src/ui/map/MAV2DIcon.cc
+++ b/src/ui/map/MAV2DIcon.cc
@@ -46,50 +46,50 @@ void MAV2DIcon::setYaw(float yaw)
void MAV2DIcon::drawIcon(QPen* pen)
{
-// mypixmap = new QPixmap(radius+1, radius+1);
-// mypixmap->fill(Qt::transparent);
-// QPainter painter(mypixmap);
+ mypixmap = new QPixmap(radius+1, radius+1);
+ mypixmap->fill(Qt::transparent);
+ QPainter painter(mypixmap);
-// // DRAW WAYPOINT
-// QPointF p(radius/2, radius/2);
+ // DRAW WAYPOINT
+ QPointF p(radius/2, radius/2);
-// float waypointSize = radius;
-// QPolygonF poly(4);
-// // Top point
-// poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
-// // Right point
-// poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
-// // Bottom point
-// poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
-// poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
+ float waypointSize = radius;
+ QPolygonF poly(4);
+ // Top point
+ poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f));
+ // Right point
+ poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y()));
+ // Bottom point
+ poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f));
+ poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y()));
-//// // Select color based on if this is the current waypoint
-//// if (list.at(i)->getCurrent())
-//// {
-//// color = QGC::colorCyan;//uas->getColor();
-//// pen.setWidthF(refLineWidthToPen(0.8f));
-//// }
-//// else
-//// {
-//// color = uas->getColor();
-//// pen.setWidthF(refLineWidthToPen(0.4f));
-//// }
-
-// //pen.setColor(color);
-// if (pen)
+// // Select color based on if this is the current waypoint
+// if (list.at(i)->getCurrent())
// {
-// pen->setWidthF(2);
-// painter.setPen(*pen);
+// color = QGC::colorCyan;//uas->getColor();
+// pen.setWidthF(refLineWidthToPen(0.8f));
// }
// else
// {
-// QPen pen2(Qt::red);
-// pen2.setWidth(2);
-// painter.setPen(pen2);
+// color = uas->getColor();
+// pen.setWidthF(refLineWidthToPen(0.4f));
// }
-// painter.setBrush(Qt::NoBrush);
-// float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
-// painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
-// painter.drawPolygon(poly);
+ //pen.setColor(color);
+ if (pen)
+ {
+ pen->setWidthF(2);
+ painter.setPen(*pen);
+ }
+ else
+ {
+ QPen pen2(Qt::red);
+ pen2.setWidth(2);
+ painter.setPen(pen2);
+ }
+ painter.setBrush(Qt::NoBrush);
+
+ float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f));
+ painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad);
+ painter.drawPolygon(poly);
}
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index 27dc8acf94cfbfe6a0939e6b6f3d5f1b24e9d0b5..0bfb64ca4beb80e5c669155058c012a79d744543 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -308,7 +308,7 @@ Pixhawk3DWidget::insertWaypoint(void)
if (wp)
{
wp->setFrame(frame);
- uas->getWaypointManager().addWaypoint(wp);
+ uas->getWaypointManager()->addWaypoint(wp);
}
}
}
@@ -325,7 +325,7 @@ Pixhawk3DWidget::setWaypoint(void)
if (uas)
{
const QVector waypoints =
- uas->getWaypointManager().getWaypointList();
+ uas->getWaypointManager()->getWaypointList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
if (frame == MAV_FRAME_GLOBAL)
@@ -366,7 +366,7 @@ Pixhawk3DWidget::deleteWaypoint(void)
{
if (uas)
{
- uas->getWaypointManager().removeWaypoint(selectedWpIndex);
+ uas->getWaypointManager()->removeWaypoint(selectedWpIndex);
}
}
@@ -377,7 +377,7 @@ Pixhawk3DWidget::setWaypointAltitude(void)
{
bool ok;
const QVector waypoints =
- uas->getWaypointManager().getWaypointList();
+ uas->getWaypointManager()->getWaypointList();
Waypoint* waypoint = waypoints.at(selectedWpIndex);
double altitude = waypoint->getZ();
@@ -409,10 +409,10 @@ Pixhawk3DWidget::clearAllWaypoints(void)
if (uas)
{
const QVector waypoints =
- uas->getWaypointManager().getWaypointList();
+ uas->getWaypointManager()->getWaypointList();
for (int i = waypoints.size() - 1; i >= 0; --i)
{
- uas->getWaypointManager().removeWaypoint(i);
+ uas->getWaypointManager()->removeWaypoint(i);
}
}
}
diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc
index 26ba1612905ad043d683df3e87105158ac21bb92..deb08f643c229d1d8312c649a10c78effa37b2c0 100644
--- a/src/ui/map3D/WaypointGroupNode.cc
+++ b/src/ui/map3D/WaypointGroupNode.cc
@@ -78,7 +78,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
removeChild(0, getNumChildren());
}
- const QVector& list = uas->getWaypointManager().getWaypointList();
+ const QVector& list = uas->getWaypointManager()->getWaypointList();
for (int i = 0; i < list.size(); i++)
{
diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc
index c387583c0b6ae6f7d2112d0037d6e05f04dfbbc6..3a3f0d7d8ce5daf7aeff5cfabc92ab4061ab0f32 100644
--- a/src/ui/uas/UASView.cc
+++ b/src/ui/uas/UASView.cc
@@ -83,7 +83,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double)));
connect(uas, SIGNAL(heartbeatTimeout()), this, SLOT(heartbeatTimeout()));
connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int)));
- connect(&(uas->getWaypointManager()), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16)));
+ connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16)));
connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool)));