UAS.cc

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

/**
 * @file
 *   @brief Represents one unmanned aerial vehicle
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QList>
#include <QTimer>
#include <QSettings>
#include <iostream>
#include <QDebug>

#include <cmath>
#include <qmath.h>

#include <limits>
#include <cstdlib>

#include "UAS.h"
#include "LinkInterface.h"
#include "HomePositionManager.h"
#include "QGC.h"
#include "GAudioOutput.h"
#include "MAVLinkProtocol.h"
#include "QGCMAVLink.h"
#include "LinkManager.h"
#ifndef __ios__
#include "SerialLink.h"
#endif
#include <Eigen/Geometry>
#include "FirmwarePluginManager.h"
#include "QGCMessageBox.h"
#include "QGCLoggingCategory.h"
#include "Vehicle.h"

QGC_LOGGING_CATEGORY(UASLog, "UASLog")

#define UAS_DEFAULT_BATTERY_WARNLEVEL 20

/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
* by calling readSettings. This means the new UAS will have the same settings
* as the previous one created unless one calls deleteSettings in the code after
* creating the UAS.
*/

UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle) : UASInterface(),
    lipoFull(4.2f),
    lipoEmpty(3.5f),
    uasId(vehicle->id()),
    unknownPackets(),
    mavlink(protocol),
    receiveDropRate(0),
    sendDropRate(0),

    name(""),
    type(MAV_TYPE_GENERIC),
    airframe(QGC_AIRFRAME_GENERIC),
    autopilot(vehicle->firmwareType()),
    systemIsArmed(false),
    base_mode(0),
    custom_mode(0),
    // custom_mode not initialized
    status(-1),
    // shortModeText not initialized
    // shortStateText not initialized

    // actuatorValues not initialized
    // actuatorNames not initialized
    // motorValues not initialized
    // motorNames mnot initialized
    thrustSum(0),
    thrustMax(10),

    startVoltage(-1.0f),
    tickVoltage(10.5f),
    lastTickVoltageValue(13.0f),
    tickLowpassVoltage(12.0f),
    warnLevelPercent(UAS_DEFAULT_BATTERY_WARNLEVEL),
    currentVoltage(12.6f),
    lpVoltage(12.0f),
    currentCurrent(0.4f),
    chargeLevel(-1),
    timeRemaining(0),
    lowBattAlarm(false),

    startTime(QGC::groundTimeMilliseconds()),
    onboardTimeOffset(0),

    controlRollManual(true),
    controlPitchManual(true),
    controlYawManual(true),
    controlThrustManual(true),
    manualRollAngle(0),
    manualPitchAngle(0),
    manualYawAngle(0),
    manualThrust(0),

    positionLock(false),
    isLocalPositionKnown(false),
    isGlobalPositionKnown(false),

    localX(0.0),
    localY(0.0),
    localZ(0.0),

    latitude(0.0),
    longitude(0.0),
    altitudeAMSL(0.0),
    altitudeAMSLFT(0.0),
    altitudeWGS84(0.0),
    altitudeRelative(0.0),

    globalEstimatorActive(false),

    latitude_gps(0.0),
    longitude_gps(0.0),
    altitude_gps(0.0),

    speedX(0.0),
    speedY(0.0),
    speedZ(0.0),

    nedPosGlobalOffset(0,0,0),
    nedAttGlobalOffset(0,0,0),

    airSpeed(std::numeric_limits<double>::quiet_NaN()),
    groundSpeed(std::numeric_limits<double>::quiet_NaN()),
    waypointManager(vehicle, this),
    fileManager(this, vehicle),

    attitudeKnown(false),
    attitudeStamped(false),
    lastAttitude(0),

    roll(0.0),
    pitch(0.0),
    yaw(0.0),

    imagePackets(0),    // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images

    blockHomePositionChanges(false),
    receivedMode(false),

    // Initialize HIL sensor noise variances to 0.  If user wants corrupted sensor values they will need to set them
    // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY
    // TODO: calibrate stand-still pixhawk variances
    xacc_var(1.2914f),
    yacc_var(0.7048f),
    zacc_var(1.9577f),
    rollspeed_var(0.8126f),
    pitchspeed_var(0.6145f),
    yawspeed_var(0.5852f),
    xmag_var(0.4786f),
    ymag_var(0.4566f),
    zmag_var(0.3333f),
    abs_pressure_var(1.1604f),
    diff_pressure_var(1.1604f),
    pressure_alt_var(1.1604f),
    temperature_var(1.4290f),

#ifndef __mobile__
    simulation(0),
#endif

    // The protected members.
    connectionLost(false),
    lastVoltageWarning(0),
    lastNonNullTime(0),
    onboardTimeOffsetInvalidCount(0),
    hilEnabled(false),
    sensorHil(false),
    lastSendTimeGPS(0),
    lastSendTimeSensors(0),
    lastSendTimeOpticalFlow(0),
    _vehicle(vehicle)
{

    for (unsigned int i = 0; i<255;++i)
    {
        componentID[i] = -1;
        componentMulti[i] = false;
    }

    connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), &fileManager, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));

    // Store a list of available actions for this UAS.
    // Basically everything exposed as a SLOT with no return value or arguments.

    QAction* newAction = new QAction(tr("Arm"), this);
    newAction->setToolTip(tr("Enable the UAS so that all actuators are online"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(armSystem()));
    actions.append(newAction);

    newAction = new QAction(tr("Disarm"), this);
    newAction->setToolTip(tr("Disable the UAS so that all actuators are offline"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(disarmSystem()));
    actions.append(newAction);

    newAction = new QAction(tr("Toggle armed"), this);
    newAction->setToolTip(tr("Toggle between armed and disarmed"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
    actions.append(newAction);

    newAction = new QAction(tr("Go home"), this);
    newAction->setToolTip(tr("Command the UAS to return to its home position"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(home()));
    actions.append(newAction);

    newAction = new QAction(tr("Land"), this);
    newAction->setToolTip(tr("Command the UAS to land"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(land()));
    actions.append(newAction);

    newAction = new QAction(tr("Launch"), this);
    newAction->setToolTip(tr("Command the UAS to launch itself and begin its mission"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(launch()));
    actions.append(newAction);

    newAction = new QAction(tr("Resume"), this);
    newAction->setToolTip(tr("Command the UAS to continue its mission"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(go()));
    actions.append(newAction);

    newAction = new QAction(tr("Stop"), this);
    newAction->setToolTip(tr("Command the UAS to halt and hold position"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(halt()));
    actions.append(newAction);

    newAction = new QAction(tr("Go autonomous"), this);
    newAction->setToolTip(tr("Set the UAS into an autonomous control mode"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(goAutonomous()));
    actions.append(newAction);

    newAction = new QAction(tr("Go manual"), this);
    newAction->setToolTip(tr("Set the UAS into a manual control mode"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(goManual()));
    actions.append(newAction);

    newAction = new QAction(tr("Toggle autonomy"), this);
    newAction->setToolTip(tr("Toggle between manual and full-autonomy"));
    connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy()));
    actions.append(newAction);

    color = UASInterface::getNextColor();
    connect(&statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
    connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings()));
    statusTimeout.start(500);
    readSettings();

    // Initial signals
    emit disarmed();
    emit armingChanged(false);
}

/**
* Saves the settings of name, airframe, autopilot type and battery specifications
* by calling writeSettings.
*/
UAS::~UAS()
{
#ifndef __mobile__
    stopHil();
    if (simulation) {
        // wait for the simulator to exit
        simulation->wait();
        simulation->deleteLater();
    }
#endif
    writeSettings();
}

/**
* Saves the settings of name, airframe, autopilot type and battery specifications
* for the next instantiation of UAS.
*/
void UAS::writeSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    settings.setValue("NAME", this->name);
    settings.setValue("AIRFRAME", this->airframe);
    settings.setValue("BATTERY_SPECS", getBatterySpecs());
    settings.endGroup();
}

/**
* Reads in the settings: name, airframe, autopilot type, and battery specifications
* for the new UAS.
*/
void UAS::readSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    this->name = settings.value("NAME", this->name).toString();
    qDebug() << "Name" << name;
    this->airframe = settings.value("AIRFRAME", this->airframe).toInt();
    if (settings.contains("BATTERY_SPECS"))
    {
        setBatterySpecs(settings.value("BATTERY_SPECS").toString());
    }
    settings.endGroup();
}

/**
*  Deletes the settings origianally read into the UAS by readSettings.
*  This is in case one does not want the old values but would rather
*  start with the values assigned by the constructor.
*/
void UAS::deleteSettings()
{
    this->name = "";
    this->airframe = QGC_AIRFRAME_GENERIC;
    warnLevelPercent = UAS_DEFAULT_BATTERY_WARNLEVEL;
}

/**
* @ return the id of the uas
*/
int UAS::getUASID() const
{
    return uasId;
}

void UAS::triggerAction(int action)
{
    if (action >= 0 && action < actions.size())
    {
        qDebug() << "Triggering action: '" << actions[action]->text() << "'";
        actions[action]->trigger();
    }
}

/**
* Update the heartbeat.
*/
void UAS::updateState()
{
    // Check if heartbeat timed out
    quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat;
    if (!connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat))
    {
        connectionLost = true;
        receivedMode = false;
        QString audiostring = QString("Link lost to system %1").arg(this->getUASID());
        GAudioOutput::instance()->say(audiostring.toLower(), GAudioOutput::AUDIO_SEVERITY_ALERT);
    }

    // Update connection loss time on each iteration
    if (connectionLost && (heartbeatInterval > timeoutIntervalHeartbeat))
    {
        connectionLossTime = heartbeatInterval;
        emit heartbeatTimeout(true, heartbeatInterval/1000);
    }

    // Connection gained
    if (connectionLost && (heartbeatInterval < timeoutIntervalHeartbeat))
    {
        QString audiostring = QString("Link regained to system %1").arg(this->getUASID());
        GAudioOutput::instance()->say(audiostring.toLower(), GAudioOutput::AUDIO_SEVERITY_NOTICE);
        connectionLost = false;
        connectionLossTime = 0;
        emit heartbeatTimeout(false, 0);
    }

    // Position lock is set by the MAVLink message handler
    // if no position lock is available, indicate an error
    if (positionLock)
    {
        positionLock = false;
    }
}

void UAS::receiveMessage(mavlink_message_t message)
{
    if (!components.contains(message.compid))
    {
        QString componentName;

        switch (message.compid)
        {
        case MAV_COMP_ID_ALL:
        {
            componentName = "ANONYMOUS";
            break;
        }
        case MAV_COMP_ID_IMU:
        {
            componentName = "IMU #1";
            break;
        }
        case MAV_COMP_ID_CAMERA:
        {
            componentName = "CAMERA";
            break;
        }
        case MAV_COMP_ID_MISSIONPLANNER:
        {
            componentName = "MISSIONPLANNER";
            break;
        }
        }

        components.insert(message.compid, componentName);
        emit componentCreated(uasId, message.compid, componentName);
    }

    //    qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq;

    // Only accept messages from this system (condition 1)
    // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled
    // and we already got one attitude packet
    if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE))
    {
        QString uasState;
        QString stateDescription;

        bool multiComponentSourceDetected = false;
        bool wrongComponent = false;

        switch (message.compid)
        {
        case MAV_COMP_ID_IMU_2:
            // Prefer IMU 2 over IMU 1 (FIXME)
            componentID[message.msgid] = MAV_COMP_ID_IMU_2;
            break;
        default:
            // Do nothing
            break;
        }

        // Store component ID
        if (componentID[message.msgid] == -1)
        {
            // Prefer the first component
            componentID[message.msgid] = message.compid;
        }
        else
        {
            // Got this message already
            if (componentID[message.msgid] != message.compid)
            {
                componentMulti[message.msgid] = true;
                wrongComponent = true;
            }
        }

        if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true;


        switch (message.msgid)
        {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            lastHeartbeat = QGC::groundTimeUsecs();
            emit heartbeat(this);
            mavlink_heartbeat_t state;
            mavlink_msg_heartbeat_decode(&message, &state);

            // Send the base_mode and system_status values to the plotter. This uses the ground time
            // so the Ground Time checkbox must be ticked for these values to display
            quint64 time = getUnixTime();
            QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid);
            emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time);
            emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time);
            emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time);

            // Set new type if it has changed
            if (this->type != state.type)
            {
                this->autopilot = state.autopilot;
                setSystemType(state.type);
            }

            bool currentlyArmed = state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY;

            if (systemIsArmed != currentlyArmed)
            {
                systemIsArmed = currentlyArmed;
                emit armingChanged(systemIsArmed);
                if (systemIsArmed)
                {
                    emit armed();
                }
                else
                {
                    emit disarmed();
                }
            }

            QString audiostring = QString("System %1").arg(uasId);
            QString stateAudio = "";
            QString modeAudio = "";
            QString navModeAudio = "";
            bool statechanged = false;
            bool modechanged = false;

            QString audiomodeText = FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)autopilot)->flightMode(state.base_mode, state.custom_mode);

            if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT)
            {
                statechanged = true;
                this->status = state.system_status;
                getStatusForCode((int)state.system_status, uasState, stateDescription);
                emit statusChanged(this, uasState, stateDescription);
                emit statusChanged(this->status);

                shortStateText = uasState;

                // Adjust for better audio
                if (uasState == QString("STANDBY")) uasState = QString("standing by");
                if (uasState == QString("EMERGENCY")) uasState = QString("emergency condition");
                if (uasState == QString("CRITICAL")) uasState = QString("critical condition");
                if (uasState == QString("SHUTDOWN")) uasState = QString("shutting down");

                stateAudio = uasState;
            }

            if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode)
            {
                modechanged = true;
                this->base_mode = state.base_mode;
                this->custom_mode = state.custom_mode;
                shortModeText = FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)autopilot)->flightMode(base_mode, custom_mode);
                emit modeChanged(this->getUASID(), shortModeText, "");

                modeAudio = " is now in " + audiomodeText + "flight mode";
            }

            // We got the mode
            receivedMode = true;

            // AUDIO
            if (modechanged && statechanged)
            {
                // Output both messages
                audiostring += modeAudio + " and " + stateAudio;
            }
            else if (modechanged || statechanged)
            {
                // Output the one message
                audiostring += modeAudio + stateAudio;
            }

            if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY))
            {
                GAudioOutput::instance()->say(QString("Emergency for system %1").arg(this->getUASID()), GAudioOutput::AUDIO_SEVERITY_EMERGENCY);
                QTimer::singleShot(3000, GAudioOutput::instance(), SLOT(startEmergency()));
            }
            else if (modechanged || statechanged)
            {
                GAudioOutput::instance()->say(audiostring.toLower());
            }
        }

            break;

        case MAVLINK_MSG_ID_BATTERY_STATUS:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_battery_status_t bat_status;
            mavlink_msg_battery_status_decode(&message, &bat_status);
            emit batteryConsumedChanged(this, (double)bat_status.current_consumed);
        }
            break;

        case MAVLINK_MSG_ID_SYS_STATUS:
        {
            if (multiComponentSourceDetected && wrongComponent)
            {
                break;
            }
            mavlink_sys_status_t state;
            mavlink_msg_sys_status_decode(&message, &state);

            // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present.
            quint64 time = getUnixTime();
            QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid);
            emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time);
            emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time);
            emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time);
            emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time);
            emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time);
            emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time);
            emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);

            // Process CPU load.
            emit loadChanged(this,state.load/10.0f);
            emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);

            // Battery charge/time remaining/voltage calculations
            currentVoltage = state.voltage_battery/1000.0f;
            lpVoltage = filterVoltage(currentVoltage);
            tickLowpassVoltage = tickLowpassVoltage*0.8f + 0.2f*currentVoltage;

            // We don't want to tick above the threshold
            if (tickLowpassVoltage > tickVoltage)
            {
                lastTickVoltageValue = tickLowpassVoltage;
            }

            if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f)
                    /* warn if lower than treshold */
                    && (lpVoltage < tickVoltage)
                    /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */
                    && (currentVoltage > 3.3f)
                    /* warn only if current voltage is really still lower by a reasonable amount */
                    && ((currentVoltage - 0.2f) < tickVoltage)
                    /* warn only every 12 seconds */
                    && (QGC::groundTimeUsecs() - lastVoltageWarning) > 12000000)
            {
                GAudioOutput::instance()->say(QString("Voltage warning for system %1: %2 volts").arg(getUASID()).arg(lpVoltage, 0, 'f', 1, QChar(' ')));
                lastVoltageWarning = QGC::groundTimeUsecs();
                lastTickVoltageValue = tickLowpassVoltage;
            }

            if (startVoltage == -1.0f && currentVoltage > 0.1f) startVoltage = currentVoltage;
            timeRemaining = calculateTimeRemaining();
            chargeLevel = state.battery_remaining;

            emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), timeRemaining);
            emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time);
            // emit voltageChanged(message.sysid, currentVoltage);
            emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time);

            // And if the battery current draw is measured, log that also.
            if (state.current_battery != -1)
            {
                currentCurrent = ((double)state.current_battery)/100.0f;
                emit valueChanged(uasId, name.arg("battery_current"), "A", currentCurrent, time);
            }

            // LOW BATTERY ALARM
            if (chargeLevel >= 0 && (getChargeLevel() < warnLevelPercent))
            {
                // An audio alarm. Does not generate any signals.
                startLowBattAlarm();
            }
            else
            {
                stopLowBattAlarm();
            }

            // control_sensors_enabled:
            // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
            emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11));
            emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12));
            emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13));
            emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14));

            // Trigger drop rate updates as needed. Here we convert the incoming
            // drop_rate_comm value from 1/100 of a percent in a uint16 to a true
            // percentage as a float. We also cap the incoming value at 100% as defined
            // by the MAVLink specifications.
            if (state.drop_rate_comm > 10000)
            {
                state.drop_rate_comm = 10000;
            }
            emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f);
            emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
        }
            break;
        case MAVLINK_MSG_ID_ATTITUDE:
        {
            mavlink_attitude_t attitude;
            mavlink_msg_attitude_decode(&message, &attitude);
            quint64 time = getUnixReferenceTime(attitude.time_boot_ms);

            emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time);

            if (!wrongComponent)
            {
                lastAttitude = time;
                setRoll(QGC::limitAngleToPMPIf(attitude.roll));
                setPitch(QGC::limitAngleToPMPIf(attitude.pitch));
                setYaw(QGC::limitAngleToPMPIf(attitude.yaw));

                attitudeKnown = true;
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
                emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
            }
        }
            break;
        case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
        {
            mavlink_attitude_quaternion_t attitude;
            mavlink_msg_attitude_quaternion_decode(&message, &attitude);
            quint64 time = getUnixReferenceTime(attitude.time_boot_ms);

            double a = attitude.q1;
            double b = attitude.q2;
            double c = attitude.q3;
            double d = attitude.q4;

            double aSq = a * a;
            double bSq = b * b;
            double cSq = c * c;
            double dSq = d * d;
            float dcm[3][3];
            dcm[0][0] = aSq + bSq - cSq - dSq;
            dcm[0][1] = 2.0 * (b * c - a * d);
            dcm[0][2] = 2.0 * (a * c + b * d);
            dcm[1][0] = 2.0 * (b * c + a * d);
            dcm[1][1] = aSq - bSq + cSq - dSq;
            dcm[1][2] = 2.0 * (c * d - a * b);
            dcm[2][0] = 2.0 * (b * d - a * c);
            dcm[2][1] = 2.0 * (a * b + c * d);
            dcm[2][2] = aSq - bSq - cSq + dSq;

            float phi, theta, psi;
            theta = asin(-dcm[2][0]);

            if (fabs(theta - M_PI_2) < 1.0e-3f) {
                phi = 0.0f;
                psi = (atan2(dcm[1][2] - dcm[0][1],
                        dcm[0][2] + dcm[1][1]) + phi);

            } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
                phi = 0.0f;
                psi = atan2f(dcm[1][2] - dcm[0][1],
                          dcm[0][2] + dcm[1][1] - phi);

            } else {
                phi = atan2f(dcm[2][1], dcm[2][2]);
                psi = atan2f(dcm[1][0], dcm[0][0]);
            }

            emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi),
                                 QGC::limitAngleToPMPIf(theta),
                                 QGC::limitAngleToPMPIf(psi), time);

            if (!wrongComponent)
            {
                lastAttitude = time;
                setRoll(QGC::limitAngleToPMPIf(phi));
                setPitch(QGC::limitAngleToPMPIf(theta));
                setYaw(QGC::limitAngleToPMPIf(psi));

                attitudeKnown = true;
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
                emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
            }
        }
            break;
        case MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET:
        {
            mavlink_local_position_ned_system_global_offset_t offset;
            mavlink_msg_local_position_ned_system_global_offset_decode(&message, &offset);
            nedPosGlobalOffset.setX(offset.x);
            nedPosGlobalOffset.setY(offset.y);
            nedPosGlobalOffset.setZ(offset.z);
            nedAttGlobalOffset.setX(offset.roll);
            nedAttGlobalOffset.setY(offset.pitch);
            nedAttGlobalOffset.setZ(offset.yaw);
        }
            break;
        case MAVLINK_MSG_ID_HIL_CONTROLS:
        {
            mavlink_hil_controls_t hil;
            mavlink_msg_hil_controls_decode(&message, &hil);
            emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
        }
            break;
        case MAVLINK_MSG_ID_VFR_HUD:
        {
            mavlink_vfr_hud_t hud;
            mavlink_msg_vfr_hud_decode(&message, &hud);
            quint64 time = getUnixTime();
            // Display updated values
            emit thrustChanged(this, hud.throttle/100.0);

            if (!attitudeKnown)
            {
                setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
                emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
            }

            setAltitudeAMSL(hud.alt);
            setGroundSpeed(hud.groundspeed);
            if (!isnan(hud.airspeed))
                setAirSpeed(hud.airspeed);
            speedZ = -hud.climb;
            emit altitudeChanged(this, altitudeAMSL, altitudeWGS84, altitudeRelative, -speedZ, time);
            emit speedChanged(this, groundSpeed, airSpeed, time);
        }
            break;
        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
            //std::cerr << std::endl;
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
        {
            mavlink_local_position_ned_t pos;
            mavlink_msg_local_position_ned_decode(&message, &pos);
            quint64 time = getUnixTime(pos.time_boot_ms);

            // Emit position always with component ID
            emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);

            if (!wrongComponent)
            {
                setLocalX(pos.x);
                setLocalY(pos.y);
                setLocalZ(pos.z);

                speedX = pos.vx;
                speedY = pos.vy;
                speedZ = pos.vz;

                // Emit
                emit localPositionChanged(this, localX, localY, localZ, time);
                emit velocityChanged_NED(this, speedX, speedY, speedZ, time);

                positionLock = true;
                isLocalPositionKnown = true;
            }
        }
            break;
        case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
        {
            mavlink_global_vision_position_estimate_t pos;
            mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
            quint64 time = getUnixTime(pos.usec);
            emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
            emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
        }
            break;
        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
            //std::cerr << std::endl;
            //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
        {
            mavlink_global_position_int_t pos;
            mavlink_msg_global_position_int_decode(&message, &pos);

            quint64 time = getUnixTime();

            setLatitude(pos.lat/(double)1E7);
            setLongitude(pos.lon/(double)1E7);
            setAltitudeWGS84(pos.alt/1000.0);
            setAltitudeRelative(pos.relative_alt/1000.0);

            globalEstimatorActive = true;

            speedX = pos.vx/100.0;
            speedY = pos.vy/100.0;
            speedZ = pos.vz/100.0;

            emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), getAltitudeWGS84(), time);
            emit altitudeChanged(this, altitudeAMSL, altitudeWGS84, altitudeRelative, -speedZ, time);
            // We had some frame mess here, global and local axes were mixed.
            emit velocityChanged_NED(this, speedX, speedY, speedZ, time);

            setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY));
            emit speedChanged(this, groundSpeed, airSpeed, time);

            positionLock = true;
            isGlobalPositionKnown = true;
        }
            break;
        case MAVLINK_MSG_ID_GPS_RAW_INT:
        {
            mavlink_gps_raw_int_t pos;
            mavlink_msg_gps_raw_int_decode(&message, &pos);

            quint64 time = getUnixTime(pos.time_usec);

            emit gpsLocalizationChanged(this, pos.fix_type);
            // TODO: track localization state not only for gps but also for other loc. sources
            int loc_type = pos.fix_type;
            if (loc_type == 1)
            {
                loc_type = 0;
            }
            emit localizationChanged(this, loc_type);
            setSatelliteCount(pos.satellites_visible);

            if (pos.fix_type > 2)
            {
                positionLock = true;
                isGlobalPositionKnown = true;

                latitude_gps = pos.lat/(double)1E7;
                longitude_gps = pos.lon/(double)1E7;
                altitude_gps = pos.alt/1000.0;

                // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
                if (!globalEstimatorActive) {
                    setLatitude(latitude_gps);
                    setLongitude(longitude_gps);
                    setAltitudeWGS84(altitude_gps);
                    emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), getAltitudeWGS84(), time);
                    emit altitudeChanged(this, altitudeAMSL, altitudeWGS84, altitudeRelative, -speedZ, time);

                    float vel = pos.vel/100.0f;
                    // Smaller than threshold and not NaN
                    if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) {
                        setGroundSpeed(vel);
                        emit speedChanged(this, groundSpeed, airSpeed, time);
                    } else {
                        emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
                    }
                }
            }
        }
            break;
        case MAVLINK_MSG_ID_GPS_STATUS:
        {
            mavlink_gps_status_t pos;
            mavlink_msg_gps_status_decode(&message, &pos);
            for(int i = 0; i < (int)pos.satellites_visible; i++)
            {
                emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
            }
            setSatelliteCount(pos.satellites_visible);
        }
            break;
        case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN:
        {
            mavlink_gps_global_origin_t pos;
            mavlink_msg_gps_global_origin_decode(&message, &pos);
            emit homePositionChanged(uasId, pos.latitude / 10000000.0, pos.longitude / 10000000.0, pos.altitude / 1000.0);
        }
            break;
        case MAVLINK_MSG_ID_RC_CHANNELS:
        {
            mavlink_rc_channels_t channels;
            mavlink_msg_rc_channels_decode(&message, &channels);

            emit remoteControlRSSIChanged(channels.rssi);

            if (channels.chan1_raw != UINT16_MAX && channels.chancount > 0)
                emit remoteControlChannelRawChanged(0, channels.chan1_raw);
            if (channels.chan2_raw != UINT16_MAX && channels.chancount > 1)
                emit remoteControlChannelRawChanged(1, channels.chan2_raw);
            if (channels.chan3_raw != UINT16_MAX && channels.chancount > 2)
                emit remoteControlChannelRawChanged(2, channels.chan3_raw);
            if (channels.chan4_raw != UINT16_MAX && channels.chancount > 3)
                emit remoteControlChannelRawChanged(3, channels.chan4_raw);
            if (channels.chan5_raw != UINT16_MAX && channels.chancount > 4)
                emit remoteControlChannelRawChanged(4, channels.chan5_raw);
            if (channels.chan6_raw != UINT16_MAX && channels.chancount > 5)
                emit remoteControlChannelRawChanged(5, channels.chan6_raw);
            if (channels.chan7_raw != UINT16_MAX && channels.chancount > 6)
                emit remoteControlChannelRawChanged(6, channels.chan7_raw);
            if (channels.chan8_raw != UINT16_MAX && channels.chancount > 7)
                emit remoteControlChannelRawChanged(7, channels.chan8_raw);
            if (channels.chan9_raw != UINT16_MAX && channels.chancount > 8)
                emit remoteControlChannelRawChanged(8, channels.chan9_raw);
            if (channels.chan10_raw != UINT16_MAX && channels.chancount > 9)
                emit remoteControlChannelRawChanged(9, channels.chan10_raw);
            if (channels.chan11_raw != UINT16_MAX && channels.chancount > 10)
                emit remoteControlChannelRawChanged(10, channels.chan11_raw);
            if (channels.chan12_raw != UINT16_MAX && channels.chancount > 11)
                emit remoteControlChannelRawChanged(11, channels.chan12_raw);
            if (channels.chan13_raw != UINT16_MAX && channels.chancount > 12)
                emit remoteControlChannelRawChanged(12, channels.chan13_raw);
            if (channels.chan14_raw != UINT16_MAX && channels.chancount > 13)
                emit remoteControlChannelRawChanged(13, channels.chan14_raw);
            if (channels.chan15_raw != UINT16_MAX && channels.chancount > 14)
                emit remoteControlChannelRawChanged(14, channels.chan15_raw);
            if (channels.chan16_raw != UINT16_MAX && channels.chancount > 15)
                emit remoteControlChannelRawChanged(15, channels.chan16_raw);
            if (channels.chan17_raw != UINT16_MAX && channels.chancount > 16)
                emit remoteControlChannelRawChanged(16, channels.chan17_raw);
            if (channels.chan18_raw != UINT16_MAX && channels.chancount > 17)
                emit remoteControlChannelRawChanged(17, channels.chan18_raw);

        }
            break;

        // TODO: (gg 20150420) PX4 Firmware does not seem to send this message. Don't know what to do about it.
        case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
        {
            mavlink_rc_channels_scaled_t channels;
            mavlink_msg_rc_channels_scaled_decode(&message, &channels);

            const unsigned int portWidth = 8; // XXX magic number

            emit remoteControlRSSIChanged(channels.rssi);
            if (static_cast<uint16_t>(channels.chan1_scaled) != UINT16_MAX)
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 0, channels.chan1_scaled/10000.0f);
            if (static_cast<uint16_t>(channels.chan2_scaled) != UINT16_MAX)
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 1, channels.chan2_scaled/10000.0f);
            if (static_cast<uint16_t>(channels.chan3_scaled) != UINT16_MAX)
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 2, channels.chan3_scaled/10000.0f);
            if (static_cast<uint16_t>(channels.chan4_scaled) != UINT16_MAX)
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 3, channels.chan4_scaled/10000.0f);
            if (static_cast<uint16_t>(channels.chan5_scaled) != UINT16_MAX)
                emit remoteControlChannelScaledChanged(channels.port * portWidth + 4, channels.chan5_scaled/10000.0f);
            if (static_cast<uint16_t>(channels.chan6_scaled) != UINT16_MAX)