HUD.cc

        drawLine(xRef, yRef+height/2.0f, xRef-width, yRef+height/2.0f, lineWidth, defaultColor, painter);
        // Horizontal bottom line
        drawLine(xRef, yRef+height, xRef-width, yRef+height, lineWidth, defaultColor, painter);

        // Text
        QString label;
        label.sprintf("%+06.2f >", value);

        QFont font("Bitstream Vera Sans");
        // Enforce minimum font size of 5 pixels
        //int fSize = qMax(5, (int)(6.0f*scalingFactor*1.26f));
        font.setPixelSize(6.0f * 1.26f);

        QFontMetrics metrics = QFontMetrics(font);
        paintText(label, defaultColor, 6.0f, (xRef-width) - metrics.width(label), yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter);
    }
    else
    {
        drawLine(xRef, yRef, xRef+width, yRef, lineWidth, defaultColor, painter);
        // Vertical main line
        drawLine(xRef+width/2.0f, yRef, xRef+width/2.0f, yRef+height, lineWidth, defaultColor, painter);
        // Zero mark
        drawLine(xRef, yRef+height/2.0f, xRef+width, yRef+height/2.0f, lineWidth, defaultColor, painter);
        // Horizontal bottom line
        drawLine(xRef, yRef+height, xRef+width, yRef+height, lineWidth, defaultColor, painter);

        // Text
        QString label;
        label.sprintf("< %+06.2f", value);
        paintText(label, defaultColor, 6.0f, xRef+width/2.0f, yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter);
    }
}

//void HUD::drawSystemIndicator(float xRef, float yRef, int maxNum, float maxWidth, float maxHeight, QPainter* painter)
//{
//    Q_UNUSED(maxWidth);
//    Q_UNUSED(maxHeight);
//    if (values.size() > 0)
//    {
//        QString selectedKey = values.begin().key();
//        //   | | | | | |
//        //   | | | | | |
//        //   x speed: 2.54

//        // One column per value
//        QMapIterator<QString, float> value(values);

//        float x = xRef;
//        float y = yRef;

//        const float vspacing = 1.0f;
//        float width = 1.5f;
//        float height = 1.5f;
//        const float hspacing = 0.6f;

//        // TODO ensure that instrument stays smaller than maxWidth and maxHeight


//        int i = 0;
//        while (value.hasNext() && i < maxNum)
//        {
//            value.next();
//            QBrush brush(Qt::SolidPattern);


//            if (value.value() < 0.01f && value.value() > -0.01f)
//            {
//                brush.setColor(Qt::gray);
//            }
//            else if (value.value() > 0.01f)
//            {
//                brush.setColor(Qt::blue);
//            }
//            else
//            {
//                brush.setColor(Qt::yellow);
//            }

//            painter->setBrush(brush);
//            painter->setPen(Qt::NoPen);

//            // Draw current value colormap
//            painter->drawRect(refToScreenX(x), refToScreenY(y), refToScreenX(width), refToScreenY(height));

//            // Draw change rate colormap
//            painter->drawRect(refToScreenX(x), refToScreenY(y+height+hspacing), refToScreenX(width), refToScreenY(height));

//            // Draw mean value colormap
//            painter->drawRect(refToScreenX(x), refToScreenY(y+2.0f*(height+hspacing)), refToScreenX(width), refToScreenY(height));

//            // Add spacing
//            x += width+vspacing;

//            // Iterate
//            i++;
//        }

//        // Draw detail label
//        QString detail = "NO DATA AVAILABLE";

//        if (values.contains(selectedKey))
//        {
//            detail = values.find(selectedKey).key();
//            detail.append(": ");
//            detail.append(QString::number(values.find(selectedKey).value()));
//        }
//        paintText(detail, QColor(255, 255, 255), 3.0f, xRef, yRef+3.0f*(height+hspacing)+1.0f, painter);
//    }
//}

void HUD::drawChangeIndicatorGauge(float xRef, float yRef, float radius, float expectedMaxChange, float value, const QColor& color, QPainter* painter, bool solid)
{
    // Draw the circle
    QPen circlePen(Qt::SolidLine);
    if (!solid) circlePen.setStyle(Qt::DotLine);
    circlePen.setColor(defaultColor);
    circlePen.setWidth(refLineWidthToPen(2.0f));
    painter->setBrush(Qt::NoBrush);
    painter->setPen(circlePen);
    drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.5f, color, painter);

    QString label;
    label.sprintf("%05.1f", value);

    float textSize = radius / 2.5;

    // Draw the value
    paintText(label, color, textSize, xRef-textSize*1.7f, yRef-textSize*0.4f, painter);

    // Draw the needle
    // Scale the rotation so that the gauge does one revolution
    // per max. change
    const float rangeScale = (2.0f * M_PI) / expectedMaxChange;
    const float maxWidth = radius / 10.0f;
    const float minWidth = maxWidth * 0.3f;

    QPolygonF p(6);

    p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f));
    p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f));
    p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f));
    p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.5f));
    p.replace(4, QPointF(xRef,               yRef-radius * 0.46f));
    p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.5f));

    rotatePolygonClockWiseRad(p, value*rangeScale, QPointF(xRef, yRef));

    QBrush indexBrush;
    indexBrush.setColor(defaultColor);
    indexBrush.setStyle(Qt::SolidPattern);
    painter->setPen(Qt::SolidLine);
    painter->setPen(defaultColor);
    painter->setBrush(indexBrush);
    drawPolygon(p, painter);
}

void HUD::drawLine(float refX1, float refY1, float refX2, float refY2, float width, const QColor& color, QPainter* painter)
{
    QPen pen(Qt::SolidLine);
    pen.setWidth(refLineWidthToPen(width));
    pen.setColor(color);
    painter->setPen(pen);
    painter->drawLine(QPoint(refToScreenX(refX1), refToScreenY(refY1)), QPoint(refToScreenX(refX2), refToScreenY(refY2)));
}

void HUD::drawEllipse(float refX, float refY, float radiusX, float radiusY, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter)
{
    Q_UNUSED(startDeg);
    Q_UNUSED(endDeg);
    QPen pen(painter->pen().style());
    pen.setWidth(refLineWidthToPen(lineWidth));
    pen.setColor(color);
    painter->setPen(pen);
    painter->drawEllipse(QPointF(refToScreenX(refX), refToScreenY(refY)), refToScreenX(radiusX), refToScreenY(radiusY));
}

void HUD::drawCircle(float refX, float refY, float radius, float startDeg, float endDeg, float lineWidth, const QColor& color, QPainter* painter)
{
    drawEllipse(refX, refY, radius, radius, startDeg, endDeg, lineWidth, color, painter);
}

void HUD::selectWaypoint(int uasId, int id)
{
    Q_UNUSED(uasId);
    waypointName = tr("WP") + QString::number(id);
}

void HUD::setImageSize(int width, int height, int depth, int channels)
{
    // Allocate raw image in correct size
    if (width != receivedWidth || height != receivedHeight || depth != receivedDepth || channels != receivedChannels || image == NULL) {
        // 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;
        if (image)
            delete image;

        // Set image format
        // 8 BIT GREYSCALE IMAGE
        if (depth <= 8 && channels == 1) {
            image = new QImage(receivedWidth, receivedHeight, QImage::Format_Indexed8);
            // Create matching color table
            image->setColorCount(256);
            for (int i = 0; i < 256; i++) {
                image->setColor(i, qRgb(i, i, i));
                //qDebug() << __FILE__ << __LINE__ << std::hex << i;
            }

        }
        // 32 BIT COLOR IMAGE WITH ALPHA VALUES (#ARGB)
        else {
            image = new QImage(receivedWidth, receivedHeight, QImage::Format_ARGB32);
        }

        // Fill first channel of image with black pixels
        if (MainWindow::instance()->getStyle() == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT)
        {
            image->fill(255);
        }
        else
        {
            image->fill(0);
        }
        glImage = *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 HUD::startImage(int imgid, int width, int height, int depth, int channels)
{
    Q_UNUSED(imgid);
    //qDebug() << "HUD: 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 HUD::finishImage()
{
    if (imageStarted) {
        commitRawDataToGL();
        imageStarted = false;
    }
}

void HUD::commitRawDataToGL()
{
    qDebug() << __FILE__ << __LINE__ << "Copying raw data to GL buffer:" << rawImage << receivedWidth << receivedHeight << image->format();
    if (image != NULL) {
        QImage::Format format = image->format();
        QImage* newImage = new QImage(rawImage, receivedWidth, receivedHeight, format);
        if (format == QImage::Format_Indexed8) {
            // Create matching color table
            newImage->setColorCount(256);
            for (int i = 0; i < 256; i++) {
                newImage->setColor(i, qRgb(i, i, i));
                //qDebug() << __FILE__ << __LINE__ << std::hex << i;
            }
        }

        glImage = *newImage;
        delete image;
        image = newImage;
        // Switch buffers
        if (rawImage == rawBuffer1) {
            rawImage = rawBuffer2;
            //qDebug() << "Now buffer 2";
        } else {
            rawImage = rawBuffer1;
            //qDebug() << "Now buffer 1";
        }
    }
    update();
}

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

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

void HUD::selectOfflineDirectory()
{
    QString fileName = QGCFileDialog::getExistingDirectory(this, tr("Select image directory"), QStandardPaths::writableLocation(QStandardPaths::DesktopLocation));
    if (fileName != "") {
        offlineDirectory = fileName;
    }
}

void HUD::enableHUDInstruments(bool enabled)
{
    HUDInstrumentsEnabled = enabled;
}

void HUD::enableVideo(bool enabled)
{
    videoEnabled = enabled;
}

void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int startIndex)
{
    Q_UNUSED(imgid);
    //    qDebug() << "at" << __FILE__ << __LINE__ << ": Received startindex" << startIndex << "and length" << length << "(" << startIndex+length << "of" << rawExpectedBytes << "bytes)";

    if (imageStarted)
    {
        //if (rawLastIndex != startIndex) qDebug() << "PACKET LOSS!";

        if (startIndex+length > rawExpectedBytes)
        {
            qDebug() << "HUD: OVERFLOW! startIndex:" << startIndex << "length:" << length << "image raw size" << ((receivedWidth * receivedHeight * receivedChannels * receivedDepth) / 8) - 1;
        }
        else
        {
            memcpy(rawImage+startIndex, imageData, length);

            rawLastIndex = startIndex+length;

            // Check if we just reached the end of the image
            if (startIndex+length == rawExpectedBytes)
            {
                //qDebug() << "HUD: 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 = (unsigned int)((startIndex+i) / receivedWidth);
        //                qDebug() << "Setting pixel" << x << "," << y << "to" << (unsigned int)*(rawImage+startIndex+i);
        //            }
        //        }
    }
}

void HUD::copyImage(UASInterface* uas)
{
    UAS* u = qobject_cast<UAS*>(uas);
    if (u)
    {
        QImage temp_im = u->getImage();
        if (temp_im.byteCount() > 0) {
            this->glImage = temp_im;
        }

        // Save to directory if logging is enabled
        if (imageLoggingEnabled)
        {
            u->getImage().save(QString("%1/%2.png").arg(imageLogDirectory).arg(imageLogCounter));
            imageLogCounter++;
        }
    }
}

void HUD::saveImages(bool save)
{
    if (save)
    {
        imageLogDirectory = QGCFileDialog::getExistingDirectory(this, tr("Select image log directory"), QStandardPaths::writableLocation(QStandardPaths::DesktopLocation));

        qDebug() << "Logging to:" << imageLogDirectory;

        if (imageLogDirectory != "")
        {
            imageLogCounter = 0;
            imageLoggingEnabled = true;
            qDebug() << "Logging on";
        }
        else
        {
            imageLoggingEnabled = false;
            selectSaveDirectoryAction->setChecked(false);
        }
    }
    else
    {
        imageLoggingEnabled = false;
        selectSaveDirectoryAction->setChecked(false);
    }

}