diff --git a/qgcvideo.pro b/qgcvideo.pro index 01c66cc4cc5d832905d8d13ae121c9299f477bbe..39246caa0bd42da47c47bd875e7e4d652fdf27fd 100644 --- a/qgcvideo.pro +++ b/qgcvideo.pro @@ -45,4 +45,4 @@ SOURCES += \ FORMS += \ src/apps/qgcvideo/QGCVideoMainWindow.ui -RESOURCES = mavground.qrc +RESOURCES = qgroundcontrol.qrc diff --git a/src/apps/qgcvideo/QGCVideoMainWindow.cc b/src/apps/qgcvideo/QGCVideoMainWindow.cc index 0fe36fb9b7ffc59215f4d2e305e9071944295638..d5458a16b88c75b15d16af18d2d39f159b6f991f 100644 --- a/src/apps/qgcvideo/QGCVideoMainWindow.cc +++ b/src/apps/qgcvideo/QGCVideoMainWindow.cc @@ -60,6 +60,18 @@ QGCVideoMainWindow::QGCVideoMainWindow(QWidget *parent) : // Open port link.connect(); + + // Show flow // FIXME + int xCount = 16; + int yCount = 5; + + unsigned char flowX[xCount][yCount]; + unsigned char flowY[xCount][yCount]; + + flowX[3][3] = 10; + flowY[3][3] = 5; + + ui->video4Widget->copyFlow((const unsigned char*)flowX, (const unsigned char*)flowY, xCount, yCount); } QGCVideoMainWindow::~QGCVideoMainWindow() @@ -316,6 +328,16 @@ void QGCVideoMainWindow::receiveBytes(LinkInterface* link, QByteArray data) imageRecBuffer2.clear(); imageRecBuffer3.clear(); imageRecBuffer4.clear(); + + ui->video4Widget->enableFlow(true); + + int xCount = 16; + int yCount = 5; + + unsigned char flowX[xCount][yCount]; + unsigned char flowY[xCount][yCount]; + + ui->video4Widget->copyFlow((const unsigned char*)flowX, (const unsigned char*)flowY, xCount, yCount); } diff --git a/src/apps/qgcvideo/QGCVideoWidget.cc b/src/apps/qgcvideo/QGCVideoWidget.cc index e6905df8a54dcb398f9f0c22ba6cf7aa43589976..8fc9b85ad832674506bfd79de3c41c3c8918beb9 100644 --- a/src/apps/qgcvideo/QGCVideoWidget.cc +++ b/src/apps/qgcvideo/QGCVideoWidget.cc @@ -131,9 +131,14 @@ QGCVideoWidget::QGCVideoWidget(QWidget* parent) nextOfflineImage(""), hudInstrumentsEnabled(false), videoEnabled(false), + flowEnabled(false), xImageFactor(1.0), yImageFactor(1.0), - imageRequested(false) + imageRequested(false), + flowFieldX(NULL), + flowFieldY(NULL), + flowWidth(0), + flowHeight(0) { // Set auto fill to false setAutoFillBackground(false); @@ -219,6 +224,7 @@ void QGCVideoWidget::contextMenuEvent (QContextMenuEvent* event) // enableVideoAction->setChecked(videoEnabled); menu.addAction(enableHUDAction); + menu.addAction(enableFlowFieldAction); //menu.addAction(selectQGCVideoWidgetColorAction); // menu.addAction(enableVideoAction); // menu.addAction(selectOfflineDirectoryAction); @@ -238,6 +244,12 @@ void QGCVideoWidget::createActions() enableVideoAction->setStatusTip(tr("Show the video live feed")); enableVideoAction->setCheckable(true); enableVideoAction->setChecked(videoEnabled); + + enableFlowFieldAction = new QAction(tr("Enable Optical Flow Field"), this); + enableFlowFieldAction->setCheckable(true); + enableFlowFieldAction->setChecked(flowEnabled); + connect(enableFlowFieldAction, SIGNAL(triggered(bool)), this, SLOT(enableFlow(bool))); + // connect(enableVideoAction, SIGNAL(triggered(bool)), this, SLOT(enableVideo(bool))); selectOfflineDirectoryAction = new QAction(tr("Select image log"), this); @@ -430,6 +442,39 @@ void QGCVideoWidget::paintEvent(QPaintEvent *event) Q_UNUSED(event); } +void QGCVideoWidget::copyFlow(const unsigned char* flowX, const unsigned char* flowY, int width, int height) +{ + flowWidth = width; + flowHeight = height; + + delete flowFieldX; + delete flowFieldY; + + flowFieldX = (unsigned char**) malloc(width*height); + flowFieldY = (unsigned char**) malloc(width*height); + + memcpy(flowFieldX, flowX, width*height); + memcpy(flowFieldY, flowY, width*height); +} + +void QGCVideoWidget::paintFlowField(QPainter* painter) +{ + if (width() > 0 && height() > 0) + { + unsigned int sX = (flowWidth+1)/width(); + unsigned int sY = (flowHeight+1)/height(); + for (unsigned int i = 0; i < flowWidth; ++i) + { + for (unsigned int j = 0; j < flowHeight; ++j) + { + // Paint vector + qDebug() << "X" << i << flowFieldX[i][j] << "Y" << j << flowFieldY[i][j]; + //painter->drawLine(QPointF(sX*i, sY*j), QPointF(sX*i+(flowFieldX[i][j]), sY*j+(flowFieldY[i][j]))); + } + } + } +} + void QGCVideoWidget::paintHUD() { if (isVisible()) { @@ -521,6 +566,15 @@ void QGCVideoWidget::paintHUD() // END OF OPENGL PAINTING + if (flowEnabled) { + QPainter painter; + painter.begin(this); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::HighQualityAntialiasing, true); + painter.setPen(Qt::red); + paintFlowField(&painter); + } + if (hudInstrumentsEnabled) { //glEnable(GL_MULTISAMPLE); diff --git a/src/apps/qgcvideo/QGCVideoWidget.h b/src/apps/qgcvideo/QGCVideoWidget.h index ec27bb540d3114ed6ddbd4ac81d40f2da914b0ab..93a44bf69427d666789190ef881ea19b4d5bde57 100644 --- a/src/apps/qgcvideo/QGCVideoWidget.h +++ b/src/apps/qgcvideo/QGCVideoWidget.h @@ -6,6 +6,7 @@ #include #include #include +#include /** @@ -31,6 +32,9 @@ public slots: void copyImage(const QImage& img); void enableHUDInstruments(bool enabled) { hudInstrumentsEnabled = enabled; } void enableVideo(bool enabled) { videoEnabled = enabled; } + void enableFlow(bool enabled) { flowEnabled = enabled; } + /** @brief Copy flow */ + void copyFlow(const unsigned char* flowX, const unsigned char* flowY, int width, int height); protected slots: @@ -42,6 +46,8 @@ protected slots: /** @brief Setup the OpenGL view for drawing a sub-component of the HUD */ void setupGLView(float referencePositionX, float referencePositionY, float referenceWidth, float referenceHeight); void paintHUD(); + /** @brief Paint an optical flow field */ + void paintFlowField(QPainter* painter); void paintPitchLinePos(QString text, float refPosX, float refPosY, QPainter* painter); void paintPitchLineNeg(QString text, float refPosX, float refPosY, QPainter* painter); @@ -151,14 +157,22 @@ protected: QString nextOfflineImage; bool hudInstrumentsEnabled; bool videoEnabled; + bool flowEnabled; float xImageFactor; float yImageFactor; QAction* enableHUDAction; QAction* enableVideoAction; + QAction* enableFlowFieldAction; QAction* selectOfflineDirectoryAction; QAction* selectVideoChannelAction; void paintEvent(QPaintEvent *event); bool imageRequested; + int flowFieldWidth; + int flowFieldHeight; + unsigned char** flowFieldX; + unsigned char** flowFieldY; + unsigned int flowWidth; + unsigned int flowHeight; }; diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 72e2dd2db0162d659ca85a32eed1e187eb0039fd..a1299ab17b8bed036e919a575277f716972e68ae 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -44,7 +44,7 @@ class LinkInterface : public QThread Q_OBJECT public: LinkInterface(QObject* parent = 0) : QThread(parent) {} - virtual ~LinkInterface()=0; + virtual ~LinkInterface() {} /* Connection management */ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index b89308501ba5796f63f168fb21f0c20370a751a7..196b5f7b31c0b6e4d1e2efe9e6039c6bc9bff89b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -683,12 +683,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // SANITY CHECK // only accept values in a realistic range // quint64 time = getUnixTime(pos.usec); - quint64 time = getUnixTime(); + quint64 time = getUnixTime(pos.usec); emit valueChanged(uasId, "latitude", "deg", pos.lat/(double)1E7, time); emit valueChanged(uasId, "longitude", "deg", pos.lon/(double)1E7, time); + emit valueChanged(uasId, "eph", "m", pos.eph/(double)1E2, time); + emit valueChanged(uasId, "epv", "m", pos.eph/(double)1E2, time); - if (pos.fix_type > 0) { + if (pos.fix_type > 2) { emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); emit valueChanged(uasId, "gps speed", "m/s", pos.vel, time); latitude = pos.lat/(double)1E7;