Vehicle.cc

{
    if (!guidedModeSupported()) {
        qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
        return;
    }
    if (!coordinate().isValid()) {
        return;
    }
    double maxDistance = _settingsManager->flyViewSettings()->maxGoToLocationDistance()->rawValue().toDouble();
    if (coordinate().distanceTo(gotoCoord) > maxDistance) {
        qgcApp()->showAppMessage(QString("New location is too far. Must be less than %1 %2.").arg(qRound(FactMetaData::metersToAppSettingsHorizontalDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsHorizontalDistanceUnitsString()));
        return;
    }
    _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
}

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

void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude)
{
    if (!orbitModeSupported()) {
        qgcApp()->showAppMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
        return;
    }
    if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
        sendMavCommandInt(
                    defaultComponentId(),
                    MAV_CMD_DO_ORBIT,
                    MAV_FRAME_GLOBAL,
                    true,                           // show error if fails
                    static_cast<float>(radius),
                    static_cast<float>(qQNaN()),    // Use default velocity
                    0,                              // Vehicle points to center
                    static_cast<float>(qQNaN()),    // reserved
                    centerCoord.latitude(), centerCoord.longitude(), static_cast<float>(amslAltitude));
    } else {
        sendMavCommand(
                    defaultComponentId(),
                    MAV_CMD_DO_ORBIT,
                    true,                           // show error if fails
                    static_cast<float>(radius),
                    static_cast<float>(qQNaN()),    // Use default velocity
                    0,                              // Vehicle points to center
                    static_cast<float>(qQNaN()),    // reserved
                    static_cast<float>(centerCoord.latitude()),
                    static_cast<float>(centerCoord.longitude()),
                    static_cast<float>(amslAltitude));
    }
}

void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord)
{
    if (!roiModeSupported()) {
        qgcApp()->showAppMessage(QStringLiteral("ROI mode not supported by Vehicle."));
        return;
    }
    if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
        sendMavCommandInt(
                    defaultComponentId(),
                    MAV_CMD_DO_SET_ROI_LOCATION,
                    MAV_FRAME_GLOBAL,
                    true,                           // show error if fails
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    centerCoord.latitude(),
                    centerCoord.longitude(),
                    static_cast<float>(centerCoord.altitude()));
    } else {
        sendMavCommand(
                    defaultComponentId(),
                    MAV_CMD_DO_SET_ROI_LOCATION,
                    true,                           // show error if fails
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(centerCoord.latitude()),
                    static_cast<float>(centerCoord.longitude()),
                    static_cast<float>(centerCoord.altitude()));
    }
}

void Vehicle::stopGuidedModeROI()
{
    if (!roiModeSupported()) {
        qgcApp()->showAppMessage(QStringLiteral("ROI mode not supported by Vehicle."));
        return;
    }
    if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
        sendMavCommandInt(
                    defaultComponentId(),
                    MAV_CMD_DO_SET_ROI_NONE,
                    MAV_FRAME_GLOBAL,
                    true,                           // show error if fails
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<double>(qQNaN()),   // Empty
                    static_cast<double>(qQNaN()),   // Empty
                    static_cast<float>(qQNaN()));   // Empty
    } else {
        sendMavCommand(
                    defaultComponentId(),
                    MAV_CMD_DO_SET_ROI_NONE,
                    true,                           // show error if fails
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()),    // Empty
                    static_cast<float>(qQNaN()));   // Empty
    }
}

void Vehicle::pauseVehicle()
{
    if (!pauseVehicleSupported()) {
        qgcApp()->showAppMessage(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
                static_cast<float>(climbOutAltitude));
}

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

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

void Vehicle::emergencyStop()
{
    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(
                static_cast<uint8_t>(_mavlink->getSystemId()),
                static_cast<uint8_t>(_mavlink->getComponentId()),
                priorityLink()->mavlinkChannel(),
                &msg,
                static_cast<uint8_t>(id()),
                _compID,
                static_cast<uint16_t>(seq));
    sendMessageOnLinkThreadSafe(priorityLink(), msg);
}

void Vehicle::sendMavCommand(int compId, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
{
    _sendMavCommandWorker(false,            // commandInt
                          false,            // requestMessage
                          showError,
                          nullptr,          // resultHandler
                          nullptr,          // resultHandlerData
                          compId,
                          command,
                          MAV_FRAME_GLOBAL,
                          param1, param2, param3, param4, param5, param6, param7);
}

void Vehicle::sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void *resultHandlerData, int compId, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
{
    _sendMavCommandWorker(false,                // commandInt
                          false,                // requestMessage,
                          false,                // showError
                          resultHandler,
                          resultHandlerData,
                          compId,
                          command,
                          MAV_FRAME_GLOBAL,
                          param1, param2, param3, param4, param5, param6, param7);
}

void Vehicle::sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7)
{
    _sendMavCommandWorker(true,         // commandInt
                          false,        // requestMessage
                          showError,
                          nullptr,      // resultHandler
                          nullptr,      // resultHandlerData
                          compId,
                          command,
                          frame,
                          param1, param2, param3, param4, param5, param6, param7);
}

void Vehicle::_sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
{
    MavCommandQueueEntry_t entry;

    entry.commandInt        = commandInt;
    entry.compId            = compId;
    entry.command           = command;
    entry.frame             = frame;
    entry.showError         = showError;
    entry.requestMessage    = requestMessage;
    entry.resultHandler     = resultHandler;
    entry.resultHandlerData = resultHandlerData;
    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.enqueue(entry);

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

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

    MavCommandQueueEntry_t& queuedCommand   = _mavCommandQueue[0];
    QString                 rawCommandName  =_toolbox->missionCommandTree()->rawName(queuedCommand.command);

    if (_mavCommandRetryCount++ >= _mavCommandMaxRetryCount) {
        if (queuedCommand.resultHandler) {
            (*queuedCommand.resultHandler)(queuedCommand.resultHandlerData, queuedCommand.compId, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
        } else {
            emit mavCommandResult(_id, queuedCommand.compId, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
        }
        if (queuedCommand.showError) {
            qgcApp()->showAppMessage(tr("Vehicle did not respond to command: %1").arg(rawCommandName));
        }
        _mavCommandQueue.dequeue();
        _sendNextQueuedMavCommand();
        return;
    }

    if (_mavCommandRetryCount > 1) {
        if (!px4Firmware() && 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" << rawCommandName << _mavCommandRetryCount;
    }

    _mavCommandAckTimer.start();

    if (queuedCommand.requestMessage) {
        RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(queuedCommand.resultHandlerData);
        _waitForMavlinkMessage(_requestMessageWaitForMessageResultHandler, pInfo, pInfo->msgId, 1000);
    }

    qCDebug(VehicleLog) << "_sendMavCommandAgain sending name:retry" << rawCommandName << _mavCommandRetryCount;

    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.compId;
        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;

        memset(&cmd, 0, sizeof(cmd));
        cmd.target_system =     _id;
        cmd.target_component =  queuedCommand.compId;
        cmd.command =           queuedCommand.command;
        cmd.confirmation =      0;
        cmd.param1 =            queuedCommand.rgParam[0];
        cmd.param2 =            queuedCommand.rgParam[1];
        cmd.param3 =            queuedCommand.rgParam[2];
        cmd.param4 =            queuedCommand.rgParam[3];
        cmd.param5 =            queuedCommand.rgParam[4];
        cmd.param6 =            queuedCommand.rgParam[5];
        cmd.param7 =            queuedCommand.rgParam[6];
        mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                             _mavlink->getComponentId(),
                                             priorityLink()->mavlinkChannel(),
                                             &msg,
                                             &cmd);
    }

    sendMessageOnLinkThreadSafe(priorityLink(), msg);
}

void Vehicle::_sendNextQueuedMavCommand()
{
    if (!_mavCommandQueue.isEmpty()) {
        _mavCommandRetryCount = 0;
        _sendMavCommandAgain();
    }
}

void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
    mavlink_command_ack_t ack;
    mavlink_msg_command_ack_decode(&message, &ack);

    QString rawCommandName  =_toolbox->missionCommandTree()->rawName(static_cast<MAV_CMD>(ack.command));
    qCDebug(VehicleLog) << QStringLiteral("_handleCommandAck command(%1) result(%2)").arg(rawCommandName).arg(QGCMAVLink::mavResultToString(static_cast<MAV_RESULT>(ack.result)));

    if (ack.command == MAV_CMD_DO_SET_ROI_LOCATION) {
        if (ack.result == MAV_RESULT_ACCEPTED) {
            _isROIEnabled = true;
            emit isROIEnabledChanged();
        }
    }

    if (ack.command == MAV_CMD_DO_SET_ROI_NONE) {
        if (ack.result == MAV_RESULT_ACCEPTED) {
            _isROIEnabled = false;
            emit isROIEnabledChanged();
        }
    }

#if !defined(NO_ARDUPILOT_DIALECT)
    if (ack.command == MAV_CMD_FLASH_BOOTLOADER && ack.result == MAV_RESULT_ACCEPTED) {
        qgcApp()->showAppMessage(tr("Bootloader flash succeeded"));
    }
#endif

    if (!_mavCommandQueue.isEmpty() && ack.command == _mavCommandQueue.head().command) {
        bool                    sendNextCommand = false;
        MavCommandQueueEntry_t  commandEntry = _mavCommandQueue.head();
        _mavCommandAckTimer.stop();

        if (commandEntry.requestMessage) {
            RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(commandEntry.resultHandlerData);
            pInfo->commandAckReceived = true;
            if (ack.result == MAV_RESULT_ACCEPTED) {
                if (pInfo->messageReceived) {
                    delete pInfo;
                    sendNextCommand = true;
                } else {
                    // We dont set sendNextCommand because we wait for the message wait to complete before sending next message
                    _waitForMavlinkMessageTimeoutActive = true;
                    _waitForMavlinkMessageElapsed.restart();
                }
            } else {
                sendNextCommand = true;
                if (pInfo->messageReceived) {
                    qCWarning(VehicleLog) << "Internal Error: _handleCommandAck for requestMessage with result failure, but message already received";
                } else {
                    _waitForMavlinkMessageClear();
                    (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), false /* noResponsefromVehicle */);
                }
            }
        } else {
            if (commandEntry.resultHandler) {
                (*commandEntry.resultHandler)(commandEntry.resultHandlerData, message.compid, static_cast<MAV_RESULT>(ack.result), false /* noResponsefromVehicle */);
            } else {
                if (commandEntry.showError) {
                    switch (ack.result) {
                    case MAV_RESULT_TEMPORARILY_REJECTED:
                        qgcApp()->showAppMessage(tr("%1 command temporarily rejected").arg(rawCommandName));
                        break;
                    case MAV_RESULT_DENIED:
                        qgcApp()->showAppMessage(tr("%1 command denied").arg(rawCommandName));
                        break;
                    case MAV_RESULT_UNSUPPORTED:
                        qgcApp()->showAppMessage(tr("%1 command not supported").arg(rawCommandName));
                        break;
                    case MAV_RESULT_FAILED:
                        qgcApp()->showAppMessage(tr("%1 command failed").arg(rawCommandName));
                        break;
                    default:
                        // Do nothing
                        break;
                    }
                }
                emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
            }
            sendNextCommand = true;
        }

        _mavCommandQueue.dequeue();
        if (sendNextCommand) {
            _sendNextQueuedMavCommand();
        }
    } else {
        qCDebug(VehicleLog) << "_handleCommandAck Ack not in queue" << rawCommandName;
    }
}

void Vehicle::_waitForMavlinkMessage(WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs)
{
    qCDebug(VehicleLog) << "_waitForMavlinkMessage msg:timeout" << messageId << timeoutMsecs;
    _waitForMavlinkMessageResultHandler     = resultHandler;
    _waitForMavlinkMessageResultHandlerData = resultHandlerData;
    _waitForMavlinkMessageId                = messageId;
    _waitForMavlinkMessageTimeoutActive     = false;
    _waitForMavlinkMessageTimeoutMsecs      = timeoutMsecs;
}

void Vehicle::_waitForMavlinkMessageClear(void)
{
    qCDebug(VehicleLog) << "_waitForMavlinkMessageClear";
    _waitForMavlinkMessageResultHandler     = nullptr;
    _waitForMavlinkMessageResultHandlerData = nullptr;
    _waitForMavlinkMessageId                = 0;
}

void Vehicle::_waitForMavlinkMessageMessageReceived(const mavlink_message_t& message)
{
    if (_waitForMavlinkMessageId != 0) {
        if (_waitForMavlinkMessageId == message.msgid) {
            WaitForMavlinkMessageResultHandler  resultHandler       = _waitForMavlinkMessageResultHandler;
            void*                               resultHandlerData   = _waitForMavlinkMessageResultHandlerData;
            qCDebug(VehicleLog) << "_waitForMavlinkMessageMessageReceived message received" << _waitForMavlinkMessageId;
            _waitForMavlinkMessageClear();
            (*resultHandler)(resultHandlerData, false /* noResponseFromVehicle */, message);
        } else if (_waitForMavlinkMessageTimeoutActive && _waitForMavlinkMessageElapsed.elapsed() > _waitForMavlinkMessageTimeoutMsecs) {
            WaitForMavlinkMessageResultHandler  resultHandler       = _waitForMavlinkMessageResultHandler;
            void*                               resultHandlerData   = _waitForMavlinkMessageResultHandlerData;
            qCDebug(VehicleLog) << "_waitForMavlinkMessageMessageReceived message timed out" << _waitForMavlinkMessageId;
            _waitForMavlinkMessageClear();
            (*resultHandler)(resultHandlerData, true /* noResponseFromVehicle */, message);
        }
    }
}

void Vehicle::requestMessage(RequestMessageResultHandler resultHandler, void* resultHandlerData, int compId, int messageId, float param1, float param2, float param3, float param4, float param5)
{
    RequestMessageInfo_t* pInfo = new RequestMessageInfo_t;

    *pInfo                      = { };
    pInfo->msgId                = messageId;
    pInfo->compId               = compId;
    pInfo->resultHandler        = resultHandler;
    pInfo->resultHandlerData    = resultHandlerData;

    _sendMavCommandWorker(false,                                    // commandInt
                          true,                                     // requestMessage,
                          false,                                    // showError
                          _requestMessageCmdResultHandler,
                          pInfo,
                          compId,
                          MAV_CMD_REQUEST_MESSAGE,
                          MAV_FRAME_GLOBAL,
                          messageId,
                          param1, param2, param3, param4, param5, 0);
}

void Vehicle::_requestMessageCmdResultHandler(void* resultHandlerData, int /*compId*/, MAV_RESULT result, bool noResponsefromVehicle)
{
    RequestMessageInfo_t* pInfo   = static_cast<RequestMessageInfo_t*>(resultHandlerData);

    pInfo->commandAckReceived = true;
    if (result != MAV_RESULT_ACCEPTED) {
        mavlink_message_t message;
        (*pInfo->resultHandler)(pInfo->resultHandlerData, result,  noResponsefromVehicle ? RequestMessageFailureCommandNotAcked : RequestMessageFailureCommandError, message);
    }
    if (pInfo->messageReceived) {
        delete pInfo;
    }
}

void Vehicle::_requestMessageWaitForMessageResultHandler(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message)
{
    RequestMessageInfo_t* pInfo = static_cast<RequestMessageInfo_t*>(resultHandlerData);

    pInfo->messageReceived = true;
    (*pInfo->resultHandler)(pInfo->resultHandlerData, noResponsefromVehicle ? MAV_RESULT_FAILED : MAV_RESULT_ACCEPTED, noResponsefromVehicle ? RequestMessageFailureMessageNotReceived : RequestMessageNoFailure, message);
}

void Vehicle::setPrearmError(const QString& prearmError)
{
    _prearmError = prearmError;
    emit prearmErrorChanged(_prearmError);
    if (!_prearmError.isEmpty()) {
        _prearmErrorTimer.start();
    }
}

void Vehicle::_prearmErrorTimeout()
{
    setPrearmError(QString());
}

void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType)
{
    _firmwareMajorVersion = majorVersion;
    _firmwareMinorVersion = minorVersion;
    _firmwarePatchVersion = patchVersion;
    _firmwareVersionType = versionType;
    emit firmwareVersionChanged();
}

void Vehicle::setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion)
{
    _firmwareCustomMajorVersion = majorVersion;
    _firmwareCustomMinorVersion = minorVersion;
    _firmwareCustomPatchVersion = patchVersion;
    emit firmwareCustomVersionChanged();
}

QString Vehicle::firmwareVersionTypeString() const
{
    switch (_firmwareVersionType) {
    case FIRMWARE_VERSION_TYPE_DEV:
        return QStringLiteral("dev");
    case FIRMWARE_VERSION_TYPE_ALPHA:
        return QStringLiteral("alpha");
    case FIRMWARE_VERSION_TYPE_BETA:
        return QStringLiteral("beta");
    case FIRMWARE_VERSION_TYPE_RC:
        return QStringLiteral("rc");
    case FIRMWARE_VERSION_TYPE_OFFICIAL:
    default:
        return QStringLiteral("");
    }
}

void Vehicle::rebootVehicle()
{
    _autoDisconnect = true;

    mavlink_message_t       msg;
    mavlink_command_long_t  cmd;

    memset(&cmd, 0, sizeof(cmd));
    cmd.target_system =     _id;
    cmd.target_component =  _defaultComponentId;
    cmd.command =           MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN;
    cmd.confirmation =      0;
    cmd.param1 =            1;
    cmd.param2 = cmd.param3 = cmd.param4 = cmd.param5 = cmd.param6 = cmd.param7 = 0;
    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                         _mavlink->getComponentId(),
                                         priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
    sendMessageOnLinkThreadSafe(priorityLink(), msg);
}

void Vehicle::setSoloFirmware(bool soloFirmware)
{
    if (soloFirmware != _soloFirmware) {
        _soloFirmware = soloFirmware;
        emit soloFirmwareChanged(soloFirmware);
    }
}

void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
{
    sendMavCommand(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, true, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs, 0, MOTOR_TEST_ORDER_BOARD);
}

QString Vehicle::brandImageIndoor() const
{
    return _firmwarePlugin->brandImageIndoor(this);
}

QString Vehicle::brandImageOutdoor() const
{
    return _firmwarePlugin->brandImageOutdoor(this);
}

void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId)
{
    if (_offlineEditingVehicle) {
        _defaultComponentId = defaultComponentId;
    } else {
        qWarning() << "Call to Vehicle::setOfflineEditingDefaultComponentId on vehicle which is not offline";
    }
}

void Vehicle::setVtolInFwdFlight(bool vtolInFwdFlight)
{
    if (_vtolInFwdFlight != vtolInFwdFlight) {
        sendMavCommand(_defaultComponentId,
                       MAV_CMD_DO_VTOL_TRANSITION,
                       true,                                                    // show errors
                       vtolInFwdFlight ? MAV_VTOL_STATE_FW : MAV_VTOL_STATE_MC, // transition state
                       0, 0, 0, 0, 0, 0);                                       // param 2-7 unused
    }
}

const char* VehicleGPSFactGroup::_latFactName =                 "lat";
const char* VehicleGPSFactGroup::_lonFactName =                 "lon";
const char* VehicleGPSFactGroup::_mgrsFactName =                "mgrs";
const char* VehicleGPSFactGroup::_hdopFactName =                "hdop";
const char* VehicleGPSFactGroup::_vdopFactName =                "vdop";
const char* VehicleGPSFactGroup::_courseOverGroundFactName =    "courseOverGround";
const char* VehicleGPSFactGroup::_countFactName =               "count";
const char* VehicleGPSFactGroup::_lockFactName =                "lock";

VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent)
    , _latFact              (0, _latFactName,               FactMetaData::valueTypeDouble)
    , _lonFact              (0, _lonFactName,               FactMetaData::valueTypeDouble)
    , _mgrsFact             (0, _mgrsFactName,              FactMetaData::valueTypeString)
    , _hdopFact             (0, _hdopFactName,              FactMetaData::valueTypeDouble)
    , _vdopFact             (0, _vdopFactName,              FactMetaData::valueTypeDouble)
    , _courseOverGroundFact (0, _courseOverGroundFactName,  FactMetaData::valueTypeDouble)
    , _countFact            (0, _countFactName,             FactMetaData::valueTypeInt32)
    , _lockFact             (0, _lockFactName,              FactMetaData::valueTypeInt32)
{
    _addFact(&_latFact,                 _latFactName);
    _addFact(&_lonFact,                 _lonFactName);
    _addFact(&_mgrsFact,                _mgrsFactName);
    _addFact(&_hdopFact,                _hdopFactName);
    _addFact(&_vdopFact,                _vdopFactName);
    _addFact(&_courseOverGroundFact,    _courseOverGroundFactName);
    _addFact(&_lockFact,                _lockFactName);
    _addFact(&_countFact,               _countFactName);

    _latFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _lonFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _mgrsFact.setRawValue("");
    _hdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _vdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _courseOverGroundFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}

void Vehicle::startMavlinkLog()
{
    sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_START, false /* showError */);
}

void Vehicle::stopMavlinkLog()
{
    sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_STOP, false /* showError */);
}

void Vehicle::_ackMavlinkLogData(uint16_t sequence)
{
    mavlink_message_t msg;
    mavlink_logging_ack_t ack;
    memset(&ack, 0, sizeof(ack));
    ack.sequence = sequence;
    ack.target_component = _defaultComponentId;
    ack.target_system = id();
    mavlink_msg_logging_ack_encode_chan(
                _mavlink->getSystemId(),
                _mavlink->getComponentId(),
                priorityLink()->mavlinkChannel(),
                &msg,
                &ack);
    sendMessageOnLinkThreadSafe(priorityLink(), msg);
}

void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
{
    mavlink_logging_data_t log;
    mavlink_msg_logging_data_decode(&message, &log);
    emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
                        log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
}

void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
{
    mavlink_logging_data_acked_t log;
    mavlink_msg_logging_data_acked_decode(&message, &log);
    _ackMavlinkLogData(log.sequence);
    emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
                        log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
}

void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
{
    firmwarePluginInstanceData->setParent(this);
    _firmwarePluginInstanceData = firmwarePluginInstanceData;
}

QString Vehicle::missionFlightMode() const
{
    return _firmwarePlugin->missionFlightMode();
}

QString Vehicle::pauseFlightMode() const
{
    return _firmwarePlugin->pauseFlightMode();
}

QString Vehicle::rtlFlightMode() const
{
    return _firmwarePlugin->rtlFlightMode();
}

QString Vehicle::smartRTLFlightMode() const
{
    return _firmwarePlugin->smartRTLFlightMode();
}

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

QString Vehicle::landFlightMode() const
{
    return _firmwarePlugin->landFlightMode();
}

QString Vehicle::takeControlFlightMode() const
{
    return _firmwarePlugin->takeControlFlightMode();
}

QString Vehicle::followFlightMode() const
{
    return _firmwarePlugin->followFlightMode();
}

QString Vehicle::vehicleImageOpaque() const
{
    if(_firmwarePlugin)
        return _firmwarePlugin->vehicleImageOpaque(this);
    else
        return QString();
}

QString Vehicle::vehicleImageOutline() const
{
    if(_firmwarePlugin)
        return _firmwarePlugin->vehicleImageOutline(this);
    else
        return QString();
}

QString Vehicle::vehicleImageCompass() const
{
    if(_firmwarePlugin)
        return _firmwarePlugin->vehicleImageCompass(this);
    else
        return QString();
}

const QVariantList& Vehicle::toolIndicators()
{
    if(_firmwarePlugin) {
        return _firmwarePlugin->toolIndicators(this);
    }
    static QVariantList emptyList;
    return emptyList;
}

const QVariantList& Vehicle::modeIndicators()
{
    if(_firmwarePlugin) {
        return _firmwarePlugin->modeIndicators(this);
    }
    static QVariantList emptyList;
    return emptyList;
}

const QVariantList& Vehicle::staticCameraList() const
{
    if (_firmwarePlugin) {
        return _firmwarePlugin->cameraList(this);
    }
    static QVariantList emptyList;
    return emptyList;
}

void Vehicle::_setupAutoDisarmSignalling()
{
    QString param = _firmwarePlugin->autoDisarmParameter(this);

    if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
        Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
        connect(fact, &Fact::rawValueChanged, this, &Vehicle::autoDisarmChanged);
        emit autoDisarmChanged();
    }
}

bool Vehicle::autoDisarm()
{
    QString param = _firmwarePlugin->autoDisarmParameter(this);

    if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
        Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
        return fact->rawValue().toDouble() > 0;
    }

    return false;
}

void Vehicle::_handleADSBVehicle(const mavlink_message_t& message)
{
    mavlink_adsb_vehicle_t adsbVehicleMsg;
    static const int maxTimeSinceLastSeen = 15;

    mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicleMsg);
    if (adsbVehicleMsg.flags | ADSB_FLAGS_VALID_COORDS && adsbVehicleMsg.tslc <= maxTimeSinceLastSeen) {
        ADSBVehicle::VehicleInfo_t vehicleInfo;

        vehicleInfo.availableFlags = 0;
        vehicleInfo.icaoAddress = adsbVehicleMsg.ICAO_address;

        vehicleInfo.location.setLatitude(adsbVehicleMsg.lat / 1e7);
        vehicleInfo.location.setLongitude(adsbVehicleMsg.lon / 1e7);
        vehicleInfo.availableFlags |= ADSBVehicle::LocationAvailable;

        vehicleInfo.callsign = adsbVehicleMsg.callsign;
        vehicleInfo.availableFlags |= ADSBVehicle::CallsignAvailable;

        if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_ALTITUDE) {
            vehicleInfo.altitude = (double)adsbVehicleMsg.altitude / 1e3;
            vehicleInfo.availableFlags |= ADSBVehicle::AltitudeAvailable;
        }

        if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_HEADING) {
            vehicleInfo.heading = (double)adsbVehicleMsg.heading / 100.0;
            vehicleInfo.availableFlags |= ADSBVehicle::HeadingAvailable;
        }

        _toolbox->adsbVehicleManager()->adsbVehicleUpdate(vehicleInfo);
    }
}

void Vehicle::_updateDistanceHeadingToHome()
{
    if (coordinate().isValid() && homePosition().isValid()) {
        _distanceToHomeFact.setRawValue(coordinate().distanceTo(homePosition()));
        if (_distanceToHomeFact.rawValue().toDouble() > 1.0) {
            _headingToHomeFact.setRawValue(coordinate().azimuthTo(homePosition()));
        } else {
            _headingToHomeFact.setRawValue(qQNaN());
        }
    } else {
        _distanceToHomeFact.setRawValue(qQNaN());
        _headingToHomeFact.setRawValue(qQNaN());
    }
}

void Vehicle::_updateHeadingToNextWP()
{
    const int currentIndex = _missionManager->currentIndex();
    QList<MissionItem*> llist = _missionManager->missionItems();

    if(llist.size()>currentIndex && currentIndex!=-1
            && llist[currentIndex]->coordinate().longitude()!=0.0
            && coordinate().distanceTo(llist[currentIndex]->coordinate())>5.0 ){

        _headingToNextWPFact.setRawValue(coordinate().azimuthTo(llist[currentIndex]->coordinate()));
    }
    else{
        _headingToNextWPFact.setRawValue(qQNaN());
    }
}

void Vehicle::_updateMissionItemIndex()
{
    const int currentIndex = _missionManager->currentIndex();

    unsigned offset = 0;
    if (!_firmwarePlugin->sendHomePositionToVehicle()) {
        offset = 1;
    }

    _missionItemIndexFact.setRawValue(currentIndex + offset);
}

void Vehicle::_updateDistanceToGCS()
{
    QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition();
    if (coordinate().isValid() && gcsPosition.isValid()) {
        _distanceToGCSFact.setRawValue(coordinate().distanceTo(gcsPosition));
    } else {
        _distanceToGCSFact.setRawValue(qQNaN());
    }
}

void Vehicle::_updateHobbsMeter()
{
    _hobbsFact.setRawValue(hobbsMeter());
}

void Vehicle::forceInitialPlanRequestComplete()
{
    _initialPlanRequestComplete = true;
    emit initialPlanRequestCompleteChanged(true);
}

void Vehicle::sendPlan(QString planFile)
{
    PlanMasterController::sendPlanToVehicle(this, planFile);
}

QString Vehicle::hobbsMeter()
{
    static const char* HOOBS_HI = "LND_FLIGHT_T_HI";
    static const char* HOOBS_LO = "LND_FLIGHT_T_LO";
    //-- TODO: Does this exist on non PX4?
    if (_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_HI) &&
            _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) {
        Fact* factHi = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_HI);
        Fact* factLo = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_LO);
        uint64_t hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000;
        int hours   = hobbsTimeSeconds / 3600;
        int minutes = (hobbsTimeSeconds % 3600) / 60;
        int seconds = hobbsTimeSeconds % 60;
        QString timeStr = QString::asprintf("%04d:%02d:%02d", hours, minutes, seconds);
        qCDebug(VehicleLog) << "Hobbs Meter:" << timeStr << "(" << factHi->rawValue().toUInt() << factLo->rawValue().toUInt() << ")";
        return timeStr;
    }
    return QString("0000:00:00");
}

void Vehicle::_vehicleParamLoaded(bool ready)
{
    //-- TODO: This seems silly but can you think of a better
    //   way to update this?
    if(ready) {
        emit hobbsMeterChanged();
    }
}

void Vehicle::_updateHighLatencyLink(bool sendCommand)
{
    if (!_priorityLink) {
        return;
    }

    if (_priorityLink->highLatency() != _highLatencyLink) {
        _highLatencyLink = _priorityLink->highLatency();
        _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
        emit highLatencyLinkChanged(_highLatencyLink);

        if (sendCommand) {
            sendMavCommand(defaultComponentId(),
                           MAV_CMD_CONTROL_HIGH_LATENCY,
                           true,
                           _highLatencyLink ? 1.0f : 0.0f); // request start/stop transmitting over high latency telemetry
        }
    }
}

void Vehicle::_trafficUpdate(bool /*alert*/, QString /*traffic_id*/, QString /*vehicle_id*/, QGeoCoordinate /*location*/, float /*heading*/)
{
#if 0
    // This is ifdef'ed out for now since this code doesn't mesh with the recent ADSB manager changes. Also airmap isn't in any
    // released build. So not going to waste time trying to fix up unused code.
    if (_trafficVehicleMap.contains(traffic_id)) {
        _trafficVehicleMap[traffic_id]->update(alert, location, heading);
    } else {
        ADSBVehicle* vehicle = new ADSBVehicle(location, heading, alert, this);
        _trafficVehicleMap[traffic_id] = vehicle;
        _adsbVehicles.append(vehicle);
    }
#endif
}

void Vehicle::_mavlinkMessageStatus(int uasId, uint64_t totalSent, uint64_t totalReceived, uint64_t totalLoss, float lossPercent)
{
    if(uasId == _id) {
        _mavlinkSentCount       = totalSent;