HUD.cc

/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK 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.

    PIXHAWK 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 PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Head Up Display (HUD)
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QDebug>
#include <cmath>
#include <limits>

#include "UASManager.h"
#include "HUD.h"
#include "MG.h"

// Fix for some platforms, e.g. windows
#ifndef GL_MULTISAMPLE
#define GL_MULTISAMPLE  0x809D
#endif

template<typename T>
inline bool isnan(T value)
{
    return value != value;

}

// requires #include <limits>
template<typename T>
inline bool isinf(T value)
{
    return std::numeric_limits<T>::has_infinity && (value == std::numeric_limits<T>::infinity() || (-1*value) == std::numeric_limits<T>::infinity());
}

/**
 * @warning The HUD widget will not start painting its content automatically
 *          to update the view, start the auto-update by calling HUD::start().
 *
 * @param width
 * @param height
 * @param parent
 */
HUD::HUD(int width, int height, QWidget* parent)
    : QGLWidget(QGLFormat(QGL::SampleBuffers), parent),
    uas(NULL),
    values(QMap<QString, float>()),
    valuesDot(QMap<QString, float>()),
    valuesMean(QMap<QString, float>()),
    valuesCount(QMap<QString, int>()),
    lastUpdate(QMap<QString, quint64>()),
    yawInt(0.0f),
    mode(tr("UNKNOWN MODE")),
    state(tr("UNKNOWN STATE")),
    fuelStatus(tr("00.0V (00m:00s)")),
    xCenterOffset(0.0f),
    yCenterOffset(0.0f),
    vwidth(200.0f),
    vheight(150.0f),
    vGaugeSpacing(50.0f),
    vPitchPerDeg(6.0f), ///< 4 mm y translation per degree)
    rawBuffer1(NULL),
    rawBuffer2(NULL),
    rawImage(NULL),
    rawLastIndex(0),
    rawExpectedBytes(0),
    bytesPerLine(1),
    imageStarted(false),
    receivedDepth(8),
    receivedChannels(1),
    receivedWidth(640),
    receivedHeight(480),
    defaultColor(QColor(70, 200, 70)),
    setPointColor(QColor(200, 20, 200)),
    warningColor(Qt::yellow),
    criticalColor(Qt::red),
    infoColor(QColor(20, 200, 20)),
    fuelColor(criticalColor),
    warningBlinkRate(5),
    refreshTimer(new QTimer(this)),
    noCamera(true),
    hardwareAcceleration(true),
    strongStrokeWidth(1.5f),
    normalStrokeWidth(1.0f),
    fineStrokeWidth(0.5f),
    waypointName("")
{
    // Set auto fill to false
    setAutoFillBackground(false);

    // Fill with black background
    QImage fill = QImage(width, height, QImage::Format_Indexed8);
    fill.setNumColors(3);
    fill.setColor(0, qRgb(0, 0, 0));
    fill.setColor(1, qRgb(0, 0, 0));
    fill.setColor(2, qRgb(0, 0, 0));
    fill.fill(0);

    //QString imagePath = MG::DIR::getIconDirectory() + "hud-template.png";
    //qDebug() << __FILE__ << __LINE__ << "template image:" << imagePath;
    //fill = QImage(imagePath);

    glImage = QGLWidget::convertToGLFormat(fill);

    // Refresh timer
    refreshTimer->setInterval(40); // 25 Hz
    connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update()));

    // Resize to correct size and fill with image
    resize(fill.size());
    glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits());

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

    fontDatabase = QFontDatabase();
    const QString fontFileName = ":/general/vera.ttf"; ///< Font file is part of the QRC file and compiled into the app
    const QString fontFamilyName = "Bitstream Vera Sans";
    if(!QFile::exists(fontFileName)) qDebug() << "ERROR! font file: " << fontFileName << " DOES NOT EXIST!";

    fontDatabase.addApplicationFont(fontFileName);
    font = fontDatabase.font(fontFamilyName, "Roman", (int)(10*scalingFactor*1.2f+0.5f));
    if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName;

    // Connect with UAS
    UASManager* manager = UASManager::instance();
    connect(manager, SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
}

HUD::~HUD()
{

}

void HUD::start()
{
    refreshTimer->start();
}

void HUD::stop()
{
    refreshTimer->stop();
}

void HUD::updateValue(UASInterface* uas, QString name, double value, quint64 msec)
{
    // UAS is not needed
    Q_UNUSED(uas);

    if (!isnan(value) && !isinf(value))
    {
        // Update mean
        const float oldMean = valuesMean.value(name, 0.0f);
        const int meanCount = valuesCount.value(name, 0);
        double mean = (oldMean * meanCount +  value) / (meanCount + 1);
        if (isnan(mean) || isinf(mean)) mean = 0.0;
        valuesMean.insert(name, mean);
        valuesCount.insert(name, meanCount + 1);
        // Two-value sliding average
        double dot = (valuesDot.value(name) + (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f))/2.0f;
        if (isnan(dot) || isinf(dot))
        {
            dot = 0.0;
        }
        valuesDot.insert(name, dot);
        values.insert(name, value);
        lastUpdate.insert(name, msec);
        //}

        //qDebug() << __FILE__ << __LINE__ << "VALUE:" << value << "MEAN:" << mean << "DOT:" << dot << "COUNT:" << meanCount;
    }
}