Skip to content

Q
qgroundcontrol
Commit 69aed4a6 authored by pixhawk's avatar pixhawk
Browse files
Options

Fixed 2D map regarding waypoints (almost done), added option to replay image logs in HUD view

parent 5795eb6b
Expand all Show whitespace changes
Inline Side-by-side
Showing with 555 additions and 261 deletions
images/earth.html
View file @ 69aed4a6
......@@ -192,20 +192,20 @@ 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)
......@@ -224,8 +224,6 @@ function initCallback(object)
ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, 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)
......
src/uas/PxQuadMAV.cc
View file @ 69aed4a6
......@@ -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;
......
src/uas/UAS.cc
View file @ 69aed4a6
......@@ -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);
......
src/uas/UAS.h
View file @ 69aed4a6
......@@ -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 */
......
src/uas/UASInterface.h
View file @ 69aed4a6
......@@ -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 */
......
src/uas/UASWaypointManager.cc
View file @ 69aed4a6
......@@ -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,7 @@ void UASWaypointManager::addWaypoint(Waypoint *wp)
waypoints.insert(waypoints.size(), wp);
emit waypointListChanged();
emit waypointListChanged(uas.getUASID());
}
}
......@@ -329,6 +337,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
waypoints[i]->setId(i);
}
emit waypointListChanged();
emit waypointListChanged(uas.getUASID());
return 0;
}
return -1;
......@@ -359,6 +368,7 @@ void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
//waypoints[new_seq]->setId(new_seq);
emit waypointListChanged();
emit waypointListChanged(uas.getUASID());
}
}
......@@ -425,6 +435,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
emit loadWPFile();
emit waypointListChanged();
emit waypointListChanged(uas.getUASID());
}
......
src/uas/UASWaypointManager.h
View file @ 69aed4a6
......@@ -86,11 +86,11 @@ public:
/** @name Waypoint list operations */
/*@{*/
const QVector<Waypoint *> &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
......@@ -99,6 +99,8 @@ public:
/*@}*/
UAS& getUAS() { return this->uas; } ///< Returns the owning UAS
/** @name Global waypoint list operations */
/*@{*/
const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; } ///< Returns a const reference to the global waypoint list.
......@@ -120,9 +122,12 @@ 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
......
src/ui/HUD.cc
View file @ 69aed4a6
......@@ -30,6 +30,10 @@ This file is part of the QGROUNDCONTROL project
*/
#include <QShowEvent>
#include <QContextMenuEvent>
#include <QMenu>
#include <QDesktopServices>
#include <QFileDialog>
#include <QDebug>
#include <cmath>
......@@ -39,6 +43,7 @@ This file is part of the QGROUNDCONTROL project
#include <limits>
#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,36 @@ void HUD::hideEvent(QHideEvent* event)
refreshTimer->stop();
}
void HUD::contextMenuEvent (QContextMenuEvent* event)
{
QMenu menu(this);
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,8 +275,17 @@ 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)));
// Try to disconnect the image link
UAS* u = dynamic_cast<UAS*>(uas);
if (u)
{
disconnect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
}
}
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)));
......@@ -244,8 +298,16 @@ void HUD::setActiveUAS(UASInterface* uas)
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*>(uas);
if (u)
{
connect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
}
// Set new UAS
this->uas = uas;
}
}
//void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec)
......@@ -609,40 +671,40 @@ 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);
......@@ -775,8 +837,17 @@ void HUD::paintHUD()
paintPitchLines(pitchLP, &painter);
painter.end();
}
else
{
QPainter painter;
painter.begin(this);
painter.end();
}
//glDisable(GL_MULTISAMPLE);
//glFlush();
}
}
......@@ -1517,6 +1588,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);
......
src/ui/HUD.h
View file @ 69aed4a6
......@@ -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);
};
......
src/ui/MainWindow.cc
View file @ 69aed4a6
......@@ -408,17 +408,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)
......@@ -853,13 +853,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
......
src/ui/MapWidget.cc
View file @ 69aed4a6
This diff is collapsed.
src/ui/MapWidget.h
View file @ 69aed4a6
......@@ -44,6 +44,7 @@ This file is part of the QGROUNDCONTROL project
class QMenu;
class Waypoint;
namespace Ui {
class MapWidget;
......@@ -72,10 +73,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();
......@@ -124,6 +133,9 @@ protected:
QMap<int, qmapcontrol::Point*> uasIcons;
QMap<int, qmapcontrol::LineString*> uasTrails;
QMap<int, QPen*> mavPens;
//QMap<int, QList<qmapcontrol::Point*> > mavWps;
//QMap<int, qmapcontrol::LineString*> waypointPaths;
UASInterface* mav;
quint64 lastUpdate;
......@@ -135,13 +147,14 @@ protected:
void createPathButtonClicked(bool checked);
void createWaypointGraphAtMap (const QPointF coordinate);
void createWaypointGraphAtMap(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);
......
src/ui/QGCMAVLinkLogPlayer.cc
View file @ 69aed4a6
......@@ -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();
}
......
src/ui/WaypointView.ui
View file @ 69aed4a6
......@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1389</width>
<height>33</height>
<height>39</height>
</rect>
</property>
<property name="sizePolicy">
......@@ -375,7 +375,7 @@ QProgressBar::chunk#thrustBar {
<string>m</string>
</property>
<property name="decimals">
<number>7</number>
<number>2</number>
</property>
<property name="minimum">
<double>-100000.000000000000000</double>
......
src/ui/linechart/LinechartWidget.cc
View file @ 69aed4a6
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment