CameraView.cc 8.37 KB
Newer Older
pixhawk's avatar
pixhawk committed
1
/*=====================================================================
lm's avatar
lm committed
2 3 4 5 6 7 8 9

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 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
pixhawk's avatar
pixhawk committed
10 11 12
    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.
lm's avatar
lm committed
13 14

    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15 16 17
    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.
lm's avatar
lm committed
18

pixhawk's avatar
pixhawk committed
19
    You should have received a copy of the GNU General Public License
lm's avatar
lm committed
20 21
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

pixhawk's avatar
pixhawk committed
22
======================================================================*/
lm's avatar
lm committed
23

pixhawk's avatar
pixhawk committed
24 25
/**
 * @file
lm's avatar
lm committed
26
 *   @brief Implementation of CameraView
pixhawk's avatar
pixhawk committed
27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include "CameraView.h"
#include <QDebug>

CameraView::CameraView(int width, int height, int depth, int channels, QWidget* parent) : QGLWidget(parent)
{
    rawImage = NULL;
    rawBuffer1 = NULL;
    rawBuffer2 = NULL;
    rawLastIndex = 0;
    image = NULL;
    imageStarted = false;
    // Init to black image
    //setImageSize(width, height, depth, channels);
    receivedWidth = width;
    receivedHeight = height;
pixhawk's avatar
pixhawk committed
46 47 48
    receivedDepth = depth;
    receivedChannels = channels;
    imageId = -1;
pixhawk's avatar
pixhawk committed
49 50 51

    // Fill with black background
    QImage fill = QImage(width, height, QImage::Format_Indexed8);
52
    fill.setColorCount(1);
pixhawk's avatar
pixhawk committed
53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82
    fill.setColor(0, qRgb(70, 200, 70));
    fill.fill(CameraView::initialColor);
    glImage = QGLWidget::convertToGLFormat(fill);

    resize(fill.size());

    // Set size once
    setFixedSize(fill.size());
    setMinimumSize(fill.size());
    setMaximumSize(fill.size());
    // Lock down the size
    setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
}

CameraView::~CameraView()
{
    delete rawImage;
    delete image;
}

void CameraView::addUAS(UASInterface* uas)
{
    // TODO Enable multi-uas support
    connect(uas, SIGNAL(imageStarted(int,int,int,int,int)), this, SLOT(startImage(int,int,int,int,int)));
    connect(uas, SIGNAL(imageDataReceived(int,const unsigned char*,int,int)), this, SLOT(setPixels(int,const unsigned char*,int,int)));
}

void CameraView::setImageSize(int width, int height, int depth, int channels)
{
    // Allocate raw image in correct size
83
    if (width != receivedWidth || height != receivedHeight || depth != receivedDepth || channels != receivedChannels || image == NULL) {
pixhawk's avatar
pixhawk committed
84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103
        // Set new size
        if (width > 0) receivedWidth  = width;
        if (height > 0) receivedHeight = height;
        if (depth > 1) receivedDepth = depth;
        if (channels > 1) receivedChannels = channels;

        rawExpectedBytes = (receivedWidth * receivedHeight * receivedDepth * receivedChannels) / 8;
        bytesPerLine = rawExpectedBytes / receivedHeight;
        // Delete old buffers if necessary
        rawImage = NULL;
        if (rawBuffer1 != NULL) delete rawBuffer1;
        if (rawBuffer2 != NULL) delete rawBuffer2;

        rawBuffer1 = (unsigned char*)malloc(rawExpectedBytes);
        rawBuffer2 = (unsigned char*)malloc(rawExpectedBytes);
        rawImage = rawBuffer1;
        // TODO check if old image should be deleted

        // Set image format
        // 8 BIT GREYSCALE IMAGE
104
        if (depth <= 8 && channels == 1) {
pixhawk's avatar
pixhawk committed
105 106
            image = new QImage(receivedWidth, receivedHeight, QImage::Format_Indexed8);
            // Create matching color table
107
            image->setColorCount(256);
108
            for (int i = 0; i < 256; i++) {
pixhawk's avatar
pixhawk committed
109 110 111 112 113 114
                image->setColor(i, qRgb(i, i, i));
                //qDebug() << __FILE__ << __LINE__ << std::hex << i;
            }

        }
        // 32 BIT COLOR IMAGE WITH ALPHA VALUES (#ARGB)
115
        else {
pixhawk's avatar
pixhawk committed
116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137
            image = new QImage(receivedWidth, receivedHeight, QImage::Format_ARGB32);
        }

        // Fill image with black pixels
        image->fill(CameraView::initialColor);
        glImage = QGLWidget::convertToGLFormat(*image);

        qDebug() << __FILE__ << __LINE__ << "Setting up image";

        // Set size once
        setFixedSize(receivedWidth, receivedHeight);
        setMinimumSize(receivedWidth, receivedHeight);
        setMaximumSize(receivedWidth, receivedHeight);
        // Lock down the size
        setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
        resize(receivedWidth, receivedHeight);
    }

}

void CameraView::startImage(int imgid, int width, int height, int depth, int channels)
{
pixhawk's avatar
pixhawk committed
138
    this->imageId = imgid;
pixhawk's avatar
pixhawk committed
139 140 141 142 143 144 145 146 147 148 149 150
    //qDebug() << "CameraView: starting image (" << width << "x" << height << ", " << depth << "bits) with " << channels << "channels";

    // Copy previous image to screen if it hasn't been finished properly
    finishImage();

    // Reset image size if necessary
    setImageSize(width, height, depth, channels);
    imageStarted = true;
}

void CameraView::finishImage()
{
151
    if (imageStarted) {
pixhawk's avatar
pixhawk committed
152 153 154 155 156 157 158 159
        commitRawDataToGL();
        imageStarted = false;
    }
}

void CameraView::commitRawDataToGL()
{
    //qDebug() << __FILE__ << __LINE__ << "Copying raw data to GL buffer:" << rawImage << receivedWidth << receivedHeight << image->format();
160
    if (image != NULL) {
pixhawk's avatar
pixhawk committed
161 162
        QImage::Format format = image->format();
        QImage* newImage = new QImage(rawImage, receivedWidth, receivedHeight, format);
163
        if (format == QImage::Format_Indexed8) {
pixhawk's avatar
pixhawk committed
164
            // Create matching color table
165
            newImage->setColorCount(256);
166
            for (int i = 0; i < 256; i++) {
pixhawk's avatar
pixhawk committed
167 168 169 170 171 172 173 174 175
                newImage->setColor(i, qRgb(i, i, i));
                //qDebug() << __FILE__ << __LINE__ << std::hex << i;
            }
        }

        glImage = QGLWidget::convertToGLFormat(*newImage);
        delete image;
        image = newImage;
        // Switch buffers
176
        if (rawImage == rawBuffer1) {
pixhawk's avatar
pixhawk committed
177 178
            rawImage = rawBuffer2;
            //qDebug() << "Now buffer 2";
179
        } else {
pixhawk's avatar
pixhawk committed
180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200
            rawImage = rawBuffer1;
            //qDebug() << "Now buffer 1";
        }
    }
    paintGL();
}

void CameraView::saveImage(QString fileName)
{
    image->save(fileName);
}

void CameraView::saveImage()
{
    //Bring up popup
    QString fileName = "output.png";
    saveImage(fileName);
}

void CameraView::setPixels(int imgid, const unsigned char* imageData, int length, unsigned int startIndex)
{
pixhawk's avatar
pixhawk committed
201 202 203 204
    // FIXME imgid can be used to receive and then display multiple images
    // the image buffer should be converted into a n image buffer.
    Q_UNUSED(imgid);

pixhawk's avatar
pixhawk committed
205 206
    //    qDebug() << "at" << __FILE__ << __LINE__ << ": Received startindex" << startIndex << "and length" << length << "(" << startIndex+length << "of" << rawExpectedBytes << "bytes)";

207
    if (imageStarted) {
pixhawk's avatar
pixhawk committed
208 209
        //if (rawLastIndex != startIndex) qDebug() << "PACKET LOSS!";

210
        if (startIndex+length > rawExpectedBytes) {
pixhawk's avatar
pixhawk committed
211
            qDebug() << "CAMERAVIEW: OVERFLOW! startIndex:" << startIndex << "length:" << length << "image raw size" << ((receivedWidth * receivedHeight * receivedChannels * receivedDepth) / 8) - 1;
212
        } else {
pixhawk's avatar
pixhawk committed
213 214 215 216 217
            memcpy(rawImage+startIndex, imageData, length);

            rawLastIndex = startIndex+length;

            // Check if we just reached the end of the image
218
            if (startIndex+length == rawExpectedBytes) {
pixhawk's avatar
pixhawk committed
219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249
                //qDebug() << "CAMERAVIEW: END OF IMAGE REACHED!";
                finishImage();
                rawLastIndex = 0;
            }
        }

        //        for (int i = 0; i < length; i++)
        //        {
        //            for (int j = 0; j < receivedChannels; j++)
        //            {
        //                unsigned int x = (startIndex+i) % receivedWidth;
        //                unsigned int y = static_cast<unsigned int>((startIndex+i) / receivedWidth);
        //                qDebug() << "Setting pixel" << x << "," << y << "to" << (unsigned int)*(rawImage+startIndex+i);
        //            }
        //        }
    }
}

void CameraView::paintGL()
{
    glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());
}

void CameraView::resizeGL(int w, int h)
{
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, w, 0, h, -1, 1);
    glMatrixMode(GL_MODELVIEW);
}