Vehicle.cc

void Vehicle::_handleScaledPressure2(mavlink_message_t& message) {
    mavlink_scaled_pressure2_t pressure;
    mavlink_msg_scaled_pressure2_decode(&message, &pressure);
    _temperatureFactGroup.temperature2()->setRawValue(pressure.temperature / 100.0);
}

void Vehicle::_handleScaledPressure3(mavlink_message_t& message) {
    mavlink_scaled_pressure3_t pressure;
    mavlink_msg_scaled_pressure3_decode(&message, &pressure);
    _temperatureFactGroup.temperature3()->setRawValue(pressure.temperature / 100.0);
}

bool Vehicle::_containsLink(LinkInterface* link)
{
    return _links.contains(link);
}

void Vehicle::_addLink(LinkInterface* link)
{
    if (!_containsLink(link)) {
        qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((qulonglong)link, 0, 16);
        _links += link;
        if (_links.count() <= 1) {
            _updatePriorityLink(true /* updateActive */, false /* sendCommand */);
        } else {
            _updatePriorityLink(true /* updateActive */, true /* sendCommand */);
        }

        connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
        connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
        connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink);
        connect(link, &LinkInterface::activeChanged, this, &Vehicle::_linkActiveChanged);
    }
}

void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
{
    qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();

    _links.removeOne(link);

    _updatePriorityLink(true /* updateActive */, true /* sendCommand */);

    if (_links.count() == 0 && !_allLinksInactiveSent) {
        qCDebug(VehicleLog) << "All links inactive";
        // Make sure to not send this more than one time
        _allLinksInactiveSent = true;
        emit allLinksInactive(this);
    }
}

bool Vehicle::sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
{
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return false;
    }

    emit _sendMessageOnLinkOnThread(link, message);

    return true;
}

void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
{
    // Make sure this is still a good link
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return;
    }

#if 0
    // Leaving in for ease in Mav 2.0 testing
    mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(link->mavlinkChannel());
    qDebug() << "_sendMessageOnLink" << mavlinkStatus << link->mavlinkChannel() << mavlinkStatus->flags << message.magic;
#endif

    // Give the plugin a chance to adjust
    _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message);

    // Write message into buffer, prepending start sign
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int len = mavlink_msg_to_send_buffer(buffer, &message);

    link->writeBytesSafe((const char*)buffer, len);
    _messagesSent++;
    emit messagesSentChanged();
}

void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
{
    emit linksPropertiesChanged();

    // if the priority link is commanded and still active don't change anything
    if (_priorityLinkCommanded) {
        if (_priorityLink.data()->link_active(_id)) {
            return;
        } else {
            _priorityLinkCommanded = false;
        }
    }

    LinkInterface* newPriorityLink = nullptr;

    // This routine specifically does not clear _priorityLink when there are no links remaining.
    // By doing this we hold a reference on the last link as the Vehicle shuts down. Thus preventing shutdown
    // ordering nullptr pointer crashes where priorityLink() is still called during shutdown sequence.
    if (_links.count() == 0) {
        return;
    }

    // Check for the existing priority link to still be valid
    for (int i=0; i<_links.count(); i++) {
        if (_priorityLink.data() == _links[i]) {
            if (!_priorityLink.data()->highLatency() && _priorityLink->link_active(_id)) {
                // Link is still valid. Continue to use it unless it is high latency. In that case we still look for a better
                // link to use as priority link.
                return;
            }
        }
    }

    // The previous priority link is no longer valid. We must no find the best link available in this priority order:
    //      First active direct USB connection
    //      Any active non high latency link
    //      An active high latency link
    //      Any link

#ifndef NO_SERIAL_LINK
    // Search for an active direct usb connection
    for (int i=0; i<_links.count(); i++) {
        LinkInterface* link = _links[i];
        SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
        if (pSerialLink) {
            LinkConfiguration* config = pSerialLink->getLinkConfiguration();
            if (config) {
                SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(config);
                if (pSerialConfig && pSerialConfig->usbDirect()) {
                    if (_priorityLink.data() != link && link->link_active(_id)) {
                        newPriorityLink = link;
                        break;
                    }
                }
            }
        }
    }
#endif

    if (!newPriorityLink) {
        // Search for an active non-high latency link
        for (int i=0; i<_links.count(); i++) {
            LinkInterface* link = _links[i];
            if (!link->highLatency() && link->link_active(_id)) {
                newPriorityLink = link;
                break;
            }
        }
    }

    if (!newPriorityLink) {
        // Search for an active high latency link
        for (int i=0; i<_links.count(); i++) {
            LinkInterface* link = _links[i];
            if (link->highLatency() && link->link_active(_id)) {
                newPriorityLink = link;
                break;
            }
        }
    }

    if (!newPriorityLink) {
        // Use any link
        newPriorityLink = _links[0];
    }

    if (_priorityLink.data() != newPriorityLink) {
        if (_priorityLink) {
            qgcApp()->showMessage((tr("switch to %2 as priority link")).arg(newPriorityLink->getName()));
        }
        _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);

        _updateHighLatencyLink(sendCommand);

        emit priorityLinkNameChanged(_priorityLink->getName());
        if (updateActive) {
            _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id);
        }
    }
}

int Vehicle::motorCount()
{
    switch (_vehicleType) {
    case MAV_TYPE_HELICOPTER:
        return 1;
    case MAV_TYPE_VTOL_DUOROTOR:
        return 2;
    case MAV_TYPE_TRICOPTER:
        return 3;
    case MAV_TYPE_QUADROTOR:
    case MAV_TYPE_VTOL_QUADROTOR:
        return 4;
    case MAV_TYPE_HEXAROTOR:
        return 6;
    case MAV_TYPE_OCTOROTOR:
        return 8;
    case MAV_TYPE_SUBMARINE:
    {
        // Supported frame types
        enum {
            SUB_FRAME_BLUEROV1,
            SUB_FRAME_VECTORED,
            SUB_FRAME_VECTORED_6DOF,
            SUB_FRAME_VECTORED_6DOF_90DEG,
            SUB_FRAME_SIMPLEROV_3,
            SUB_FRAME_SIMPLEROV_4,
            SUB_FRAME_SIMPLEROV_5,
            SUB_FRAME_CUSTOM
        };

        uint8_t frameType = parameterManager()->getParameter(_compID, "FRAME_CONFIG")->rawValue().toInt();

        switch (frameType) {  // ardupilot/libraries/AP_Motors/AP_Motors6DOF.h sub_frame_t

        case SUB_FRAME_BLUEROV1:
        case SUB_FRAME_VECTORED:
            return 6;

        case SUB_FRAME_SIMPLEROV_3:
            return 3;

        case SUB_FRAME_SIMPLEROV_4:
            return 4;

        case SUB_FRAME_SIMPLEROV_5:
            return 5;

        case SUB_FRAME_VECTORED_6DOF:
        case SUB_FRAME_VECTORED_6DOF_90DEG:
        case SUB_FRAME_CUSTOM:
            return 8;

        default:
            return -1;
        }
    }

    default:
        return -1;
    }
}

bool Vehicle::coaxialMotors()
{
    return _firmwarePlugin->multiRotorCoaxialMotors(this);
}

bool Vehicle::xConfigMotors()
{
    return _firmwarePlugin->multiRotorXConfig(this);
}

QString Vehicle::formatedMessages()
{
    QString messages;
    for(UASMessage* message: _toolbox->uasMessageHandler()->messages()) {
        messages += message->getFormatedText();
    }
    return messages;
}

void Vehicle::clearMessages()
{
    _toolbox->uasMessageHandler()->clearMessages();
}

void Vehicle::_handletextMessageReceived(UASMessage* message)
{
    if(message)
    {
        _formatedMessage = message->getFormatedText();
        emit formatedMessageChanged();
    }
}

void Vehicle::_handleTextMessage(int newCount)
{
    // Reset?
    if(!newCount) {
        _currentMessageCount = 0;
        _currentNormalCount  = 0;
        _currentWarningCount = 0;
        _currentErrorCount   = 0;
        _messageCount        = 0;
        _currentMessageType  = MessageNone;
        emit newMessageCountChanged();
        emit messageTypeChanged();
        emit messageCountChanged();
        return;
    }

    UASMessageHandler* pMh = _toolbox->uasMessageHandler();
    MessageType_t type = newCount ? _currentMessageType : MessageNone;
    int errorCount     = _currentErrorCount;
    int warnCount      = _currentWarningCount;
    int normalCount    = _currentNormalCount;
    //-- Add current message counts
    errorCount  += pMh->getErrorCount();
    warnCount   += pMh->getWarningCount();
    normalCount += pMh->getNormalCount();
    //-- See if we have a higher level
    if(errorCount != _currentErrorCount) {
        _currentErrorCount = errorCount;
        type = MessageError;
    }
    if(warnCount != _currentWarningCount) {
        _currentWarningCount = warnCount;
        if(_currentMessageType != MessageError) {
            type = MessageWarning;
        }
    }
    if(normalCount != _currentNormalCount) {
        _currentNormalCount = normalCount;
        if(_currentMessageType != MessageError && _currentMessageType != MessageWarning) {
            type = MessageNormal;
        }
    }
    int count = _currentErrorCount + _currentWarningCount + _currentNormalCount;
    if(count != _currentMessageCount) {
        _currentMessageCount = count;
        // Display current total new messages count
        emit newMessageCountChanged();
    }
    if(type != _currentMessageType) {
        _currentMessageType = type;
        // Update message level
        emit messageTypeChanged();
    }
    // Update message count (all messages)
    if(newCount != _messageCount) {
        _messageCount = newCount;
        emit messageCountChanged();
    }
    QString errMsg = pMh->getLatestError();
    if(errMsg != _latestError) {
        _latestError = errMsg;
        emit latestErrorChanged();
    }
}

void Vehicle::resetMessages()
{
    // Reset Counts
    int count = _currentMessageCount;
    MessageType_t type = _currentMessageType;
    _currentErrorCount   = 0;
    _currentWarningCount = 0;
    _currentNormalCount  = 0;
    _currentMessageCount = 0;
    _currentMessageType = MessageNone;
    if(count != _currentMessageCount) {
        emit newMessageCountChanged();
    }
    if(type != _currentMessageType) {
        emit messageTypeChanged();
    }
}

void Vehicle::_loadSettings()
{
    if (!_active) {
        return;
    }
    QSettings settings;
    settings.beginGroup(QString(_settingsGroup).arg(_id));
    bool convertOk;
    _joystickMode = static_cast<JoystickMode_t>(settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk));
    if (!convertOk) {
        _joystickMode = JoystickModeRC;
    }
    // Joystick enabled is a global setting so first make sure there are any joysticks connected
    if (_toolbox->joystickManager()->joysticks().count()) {
        setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool());
        _startJoystick(true);
    }
}

void Vehicle::_saveSettings()
{
    QSettings settings;

    settings.beginGroup(QString(_settingsGroup).arg(_id));

    settings.setValue(_joystickModeSettingsKey, _joystickMode);

    // The joystick enabled setting should only be changed if a joystick is present
    // since the checkbox can only be clicked if one is present
    if (_toolbox->joystickManager()->joysticks().count()) {
        settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled);
    }
}

int Vehicle::joystickMode()
{
    return _joystickMode;
}

void Vehicle::setJoystickMode(int mode)
{
    if (mode < 0 || mode >= JoystickModeMax) {
        qCWarning(VehicleLog) << "Invalid joystick mode" << mode;
        return;
    }

    _joystickMode = (JoystickMode_t)mode;
    _saveSettings();
    emit joystickModeChanged(mode);
}

QStringList Vehicle::joystickModes()
{
    QStringList list;

    list << "Normal" << "Attitude" << "Position" << "Force" << "Velocity";

    return list;
}

bool Vehicle::joystickEnabled()
{
    return _joystickEnabled;
}

void Vehicle::setJoystickEnabled(bool enabled)
{
    _joystickEnabled = enabled;
    _startJoystick(_joystickEnabled);
    _saveSettings();
    emit joystickEnabledChanged(_joystickEnabled);
}

void Vehicle::_startJoystick(bool start)
{
    Joystick* joystick = _joystickManager->activeJoystick();
    if (joystick) {
        if (start) {
            joystick->startPolling(this);
        } else {
            joystick->stopPolling();
        }
    }
}

bool Vehicle::active()
{
    return _active;
}

void Vehicle::setActive(bool active)
{
    if (_active != active) {
        _active = active;
        _startJoystick(false);
        emit activeChanged(_active);
    }
}

QGeoCoordinate Vehicle::homePosition()
{
    return _homePosition;
}

void Vehicle::setArmed(bool armed)
{
    // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks.
    sendMavCommand(_defaultComponentId,
                   MAV_CMD_COMPONENT_ARM_DISARM,
                   true,    // show error if fails
                   armed ? 1.0f : 0.0f);
}

bool Vehicle::flightModeSetAvailable()
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
}

QStringList Vehicle::flightModes()
{
    return _firmwarePlugin->flightModes(this);
}

QStringList Vehicle::extraJoystickFlightModes()
{
    return _firmwarePlugin->extraJoystickFlightModes(this);
}

QString Vehicle::flightMode() const
{
    return _firmwarePlugin->flightMode(_base_mode, _custom_mode);
}

void Vehicle::setFlightMode(const QString& flightMode)
{
    uint8_t     base_mode;
    uint32_t    custom_mode;

    if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) {
        // setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing
        // states.
        uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE;
        newBaseMode |= base_mode;

        mavlink_message_t msg;
        mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
                                       _mavlink->getComponentId(),
                                       priorityLink()->mavlinkChannel(),
                                       &msg,
                                       id(),
                                       newBaseMode,
                                       custom_mode);
        sendMessageOnLink(priorityLink(), msg);
    } else {
        qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;
    }
}

QString Vehicle::priorityLinkName() const
{
    if (_priorityLink) {
        return _priorityLink->getName();
    }

    return "none";
}

QVariantList Vehicle::links() const {
    QVariantList ret;

    for( const auto &item: _links )
        ret << QVariant::fromValue(item);

    return ret;
}

void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
{
    if (!_priorityLink) {
        return;
    }

    if (priorityLinkName == _priorityLink->getName()) {
        // The link did not change
        return;
    }

    LinkInterface* newPriorityLink = nullptr;


    for (int i=0; i<_links.count(); i++) {
        if (_links[i]->getName() == priorityLinkName) {
            newPriorityLink = _links[i];
        }
    }

    if (newPriorityLink) {
        _priorityLinkCommanded = true;
        _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
        _updateHighLatencyLink(true);
        emit priorityLinkNameChanged(_priorityLink->getName());
        _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id);
    }
}

void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
{
    mavlink_message_t               msg;
    mavlink_request_data_stream_t   dataStream;

    memset(&dataStream, 0, sizeof(dataStream));

    dataStream.req_stream_id = stream;
    dataStream.req_message_rate = rate;
    dataStream.start_stop = 1;  // start
    dataStream.target_system = id();
    dataStream.target_component = _defaultComponentId;

    mavlink_msg_request_data_stream_encode_chan(_mavlink->getSystemId(),
                                                _mavlink->getComponentId(),
                                                priorityLink()->mavlinkChannel(),
                                                &msg,
                                                &dataStream);

    if (sendMultiple) {
        // We use sendMessageMultiple since we really want these to make it to the vehicle
        sendMessageMultiple(msg);
    } else {
        sendMessageOnLink(priorityLink(), msg);
    }
}

void Vehicle::_sendMessageMultipleNext()
{
    if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) {
        qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid;

        sendMessageOnLink(priorityLink(), _sendMessageMultipleList[_nextSendMessageMultipleIndex].message);

        if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) {
            _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex);
        } else {
            _nextSendMessageMultipleIndex++;
        }
    }

    if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) {
        _nextSendMessageMultipleIndex = 0;
    }
}

void Vehicle::sendMessageMultiple(mavlink_message_t message)
{
    SendMessageMultipleInfo_t   info;

    info.message =      message;
    info.retryCount =   _sendMessageMultipleRetries;

    _sendMessageMultipleList.append(info);
}

void Vehicle::_missionManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
    qgcApp()->showMessage(tr("Mission transfer failed. Error: %1").arg(errorMsg));
}

void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
    qgcApp()->showMessage(tr("GeoFence transfer failed. Error: %1").arg(errorMsg));
}

void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
    qgcApp()->showMessage(tr("Rally Point transfer failed. Error: %1").arg(errorMsg));
}

void Vehicle::_clearCameraTriggerPoints()
{
    _cameraTriggerPoints.clearAndDeleteContents();
}

void Vehicle::_flightTimerStart()
{
    _flightTimer.start();
    _flightTimeUpdater.start();
    _flightDistanceFact.setRawValue(0);
    _flightTimeFact.setRawValue(0);
}

void Vehicle::_flightTimerStop()
{
    _flightTimeUpdater.stop();
}

void Vehicle::_updateFlightTime()
{
    _flightTimeFact.setRawValue((double)_flightTimer.elapsed() / 1000.0);
}

void Vehicle::_startPlanRequest()
{
    if (_missionManagerInitialRequestSent) {
        // We have already started (or possibly completed) the sequence of requesting the plan for the first time
        return;
    }

    // We don't start the Plan request until the following things are satisfied:
    //  - Parameter download is complete
    //  - We know the vehicle capabilities
    //  - We know the max mavlink protocol version
    if (_parameterManager->parametersReady() && _capabilityBitsKnown && _mavlinkProtocolRequestComplete) {
        qCDebug(VehicleLog) << "_startPlanRequest";
        _missionManagerInitialRequestSent = true;
        if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) {
            QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionSavePath();
            if (!missionAutoLoadDirPath.isEmpty()) {
                QDir missionAutoLoadDir(missionAutoLoadDirPath);
                QString autoloadFilename = missionAutoLoadDir.absoluteFilePath(tr("AutoLoad%1.%2").arg(_id).arg(AppSettings::planFileExtension));
                if (QFile(autoloadFilename).exists()) {
                    _initialPlanRequestComplete = true; // We aren't going to load from the vehicle, so we are done
                    PlanMasterController::sendPlanToVehicle(this, autoloadFilename);
                    return;
                }
            }
        }
        _missionManager->loadFromVehicle();
    } else {
        if (!_parameterManager->parametersReady()) {
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready";
        } else if (!_capabilityBitsKnown) {
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known";
        } else if (!_mavlinkProtocolRequestComplete) {
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to mavlink protocol request not complete";
        }
    }
}

void Vehicle::_missionLoadComplete()
{
    // After the initial mission request completes we ask for the geofence
    if (!_geoFenceManagerInitialRequestSent) {
        _geoFenceManagerInitialRequestSent = true;
        if (_geoFenceManager->supported()) {
            qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence";
            _geoFenceManager->loadFromVehicle();
        } else {
            qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping";
            _geoFenceLoadComplete();
        }
    }
}

void Vehicle::_geoFenceLoadComplete()
{
    // After geofence request completes we ask for the rally points
    if (!_rallyPointManagerInitialRequestSent) {
        _rallyPointManagerInitialRequestSent = true;
        if (_rallyPointManager->supported()) {
            qCDebug(VehicleLog) << "_missionLoadComplete requesting Rally Points";
            _rallyPointManager->loadFromVehicle();
        } else {
            qCDebug(VehicleLog) << "_missionLoadComplete Rally Points not supported skipping";
            _rallyPointLoadComplete();
        }
    }
}

void Vehicle::_rallyPointLoadComplete()
{
    qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
    if (!_initialPlanRequestComplete) {
        _initialPlanRequestComplete = true;
        emit initialPlanRequestCompleteChanged(true);
    }
}

void Vehicle::_parametersReady(bool parametersReady)
{
    qDebug() << "_parametersReady" << parametersReady;
    // Try to set current unix time to the vehicle
    _sendQGCTimeToVehicle();
    // Send time twice, more likely to get to the vehicle on a noisy link
    _sendQGCTimeToVehicle();
    if (parametersReady) {
        _setupAutoDisarmSignalling();
        _startPlanRequest();
    }
}

void Vehicle::_sendQGCTimeToVehicle()
{
    mavlink_message_t       msg;
    mavlink_system_time_t   cmd;

    // Timestamp of the master clock in microseconds since UNIX epoch.
    cmd.time_unix_usec = QDateTime::currentDateTime().currentMSecsSinceEpoch()*1000;
    // Timestamp of the component clock since boot time in milliseconds (Not necessary).
    cmd.time_boot_ms = 0;
    mavlink_msg_system_time_encode_chan(_mavlink->getSystemId(),
                                        _mavlink->getComponentId(),
                                        priorityLink()->mavlinkChannel(),
                                        &msg,
                                        &cmd);

    sendMessageOnLink(priorityLink(), msg);
}

void Vehicle::disconnectInactiveVehicle()
{
    // Vehicle is no longer communicating with us, disconnect all links

    LinkManager* linkMgr = _toolbox->linkManager();
    for (int i=0; i<_links.count(); i++) {
        // FIXME: This linkInUse check is a hack fix for multiple vehicles on the same link.
        // The real fix requires significant restructuring which will come later.
        if (!_toolbox->multiVehicleManager()->linkInUse(_links[i], this)) {
            linkMgr->disconnectLink(_links[i]);
        }
    }
    emit linksChanged();
}

void Vehicle::_imageReady(UASInterface*)
{
    if(_uas)
    {
        QImage img = _uas->getImage();
        _toolbox->imageProvider()->setImage(&img, _id);
        _flowImageIndex++;
        emit flowImageIndexChanged();
    }
}

void Vehicle::_remoteControlRSSIChanged(uint8_t rssi)
{
    //-- 0 <= rssi <= 100 - 255 means "invalid/unknown"
    if(rssi > 100) { // Anything over 100 doesn't make sense
        if(_rcRSSI != 255) {
            _rcRSSI = 255;
            emit rcRSSIChanged(_rcRSSI);
        }
        return;
    }
    //-- Initialize it
    if(_rcRSSIstore == 255.) {
        _rcRSSIstore = (double)rssi;
    }
    // Low pass to git rid of jitter
    _rcRSSIstore = (_rcRSSIstore * 0.9f) + ((float)rssi * 0.1);
    uint8_t filteredRSSI = (uint8_t)ceil(_rcRSSIstore);
    if(_rcRSSIstore < 0.1) {
        filteredRSSI = 0;
    }
    if(_rcRSSI != filteredRSSI) {
        _rcRSSI = filteredRSSI;
        emit rcRSSIChanged(_rcRSSI);
    }
}

void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust)
{
    // The following if statement prevents the virtualTabletJoystick from sending values if the standard joystick is enabled
    if ( !_joystickEnabled && !_highLatencyLink) {
        _uas->setExternalControlSetpoint(
            static_cast<float>(roll),
            static_cast<float>(pitch),
            static_cast<float>(yaw),
            static_cast<float>(thrust),
            0, JoystickModeRC);
    }
}

void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled)
{
    if (_connectionLostEnabled != connectionLostEnabled) {
        _connectionLostEnabled = connectionLostEnabled;
        emit connectionLostEnabledChanged(_connectionLostEnabled);
    }
}

void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID)
{
    // only continue if the vehicle id is correct
    if (vehicleID != _id) {
        return;
    }

    emit linksPropertiesChanged();

    bool communicationLost = false;
    bool communicationRegained = false;

    if (link == _priorityLink) {
        if (active && _connectionLost) {
            // communication to priority link regained
            _connectionLost = false;
            communicationRegained = true;
            emit connectionLostChanged(false);

            if (_priorityLink->highLatency()) {
                _setMaxProtoVersion(100);
            } else {
                // Re-negotiate protocol version for the link
                sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
                               MAV_CMD_REQUEST_PROTOCOL_VERSION,
                               false,                                   // No error shown if fails
                               1);                                     // Request protocol version
            }
        } else if (!active && !_connectionLost) {
            _updatePriorityLink(false /* updateActive */, false /* sendCommand */);

            if (link == _priorityLink) {
                // No other link to switch to was found, notify user of comm lossf
                _connectionLost = true;
                communicationLost = true;
                _heardFrom = false;
                emit connectionLostChanged(true);

                if (_autoDisconnect) {
                    // Reset link state
                    for (int i = 0; i < _links.length(); i++) {
                        _mavlink->resetMetadataForLink(_links.at(i));
                    }

                    disconnectInactiveVehicle();
                }
            }
        }
    } else {
        qgcApp()->showMessage((tr("%1 communication to auxiliary link %2 %3")).arg(_vehicleIdSpeech()).arg(link->getName()).arg(active ? "regained" : "lost"));
        _updatePriorityLink(false /* updateActive */, true /* sendCommand */);
    }

    QString commSpeech;
    bool multiVehicle = _toolbox->multiVehicleManager()->vehicles()->count() > 1;
    if (communicationRegained) {
        commSpeech = tr("Communication regained");
        if (_links.count() > 1) {
            qgcApp()->showMessage(tr("Communication regained to vehicle %1 on %2 link %3").arg(_id).arg(_links.count() > 1 ? tr("priority") : tr("auxiliary")).arg(link->getName()));
        } else if (multiVehicle) {
            qgcApp()->showMessage(tr("Communication regained to vehicle %1").arg(_id));
        }
    }
    if (communicationLost) {
        commSpeech = tr("Communication lost");
        if (_links.count() > 1) {
            qgcApp()->showMessage(tr("Communication lost to vehicle %1 on %2 link %3").arg(_id).arg(_links.count() > 1 ? tr("priority") : tr("auxiliary")).arg(link->getName()));
        } else if (multiVehicle) {
            qgcApp()->showMessage(tr("Communication lost to vehicle %1").arg(_id));
        }
    }
    if (multiVehicle && (communicationLost || communicationRegained)) {
        commSpeech.append(tr(" to vehicle %1").arg(_id));
    }
    if (!commSpeech.isEmpty()) {
        _say(commSpeech);
    }
}

void Vehicle::_say(const QString& text)
{
    _toolbox->audioOutput()->say(text.toLower());
}

bool Vehicle::fixedWing() const
{
    return QGCMAVLink::isFixedWing(vehicleType());
}

bool Vehicle::rover() const
{
    return QGCMAVLink::isRover(vehicleType());
}

bool Vehicle::sub() const
{
    return QGCMAVLink::isSub(vehicleType());
}

bool Vehicle::multiRotor() const
{
    return QGCMAVLink::isMultiRotor(vehicleType());
}

bool Vehicle::vtol() const
{
    return _firmwarePlugin->isVtol(this);
}

bool Vehicle::supportsThrottleModeCenterZero() const
{
    return _firmwarePlugin->supportsThrottleModeCenterZero();
}

bool Vehicle::supportsNegativeThrust()
{
    return _firmwarePlugin->supportsNegativeThrust(this);
}

bool Vehicle::supportsRadio() const
{
    return _firmwarePlugin->supportsRadio();
}

bool Vehicle::supportsJSButton() const
{
    return _firmwarePlugin->supportsJSButton();
}

bool Vehicle::supportsMotorInterference() const
{
    return _firmwarePlugin->supportsMotorInterference();
}

bool Vehicle::supportsTerrainFrame() const
{
    return _capabilityBits & MAV_PROTOCOL_CAPABILITY_TERRAIN;
}

QString Vehicle::vehicleTypeName() const {
    static QMap<int, QString> typeNames = {
        { MAV_TYPE_GENERIC,         tr("Generic micro air vehicle" )},
        { MAV_TYPE_FIXED_WING,      tr("Fixed wing aircraft")},
        { MAV_TYPE_QUADROTOR,       tr("Quadrotor")},
        { MAV_TYPE_COAXIAL,         tr("Coaxial helicopter")},
        { MAV_TYPE_HELICOPTER,      tr("Normal helicopter with tail rotor.")},
        { MAV_TYPE_ANTENNA_TRACKER, tr("Ground installation")},
        { MAV_TYPE_GCS,             tr("Operator control unit / ground control station")},
        { MAV_TYPE_AIRSHIP,         tr("Airship, controlled")},
        { MAV_TYPE_FREE_BALLOON,    tr("Free balloon, uncontrolled")},