first import

[mardrone] / mardrone / video.cpp

diff --git a/mardrone/video.cpp b/mardrone/video.cpp
new file mode 100644 (file)
index 0000000..43a12ac
--- /dev/null
+++ b/mardrone/video.cpp
@@ -0,0 +1,154 @@
+#include "video.h"
+#include <QGraphicsView>
+
+DroneVideo::DroneVideo()
+{
+     droneHost.setAddress("192.168.1.1");
+     initialized=false;
+}
+
+VideoThread::VideoThread(DroneVideo *parentp,QHostAddress host,QImage *_image)
+{
+    image=_image;
+    qDebug() << "videoThread::videoThread";
+    stopped=false;
+    parent=parentp;
+    videoSock=new QUdpSocket();
+    videoSock->bind(QHostAddress::Any,5555);
+    droneHost=host;
+    start();
+
+};
+
+void DroneVideo::paint(QPainter *painter,const QStyleOptionGraphicsItem *option,
+                        QWidget *widget)
+ {
+    if(!initialized) { // We need initialize QImage here because we don't know display depth before
+        int depth=0;
+
+        depth=painter->device()->depth();
+        qDebug() << "depth=" << depth;
+
+        if(depth==24) {
+            image=new QImage(320,240, QImage::Format_RGB32);
+             image->fill(0x555555);
+        }
+        else {
+            image=new QImage(320,240, QImage::Format_RGB16);
+            image->fill(0x5555);
+        }
+        QPainter p(image);
+        p.drawLine(0,0,image->width(),image->height());
+        p.drawLine(image->width(),0,0,image->height());
+        update(boundingRect());
+        videoThread=new VideoThread(this,droneHost,image);
+        initialized=true;
+    } else
+    painter->drawImage(boundingRect(),*image,image->rect());
+ };
+
+QRectF DroneVideo::boundingRect() const
+{
+    return QRectF(0.0,0.0,size().width(),size().height());
+}
+
+
+void VideoThread::run()
+{
+#define ACQ_WIDTH     320
+#define ACQ_HEIGHT    240
+#undef memset
+    memset(&controller,0,sizeof(controller));
+    memset(&picture,0,sizeof(picture));
+    pictureWidth= image->width();
+    pictureHeight=image->height();
+    int codec_type=UVLC_CODEC;
+    qDebug() << "videoThread::run()";
+    stateTimer=new QTimer();
+    connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
+    connect(videoSock,SIGNAL(readyRead()),this,SLOT(videoDataReady()));
+    qDebug() << "videoThread::run() 2";
+    luma_only=FALSE;
+    num_picture_decoded=0;
+    /// Picture configuration
+    picture.format        = PIX_FMT_YUV420P;
+    picture.width         = pictureWidth;
+    picture.height        = pictureHeight;
+    picture.framerate     = 30;
+    picture.y_buf         = (uint8_t*)(void*)vp_os_malloc((size_t) pictureWidth*pictureHeight );
+    picture.cr_buf        = (uint8_t*)vp_os_malloc( pictureWidth*pictureHeight/4 );
+    picture.cb_buf        = (uint8_t*)vp_os_malloc( pictureWidth*pictureHeight/4 );
+    picture.y_line_size   = pictureWidth;
+    picture.cb_line_size  = pictureWidth / 2;
+    picture.cr_line_size  = pictureWidth / 2;
+    picture.y_pad         = 0;
+    picture.c_pad         = 0;
+    qDebug() << "videoThread::run() 3";
+    video_codec_open(&controller, (codec_type_t)UVLC_CODEC);
+    //stateTimer->start(1000);
+    qDebug() << "videoThread::run() initialized";
+    sendVideoPort("AT");
+    while(!stopped) {
+        exec();
+    }
+
+}
+
+void VideoThread::timer()
+{
+  //  qDebug() << "thread Timer";
+
+}
+
+void VideoThread::sendVideoPort(QString cmd)
+{
+    QByteArray dgram;
+    dgram=cmd.toLatin1();
+    qDebug() << "videoThread::sendCmd= " << cmd+"\n" << "to " << droneHost ;
+    videoSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5555);
+}
+
+void VideoThread::videoDataReady()
+{
+   qint64 l;
+   QByteArray videoData;
+
+   QHostAddress host;
+   quint16 port;
+   videoData.resize(videoSock->pendingDatagramSize ());
+   l=videoSock->readDatagram(videoData.data(),videoData.size(),&host,&port);
+   qDebug() << "videoThread::videoDataReady" <<" l=" << l << "from"  << host ;
+   decodeTransform(videoData);
+}
+
+void VideoThread::decodeTransform(QByteArray &videoData)
+{
+    controller.in_stream.bytes   = (uint32_t*)videoData.data();
+    controller.in_stream.used    = videoData.size();
+    controller.in_stream.size    = videoData.size();
+    controller.in_stream.index   = 0;
+    controller.in_stream.length  = 32;
+    controller.in_stream.code    = 0;
+
+    bool_t got_image = FALSE;
+    //qDebug() <<"VideoThread::decodeTransform";
+    video_decode_blockline( &controller, &picture, &got_image );
+    //qDebug() <<"VideoThread::decodeTransform 2";
+    //video_decode_picture( &controller, &picture, &stream, &got_image );
+    if( got_image )
+        {
+            qDebug() <<"VideoThread::decodeTransform got image" << picture.width << picture.height << image->byteCount() << image->bytesPerLine();
+          // we got one picture
+          // out->size = 1;
+          picture.complete     = 1;
+          num_picture_decoded++;
+          vp_stages_YUV420P_to_RGB565(NULL,&picture,image->bits(),image->bytesPerLine());
+
+
+          qDebug() << "pic " << num_picture_decoded;
+        }
+
+
+};
+
+