/*===================================================================== QGroundControl Open Source Ground Control Station (c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org> This file is part of the QGROUNDCONTROL project QGROUNDCONTROL is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. QGROUNDCONTROL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>. ======================================================================*/ /** * @file * @brief Implementation of main window * * @author Dominik Honegger * */ #include "QGCVideoMainWindow.h" #include "ui_QGCVideoMainWindow.h" #include "UDPLink.h" #include <QDebug> QByteArray imageRecBuffer1 = QByteArray(376*240,255); QByteArray imageRecBuffer2 = QByteArray(376*240,255); QByteArray imageRecBuffer3 = QByteArray(376*240,255); QByteArray imageRecBuffer4 = QByteArray(376*240,255); static int part = 0; unsigned char last_id = 0; QGCVideoMainWindow::QGCVideoMainWindow(QWidget *parent) : QMainWindow(parent), link(QHostAddress::Any, 5555), ui(new Ui::QGCVideoMainWindow) { ui->setupUi(this); // Set widgets in video mode ui->video1Widget->enableVideo(true); ui->video2Widget->enableVideo(true); ui->video3Widget->enableVideo(true); ui->video4Widget->enableVideo(true); // Connect link to this widget, receive all bytes connect(&link, SIGNAL(bytesReceived(LinkInterface*,QByteArray)), this, SLOT(receiveBytes(LinkInterface*,QByteArray))); // 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() { delete ui; } void QGCVideoMainWindow::receiveBytes(LinkInterface* link, QByteArray data) { // There is no need to differentiate between links // for this use case here Q_UNUSED(link); // Image data is stored in QByteArray // Output bytes and load Lenna! QString bytes; QString index; QString imageid; QString ascii; // TODO FIXME Fabian // RAW hardcoded to 22x22 int imgWidth = 376; int imgHeight = 240; int imgColors = 255; //const int headerSize = 15; // Construct PGM header QString header("P5\n%1 %2\n%3\n"); header = header.arg(imgWidth).arg(imgHeight).arg(imgColors); unsigned char i0 = data[0]; unsigned char id = data[1]; index.append(QString().sprintf("%02x", i0)); imageid.append(QString().sprintf("%02x", id)); qDebug() << "Received" << data.size() << "bytes"<< " part: " <<index<< " imageid: " <<imageid; switch (i0) { case 0x01: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i] = data[i*4+4]; imageRecBuffer2[i] = data[i*4+5]+127; imageRecBuffer3[i] = data[i*4+6]+127; imageRecBuffer4[i] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 1; break; } case 0x02: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i+45120/4] = data[i*4+4]; imageRecBuffer2[i+45120/4] = data[i*4+5]+127; imageRecBuffer3[i+45120/4] = data[i*4+6]+127; imageRecBuffer4[i+45120/4] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 2; break; } case 0x03: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i+45120/4*2] = data[i*4+4]; imageRecBuffer2[i+45120/4*2] = data[i*4+5]+127; imageRecBuffer3[i+45120/4*2] = data[i*4+6]+127; imageRecBuffer4[i+45120/4*2] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 4; break; } case 0x04: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i+45120/4*3] = data[i*4+4]; imageRecBuffer2[i+45120/4*3] = data[i*4+5]+127; imageRecBuffer3[i+45120/4*3] = data[i*4+6]+127; imageRecBuffer4[i+45120/4*3] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 8; break; } case 0x05: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i+45120/4*4] = data[i*4+4]; imageRecBuffer2[i+45120/4*4] = data[i*4+5]+127; imageRecBuffer3[i+45120/4*4] = data[i*4+6]+127; imageRecBuffer4[i+45120/4*4] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 16; break; } case 0x06: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i+45120/4*5] = data[i*4+4]; imageRecBuffer2[i+45120/4*5] = data[i*4+5]+127; imageRecBuffer3[i+45120/4*5] = data[i*4+6]+127; imageRecBuffer4[i+45120/4*5] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 32; break; } case 0x07: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i+45120/4*6] = data[i*4+4]; imageRecBuffer2[i+45120/4*6] = data[i*4+5]+127; imageRecBuffer3[i+45120/4*6] = data[i*4+6]+127; imageRecBuffer4[i+45120/4*6] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 64; break; } case 0x08: { for (int i=0; i<data.size()/4-1; i++) { imageRecBuffer1[i+45120/4*7] = data[i*4+4]; imageRecBuffer2[i+45120/4*7] = data[i*4+5]+127; imageRecBuffer3[i+45120/4*7] = data[i*4+6]+127; imageRecBuffer4[i+45120/4*7] = data[i*4+7]; } if(id != last_id) { part = 0; } part = part | 128; break; } } last_id = id; if(part==255) { QByteArray tmpImage1(header.toStdString().c_str(), header.toStdString().size()); tmpImage1.append(imageRecBuffer1); QByteArray tmpImage2(header.toStdString().c_str(), header.toStdString().size()); tmpImage2.append(imageRecBuffer2); QByteArray tmpImage3(header.toStdString().c_str(), header.toStdString().size()); tmpImage3.append(imageRecBuffer3); QByteArray tmpImage4(header.toStdString().c_str(), header.toStdString().size()); tmpImage4.append(imageRecBuffer4); // Load image into window //QImage test(":images/patterns/lenna.jpg"); QImage image1; QImage image2; QImage image3; QImage image4; if (imageRecBuffer1.isNull()) { qDebug()<< "could not convertToPGM()"; } if (!image1.loadFromData(tmpImage1, "PGM")) { qDebug()<< "could not create extracted image1"; } if (imageRecBuffer2.isNull()) { qDebug()<< "could not convertToPGM()"; } if (!image2.loadFromData(tmpImage2, "PGM")) { qDebug()<< "could not create extracted image2"; } if (imageRecBuffer3.isNull()) { qDebug()<< "could not convertToPGM()"; } if (!image3.loadFromData(tmpImage3, "PGM")) { qDebug()<< "could not create extracted image3"; } if (imageRecBuffer4.isNull()) { qDebug()<< "could not convertToPGM()"; } if (!image4.loadFromData(tmpImage4, "PGM")) { qDebug()<< "could not create extracted image3"; } tmpImage1.clear(); tmpImage2.clear(); tmpImage3.clear(); tmpImage4.clear(); //ui->video1Widget->copyImage(test); ui->video1Widget->copyImage(image1); ui->video2Widget->copyImage(image2); ui->video3Widget->copyImage(image3); ui->video4Widget->copyImage(image4); part = 0; imageRecBuffer1.clear(); 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); } /* for (int j=0; j<data.size(); j++) { unsigned char v = data[j]; bytes.append(QString().sprintf("%02x ", v)); if (data.at(j) > 31 && data.at(j) < 127) { ascii.append(data.at(j)); } else { ascii.append(219); } }*/ //qDebug() << bytes; //qDebug() << "ASCII:" << ascii; }