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 "../MainWindow.h"
#include "PixhawkCheetahGeode.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);

    // generate map model
    mImageryNode = createImagery();
    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)));

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

    initializeSystem(systemId, uas->getColor());

    emit systemCreatedSignal(uas);
}

void
Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
                                      double x, double y, double z,
                                      quint64 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);

        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)
{
    int systemId = uas->getUASID();

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

    // 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)
{
    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)
{
    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::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() > 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::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::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::selectTargetHeading(void)
{
    if (!mActiveUAS)
    {
        return;
    }

    osg::Vec2d p;

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

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

        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->getAltitude();
        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 QVector<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->getAltitude();
        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 QVector<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 QVector<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 QVector<Waypoint *> waypoints =
            mActiveUAS->getWaypointManager()->getWaypointEditableList();
        for (int i = waypoints.size() - 1; i >= 0; --i)
        {
            mActiveUAS->getWaypointManager()->removeWaypoint(i);
        }
    }
}

void
Pixhawk3DWidget::sizeChanged(int width, int height)
{
    resizeHUD(width, height);
}

void
Pixhawk3DWidget::update(void)
{
    MAV_FRAME frame = mGlobalViewParams->frame();

    // set node visibility
    m3DWidget->worldMap()->setChildValue(mWorldGridNode,
                                         mGlobalViewParams->displayWorldGrid());
    if (mGlobalViewParams->imageryType() == Imagery::BLANK_MAP)
    {
        m3DWidget->worldMap()->setChildValue(mImageryNode, false);
    }
    else
    {
        m3DWidget->worldMap()->setChildValue(mImageryNode, true);
    }

    // set system-specific node visibility
    QMutableMapIterator<int, SystemViewParamsPtr> it(mSystemViewParamMap);
    while (it.hasNext())
    {
        it.next();

        osg::ref_ptr<SystemGroupNode>& systemNode = m3DWidget->systemGroup(it.key());
        SystemContainer& systemData = mSystemContainerMap[it.key()];
        const SystemViewParamsPtr& systemViewParams = it.value();

        osg::ref_ptr<osg::Switch>& allocentricMap = systemNode->allocentricMap();
        allocentricMap->setChildValue(systemData.setpointGroupNode(),
                                      systemViewParams->displaySetpoints());

        osg::ref_ptr<osg::Switch>& rollingMap = systemNode->rollingMap();
        rollingMap->setChildValue(systemData.localGridNode(),
                                  systemViewParams->displayLocalGrid());
        rollingMap->setChildValue(systemData.orientationNode(),
                                  systemViewParams->displayTrails());
        rollingMap->setChildValue(systemData.pointCloudNode(),
                                  systemViewParams->displayPointCloud());
        rollingMap->setChildValue(systemData.targetNode(),
                                  systemViewParams->displayTarget());
        rollingMap->setChildValue(systemData.trailNode(),
                                  systemViewParams->displayTrails());
        rollingMap->setChildValue(systemData.waypointGroupNode(),
                                  systemViewParams->displayWaypoints());

#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
        rollingMap->setChildValue(systemData.obstacleGroupNode(),
                                  systemViewParams->displayObstacleList());
        rollingMap->setChildValue(systemData.plannedPathNode(),
                                  systemViewParams->displayPlannedPath());

        m3DWidget->hudGroup()->setChildValue(systemData.depthImageNode(),
                                             systemViewParams->displayRGBD());
        m3DWidget->hudGroup()->setChildValue(systemData.rgbImageNode(),
                                             systemViewParams->displayRGBD());
#endif
    }

    mImageryNode->setImageryType(mGlobalViewParams->imageryType());

    if (mFollowCameraId != -1)
    {
        UASInterface* uas = UASManager::instance()->getUASForId(mFollowCameraId);
        if (uas)
        {
            double x = 0.0, y = 0.0, z = 0.0;
            getPosition(uas, mGlobalViewParams->frame(), x, y, z);

            double dx = y - mCameraPos.y();
            double dy = x - mCameraPos.x();
            double dz = mCameraPos.z() - z;

            m3DWidget->moveCamera(dx, dy, dz);

            mCameraPos = QVector3D(x, y, z);
        }
    }
    else
    {
        if (!mInitCameraPos && mActiveUAS)
        {
            double x = 0.0, y = 0.0, z = 0.0;
            getPosition(mActiveUAS, frame, x, y, z);
            m3DWidget->recenterCamera(y, x, -z);

            mCameraPos = QVector3D(x, y, z);

            setBirdEyeView();
            mInitCameraPos = true;
        }
    }

    // update system-specific data
    it.toFront();
    while (it.hasNext())
    {
        it.next();

        int systemId = it.key();

        UASInterface* uas = UASManager::instance()->getUASForId(systemId);

        osg::ref_ptr<SystemGroupNode>& systemNode = m3DWidget->systemGroup(systemId);
        SystemContainer& systemData = mSystemContainerMap[systemId];
        SystemViewParamsPtr& systemViewParams = it.value();

        double x = 0.0;
        double y = 0.0;
        double z = 0.0;
        double roll = 0.0;
        double pitch = 0.0;
        double yaw = 0.0;

        getPose(uas, frame, x, y, z, roll, pitch, yaw);

        if (systemViewParams->displaySetpoints())
        {

        }
        else
        {
            systemData.setpointGroupNode()->removeChildren(0, systemData.setpointGroupNode()->getNumChildren());
        }
        if (systemViewParams->displayTarget())
        {
            if (systemData.target().isNull())
            {
                systemViewParams->displayTarget() = false;
            }
            else
            {
                updateTarget(uas, frame, x, y, z, systemData.target(),
                             systemData.targetNode());
            }
        }
        if (systemViewParams->displayTrails())
        {
            updateTrails(x, y, z, systemData.trailNode(), systemData.orientationNode(),
                         systemData.trailMap(), systemData.trailIndexMap());
        }
        else
        {
            systemData.trailMap().clear();
        }
        if (systemViewParams->displayWaypoints())
        {
            updateWaypoints(uas, frame, systemData.waypointGroupNode());
        }

#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
        if (systemViewParams->displayObstacleList())
        {
            updateObstacles(uas, frame, x, y, z, systemData.obstacleGroupNode());
        }
        if (systemViewParams->displayPlannedPath())
        {
            updatePlannedPath(uas, frame, x, y, z, systemData.plannedPathNode());
        }
        if (systemViewParams->displayPointCloud())
        {
            updatePointCloud(uas, frame, x, y, z, systemData.pointCloudNode(),
                             systemViewParams->colorPointCloudByDistance());
        }
        if (systemViewParams->displayRGBD())
        {
            updateRGBD(uas, frame, systemData.rgbImageNode(),
                       systemData.depthImageNode());
        }
#endif
    }

    if (frame == MAV_FRAME_GLOBAL &&
        mGlobalViewParams->imageryType() != Imagery::BLANK_MAP)
    {
//        updateImagery(robotX, robotY, robotZ, utmZone);
    }

    if (mActiveUAS)
    {
      updateHUD(mActiveUAS, frame);
    }

    layout()->update();
}

void
Pixhawk3DWidget::addModels(QVector< osg::ref_ptr<osg::Node> >& models,
                           const QColor& systemColor)
{
    QDir directory("models");
    QStringList files = directory.entryList(QStringList("*.osg"), QDir::Files);

    // add Pixhawk Bravo model
    models.push_back(PixhawkCheetahGeode::create(systemColor));

    // add sphere of 0.05m radius
    osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.05f);
    osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
    sphereDrawable->setColor(osg::Vec4f(systemColor.redF(), systemColor.greenF(), systemColor.blueF(), 1.0f));
    osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
    sphereGeode->addDrawable(sphereDrawable);
    sphereGeode->setName("Sphere (0.1m)");
    models.push_back(sphereGeode);

    // add all other models in folder
    for (int i = 0; i < files.size(); ++i)
    {
        osg::ref_ptr<osg::Node> node =
            osgDB::readNodeFile(directory.absoluteFilePath(files[i]).toStdString().c_str());

        if (node)
        {
            models.push_back(node);
        }
        else
        {
            printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
        }
    }
}