Working on video streaming

qgcvideo.pro

@@ -45,4 +45,4 @@ SOURCES += \
 FORMS += \
     src/apps/qgcvideo/QGCVideoMainWindow.ui
 
-RESOURCES = mavground.qrc
+RESOURCES = qgroundcontrol.qrc

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);
     }
 
 

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);

src/apps/qgcvideo/QGCVideoWidget.h

@@ -6,6 +6,7 @@
 #include <QPainter>
 #include <QFontDatabase>
 #include <QTimer>
+#include <QVector>
 
 
 /**
@@ -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;
 
 };
 

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 */
 

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;