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 <qmath.h>
//#include "Waypoint2DIcon.h"
#include "MAV2DIcon.h"

#include <QDebug>


HSIDisplay::HSIDisplay(QWidget *parent) :
    HDDisplay(NULL, "HSI", parent),
    dragStarted(false),
    leftDragStarted(false),
    mouseHasMoved(false),
    startX(0.0f),
    startY(0.0f),
    actionPending(false),
    directSending(false),
    gpsSatellites(),
    satellitesUsed(0),
    attXSet(0.0f),
    attYSet(0.0f),
    attYawSet(0.0f),
    altitudeSet(1.0f),
    posXSet(0.0f),
    posYSet(0.0f),
    posZSet(0.0f),
    attXSaturation(0.2f),
    attYSaturation(0.2f),
    attYawSaturation(0.5f),
    posXSaturation(0.05f),
    posYSaturation(0.05f),
    altitudeSaturation(1.0f),
    lat(0.0f),
    lon(0.0f),
    alt(0.0f),
    globalAvailable(0),
    x(0.0f),
    y(0.0f),
    z(0.0f),
    vx(0.0f),
    vy(0.0f),
    vz(0.0f),
    speed(0.0f),
    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.0),
    xCenterPos(0),
    yCenterPos(0),
    positionLock(false),
    attControlEnabled(false),
    xyControlEnabled(false),
    zControlEnabled(false),
    yawControlEnabled(false),
    positionFix(0),
    gpsFix(0),
    visionFix(0),
    laserFix(0),
    iruFix(0),
    mavInitialized(false),
    topMargin(20.0f),
    bottomMargin(14.0f),
    attControlKnown(false),
    xyControlKnown(false),
    zControlKnown(false),
    yawControlKnown(false),
    positionFixKnown(false),
    visionFixKnown(false),
    gpsFixKnown(false),
    iruFixKnown(false),
    setPointKnown(false),
    positionSetPointKnown(false),
    userSetPointSet(false),
    userXYSetPointSet(false),
    userZSetPointSet(false),
    userYawSetPointSet(false)
{
    refreshTimer->setInterval(updateInterval);

    columns = 1;
    this->setAutoFillBackground(true);
    QPalette pal = palette();
    pal.setColor(backgroundRole(), QGC::colorBlack);
    setPalette(pal);

    vwidth = 80.0f;
    vheight = 100.0f;

    xCenterPos = vwidth/2.0f;
    yCenterPos = vheight/2.0f + topMargin - bottomMargin;

    uas = NULL;
    resetMAVState();

    // Do first update
    setMetricWidth(metricWidth);
    // Set tooltip
    setToolTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));
    setStatusTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));

    connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage()));
    statusClearTimer.start(3000);

    setFocusPolicy(Qt::StrongFocus);
}

HSIDisplay::~HSIDisplay()
{

}

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;

    // Setpoints
    positionSetPointKnown = false;
    setPointKnown = false;
}

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();