Vehicle.cc

void Vehicle::_saveSettings(void)
{
    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(void)
{
    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(void)
{
    QStringList list;

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

    return list;
}

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

void Vehicle::setJoystickEnabled(bool enabled)
{
    _joystickEnabled = enabled;
    _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(void)
{
    return _active;
}

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

QGeoCoordinate Vehicle::homePosition(void)
{
    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(void)
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
}

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

QString Vehicle::flightMode(void) 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(void) const
{
    if (_priorityLink) {
        return _priorityLink->getName();
    }

    return "none";
}

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

    foreach( 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 = NULL;


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

bool Vehicle::hilMode(void)
{
    return _base_mode & MAV_MODE_FLAG_HIL_ENABLED;
}

void Vehicle::setHilMode(bool hilMode)
{
    mavlink_message_t msg;

    uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_HIL;
    if (hilMode) {
        newBaseMode |= MAV_MODE_FLAG_HIL_ENABLED;
    }

    mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
                                   _mavlink->getComponentId(),
                                   priorityLink()->mavlinkChannel(),
                                   &msg,
                                   id(),
                                   newBaseMode,
                                   _custom_mode);
    sendMessageOnLink(priorityLink(), msg);
}

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(void)
{
    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. Retry transfer. Error: %1").arg(errorMsg));
}

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

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

void Vehicle::_addNewMapTrajectoryPoint(void)
{
    if (_mapTrajectoryHaveFirstCoordinate) {
        // Keep three minutes of trajectory on mobile due to perf impact of lines
#ifdef __mobile__
        if (_mapTrajectoryList.count() * _mapTrajectoryMsecsBetweenPoints > 3 * 1000 * 60) {
            _mapTrajectoryList.removeAt(0)->deleteLater();
        }
#endif
        _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this));
        _flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + _mapTrajectoryLastCoordinate.distanceTo(_coordinate));
    }
    _mapTrajectoryHaveFirstCoordinate = true;
    _mapTrajectoryLastCoordinate = _coordinate;
    _flightTimeFact.setRawValue((double)_flightTimer.elapsed() / 1000.0);
}

void Vehicle::_clearTrajectoryPoints(void)
{
    _mapTrajectoryList.clearAndDeleteContents();
}

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

void Vehicle::_mapTrajectoryStart(void)
{
    _mapTrajectoryHaveFirstCoordinate = false;
    _clearTrajectoryPoints();
    _mapTrajectoryTimer.start();
    _flightTimer.start();
    _flightDistanceFact.setRawValue(0);
    _flightTimeFact.setRawValue(0);
}

void Vehicle::_mapTrajectoryStop()
{
    _mapTrajectoryTimer.stop();
}

void Vehicle::_startPlanRequest(void)
{
    if (_missionManagerInitialRequestSent) {
        return;
    }

    if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
        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 (!_vehicleCapabilitiesKnown) {
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known";
        }
    }
}

void Vehicle::_missionLoadComplete(void)
{
    // 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(void)
{
    // 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(void)
{
    qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
    if (!_initialPlanRequestComplete) {
        _initialPlanRequestComplete = true;
        emit initialPlanRequestCompleteChanged(true);
    }
}

void Vehicle::_parametersReady(bool 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(void)
{
    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(void)
{
    // 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)
{
    if (_rcRSSIstore < 0 || _rcRSSIstore > 100) {
        _rcRSSIstore = 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(roll, pitch, yaw, 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) {
            if (_connectionLostEnabled) {
                _connectionLost = true;
                communicationLost = true;
                _heardFrom = false;
                _maxProtoVersion = 0;
                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(void) const
{
    return QGCMAVLink::isFixedWing(vehicleType());
}

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

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

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

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

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

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

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

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

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

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

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")},
        { MAV_TYPE_ROCKET,          tr("Rocket")},
        { MAV_TYPE_GROUND_ROVER,    tr("Ground rover")},
        { MAV_TYPE_SURFACE_BOAT,    tr("Surface vessel, boat, ship")},
        { MAV_TYPE_SUBMARINE,       tr("Submarine")},
        { MAV_TYPE_HEXAROTOR,       tr("Hexarotor")},
        { MAV_TYPE_OCTOROTOR,       tr("Octorotor")},
        { MAV_TYPE_TRICOPTER,       tr("Octorotor")},
        { MAV_TYPE_FLAPPING_WING,   tr("Flapping wing")},
        { MAV_TYPE_KITE,            tr("Flapping wing")},
        { MAV_TYPE_ONBOARD_CONTROLLER, tr("Onboard companion controller")},
        { MAV_TYPE_VTOL_DUOROTOR,   tr("Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter")},
        { MAV_TYPE_VTOL_QUADROTOR,  tr("Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter")},
        { MAV_TYPE_VTOL_TILTROTOR,  tr("Tiltrotor VTOL")},
        { MAV_TYPE_VTOL_RESERVED2,  tr("VTOL reserved 2")},
        { MAV_TYPE_VTOL_RESERVED3,  tr("VTOL reserved 3")},
        { MAV_TYPE_VTOL_RESERVED4,  tr("VTOL reserved 4")},
        { MAV_TYPE_VTOL_RESERVED5,  tr("VTOL reserved 5")},
        { MAV_TYPE_GIMBAL,          tr("Onboard gimbal")},
        { MAV_TYPE_ADSB,            tr("Onboard ADSB peripheral")},
    };
    return typeNames[_vehicleType];
}

/// Returns the string to speak to identify the vehicle
QString Vehicle::_vehicleIdSpeech(void)
{
    if (_toolbox->multiVehicleManager()->vehicles()->count() > 1) {
        return QString(tr("vehicle %1")).arg(id());
    } else {
        return QString();
    }
}

void Vehicle::_handleFlightModeChanged(const QString& flightMode)
{
    _say(QString(tr("%1 %2 flight mode")).arg(_vehicleIdSpeech()).arg(flightMode));
    emit guidedModeChanged(_firmwarePlugin->isGuidedMode(this));
}

void Vehicle::_announceArmedChanged(bool armed)
{
    _say(QString("%1 %2").arg(_vehicleIdSpeech()).arg(armed ? QString(tr("armed")) : QString(tr("disarmed"))));
}

void Vehicle::_setFlying(bool flying)
{
    if (_flying != flying) {
        _flying = flying;
        emit flyingChanged(flying);
    }
}

void Vehicle::_setLanding(bool landing)
{
    if (armed() && _landing != landing) {
        _landing = landing;
        emit landingChanged(landing);
    }
}

bool Vehicle::guidedModeSupported(void) const
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability);
}

bool Vehicle::pauseVehicleSupported(void) const
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::PauseVehicleCapability);
}

bool Vehicle::orbitModeSupported() const
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
}

bool Vehicle::takeoffVehicleSupported() const
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::TakeoffVehicleCapability);
}

void Vehicle::guidedModeRTL(void)
{
    if (!guidedModeSupported()) {
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
        return;
    }
    _firmwarePlugin->guidedModeRTL(this);
}

void Vehicle::guidedModeLand(void)
{
    if (!guidedModeSupported()) {
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
        return;
    }
    _firmwarePlugin->guidedModeLand(this);
}

void Vehicle::guidedModeTakeoff(double altitudeRelative)
{
    if (!guidedModeSupported()) {
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
        return;
    }
    _firmwarePlugin->guidedModeTakeoff(this, altitudeRelative);
}

double Vehicle::minimumTakeoffAltitude(void)
{
    return _firmwarePlugin->minimumTakeoffAltitude(this);
}

void Vehicle::startMission(void)
{
    _firmwarePlugin->startMission(this);
}

void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
{
    if (!guidedModeSupported()) {
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
        return;
    }
    if (!coordinate().isValid()) {
        return;
    }
    double maxDistance = 1000.0;
    if (coordinate().distanceTo(gotoCoord) > maxDistance) {
        qgcApp()->showMessage(QString("New location is too far. Must be less than %1 %2").arg(qRound(FactMetaData::metersToAppSettingsDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsDistanceUnitsString()));
        return;
    }
    _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
}

void Vehicle::guidedModeChangeAltitude(double altitudeChange)
{
    if (!guidedModeSupported()) {
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
        return;
    }
    _firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange);
}

void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude)
{
    if (!orbitModeSupported()) {
        qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
        return;
    }

    double lat, lon, alt;
    if (centerCoord.isValid()) {
        lat = lon = alt = qQNaN();
    } else {
        lat = centerCoord.latitude();
        lon = centerCoord.longitude();
        alt = amslAltitude;
    }

    if (capabilityBits() && MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
        sendMavCommandInt(defaultComponentId(),
                          MAV_CMD_DO_ORBIT,
                          MAV_FRAME_GLOBAL,
                          true,            // show error if fails
                          radius,
                          qQNaN(),         // Use default velocity
                          0,               // Vehicle points to center
                          qQNaN(),         // reserved
                          lat, lon, alt);
    } else {
        sendMavCommand(defaultComponentId(),
                       MAV_CMD_DO_ORBIT,
                       true,            // show error if fails
                       radius,
                       qQNaN(),         // Use default velocity
                       0,               // Vehicle points to center
                       qQNaN(),         // reserved
                       lat, lon, alt);
    }
}

void Vehicle::pauseVehicle(void)
{
    if (!pauseVehicleSupported()) {
        qgcApp()->showMessage(QStringLiteral("Pause not supported by vehicle."));
        return;
    }
    _firmwarePlugin->pauseVehicle(this);
}

void Vehicle::abortLanding(double climbOutAltitude)
{
    sendMavCommand(defaultComponentId(),
                   MAV_CMD_DO_GO_AROUND,
                   true,        // show error if fails
                   climbOutAltitude);
}

bool Vehicle::guidedMode(void) const
{
    return _firmwarePlugin->isGuidedMode(this);
}

void Vehicle::setGuidedMode(bool guidedMode)
{
    return _firmwarePlugin->setGuidedMode(this, guidedMode);
}

void Vehicle::emergencyStop(void)
{
    sendMavCommand(_defaultComponentId,
                   MAV_CMD_COMPONENT_ARM_DISARM,
                   true,        // show error if fails
                   0.0f,
                   21196.0f);  // Magic number for emergency stop
}

void Vehicle::setCurrentMissionSequence(int seq)
{
    if (!_firmwarePlugin->sendHomePositionToVehicle()) {
        seq--;
    }
    mavlink_message_t msg;
    mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(),
                                              _mavlink->getComponentId(),
                                              priorityLink()->mavlinkChannel(),
                                              &msg,
                                              id(),
                                              _compID,
                                              seq);
    sendMessageOnLink(priorityLink(), msg);
}

void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
{
    MavCommandQueueEntry_t entry;

    entry.commandInt = false;
    entry.component = component;
    entry.command = command;
    entry.showError = showError;
    entry.rgParam[0] = param1;
    entry.rgParam[1] = param2;
    entry.rgParam[2] = param3;
    entry.rgParam[3] = param4;
    entry.rgParam[4] = param5;
    entry.rgParam[5] = param6;
    entry.rgParam[6] = param7;

    _mavCommandQueue.append(entry);

    if (_mavCommandQueue.count() == 1) {
        _mavCommandRetryCount = 0;
        _sendMavCommandAgain();
    }
}

void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7)
{
    MavCommandQueueEntry_t entry;

    entry.commandInt = true;
    entry.component = component;
    entry.command = command;
    entry.frame = frame;
    entry.showError = showError;
    entry.rgParam[0] = param1;
    entry.rgParam[1] = param2;
    entry.rgParam[2] = param3;
    entry.rgParam[3] = param4;
    entry.rgParam[4] = param5;
    entry.rgParam[5] = param6;
    entry.rgParam[6] = param7;

    _mavCommandQueue.append(entry);

    if (_mavCommandQueue.count() == 1) {
        _mavCommandRetryCount = 0;
        _sendMavCommandAgain();
    }
}

void Vehicle::_sendMavCommandAgain(void)
{
    if(!_mavCommandQueue.size()) {
        qWarning() << "Command resend with no commands in queue";
        _mavCommandAckTimer.stop();
        return;
    }

    MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];

    if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
        if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
            // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
            qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES. Setting no capabilities. Starting Plan request.";
            _setCapabilities(0);
            _startPlanRequest();
        }

        if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
            // We aren't going to get a response back for the protocol version, so assume v1 is all we can do.
            // If the max protocol version is uninitialized, fall back to v1.
            qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_PROTOCOL_VERSION. Starting Plan request.";
            if (_maxProtoVersion == 0) {
                qCDebug(VehicleLog) << "Setting _maxProtoVersion to 100 since not yet set.";
                _setMaxProtoVersion(100);
            } else {
                qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion;
            }
        }

        emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
        if (queuedCommand.showError) {
            qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(_toolbox->missionCommandTree()->friendlyName(queuedCommand.command)));
        }
        _mavCommandQueue.removeFirst();
        _sendNextQueuedMavCommand();
        return;
    }

    if (_mavCommandRetryCount > 1) {
        // We always let AUTOPILOT_CAPABILITIES go through multiple times even if we don't get acks. This is because
        // we really need to get capabilities and version info back over a lossy link.
        if (queuedCommand.command != MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
            if (px4Firmware()) {
                // Older PX4 firmwares are inconsistent with repect to sending back an Ack from a COMMAND_LONG, hence we can't support retry logic for it.
                if (_firmwareMajorVersion != versionNotSetValue) {
                    // If no version set assume lastest master dev build, so acks are suppored
                    if (_firmwareMajorVersion <= 1 && _firmwareMinorVersion <= 5 && _firmwarePatchVersion <= 3) {
                        // Acks not supported in this version
                        return;
                    }
                }
            } else {
                if (queuedCommand.command == MAV_CMD_START_RX_PAIR) {
                    // The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares
                    // we aren't really sure whether they are correct or not.
                    return;
                }
            }
        }
        qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
    }

    _mavCommandAckTimer.start();

    mavlink_message_t       msg;
    if (queuedCommand.commandInt) {
        mavlink_command_int_t  cmd;

        memset(&cmd, 0, sizeof(cmd));
        cmd.target_system =     _id;
        cmd.target_component =  queuedCommand.component;
        cmd.command =           queuedCommand.command;
        cmd.frame =             queuedCommand.frame;
        cmd.param1 =            queuedCommand.rgParam[0];
        cmd.param2 =            queuedCommand.rgParam[1];
        cmd.param3 =            queuedCommand.rgParam[2];
        cmd.param4 =            queuedCommand.rgParam[3];
        cmd.x =                 queuedCommand.rgParam[4] * qPow(10.0, 7.0);
        cmd.y =                 queuedCommand.rgParam[5] * qPow(10.0, 7.0);
        cmd.z =                 queuedCommand.rgParam[6];
        mavlink_msg_command_int_encode_chan(_mavlink->getSystemId(),
                                            _mavlink->getComponentId(),
                                            priorityLink()->mavlinkChannel(),
                                            &msg,
                                            &cmd);
    } else {
        mavlink_command_long_t  cmd;