UAS.cc

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

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

#include <QList>
#include <QMessageBox>
#include <QTimer>
#include <QSettings>
#include <iostream>
#include <QDebug>
#include <cmath>
#include <qmath.h>
#include "UAS.h"
#include "LinkInterface.h"
#include "UASManager.h"
#include "QGC.h"
#include "GAudioOutput.h"
#include "MAVLinkProtocol.h"
#include "QGCMAVLink.h"
#include "LinkManager.h"
#include "SerialLink.h"

UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
    uasId(id),
    startTime(QGC::groundTimeMilliseconds()),
    commStatus(COMM_DISCONNECTED),
    name(""),
    autopilot(-1),
    links(new QList<LinkInterface*>()),
    unknownPackets(),
    mavlink(protocol),
    waypointManager(this),
    thrustSum(0),
    thrustMax(10),
    startVoltage(0),
    warnVoltage(9.5f),
    warnLevelPercent(20.0f),
    currentVoltage(12.0f),
    lpVoltage(12.0f),
    batteryRemainingEstimateEnabled(true),
    mode(-1),
    status(-1),
    navMode(-1),
    onboardTimeOffset(0),
    controlRollManual(true),
    controlPitchManual(true),
    controlYawManual(true),
    controlThrustManual(true),
    manualRollAngle(0),
    manualPitchAngle(0),
    manualYawAngle(0),
    manualThrust(0),
    receiveDropRate(0),
    sendDropRate(0),
    lowBattAlarm(false),
    positionLock(false),
    localX(0.0),
    localY(0.0),
    localZ(0.0),
    latitude(0.0),
    longitude(0.0),
    altitude(0.0),
    roll(0.0),
    pitch(0.0),
    yaw(0.0),
    statusTimeout(new QTimer(this)),
    paramsOnceRequested(false),
    airframe(QGC_AIRFRAME_EASYSTAR),
    attitudeKnown(false),
    paramManager(NULL),
    attitudeStamped(false),
    lastAttitude(0),
    simulation(new QGCFlightGearLink(this)),
    isLocalPositionKnown(false),
    isGlobalPositionKnown(false),
    systemIsArmed(false)
{
    color = UASInterface::getNextColor();
    setBatterySpecs(QString("9V,9.5V,12.6V"));
    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);
}

UAS::~UAS()
{
    writeSettings();
    delete links;
    links=NULL;
}

void UAS::writeSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    settings.setValue("NAME", this->name);
    settings.setValue("AIRFRAME", this->airframe);
    settings.setValue("AP_TYPE", this->autopilot);
    settings.setValue("BATTERY_SPECS", getBatterySpecs());
    settings.endGroup();
    settings.sync();
}

void UAS::readSettings()
{
    QSettings settings;
    settings.beginGroup(QString("MAV%1").arg(uasId));
    this->name = settings.value("NAME", this->name).toString();
    this->airframe = settings.value("AIRFRAME", this->airframe).toInt();
    this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt();
    if (settings.contains("BATTERY_SPECS")) {
        setBatterySpecs(settings.value("BATTERY_SPECS").toString());
    }
    settings.endGroup();
}

int UAS::getUASID() const
{
    return uasId;
}

void UAS::updateState()
{
    // Check if heartbeat timed out
    quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat;
    if (heartbeatInterval > timeoutIntervalHeartbeat)
    {
        emit heartbeatTimeout(heartbeatInterval);
        emit heartbeatTimeout();
    }

    // Position lock is set by the MAVLink message handler
    // if no position lock is available, indicate an error
    if (positionLock)
    {
        positionLock = false;
    }
    else
    {
        if (((mode&MAV_MODE_FLAG_DECODE_POSITION_AUTO) || (mode&MAV_MODE_FLAG_DECODE_POSITION_GUIDED)) && positionLock)
        {
            GAudioOutput::instance()->notifyNegative();
        }
    }
}

void UAS::setSelected()
{
    if (UASManager::instance()->getActiveUAS() != this) {
        UASManager::instance()->setActiveUAS(this);
        emit systemSelected(true);
    }
}

bool UAS::getSelected() const
{
    return (UASManager::instance()->getActiveUAS() == this);
}

void UAS::receiveMessageNamedValue(const mavlink_message_t& message)
{
    if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT)
    {
        mavlink_named_value_float_t val;
        mavlink_msg_named_value_float_decode(&message, &val);
        QByteArray bytes(val.name, MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN);
        emit valueChanged(this->getUASID(), QString(bytes), tr("raw"), val.value, getUnixTime());
    }
    else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT)
    {
        mavlink_named_value_int_t val;
        mavlink_msg_named_value_int_decode(&message, &val);
        QByteArray bytes(val.name, MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN);
        emit valueChanged(this->getUASID(), QString(bytes), tr("raw"), val.value, getUnixTime());
    }
}

void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
    if (!link) return;
    if (!links->contains(link))
    {
        addLink(link);
        //        qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName();
    }
    //    else
    //    {
    //        qDebug() << __FILE__ << __LINE__ << "DID NOT ADD LINK" << link->getName() << "ALREADY IN LIST";
    //    }

    //    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;

        switch (message.msgid)
        {
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
            lastHeartbeat = QGC::groundTimeUsecs();
            emit heartbeat(this);
            mavlink_heartbeat_t state;
            mavlink_msg_heartbeat_decode(&message, &state);
            // Set new type if it has changed
            if (this->type != state.type)
            {
                this->type = state.type;
                if (airframe == 0)
                {
                    switch (type)
                    {
                    case MAV_TYPE_FIXED_WING:
                        setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR);
                        break;
                    case MAV_TYPE_QUADROTOR:
                        setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH);
                        break;
                    case MAV_TYPE_HEXAROTOR:
                        setAirframe(UASInterface::QGC_AIRFRAME_HEXCOPTER);
                        break;
                    default:
                        // Do nothing
                        break;
                    }
                }
                this->autopilot = state.autopilot;
                emit systemTypeSet(this, 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 = "System " + getUASName();
            QString stateAudio = "";
            QString modeAudio = "";
            QString navModeAudio = "";
            bool statechanged = false;
            bool modechanged = false;


            if (state.system_status != this->status)
            {
                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;

                stateAudio = tr(" changed status to ") + uasState;
            }

            if (this->mode != static_cast<int>(state.base_mode))
            {
                modechanged = true;
                this->mode = static_cast<int>(state.base_mode);
                shortModeText = getShortModeTextFor(this->mode);

                emit modeChanged(this->getUASID(), shortModeText, "");

                modeAudio = " is now in " + shortModeText;
            }

            if (navMode != state.custom_mode)
            {
                emit navModeChanged(uasId, state.custom_mode, getNavModeText(state.custom_mode));
                navMode = state.custom_mode;
                navModeAudio = tr(" changed nav mode to ") + tr("FIXME");
            }

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

            if ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY)
            {
                GAudioOutput::instance()->startEmergency();
            }
            else if (modechanged || statechanged)
            {
                GAudioOutput::instance()->stopEmergency();
                GAudioOutput::instance()->say(audiostring.toLower());
            }

//            if (state.system_status == MAV_STATE_POWEROFF)
//            {
//                emit systemRemoved(this);
//                emit systemRemoved();
//            }
}

            break;
        case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
        case MAVLINK_MSG_ID_NAMED_VALUE_INT:
            // Receive named value message
            receiveMessageNamedValue(message);
            break;
        case MAVLINK_MSG_ID_SYS_STATUS:
        {
                mavlink_sys_status_t state;
                mavlink_msg_sys_status_decode(&message, &state);

                emit loadChanged(this,state.load/10.0f);

                currentVoltage = state.voltage_battery/1000.0f;
                lpVoltage = filterVoltage(currentVoltage);

                if (startVoltage == 0) startVoltage = currentVoltage;
                timeRemaining = calculateTimeRemaining();
                if (!batteryRemainingEstimateEnabled && chargeLevel != -1)
                {
                    chargeLevel = state.battery_remaining;
                }
                //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
                emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining);
                emit voltageChanged(message.sysid, state.voltage_battery/1000);

                // LOW BATTERY ALARM
                if (lpVoltage < warnVoltage)
                {
                    startLowBattAlarm();
                }
                else
                {
                    stopLowBattAlarm();
                }

                // COMMUNICATIONS DROP RATE
                // FIXME
                emit dropRateChanged(this->getUASID(), state.drop_rate_comm/10000.0f);
            }
            break;
        case MAVLINK_MSG_ID_ATTITUDE:
            {
                mavlink_attitude_t attitude;
                mavlink_msg_attitude_decode(&message, &attitude);
                quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
                lastAttitude = time;
                roll = QGC::limitAngleToPMPIf(attitude.roll);
                pitch = QGC::limitAngleToPMPIf(attitude.pitch);
                yaw = QGC::limitAngleToPMPIf(attitude.yaw);

//                // Emit in angles

//                // Convert yaw angle to compass value
//                // in 0 - 360 deg range
//                float compass = (yaw/M_PI)*180.0+360.0f;
//                if (compass > -10000 && compass < 10000)
//                {
//                    while (compass > 360.0f) {
//                        compass -= 360.0f;
//                    }
//                }
//                else
//                {
//                    // Set to 0, since it is an invalid value
//                    compass = 0.0f;
//                }

                attitudeKnown = true;
                emit attitudeChanged(this, roll, pitch, yaw, time);
                emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
            }
            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)
                {
                    yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI);
                    emit attitudeChanged(this, roll, pitch, yaw, time);
                }

                emit altitudeChanged(uasId, hud.alt);
                emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, 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);
                localX = pos.x;
                localY = pos.y;
                localZ = pos.z;
                emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
                emit speedChanged(this, pos.vx, pos.vy, pos.vz, time);

                // Set internal state
                if (!positionLock) {
                    // If position was not locked before, notify positive
                    GAudioOutput::instance()->notifyPositive();
                }
                positionLock = true;
                isLocalPositionKnown = true;
            }
            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();
                latitude = pos.lat/(double)1E7;
                longitude = pos.lon/(double)1E7;
                altitude = pos.alt/1000.0;
                speedX = pos.vx/100.0;
                speedY = pos.vy/100.0;
                speedZ = pos.vz/100.0;
                emit globalPositionChanged(this, latitude, longitude, altitude, time);
                emit speedChanged(this, speedX, speedY, speedZ, time);
                // Set internal state
                if (!positionLock)
                {
                    // If position was not locked before, notify positive
                    GAudioOutput::instance()->notifyPositive();
                }
                positionLock = true;
                isGlobalPositionKnown = true;
                //TODO fix this hack for forwarding of global position for patch antenna tracking
                forwardMessage(message);
            }
            break;
        case MAVLINK_MSG_ID_GPS_RAW_INT:
            {
                mavlink_gps_raw_int_t pos;
                mavlink_msg_gps_raw_int_decode(&message, &pos);

                // SANITY CHECK
                // only accept values in a realistic range
                // quint64 time = getUnixTime(pos.time_usec);
                quint64 time = getUnixTime(pos.time_usec);

                if (pos.fix_type > 2)
                {
                    emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
                    latitude = pos.lat/(double)1E7;
                    longitude = pos.lon/(double)1E7;
                    altitude = pos.alt/1000.0;
                    positionLock = true;
                    isGlobalPositionKnown = true;

                    // Check for NaN
                    int alt = pos.alt;
                    if (!isnan(alt) && !isinf(alt))
                    {
                        alt = 0;
                        emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
                    }
                    // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
                    // Smaller than threshold and not NaN

                    float vel = pos.vel/100.0f;

                    if (vel < 1000000 && !isnan(vel) && !isinf(vel))
                    {
                        // FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time);
                        //qDebug() << "GOT GPS RAW";
                        // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
                    }
                    else
                    {
                        emit textMessageReceived(uasId, message.compid, 255, 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]));
                }
            }
            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, pos.longitude, pos.altitude);
            }
            break;
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
            {
                mavlink_rc_channels_raw_t channels;
                mavlink_msg_rc_channels_raw_decode(&message, &channels);
                emit remoteControlRSSIChanged(channels.rssi/255.0f);
                emit remoteControlChannelRawChanged(0, channels.chan1_raw);
                emit remoteControlChannelRawChanged(1, channels.chan2_raw);
                emit remoteControlChannelRawChanged(2, channels.chan3_raw);
                emit remoteControlChannelRawChanged(3, channels.chan4_raw);
                emit remoteControlChannelRawChanged(4, channels.chan5_raw);
                emit remoteControlChannelRawChanged(5, channels.chan6_raw);
                emit remoteControlChannelRawChanged(6, channels.chan7_raw);
                emit remoteControlChannelRawChanged(7, channels.chan8_raw);
            }
            break;
        case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
            {
                mavlink_rc_channels_scaled_t channels;
                mavlink_msg_rc_channels_scaled_decode(&message, &channels);
                emit remoteControlRSSIChanged(channels.rssi/255.0f);
                emit remoteControlChannelScaledChanged(0, channels.chan1_scaled/10000.0f);
                emit remoteControlChannelScaledChanged(1, channels.chan2_scaled/10000.0f);
                emit remoteControlChannelScaledChanged(2, channels.chan3_scaled/10000.0f);
                emit remoteControlChannelScaledChanged(3, channels.chan4_scaled/10000.0f);
                emit remoteControlChannelScaledChanged(4, channels.chan5_scaled/10000.0f);
                emit remoteControlChannelScaledChanged(5, channels.chan6_scaled/10000.0f);
                emit remoteControlChannelScaledChanged(6, channels.chan7_scaled/10000.0f);
                emit remoteControlChannelScaledChanged(7, channels.chan8_scaled/10000.0f);
            }
            break;
        case MAVLINK_MSG_ID_PARAM_VALUE:
            {
                mavlink_param_value_t value;
                mavlink_msg_param_value_decode(&message, &value);
                QByteArray bytes(value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
                QString parameterName = QString(bytes);
                int component = message.compid;
                mavlink_param_union_t val;
                val.param_float = value.param_value;
                val.type = value.param_type;

                // Insert component if necessary
                if (!parameters.contains(component))
                {
                    parameters.insert(component, new QMap<QString, QVariant>());
                }

                // Insert parameter into registry
                if (parameters.value(component)->contains(parameterName)) parameters.value(component)->remove(parameterName);

                // Insert with correct type
                switch (value.param_type)
                {
                case MAVLINK_TYPE_FLOAT:
                    {
                    // Variant
                    QVariant param(val.param_float);
                    parameters.value(component)->insert(parameterName, param);
                    // Emit change
                    emit parameterChanged(uasId, message.compid, parameterName, param);
                    emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param);
                    qDebug() << "RECEIVED PARAM:" << param;
                }
                    break;
                case MAVLINK_TYPE_UINT32_T:
                    {
                    // Variant
                    QVariant param(val.param_uint32);
                    parameters.value(component)->insert(parameterName, param);
                    // Emit change
                    emit parameterChanged(uasId, message.compid, parameterName, param);
                    emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param);
                    qDebug() << "RECEIVED PARAM:" << param;
                }
                    break;
                case MAVLINK_TYPE_INT32_T:
                    {
                    // Variant
                    QVariant param(val.param_int32);
                    parameters.value(component)->insert(parameterName, param);
                    // Emit change
                    emit parameterChanged(uasId, message.compid, parameterName, param);
                    emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param);
                    qDebug() << "RECEIVED PARAM:" << param;
                }
                    break;
                default:
                    qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << value.param_type;
                }
            }
            break;
        case MAVLINK_MSG_ID_COMMAND_ACK:
            mavlink_command_ack_t ack;
            mavlink_msg_command_ack_decode(&message, &ack);
            if (ack.result == 1)
            {
                emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed CMD: %1").arg(ack.command));
            }
            else
            {
                emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected CMD: %1").arg(ack.command));
            }
            break;
        case MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT:
            {
                mavlink_roll_pitch_yaw_thrust_setpoint_t out;
                mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out);
                quint64 time = getUnixTimeFromMs(out.time_boot_ms);
                emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time);
            }
            break;
        case MAVLINK_MSG_ID_MISSION_COUNT:
            {
                mavlink_mission_count_t wpc;
                mavlink_msg_mission_count_decode(&message, &wpc);
                if (wpc.target_system == mavlink->getSystemId())
                {
                    waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
                }
                else
                {
                    qDebug() << "Got waypoint message, but was not for me";
                }
            }
            break;

        case MAVLINK_MSG_ID_MISSION_ITEM:
            {
                mavlink_mission_item_t wp;
                mavlink_msg_mission_item_decode(&message, &wp);
                //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
                if(wp.target_system == mavlink->getSystemId())
                {
                    waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
                }
                else
                {
                    qDebug() << "Got waypoint message, but was not for me";
                }
            }
            break;

        case MAVLINK_MSG_ID_MISSION_ACK:
            {
                mavlink_mission_ack_t wpa;
                mavlink_msg_mission_ack_decode(&message, &wpa);
                if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId())
                {
                    waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
                }
            }
            break;

        case MAVLINK_MSG_ID_MISSION_REQUEST:
            {
                mavlink_mission_request_t wpr;
                mavlink_msg_mission_request_decode(&message, &wpr);
                if(wpr.target_system == mavlink->getSystemId())
                {
                    waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
                }
                else
                {
                    qDebug() << "Got waypoint message, but was not for me";
                }
            }
            break;

        case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
            {
                mavlink_mission_item_reached_t wpr;
                mavlink_msg_mission_item_reached_decode(&message, &wpr);
                waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr);
                QString text = QString("System %1 reached waypoint %2").arg(getUASName()).arg(wpr.seq);
                GAudioOutput::instance()->say(text);
                emit textMessageReceived(message.sysid, message.compid, 0, text);
            }
            break;

        case MAVLINK_MSG_ID_MISSION_CURRENT:
            {
                mavlink_mission_current_t wpc;
                mavlink_msg_mission_current_decode(&message, &wpc);
                waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc);
            }
            break;

        case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT:
            {
                mavlink_local_position_setpoint_t p;
                mavlink_msg_local_position_setpoint_decode(&message, &p);
                emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
            }
            break;
        case MAVLINK_MSG_ID_STATUSTEXT:
            {
                QByteArray b;
                b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
                mavlink_msg_statustext_get_text(&message, b.data());
                //b.append('\0');
                QString text = QString(b);
                int severity = mavlink_msg_statustext_get_severity(&message);
                //qDebug() << "RECEIVED STATUS:" << text;false
                //emit statusTextReceived(severity, text);
                emit textMessageReceived(uasId, message.compid, severity, text);
            }
            break;
#ifdef MAVLINK_ENABLED_PIXHAWK
        case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE:
            {
                qDebug() << "RECIEVED ACK TO GET IMAGE";
                mavlink_data_transmission_handshake_t p;
                mavlink_msg_data_transmission_handshake_decode(&message, &p);
                imageSize = p.size;
                imagePackets = p.packets;
                imagePayload = p.payload;
                imageQuality = p.jpg_quality;
                imageType = p.type;
                imageStart = QGC::groundTimeMilliseconds();
            }
            break;

        case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
            {
                mavlink_encapsulated_data_t img;
                mavlink_msg_encapsulated_data_decode(&message, &img);
                int seq = img.seqnr;
                int pos = seq * imagePayload;

                // Check if we have a valid transaction
                if (imagePackets == 0)
                {
                    // NO VALID TRANSACTION - ABORT
                    // Restart statemachine
                    imagePacketsArrived = 0;
                }

                for (int i = 0; i < imagePayload; ++i)
                {
                    if (pos <= imageSize) {
                        imageRecBuffer[pos] = img.data[i];
                    }
                    ++pos;
                }

                ++imagePacketsArrived;

                // emit signal if all packets arrived
                if ((imagePacketsArrived >= imagePackets))
                {
                    // Restart statemachine
                    imagePacketsArrived = 0;
                    emit imageReady(this);
                    qDebug() << "imageReady emitted. all packets arrived";
                }
            }
            break;
#endif
//        case MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT:
//        {
//            mavlink_object_detection_event_t event;
//            mavlink_msg_object_detection_event_decode(&message, &event);
//            QString str(event.name);
//            emit objectDetected(event.time, event.object_id, event.type, str, event.quality, event.bearing, event.distance);
//        }
//        break;
        // WILL BE ENABLED ONCE MESSAGE IS IN COMMON MESSAGE SET
//        case MAVLINK_MSG_ID_MEMORY_VECT:
//        {
//            mavlink_memory_vect_t vect;
//            mavlink_msg_memory_vect_decode(&message, &vect);
//            QString str("mem_%1");
//            quint64 time = getUnixTime(0);
//            int16_t *mem0 = (int16_t *)&vect.value[0];
//            uint16_t *mem1 = (uint16_t *)&vect.value[0];
//            int32_t *mem2 = (int32_t *)&vect.value[0];
//            // uint32_t *mem3 = (uint32_t *)&vect.value[0]; causes overload problem
//            float *mem4 = (float *)&vect.value[0];
//            if ( vect.ver == 0) vect.type = 0, vect.ver = 1; else ;
//            if ( vect.ver == 1)
//            {
//                switch (vect.type) {
//                default:
//                case 0:
//                    for (int i = 0; i < 16; i++)
//                        // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "i16", mem0[i], time);
//                    break;
//                case 1:
//                    for (int i = 0; i < 16; i++)
//                        // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "ui16", mem1[i], time);
//                    break;
//                case 2:
//                    for (int i = 0; i < 16; i++)
//                        // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "Q15", (float)mem0[i]/32767.0, time);
//                    break;
//                case 3:
//                    for (int i = 0; i < 16; i++)
//                        // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "1Q14", (float)mem0[i]/16383.0, time);
//                    break;
//                case 4:
//                    for (int i = 0; i < 8; i++)
//                        // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time);
//                    break;
//                case 5:
//                    for (int i = 0; i < 8; i++)
//                        // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time);
//                    break;
//                case 6:
//                    for (int i = 0; i < 8; i++)
//                        // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "float", mem4[i], time);
//                    break;
//                }
//            }
//        }
//        break;
#ifdef MAVLINK_ENABLED_UALBERTA
        case MAVLINK_MSG_ID_NAV_FILTER_BIAS:
            {
                mavlink_nav_filter_bias_t bias;
                mavlink_msg_nav_filter_bias_decode(&message, &bias);
                quint64 time = getUnixTime();
                // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time);
                // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time);
                // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time);
                // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time);
                // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time);
                // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time);
            }
            break;
        case MAVLINK_MSG_ID_RADIO_CALIBRATION:
            {
                mavlink_radio_calibration_t radioMsg;
                mavlink_msg_radio_calibration_decode(&message, &radioMsg);
                QVector<uint16_t> aileron;
                QVector<uint16_t> elevator;
                QVector<uint16_t> rudder;
                QVector<uint16_t> gyro;
                QVector<uint16_t> pitch;
                QVector<uint16_t> throttle;

                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN; ++i)
                    aileron << radioMsg.aileron[i];
                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN; ++i)
                    elevator << radioMsg.elevator[i];
                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN; ++i)
                    rudder << radioMsg.rudder[i];
                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_GYRO_LEN; ++i)
                    gyro << radioMsg.gyro[i];
                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN; ++i)
                    pitch << radioMsg.pitch[i];
                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN; ++i)
                    throttle << radioMsg.throttle[i];

                QPointer<RadioCalibrationData> radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle);
                emit radioCalibrationReceived(radioData);
                delete radioData;
            }
            break;

#endif
            // Messages to ignore
        case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT:
        case MAVLINK_MSG_ID_RAW_IMU:
        case MAVLINK_MSG_ID_SCALED_IMU:
        case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
        case MAVLINK_MSG_ID_RAW_PRESSURE:
        case MAVLINK_MSG_ID_SCALED_PRESSURE:
        case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
        case MAVLINK_MSG_ID_OPTICAL_FLOW:
            break;
        case MAVLINK_MSG_ID_DEBUG_VECT:
            {
                mavlink_debug_vect_t debug;
                mavlink_msg_debug_vect_decode(&message, &debug);
                debug.name[MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN-1] = '\0';
                QString name(debug.name);
                quint64 time = getUnixTime(debug.time_usec);
                emit valueChanged(uasId, name+".x", "raw", debug.x, time);
                emit valueChanged(uasId, name+".y", "raw", debug.y, time);
                emit valueChanged(uasId, name+".z", "raw", debug.z, time);
            }
            break;
        case MAVLINK_MSG_ID_DEBUG:
            break;
        default:
            {
                if (!unknownPackets.contains(message.msgid))
                {
                    unknownPackets.append(message.msgid);
                    QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid);
                    GAudioOutput::instance()->say(errString+tr(", please check console for details."));
                    emit textMessageReceived(uasId, message.compid, 255, errString);
                    std::cout << "Unable to decode message from system " << std::dec << static_cast<int>(message.sysid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                    //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.acid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                }
            }
            break;
        }
    }
}

void UAS::setHomePosition(double lat, double lon, double alt)
{
    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Warning);
    msgBox.setText("Setting new World Coordinate Frame Origin");
    msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::Cancel);
    int ret = msgBox.exec();

    // Close the message box shortly after the click to prevent accidental clicks
    QTimer::singleShot(5000, &msgBox, SLOT(reject()));


    if (ret == QMessageBox::Yes)
    {
        mavlink_message_t msg;
        mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 0, 0, 0, 0, lat, lon, alt);
        // Send message twice to increase chance that it reaches its goal
        sendMessage(msg);
        // Wait 15 ms
        QGC::SLEEP::usleep(15000);
        // Send again
        sendMessage(msg);

        // Send new home position to UAS
        mavlink_set_gps_global_origin_t home;
        home.target_system = uasId;
        home.latitude = lat*1E7;
        home.longitude = lon*1E7;
        home.altitude = alt*1000;
        qDebug() << "lat:" << home.latitude << " lon:" << home.longitude;
        mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home);
        sendMessage(msg);
    }
}

void UAS::setLocalOriginAtCurrentGPSPosition()
{
    QMessageBox msgBox;
    msgBox.setIcon(QMessageBox::Warning);
    msgBox.setText("Setting new World Coordinate Frame Origin");
    msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::Cancel);
    int ret = msgBox.exec();

    // Close the message box shortly after the click to prevent accidental clicks
    QTimer::singleShot(5000, &msgBox, SLOT(reject()));


    if (ret == QMessageBox::Yes)
    {
        mavlink_message_t msg;
        mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 1, 0, 0, 0, 0, 0, 0);
        // Send message twice to increase chance that it reaches its goal
        sendMessage(msg);
        // Wait 15 ms
        QGC::SLEEP::usleep(15000);
        // Send again
        sendMessage(msg);