Pixhawk3DWidget.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 Definition of the class Pixhawk3DWidget.
 *
 *   @author Lionel Heng <hengli@inf.ethz.ch>
 *
 */

#include "Pixhawk3DWidget.h"

#include <sstream>

#include <osg/Geode>
#include <osg/Image>
#include <osgDB/ReadFile>
#include <osg/LineWidth>
#include <osg/ShapeDrawable>
#include <osgText/Text>

#include "../MainWindow.h"
#include "PixhawkCheetahNode.h"
#include "TerrainParamDialog.h"
#include "UASManager.h"

#include "QGC.h"
#include "gpl.h"

#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
#include <tr1/memory>
#include <pixhawk/pixhawk.pb.h>
#endif

Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 : kMessageTimeout(4.0)
 , mMode(DEFAULT_MODE)
 , mSelectedWpIndex(-1)
 , mActiveSystemId(-1)
 , mActiveUAS(NULL)
 , mGlobalViewParams(new GlobalViewParams)
 , mFollowCameraId(-1)
 , mInitCameraPos(false)
 , m3DWidget(new Q3DWidget(this))
 , mViewParamWidget(new ViewParamWidget(mGlobalViewParams, mSystemViewParamMap, this, parent))
{
    connect(m3DWidget, SIGNAL(sizeChanged(int,int)), this, SLOT(sizeChanged(int,int)));
    connect(m3DWidget, SIGNAL(update()), this, SLOT(update()));

    m3DWidget->setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f);
    m3DWidget->init(15.0f);
    m3DWidget->handleDeviceEvents() = false;

    mWorldGridNode = createWorldGrid();
    m3DWidget->worldMap()->addChild(mWorldGridNode, false);

    mTerrainPAT = new osg::PositionAttitudeTransform;
    m3DWidget->worldMap()->addChild(mTerrainPAT);

    // generate map model
    mImageryNode = createImagery();
    mImageryNode->setName("imagery");
    m3DWidget->worldMap()->addChild(mImageryNode, false);

    setupHUD();

    buildLayout();

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
            this, SLOT(activeSystemChanged(UASInterface*)));
    connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)),
            this, SLOT(systemCreated(UASInterface*)));
    connect(mGlobalViewParams.data(), SIGNAL(followCameraChanged(int)),
            this, SLOT(followCameraChanged(int)));
    connect(mGlobalViewParams.data(), SIGNAL(imageryParamsChanged(void)),
            this, SLOT(imageryParamsChanged(void)));

    MainWindow* parentWindow = qobject_cast<MainWindow*>(parent);
    parentWindow->addDockWidget(Qt::LeftDockWidgetArea, mViewParamWidget);

    mViewParamWidget->hide();

    setFocusPolicy(Qt::StrongFocus);
    setMouseTracking(true);
}

Pixhawk3DWidget::~Pixhawk3DWidget()
{

}

void
Pixhawk3DWidget::activeSystemChanged(UASInterface* uas)
{
    if (uas)
    {
        mActiveSystemId = uas->getUASID();
    }

    mActiveUAS = uas;

    mMode = DEFAULT_MODE;
}

void
Pixhawk3DWidget::systemCreated(UASInterface *uas)
{
    int systemId = uas->getUASID();

    if (mSystemContainerMap.contains(systemId))
    {
        return;
    }

    mSystemViewParamMap.insert(systemId, SystemViewParamsPtr(new SystemViewParams(systemId)));
    mSystemContainerMap.insert(systemId, SystemContainer());

    connect(uas, SIGNAL(localPositionChanged(UASInterface*,int,double,double,double,quint64)),
            this, SLOT(localPositionChanged(UASInterface*,int,double,double,double,quint64)));
    connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),
            this, SLOT(localPositionChanged(UASInterface*,double,double,double,quint64)));
    connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)),
            this, SLOT(attitudeChanged(UASInterface*,int,double,double,double,quint64)));
    connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),
            this, SLOT(attitudeChanged(UASInterface*,double,double,double,quint64)));
    connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)),
            this, SLOT(setpointChanged(int,float,float,float,float)));
    connect(uas, SIGNAL(homePositionChanged(int,double,double,double)),
            this, SLOT(homePositionChanged(int,double,double,double)));
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
    connect(uas, SIGNAL(overlayChanged(UASInterface*)),
            this, SLOT(addOverlay(UASInterface*)));
#endif

//    mSystemContainerMap[systemId].gpsLocalOrigin() = QVector3D(47.397786, 8.544476, 428);
    initializeSystem(systemId, uas->getColor());

    emit systemCreatedSignal(uas);
}

void
Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
                                      double x, double y, double z,
                                      quint64 time)
{
    Q_UNUSED(time);

    int systemId = uas->getUASID();

    if (!mSystemContainerMap.contains(systemId))
    {
        return;
    }

    SystemContainer& systemData = mSystemContainerMap[systemId];

    // update trail data
    if (!systemData.trailMap().contains(component))
    {
        systemData.trailMap().insert(component, QVector<osg::Vec3d>());
        systemData.trailMap()[component].reserve(10000);
        systemData.trailIndexMap().insert(component,
                                          systemData.trailMap().size() - 1);

        // generate nice bright random color
        float golden_ratio_conjugate = 0.618033988749895f;

        float h = (float)qrand() / RAND_MAX + golden_ratio_conjugate;
        if (h > 1.0f)
        {
            h -= 1.0f;
        }

        QColor colorHSV;
        colorHSV.setHsvF(h, 0.99, 0.99, 0.5);

        QColor colorRGB = colorHSV.toRgb();

        osg::Vec4f color(colorRGB.redF(), colorRGB.greenF(), colorRGB.blueF(), colorRGB.alphaF());

        systemData.trailNode()->addDrawable(createTrail(color));
        systemData.trailNode()->addDrawable(createLink(uas->getColor()));

        double radius = 0.5;

        osg::ref_ptr<osg::Group> group = new osg::Group;

        // cone indicates orientation
        osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
        double coneRadius = radius / 2.0;
        osg::ref_ptr<osg::Cone> cone =
            new osg::Cone(osg::Vec3d(0.0, 0.0, 0.0),
                          coneRadius, radius * 2.0);

        sd->setShape(cone);
        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
        sd->setColor(color);

        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
        geode->addDrawable(sd);

        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
            new osg::PositionAttitudeTransform;
        pat->addChild(geode);
        pat->setAttitude(osg::Quat(- M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                   M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
                                   0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
        group->addChild(pat);

        // cylinder indicates position
        sd = new osg::ShapeDrawable;
        osg::ref_ptr<osg::Cylinder> cylinder =
            new osg::Cylinder(osg::Vec3d(0.0, 0.0, 0.0),
                              radius, 0);

        sd->setShape(cylinder);
        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
        sd->setColor(color);

        geode = new osg::Geode;
        geode->addDrawable(sd);
        group->addChild(geode);

        // text indicates component id
        colorRGB.lighter();
        color = osg::Vec4(colorRGB.redF(), colorRGB.greenF(), colorRGB.blueF(), 1.0f);

        osg::ref_ptr<osgText::Text> text = new osgText::Text;
        text->setFont(m3DWidget->font());
        text->setText(QString::number(component).toStdString().c_str());
        text->setColor(color);
        text->setCharacterSize(0.3f);
        text->setAxisAlignment(osgText::Text::XY_PLANE);
        text->setAlignment(osgText::Text::CENTER_CENTER);
        text->setPosition(osg::Vec3(0.0, -0.8, 0.0));

        sd = new osg::ShapeDrawable;
        osg::ref_ptr<osg::Box> textBox =
            new osg::Box(osg::Vec3(0.0, -0.8, -0.01), 0.7, 0.4, 0.01);

        sd->setShape(textBox);
        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
        sd->setColor(osg::Vec4(0.0, 0.0, 0.0, 1.0));

        geode = new osg::Geode;
        geode->addDrawable(text);
        geode->addDrawable(sd);
        group->addChild(geode);

        pat = new osg::PositionAttitudeTransform;
        pat->addChild(group);
        systemData.orientationNode()->addChild(pat);
    }

    QVector<osg::Vec3d>& trail = systemData.trailMap()[component];

    bool addToTrail = false;
    if (trail.size() > 0)
    {
        if (fabs(x - trail[trail.size() - 1].x()) > 0.01f ||
                fabs(y - trail[trail.size() - 1].y()) > 0.01f ||
                fabs(z - trail[trail.size() - 1].z()) > 0.01f)
        {
            addToTrail = true;
        }
    }
    else
    {
        addToTrail = true;
    }

    if (addToTrail)
    {
        osg::Vec3d p(x, y, z);
        if (trail.size() == trail.capacity())
        {
            memcpy(trail.data(), trail.data() + 1,
                   (trail.size() - 1) * sizeof(osg::Vec3d));
            trail[trail.size() - 1] = p;
        }
        else
        {
            trail.append(p);
        }
    }
}

void
Pixhawk3DWidget::localPositionChanged(UASInterface* uas,
                                      double x, double y, double z,
                                      quint64 time)
{
    Q_UNUSED(time);

    int systemId = uas->getUASID();

    if (!mSystemContainerMap.contains(systemId))
    {
        return;
    }

//    // Add offset
//    UAS* mav = qobject_cast<UAS*>(uas);

//    float offX = mav->getNedPosGlobalOffset().x();
//    float offY = mav->getNedPosGlobalOffset().y();
//    float offZ = mav->getNedPosGlobalOffset().z();
//    float offYaw = mav->getNedAttGlobalOffset().z();

    // update system position
    m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z));
}

void
Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component,
                                 double roll, double pitch, double yaw,
                                 quint64 time)
{
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    Q_UNUSED(time);

    int systemId = uas->getUASID();

    if (!mSystemContainerMap.contains(systemId))
    {
        return;
    }

    SystemContainer& systemData = mSystemContainerMap[systemId];

    // update trail data
    if (!systemData.trailMap().contains(component))
    {
        return;
    }

    int idx = systemData.trailIndexMap().value(component);

    osg::PositionAttitudeTransform* pat =
        dynamic_cast<osg::PositionAttitudeTransform*>(systemData.orientationNode()->getChild(idx));

    pat->setAttitude(osg::Quat(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
                               0.0, osg::Vec3d(1.0f, 0.0f, 0.0f),
                               0.0, osg::Vec3d(0.0f, 1.0f, 0.0f)));
}

void
Pixhawk3DWidget::attitudeChanged(UASInterface* uas,
                                 double roll, double pitch, double yaw,
                                 quint64 time)
{
    Q_UNUSED(time);

    int systemId = uas->getUASID();

    if (!mSystemContainerMap.contains(systemId))
    {
        return;
    }

    // update system attitude
    osg::Quat q(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f),
                pitch, osg::Vec3d(1.0f, 0.0f, 0.0f),
                roll, osg::Vec3d(0.0f, 1.0f, 0.0f));
    m3DWidget->systemGroup(systemId)->attitude()->setAttitude(q);
}

void
Pixhawk3DWidget::homePositionChanged(int uasId, double lat, double lon,
                                     double alt)
{
    if (!mSystemContainerMap.contains(uasId))
    {
        return;
    }

    SystemContainer& systemData = mSystemContainerMap[uasId];

    systemData.gpsLocalOrigin() = QVector3D(lat, lon, alt);
}

void
Pixhawk3DWidget::setpointChanged(int uasId, float x, float y, float z,
                                 float yaw)
{
    if (!mSystemContainerMap.contains(uasId))
    {
        return;
    }

    UASInterface* uas = UASManager::instance()->getUASForId(uasId);
    if (!uas)
    {
        return;
    }

    QColor color = uas->getColor();
    const SystemViewParamsPtr& systemViewParams = mSystemViewParamMap.value(uasId);

    osg::ref_ptr<osg::PositionAttitudeTransform> pat =
        new osg::PositionAttitudeTransform;

    pat->setPosition(osg::Vec3d(y, x, -z));
    pat->setAttitude(osg::Quat(osg::DegreesToRadians(yaw) - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
                               M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
                               0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));

    osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f, 0.3f);
    osg::ref_ptr<osg::ShapeDrawable> coneDrawable = new osg::ShapeDrawable(cone);
    coneDrawable->setColor(osg::Vec4f(color.redF(), color.greenF(), color.blueF(), 1.0f));
    coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
    osg::ref_ptr<osg::Geode> coneGeode = new osg::Geode;
    coneGeode->addDrawable(coneDrawable);

    pat->addChild(coneGeode);

    osg::ref_ptr<osg::Group>& setpointGroupNode = mSystemContainerMap[uasId].setpointGroupNode();

    setpointGroupNode->addChild(pat);
    if (setpointGroupNode->getNumChildren() > static_cast<unsigned int>(systemViewParams->setpointHistoryLength()))
    {
        setpointGroupNode->removeChildren(0, setpointGroupNode->getNumChildren() - systemViewParams->setpointHistoryLength());
    }

    osg::Vec4f setpointColor(color.redF(), color.greenF(), color.blueF(), 1.0f);
    int setpointCount = setpointGroupNode->getNumChildren();

    // update colors
    for (int i = 0; i < setpointCount; ++i)
    {
        osg::PositionAttitudeTransform* pat =
            dynamic_cast<osg::PositionAttitudeTransform*>(setpointGroupNode->getChild(i));

        osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
        osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));

        setpointColor.a() = static_cast<float>(i + 1) / setpointCount;
        sd->setColor(setpointColor);
    }
}

void
Pixhawk3DWidget::clearData(void)
{
    QMutableMapIterator<int, SystemContainer> it(mSystemContainerMap);
    while (it.hasNext())
    {
        it.next();

        SystemContainer& systemData = it.value();

        // clear setpoint data
        systemData.setpointGroupNode()->removeChildren(0, systemData.setpointGroupNode()->getNumChildren());

        // clear trail data
        systemData.orientationNode()->removeChildren(0, systemData.orientationNode()->getNumChildren());
        systemData.trailIndexMap().clear();
        systemData.trailMap().clear();
        systemData.trailNode()->removeDrawables(0, systemData.trailNode()->getNumDrawables());
    }
}

void
Pixhawk3DWidget::showTerrainParamWindow(void)
{
    TerrainParamDialog::getTerrainParams(mGlobalViewParams);

    const QVector3D& positionOffset = mGlobalViewParams->terrainPositionOffset();
    const QVector3D& attitudeOffset = mGlobalViewParams->terrainAttitudeOffset();

    mTerrainPAT->setPosition(osg::Vec3d(positionOffset.y(), positionOffset.x(), -positionOffset.z()));
    mTerrainPAT->setAttitude(osg::Quat(- attitudeOffset.z(), osg::Vec3d(0.0f, 0.0f, 1.0f),
                                       attitudeOffset.y(), osg::Vec3d(1.0f, 0.0f, 0.0f),
                                       attitudeOffset.x(), osg::Vec3d(0.0f, 1.0f, 0.0f)));
}

void
Pixhawk3DWidget::showViewParamWindow(void)
{
    if (mViewParamWidget->isVisible())
    {
        mViewParamWidget->hide();
    }
    else
    {
        mViewParamWidget->show();
    }
}

void
Pixhawk3DWidget::followCameraChanged(int systemId)
{
    if (systemId == -1)
    {
        mFollowCameraId = -1;
    }

    UASInterface* uas = UASManager::instance()->getUASForId(systemId);
    if (!uas)
    {
        return;
    }

    if (mFollowCameraId != systemId)
    {
        double x = 0.0, y = 0.0, z = 0.0;
        getPosition(uas, mGlobalViewParams->frame(), x, y, z);

        mCameraPos = QVector3D(x, y, z);

        m3DWidget->recenterCamera(y, x, -z);

        mFollowCameraId = systemId;
    }
}

void
Pixhawk3DWidget::imageryParamsChanged(void)
{
    mImageryNode->setImageryType(mGlobalViewParams->imageryType());
    mImageryNode->setPath(mGlobalViewParams->imageryPath());

    const QVector3D& offset = mGlobalViewParams->imageryOffset();
    mImageryNode->setOffset(offset.x(), offset.y(), offset.z());
}

void
Pixhawk3DWidget::recenterActiveCamera(void)
{
    if (mFollowCameraId != -1)
    {
        UASInterface* uas = UASManager::instance()->getUASForId(mFollowCameraId);
        if (!uas)
        {
            return;
        }

        double x = 0.0, y = 0.0, z = 0.0;
        getPosition(uas, mGlobalViewParams->frame(), x, y, z);

        mCameraPos = QVector3D(x, y, z);

        m3DWidget->recenterCamera(y, x, -z);
    }
}

void
Pixhawk3DWidget::modelChanged(int systemId, int index)
{
    if (!mSystemContainerMap.contains(systemId))
    {
        return;
    }

    SystemContainer& systemData = mSystemContainerMap[systemId];
    osg::ref_ptr<SystemGroupNode>& systemGroupNode = m3DWidget->systemGroup(systemId);

    systemGroupNode->egocentricMap()->removeChild(systemData.modelNode());
    systemData.modelNode() = systemData.models().at(index);
    systemGroupNode->egocentricMap()->addChild(systemData.modelNode());
}

void
Pixhawk3DWidget::setBirdEyeView(void)
{
    mViewParamWidget->setFollowCameraId(-1);

    m3DWidget->rotateCamera(0.0, 0.0, 0.0);
    m3DWidget->setCameraDistance(100.0);
}

void
Pixhawk3DWidget::loadTerrainModel(void)
{
    QString filename = QFileDialog::getOpenFileName(this, "Load Terrain Model",
                                                    QDesktopServices::storageLocation(QDesktopServices::DesktopLocation),
                                                    tr("Collada (*.dae)"));

    if (filename.isNull())
    {
        return;
    }

    osg::ref_ptr<osg::Node> node =
        osgDB::readNodeFile(filename.toStdString().c_str());

    if (node)
    {
        if (mTerrainNode.get())
        {
            mTerrainPAT->removeChild(mTerrainNode);
        }
        mTerrainNode = node;
        mTerrainNode->setName("terrain");
        mTerrainPAT->addChild(mTerrainNode);

        mGlobalViewParams->terrainPositionOffset() = QVector3D();
        mGlobalViewParams->terrainAttitudeOffset() = QVector3D();

        mTerrainPAT->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
        mTerrainPAT->setAttitude(osg::Quat(0.0, osg::Vec3d(0.0f, 0.0f, 1.0f),
                                           0.0, osg::Vec3d(1.0f, 0.0f, 0.0f),
                                           0.0, osg::Vec3d(0.0f, 1.0f, 0.0f)));
    }
    else
    {
        QMessageBox msgBox(QMessageBox::Warning,
                           "Error loading model",
                           QString("Error: Unable to load terrain model (%1).").arg(filename));
        msgBox.exec();
    }
}

#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
void
Pixhawk3DWidget::addOverlay(UASInterface *uas)
{
    int systemId = uas->getUASID();

    if (!mSystemContainerMap.contains(systemId))
    {
        return;
    }

    SystemContainer& systemData = mSystemContainerMap[systemId];

    qreal receivedTimestamp;
    px::GLOverlay overlay = uas->getOverlay(receivedTimestamp);

    QString overlayName = QString::fromStdString(overlay.name());

    osg::ref_ptr<SystemGroupNode>& systemNode = m3DWidget->systemGroup(systemId);

    if (!systemData.overlayNodeMap().contains(overlayName))
    {
        osg::ref_ptr<GLOverlayGeode> overlayNode = new GLOverlayGeode;
        systemData.overlayNodeMap().insert(overlayName, overlayNode);

        systemNode->allocentricMap()->addChild(overlayNode, false);
        systemNode->rollingMap()->addChild(overlayNode, false);

        emit overlayCreatedSignal(systemId, overlayName);
    }

    osg::ref_ptr<GLOverlayGeode>& overlayNode = systemData.overlayNodeMap()[overlayName];
    overlayNode->setOverlay(overlay);
    overlayNode->setMessageTimestamp(receivedTimestamp);
}
#endif

void
Pixhawk3DWidget::selectTargetHeading(void)
{
    if (!mActiveUAS)
    {
        return;
    }

    osg::Vec2d p;

    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
    {
        double altitude = mActiveUAS->getAltitudeAMSL();

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude);

        p.set(cursorWorldCoords.x(), cursorWorldCoords.y());
    }
    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
    {
        double z = mActiveUAS->getLocalZ();

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);

        p.set(cursorWorldCoords.x(), cursorWorldCoords.y());
    }

    SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()];
    QVector4D& target = systemData.target();

    target.setW(atan2(p.y() - target.y(), p.x() - target.x()));
}

void
Pixhawk3DWidget::selectTarget(void)
{
    if (!mActiveUAS)
    {
        return;
    }
    if (!mActiveUAS->getParamManager())
    {
        return;
    }

    SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()];
    QVector4D& target = systemData.target();

    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
    {
        double altitude = mActiveUAS->getAltitudeAMSL();

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(mCachedMousePos, altitude);

        QVariant zTarget;
        if (!mActiveUAS->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
        {
            zTarget = -altitude;
        }

        target = QVector4D(cursorWorldCoords.x(), cursorWorldCoords.y(),
                           zTarget.toReal(), 0.0);
    }
    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
    {
        double z = mActiveUAS->getLocalZ();

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(mCachedMousePos, -z);

        QVariant zTarget;
        if (!mActiveUAS->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
        {
            zTarget = z;
        }

        target = QVector4D(cursorWorldCoords.x(), cursorWorldCoords.y(),
                           zTarget.toReal(), 0.0);
    }

    int systemId = mActiveUAS->getUASID();

    QMap<int, SystemViewParamsPtr>::iterator it = mSystemViewParamMap.find(systemId);
    if (it != mSystemViewParamMap.end())
    {
        it.value()->displayTarget() = true;
    }

    mMode = SELECT_TARGET_HEADING_MODE;
}

void
Pixhawk3DWidget::setTarget(void)
{
    selectTargetHeading();

    SystemContainer& systemData = mSystemContainerMap[mActiveUAS->getUASID()];
    QVector4D& target = systemData.target();

    mActiveUAS->setTargetPosition(target.x(), target.y(), target.z(),
                                  osg::RadiansToDegrees(target.w()));
}

void
Pixhawk3DWidget::insertWaypoint(void)
{
    if (!mActiveUAS)
    {
        return;
    }

    Waypoint* wp = NULL;
    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
    {
        double latitude = mActiveUAS->getLatitude();
        double longitude = mActiveUAS->getLongitude();
        double altitude = mActiveUAS->getAltitudeAMSL();
        double x, y;
        QString utmZone;
        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(mCachedMousePos, altitude);

        Imagery::UTMtoLL(cursorWorldCoords.x(), cursorWorldCoords.y(), utmZone,
                         latitude, longitude);

        wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25);
    }
    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
    {
        double z = mActiveUAS->getLocalZ();

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(mCachedMousePos, -z);

        wp = new Waypoint(0, cursorWorldCoords.x(),
                          cursorWorldCoords.y(), z, 0.0, 0.25);
    }

    if (wp)
    {
        wp->setFrame(mGlobalViewParams->frame());
        mActiveUAS->getWaypointManager()->addWaypointEditable(wp);
    }

    mSelectedWpIndex = wp->getId();
    mMode = MOVE_WAYPOINT_HEADING_MODE;
}

void
Pixhawk3DWidget::moveWaypointPosition(void)
{
    if (mMode != MOVE_WAYPOINT_POSITION_MODE)
    {
        mMode = MOVE_WAYPOINT_POSITION_MODE;
        return;
    }

    if (!mActiveUAS)
    {
        return;
    }

    const QList<Waypoint *> waypoints =
        mActiveUAS->getWaypointManager()->getWaypointEditableList();
    Waypoint* waypoint = waypoints.at(mSelectedWpIndex);

    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
    {
        double latitude = mActiveUAS->getLatitude();
        double longitude = mActiveUAS->getLongitude();
        double altitude = mActiveUAS->getAltitudeAMSL();
        double x, y;
        QString utmZone;
        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude);

        Imagery::UTMtoLL(cursorWorldCoords.x(), cursorWorldCoords.y(),
                         utmZone, latitude, longitude);

        waypoint->setX(longitude);
        waypoint->setY(latitude);
    }
    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
    {
        double z = mActiveUAS->getLocalZ();

        QPointF cursorWorldCoords =
            m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);

        waypoint->setX(cursorWorldCoords.x());
        waypoint->setY(cursorWorldCoords.y());
    }
}

void
Pixhawk3DWidget::moveWaypointHeading(void)
{
    if (mMode != MOVE_WAYPOINT_HEADING_MODE)
    {
        mMode = MOVE_WAYPOINT_HEADING_MODE;
        return;
    }

    if (!mActiveUAS)
    {
        return;
    }

    const QList<Waypoint *> waypoints =
        mActiveUAS->getWaypointManager()->getWaypointEditableList();
    Waypoint* waypoint = waypoints.at(mSelectedWpIndex);

    double x = 0.0, y = 0.0, z = 0.0;

    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
    {
        double latitude = waypoint->getY();
        double longitude = waypoint->getX();
        z = -waypoint->getZ();
        QString utmZone;
        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
    }
    else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
    {
        z = mActiveUAS->getLocalZ();
    }

    QPointF cursorWorldCoords =
        m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);

    double yaw = atan2(cursorWorldCoords.y() - waypoint->getY(),
                       cursorWorldCoords.x() - waypoint->getX());
    yaw = osg::RadiansToDegrees(yaw);

    waypoint->setYaw(yaw);
}

void
Pixhawk3DWidget::deleteWaypoint(void)
{
    if (mActiveUAS)
    {
        mActiveUAS->getWaypointManager()->removeWaypoint(mSelectedWpIndex);
    }
}

void
Pixhawk3DWidget::setWaypointAltitude(void)
{
    if (!mActiveUAS)
    {
        return;
    }

    bool ok;
    const QList<Waypoint *> waypoints =
        mActiveUAS->getWaypointManager()->getWaypointEditableList();
    Waypoint* waypoint = waypoints.at(mSelectedWpIndex);

    double altitude = waypoint->getZ();
    if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
    {
        altitude = -altitude;
    }

    double newAltitude =
        QInputDialog::getDouble(this, tr("Set altitude of waypoint %1").arg(mSelectedWpIndex),
                                tr("Altitude (m):"), waypoint->getZ(), -1000.0, 1000.0, 1, &ok);
    if (ok)
    {
        if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
        {
            waypoint->setZ(newAltitude);
        }
        else if (mGlobalViewParams->frame() == MAV_FRAME_LOCAL_NED)
        {
            waypoint->setZ(-newAltitude);
        }
    }
}

void
Pixhawk3DWidget::clearAllWaypoints(void)
{
    if (mActiveUAS)
    {
        const QList<Waypoint *> waypoints =
            mActiveUAS->getWaypointManager()->getWaypointEditableList();
        for (int i = waypoints.size() - 1; i >= 0; --i)
        {
            mActiveUAS->getWaypointManager()->removeWaypoint(i);
        }
    }
}

void
Pixhawk3DWidget::moveImagery(void)
{
    if (mMode != MOVE_IMAGERY_MODE)
    {
        mMode = MOVE_IMAGERY_MODE;
        return;
    }

    if (!mActiveUAS)
    {
        return;
    }

    if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
    {
        return;
    }

    double z = mActiveUAS->getLocalZ();

    QPointF cursorWorldCoords =
        m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), -z);

    QVector3D& offset = mGlobalViewParams->imageryOffset();
    offset.setX(cursorWorldCoords.x());