HSIDisplay.cc

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

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
    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 Horizontal Situation Indicator class
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QFile>
#include <QStringList>
#include <QPainter>
#include <QGraphicsScene>
#include <QHBoxLayout>
#include <QDoubleSpinBox>
#include "UASManager.h"
#include "HSIDisplay.h"
#include "QGC.h"
#include "Waypoint.h"
#include "UASWaypointManager.h"
#include "Waypoint2DIcon.h"

#include <QDebug>

HSIDisplay::HSIDisplay(QWidget *parent) :
        HDDisplay(NULL, "HSI", parent),
        gpsSatellites(),
        satellitesUsed(0),
        attXSet(0),
        attYSet(0),
        attYawSet(0),
        altitudeSet(1.0),
        posXSet(0),
        posYSet(0),
        posZSet(0),
        attXSaturation(0.5f),
        attYSaturation(0.5f),
        attYawSaturation(0.5f),
        posXSaturation(0.05),
        posYSaturation(0.05),
        altitudeSaturation(1.0),
        lat(0),
        lon(0),
        alt(0),
        globalAvailable(0),
        x(0),
        y(0),
        z(0),
        vx(0),
        vy(0),
        vz(0),
        speed(0),
        localAvailable(0),
        roll(0.0f),
        pitch(0.0f),
        yaw(0.0f),
        bodyXSetCoordinate(0.0f),
        bodyYSetCoordinate(0.0f),
        bodyZSetCoordinate(0.0f),
        bodyYawSet(0.0f),
        uiXSetCoordinate(0.0f),
        uiYSetCoordinate(0.0f),
        uiZSetCoordinate(0.0f),
        uiYawSet(0.0f),
        metricWidth(4.0f),
        positionLock(false),
        attControlEnabled(false),
        xyControlEnabled(false),
        zControlEnabled(false),
        yawControlEnabled(false),
        positionFix(0),
        gpsFix(0),
        visionFix(0),
        laserFix(0),
        mavInitialized(false),
        bottomMargin(10.0f),
        topMargin(10.0f)
{
    refreshTimer->setInterval(updateInterval);

    columns = 1;
    this->setAutoFillBackground(true);

    vwidth = 80;
    vheight = 80;

    xCenterPos = vwidth/2.0f;
    yCenterPos = vheight/2.0f + topMargin - bottomMargin;
    //qDebug() << "CENTER" << xCenterPos << yCenterPos;

    // Add interaction elements
    QHBoxLayout* layout = new QHBoxLayout(this);
    layout->setMargin(2);
    layout->setSpacing(0);
    QDoubleSpinBox* spinBox = new QDoubleSpinBox(this);
    spinBox->setMinimum(0.1);
    spinBox->setMaximum(9999);
    spinBox->setValue(metricWidth);
    spinBox->setToolTip(tr("Ground width in meters shown on instrument"));
    spinBox->setStatusTip(tr("Ground width in meters shown on instrument"));
    connect(spinBox, SIGNAL(valueChanged(double)), this, SLOT(setMetricWidth(double)));
    connect(this, SIGNAL(metricWidthChanged(double)), spinBox, SLOT(setValue(double)));
    layout->addWidget(spinBox);
    layout->setAlignment(spinBox, Qt::AlignBottom | Qt::AlignLeft);
    this->setLayout(layout);

    uas = NULL;
    resetMAVState();

    // Do first update
    setMetricWidth(metricWidth);
}

void HSIDisplay::resetMAVState()
{
    mavInitialized = false;
    attControlKnown = false;
    attControlEnabled = false;
    xyControlKnown = false;
    xyControlEnabled = false;
    zControlKnown = false;
    zControlEnabled = false;
    yawControlKnown = false;
    yawControlEnabled = false;

    // Draw position lock indicators
    positionFixKnown = false;
    positionFix = 0;
    visionFixKnown = false;
    visionFix = 0;
    gpsFixKnown = false;
    gpsFix = 0;
    iruFixKnown = false;
    iruFix = 0;

    // Data
    setPointKnown = false;
    localAvailable = 0;
    globalAvailable = 0;
}

void HSIDisplay::paintEvent(QPaintEvent * event)
{
    Q_UNUSED(event);
    //paintGL();
//    static quint64 interval = 0;
//    //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
//    interval = MG::TIME::getGroundTimeNow();
    renderOverlay();
}

void HSIDisplay::renderOverlay()
{
#if (QGC_EVENTLOOP_DEBUG)
    qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
    // Center location of the HSI gauge items

    //float bottomMargin = 3.0f;

    // Size of the ring instrument
    //const float margin = 0.1f;  // 10% margin of total width on each side
    float baseRadius = (vheight - topMargin - bottomMargin) / 2.0f - bottomMargin / 2.0f;

    // Draw instruments
    // TESTING THIS SHOULD BE MOVED INTO A QGRAPHICSVIEW
    // Update scaling factor
    // adjust scaling to fit both horizontally and vertically
    scalingFactor = this->width()/vwidth;
    double scalingFactorH = this->height()/vheight;
    if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH;

    QPainter painter(viewport());
    painter.setRenderHint(QPainter::Antialiasing, true);
    painter.setRenderHint(QPainter::HighQualityAntialiasing, true);

    // Draw base instrument
    // ----------------------