APMFirmwarePlugin.cc 40.8 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
Don Gagne's avatar
Don Gagne committed
9

Don Gagne's avatar
Don Gagne committed
10
#include "APMFirmwarePlugin.h"
11
#include "APMAutoPilotPlugin.h"
12
#include "QGCMAVLink.h"
13
#include "QGCApplication.h"
14 15 16
#include "APMFlightModesComponentController.h"
#include "APMAirframeComponentController.h"
#include "APMSensorsComponentController.h"
17
#include "MissionManager.h"
18
#include "ParameterManager.h"
19
#include "QGCFileDownload.h"
20 21
#include "SettingsManager.h"
#include "AppSettings.h"
22
#include "APMMavlinkStreamRateSettings.h"
23 24 25 26
#include "ArduPlaneFirmwarePlugin.h"
#include "ArduCopterFirmwarePlugin.h"
#include "ArduRoverFirmwarePlugin.h"
#include "ArduSubFirmwarePlugin.h"
Don Gagne's avatar
Don Gagne committed
27

Don Gagne's avatar
Don Gagne committed
28 29
#include <QTcpSocket>

Pritam Ghanghas's avatar
Pritam Ghanghas committed
30
QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")
Don Gagne's avatar
Don Gagne committed
31

32
static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)");
33
static const QRegExp APM_SOLO_REXP("^(APM:Copter solo-)");
34 35
static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)");
static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)");
36
static const QRegExp APM_SUB_REXP("^(ArduSub|APM:Sub)");
37 38 39
static const QRegExp APM_PX4NUTTX_REXP("^PX4: .*NuttX: .*");
static const QRegExp APM_FRAME_REXP("^Frame: ");
static const QRegExp APM_SYSID_REXP("^PX4v2 ");
40 41

// Regex to parse version text coming from APM, gives out firmware type, major, minor and patch level numbers
42
static const QRegExp VERSION_REXP("^(APM:Copter|APM:Plane|APM:Rover|APM:Sub|ArduCopter|ArduPlane|ArduRover|ArduSub) +[vV](\\d*)\\.*(\\d*)*\\.*(\\d*)*");
43 44 45

// minimum firmware versions that don't suffer from mavlink severity inversion bug.
// https://github.com/diydrones/apm_planner/issues/788
46
static const QString MIN_SOLO_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter solo-1.2.0");
47 48
static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V3.4.0");
static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0");
49
static const QString MIN_SUB_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Sub V3.4.0");
50 51
static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0");

Don Gagne's avatar
Don Gagne committed
52 53
const char* APMFirmwarePlugin::_artooIP =                   "10.1.1.1"; ///< IP address of ARTOO controller
const int   APMFirmwarePlugin::_artooVideoHandshakePort =   5502;       ///< Port for video handshake on ARTOO controller
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100

/*
 * @brief APMFirmwareVersion is a small class to represent the firmware version
 * It encabsules vehicleType, major version, minor version and patch level version
 * and provides accessors for the same.
 * isValid() can be used, to know whether version infromation is available or not
 * supports < operator
 */
APMFirmwareVersion::APMFirmwareVersion(const QString &versionText)
{
    _major         = 0;
    _minor         = 0;
    _patch         = 0;

    _parseVersion(versionText);
}

bool APMFirmwareVersion::isValid() const
{
    return !_versionString.isEmpty();
}

bool APMFirmwareVersion::isBeta() const
{
    return _versionString.contains(QStringLiteral(".rc"));
}

bool APMFirmwareVersion::isDev() const
{
    return _versionString.contains(QStringLiteral(".dev"));
}

bool APMFirmwareVersion::operator <(const APMFirmwareVersion& other) const
{
    int myVersion = _major << 16 | _minor << 8 | _patch ;
    int otherVersion = other.majorNumber() << 16 | other.minorNumber() << 8 | other.patchNumber();
    return myVersion < otherVersion;
}

void APMFirmwareVersion::_parseVersion(const QString &versionText)
{
    if (versionText.isEmpty()) {
        return;
    }


    if (VERSION_REXP.indexIn(versionText) == -1) {
Pritam Ghanghas's avatar
Pritam Ghanghas committed
101 102
        qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything"
                                        << "version text to be parsed" << versionText;
103 104 105 106 107 108
        return;
    }

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

    if (capturedTexts.count() < 5) {
Pritam Ghanghas's avatar
Pritam Ghanghas committed
109 110
        qCWarning(APMFirmwarePluginLog) << "something wrong with parsing the version text, not hitting anything"
                                        << VERSION_REXP.captureCount() << VERSION_REXP.capturedTexts();
111 112 113 114 115 116 117 118 119 120 121 122 123
        return;
    }

    // successful extraction of version numbers
    // even though we could have collected the version string atleast
    // but if the parsing has faild, not much point
    _versionString = versionText;
    _vehicleType   = capturedTexts[1];
    _major         = capturedTexts[2].toInt();
    _minor         = capturedTexts[3].toInt();
    _patch         = capturedTexts[4].toInt();
}

124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147

/*
 * @brief APMCustomMode encapsulates the custom modes for APM
 */
APMCustomMode::APMCustomMode(uint32_t mode, bool settable) :
    _mode(mode),
    _settable(settable)
{
}

void APMCustomMode::setEnumToStringMapping(const QMap<uint32_t, QString>& enumToString)
{
    _enumToString = enumToString;
}

QString APMCustomMode::modeString() const
{
    QString mode = _enumToString.value(modeAsInt());
    if (mode.isEmpty()) {
        mode = "mode" + QString::number(modeAsInt());
    }
    return mode;
}

148 149 150 151 152 153 154
APMFirmwarePluginInstanceData::APMFirmwarePluginInstanceData(QObject* parent)
    : QObject(parent)
    , textSeverityAdjustmentNeeded(false)
{

}

155
APMFirmwarePlugin::APMFirmwarePlugin(void)
Don Gagne's avatar
Don Gagne committed
156
    : _coaxialMotors(false)
Don Gagne's avatar
Don Gagne committed
157
{
158 159 160 161
    qmlRegisterType<APMFlightModesComponentController>  ("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController");
    qmlRegisterType<APMAirframeComponentController>     ("QGroundControl.Controllers", 1, 0, "APMAirframeComponentController");
    qmlRegisterType<APMSensorsComponentController>      ("QGroundControl.Controllers", 1, 0, "APMSensorsComponentController");
}
Don Gagne's avatar
Don Gagne committed
162

163 164 165
AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
{
    return new APMAutoPilotPlugin(vehicle, vehicle);
Don Gagne's avatar
Don Gagne committed
166 167
}

168
bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
Don Gagne's avatar
Don Gagne committed
169
{
170 171 172
    uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
    if (vehicle->multiRotor()) {
        available |= TakeoffVehicleCapability;
173
    } else if (vehicle->vtol()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
174
        available |= TakeoffVehicleCapability;
175
    }
176

177
    return (capabilities & available) == capabilities;
Don Gagne's avatar
Don Gagne committed
178 179 180 181 182
}

QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);
183

Don Gagne's avatar
Don Gagne committed
184 185 186
    return QList<VehicleComponent*>();
}

Daniel Agar's avatar
Daniel Agar committed
187
QStringList APMFirmwarePlugin::flightModes(Vehicle* vehicle)
188
{
Daniel Agar's avatar
Daniel Agar committed
189
    Q_UNUSED(vehicle)
190 191 192 193 194 195 196
    QStringList flightModesList;
    foreach (const APMCustomMode& customMode, _supportedModes) {
        if (customMode.canBeSet()) {
            flightModesList << customMode.modeString();
        }
    }
    return flightModesList;
Don Gagne's avatar
Don Gagne committed
197 198
}

Don Gagne's avatar
Don Gagne committed
199
QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
200
{
201 202 203 204 205 206 207 208 209 210
    QString flightMode = "Unknown";

    if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        foreach (const APMCustomMode& customMode, _supportedModes) {
            if (customMode.modeAsInt() == custom_mode) {
                flightMode = customMode.modeString();
            }
        }
    }
    return flightMode;
Don Gagne's avatar
Don Gagne committed
211 212 213 214
}

bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233
    *base_mode = 0;
    *custom_mode = 0;

    bool found = false;

    foreach(const APMCustomMode& mode, _supportedModes) {
        if (flightMode.compare(mode.modeString(), Qt::CaseInsensitive) == 0) {
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = mode.modeAsInt();
            found = true;
            break;
        }
    }

    if (!found) {
        qCWarning(APMFirmwarePluginLog) << "Unknown flight Mode" << flightMode;
    }

    return found;
Don Gagne's avatar
Don Gagne committed
234 235
}

236
void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
237
{
Don Gagne's avatar
Don Gagne committed
238 239 240 241 242
    Q_UNUSED(vehicle);

    mavlink_param_value_t paramValue;
    mavlink_param_union_t paramUnion;

Don Gagne's avatar
Don Gagne committed
243 244
    memset(&paramValue, 0, sizeof(paramValue));

Don Gagne's avatar
Don Gagne committed
245 246 247 248 249 250 251
    // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
    // type they are. Fix that up to correct usage.

    mavlink_msg_param_value_decode(message, &paramValue);

    switch (paramValue.param_type) {
    case MAV_PARAM_TYPE_UINT8:
252
        paramUnion.param_uint8 = static_cast<uint8_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
253 254
        break;
    case MAV_PARAM_TYPE_INT8:
255
        paramUnion.param_int8  = static_cast<int8_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
256 257
        break;
    case MAV_PARAM_TYPE_UINT16:
258
        paramUnion.param_uint16 = static_cast<uint16_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
259 260
        break;
    case MAV_PARAM_TYPE_INT16:
261
        paramUnion.param_int16 = static_cast<int16_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
262 263
        break;
    case MAV_PARAM_TYPE_UINT32:
264
        paramUnion.param_uint32 = static_cast<uint32_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
265 266
        break;
    case MAV_PARAM_TYPE_INT32:
267
        paramUnion.param_int32 = static_cast<int32_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
268 269 270 271 272 273
        break;
    case MAV_PARAM_TYPE_REAL32:
        paramUnion.param_float = paramValue.param_value;
        break;
    default:
        qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
274 275
    }

Don Gagne's avatar
Don Gagne committed
276 277
    paramValue.param_value = paramUnion.param_float;

278 279 280
    // Re-Encoding is always done using mavlink 1.0
    mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0);
    mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
281 282
    mavlink_msg_param_value_encode_chan(message->sysid,
                                        message->compid,
283
                                        0,                  // Re-encoding uses reserved channel 0
284 285
                                        message,
                                        &paramValue);
Don Gagne's avatar
Don Gagne committed
286 287
}

288
void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
289 290 291 292 293 294
{
    Q_UNUSED(vehicle);

    mavlink_param_set_t     paramSet;
    mavlink_param_union_t   paramUnion;

Don Gagne's avatar
Don Gagne committed
295 296
    memset(&paramSet, 0, sizeof(paramSet));

Don Gagne's avatar
Don Gagne committed
297 298 299 300 301
    // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
    // type they are. Fix it back to the wrong way on the way out.

    mavlink_msg_param_set_decode(message, &paramSet);

302 303 304 305 306
    if (!_ardupilotComponentMap[paramSet.target_system][paramSet.target_component]) {
        // Message is targetted to non-ArduPilot firmware component, assume it uses current mavlink spec
        return;
    }

Don Gagne's avatar
Don Gagne committed
307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332
    paramUnion.param_float = paramSet.param_value;

    switch (paramSet.param_type) {
    case MAV_PARAM_TYPE_UINT8:
        paramSet.param_value = paramUnion.param_uint8;
        break;
    case MAV_PARAM_TYPE_INT8:
        paramSet.param_value = paramUnion.param_int8;
        break;
    case MAV_PARAM_TYPE_UINT16:
        paramSet.param_value = paramUnion.param_uint16;
        break;
    case MAV_PARAM_TYPE_INT16:
        paramSet.param_value = paramUnion.param_int16;
        break;
    case MAV_PARAM_TYPE_UINT32:
        paramSet.param_value = paramUnion.param_uint32;
        break;
    case MAV_PARAM_TYPE_INT32:
        paramSet.param_value = paramUnion.param_int32;
        break;
    case MAV_PARAM_TYPE_REAL32:
        // Already in param_float
        break;
    default:
        qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
Don Gagne's avatar
Don Gagne committed
333 334
    }

335
    mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, &paramSet);
Don Gagne's avatar
Don Gagne committed
336 337
}

Don Gagne's avatar
Don Gagne committed
338
bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message, bool longVersion)
Don Gagne's avatar
Don Gagne committed
339 340
{
    QString messageText;
341
    APMFirmwarePluginInstanceData* instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
342

Don Gagne's avatar
Don Gagne committed
343 344 345 346 347 348
    int severity;
    if (longVersion) {
        severity = mavlink_msg_statustext_long_get_severity(message);
    } else {
        severity = mavlink_msg_statustext_get_severity(message);
    }
Don Gagne's avatar
Don Gagne committed
349

Don Gagne's avatar
Don Gagne committed
350 351
    if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || severity < MAV_SEVERITY_NOTICE) {
        messageText = _getMessageText(message, longVersion);
Don Gagne's avatar
Don Gagne committed
352 353
        qCDebug(APMFirmwarePluginLog) << messageText;

Don Gagne's avatar
Don Gagne committed
354
        if (!messageText.contains(APM_SOLO_REXP)) {
Don Gagne's avatar
Don Gagne committed
355
            // if don't know firmwareVersion yet, try and see if this message contains it
356
            if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || messageText.contains(APM_SUB_REXP)) {
Don Gagne's avatar
Don Gagne committed
357
                // found version string
Don Gagne's avatar
Don Gagne committed
358
                APMFirmwareVersion firmwareVersion(messageText);
359
                instanceData->textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(firmwareVersion);
Don Gagne's avatar
Don Gagne committed
360 361 362 363 364 365 366

                vehicle->setFirmwareVersion(firmwareVersion.majorNumber(), firmwareVersion.minorNumber(), firmwareVersion.patchNumber());

                int supportedMajorNumber = -1;
                int supportedMinorNumber = -1;

                switch (vehicle->vehicleType()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
367 368 369 370 371 372 373
                case MAV_TYPE_VTOL_DUOROTOR:
                case MAV_TYPE_VTOL_QUADROTOR:
                case MAV_TYPE_VTOL_TILTROTOR:
                case MAV_TYPE_VTOL_RESERVED2:
                case MAV_TYPE_VTOL_RESERVED3:
                case MAV_TYPE_VTOL_RESERVED4:
                case MAV_TYPE_VTOL_RESERVED5:
Don Gagne's avatar
Don Gagne committed
374 375
                case MAV_TYPE_FIXED_WING:
                    supportedMajorNumber = 3;
376
                    supportedMinorNumber = 8;
Don Gagne's avatar
Don Gagne committed
377 378
                    break;
                case MAV_TYPE_QUADROTOR:
DonLakeFlyer's avatar
DonLakeFlyer committed
379 380
                    // Start TCP video handshake with ARTOO in case it's a Solo running ArduPilot firmware
                    _soloVideoHandshake(vehicle, false /* originalSoloFirmware */);
Don Gagne's avatar
Don Gagne committed
381 382 383 384 385 386 387
                case MAV_TYPE_COAXIAL:
                case MAV_TYPE_HELICOPTER:
                case MAV_TYPE_SUBMARINE:
                case MAV_TYPE_HEXAROTOR:
                case MAV_TYPE_OCTOROTOR:
                case MAV_TYPE_TRICOPTER:
                    supportedMajorNumber = 3;
388
                    supportedMinorNumber = 5;
Don Gagne's avatar
Don Gagne committed
389
                    break;
390 391 392 393
                case MAV_TYPE_GROUND_ROVER:
                case MAV_TYPE_SURFACE_BOAT:
                    supportedMajorNumber = 3;
                    supportedMinorNumber = 4;
Don Gagne's avatar
Don Gagne committed
394 395 396
                default:
                    break;
                }
Don Gagne's avatar
Don Gagne committed
397

Don Gagne's avatar
Don Gagne committed
398 399
                if (supportedMajorNumber != -1) {
                    if (firmwareVersion.majorNumber() < supportedMajorNumber || firmwareVersion.minorNumber() < supportedMinorNumber) {
400
                        qgcApp()->showMessage(tr("QGroundControl fully supports Version %1.%2 and above. You are using a version prior to that. This combination is untested, you may run into unpredictable results.").arg(supportedMajorNumber).arg(supportedMinorNumber));
401
                    }
Don Gagne's avatar
Don Gagne committed
402 403
                }
            }
Don Gagne's avatar
Don Gagne committed
404
        }
Don Gagne's avatar
Don Gagne committed
405

Don Gagne's avatar
Don Gagne committed
406 407
        // APM user facing calibration messages come through as high severity, we need to parse them out
        // and lower the severity on them so that they don't pop in the users face.
408

Don Gagne's avatar
Don Gagne committed
409 410
        if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
            _adjustCalibrationMessageSeverity(message);
411
            return true;
412
        }
Don Gagne's avatar
Don Gagne committed
413
    }
414

Don Gagne's avatar
Don Gagne committed
415
    // adjust mesasge if needed
416
    if (instanceData->textSeverityAdjustmentNeeded) {
Don Gagne's avatar
Don Gagne committed
417 418
        _adjustSeverity(message);
    }
419

Don Gagne's avatar
Don Gagne committed
420
    if (messageText.isEmpty()) {
Don Gagne's avatar
Don Gagne committed
421
        messageText = _getMessageText(message, longVersion);
Don Gagne's avatar
Don Gagne committed
422 423 424 425
    }

    // The following messages are incorrectly labeled as warning message.
    // Fixed in newer firmware (unreleased at this point), but still in older firmware.
426
    if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || messageText.contains(APM_SUB_REXP) ||
Don Gagne's avatar
Don Gagne committed
427
            messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
Don Gagne's avatar
Don Gagne committed
428
        _setInfoSeverity(message, longVersion);
Don Gagne's avatar
Don Gagne committed
429
    }
430

Don Gagne's avatar
Don Gagne committed
431 432
    if (messageText.contains(APM_SOLO_REXP)) {
        qDebug() << "Found Solo";
433
        vehicle->setSoloFirmware(true);
Don Gagne's avatar
Don Gagne committed
434 435

        // Fix up severity
Don Gagne's avatar
Don Gagne committed
436
        _setInfoSeverity(message, longVersion);
Don Gagne's avatar
Don Gagne committed
437 438

        // Start TCP video handshake with ARTOO
DonLakeFlyer's avatar
DonLakeFlyer committed
439
        _soloVideoHandshake(vehicle, true /* originalSoloFirmware */);
Don Gagne's avatar
Don Gagne committed
440 441 442 443 444 445 446 447 448
    } else if (messageText.contains(APM_FRAME_REXP)) {
        // We need to parse the Frame: message in order to determine whether the motors are coaxial or not
        QRegExp frameTypeRegex("^Frame: (\\S*)");
        if (frameTypeRegex.indexIn(messageText) != -1) {
            QString frameType = frameTypeRegex.cap(1);
            if (!frameType.isEmpty() && (frameType == QStringLiteral("Y6") || frameType == QStringLiteral("OCTA_QUAD") || frameType == QStringLiteral("COAX"))) {
                _coaxialMotors = true;
            }
        }
Don Gagne's avatar
Don Gagne committed
449 450
    }

451
    return true;
Don Gagne's avatar
Don Gagne committed
452 453
}

454
void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
455 456
{
    bool flying = false;
457

458 459
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(message, &heartbeat);
Don Gagne's avatar
Don Gagne committed
460

461 462 463
    if (message->compid == MAV_COMP_ID_AUTOPILOT1) {
        // We pull Vehicle::flying state from HEARTBEAT on ArduPilot. This is a firmware specific test.
        if (vehicle->armed()) {
Don Gagne's avatar
Don Gagne committed
464

465 466 467 468 469
            flying = heartbeat.system_status == MAV_STATE_ACTIVE;
            if (!flying && vehicle->flying()) {
                // If we were previously flying, and we go into critical or emergency assume we are still flying
                flying = heartbeat.system_status == MAV_STATE_CRITICAL || heartbeat.system_status == MAV_STATE_EMERGENCY;
            }
470
        }
471
        vehicle->_setFlying(flying);
472
    }
Don Gagne's avatar
Don Gagne committed
473

474 475 476 477 478 479
    // We need to know whether this component is part of the ArduPilot stack code or not so we can adjust mavlink quirks appropriately.
    // If the component sends a heartbeat we can know that. If it's doesn't there is pretty much no way to know.
    _ardupilotComponentMap[message->sysid][message->compid] = heartbeat.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA;

    // Force the ESP8266 to be non-ArduPilot code (it doesn't send heartbeats)
    _ardupilotComponentMap[message->sysid][MAV_COMP_ID_UDP_BRIDGE] = false;
Don Gagne's avatar
Don Gagne committed
480 481
}

482
bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
483
{
484 485 486
    if (message->msgid == MAVLINK_MSG_ID_HEARTBEAT) {
        // We need to look at all heartbeats that go by from any component
        _handleIncomingHeartbeat(vehicle, message);
487
        return true;
Don Gagne's avatar
Don Gagne committed
488 489
    }

490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506
    // Only translate messages which come from ArduPilot code. All other components are expected to follow current mavlink spec.
    if (_ardupilotComponentMap[vehicle->id()][message->compid]) {
        switch (message->msgid) {
        case MAVLINK_MSG_ID_PARAM_VALUE:
            _handleIncomingParamValue(vehicle, message);
            break;
        case MAVLINK_MSG_ID_STATUSTEXT:
            return _handleIncomingStatusText(vehicle, message, false /* longVersion */);
        case MAVLINK_MSG_ID_STATUSTEXT_LONG:
            return _handleIncomingStatusText(vehicle, message, true /* longVersion */);
        case MAVLINK_MSG_ID_RC_CHANNELS:
            _handleRCChannels(vehicle, message);
            break;
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
            _handleRCChannelsRaw(vehicle, message);
            break;
        }
Don Gagne's avatar
Don Gagne committed
507
    }
508 509

    return true;
Don Gagne's avatar
Don Gagne committed
510 511
}

512
void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
513
{
514 515 516 517
    switch (message->msgid) {
    case MAVLINK_MSG_ID_PARAM_SET:
        _handleOutgoingParamSet(vehicle, outgoingLink, message);
        break;
Don Gagne's avatar
Don Gagne committed
518
    }
519 520
}

Don Gagne's avatar
Don Gagne committed
521
QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message, bool longVersion) const
522 523 524
{
    QByteArray b;

Don Gagne's avatar
Don Gagne committed
525 526 527 528 529 530 531
    if (longVersion) {
        b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1);
        mavlink_msg_statustext_long_get_text(message, b.data());
    } else {
        b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
        mavlink_msg_statustext_get_text(message, b.data());
    }
532 533 534 535 536 537

    // Ensure NUL-termination
    b[b.length()-1] = '\0';
    return QString(b);
}

Pritam Ghanghas's avatar
Pritam Ghanghas committed
538
bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
539
{
Don Gagne's avatar
Don Gagne committed
540 541 542
    if (!firmwareVersion.isValid()) {
        return false;
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
543

Don Gagne's avatar
Don Gagne committed
544 545 546 547 548 549 550 551 552 553 554 555 556
    bool adjustmentNeeded = false;
    if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) {
        if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
            adjustmentNeeded = true;
        }
    } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) {
        if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
            adjustmentNeeded = true;
        }
    } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) {
        if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
            adjustmentNeeded = true;
        }
557 558 559 560
    } else if (firmwareVersion.vehicleType().contains(APM_SUB_REXP)) {
        if (firmwareVersion < APMFirmwareVersion(MIN_SUB_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
            adjustmentNeeded = true;
        }
Don Gagne's avatar
Don Gagne committed
561
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
562

Don Gagne's avatar
Don Gagne committed
563
    return adjustmentNeeded;
Pritam Ghanghas's avatar
Pritam Ghanghas committed
564
}
565

Pritam Ghanghas's avatar
Pritam Ghanghas committed
566 567 568
void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
{
    // lets make QGC happy with right severity values
569 570 571
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
    switch(statusText.severity) {
Don Gagne's avatar
Don Gagne committed
572 573 574 575 576 577 578 579 580
    case MAV_SEVERITY_ALERT:    /* SEVERITY_LOW according to old codes */
        statusText.severity = MAV_SEVERITY_WARNING;
        break;
    case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes  */
        statusText.severity = MAV_SEVERITY_ALERT;
        break;
    case MAV_SEVERITY_ERROR:    /*SEVERITY_HIGH according to old codes */
        statusText.severity = MAV_SEVERITY_CRITICAL;
        break;
581 582
    }

583 584 585 586 587 588 589 590
    // Re-Encoding is always done using mavlink 1.0
    mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0);
    mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
    mavlink_msg_statustext_encode_chan(message->sysid,
                                       message->compid,
                                       0,                  // Re-encoding uses reserved channel 0
                                       message,
                                       &statusText);
Don Gagne's avatar
Don Gagne committed
591
}
592

Don Gagne's avatar
Don Gagne committed
593
void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message, bool longVersion) const
594
{
595 596 597
    // Re-Encoding is always done using mavlink 1.0
    mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0);
    mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
Don Gagne's avatar
Don Gagne committed
598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619

    if (longVersion) {
        mavlink_statustext_long_t statusTextLong;
        mavlink_msg_statustext_long_decode(message, &statusTextLong);

        statusTextLong.severity = MAV_SEVERITY_INFO;
        mavlink_msg_statustext_long_encode_chan(message->sysid,
                                                message->compid,
                                                0,                  // Re-encoding uses reserved channel 0
                                                message,
                                                &statusTextLong);
    } else {
        mavlink_statustext_t statusText;
        mavlink_msg_statustext_decode(message, &statusText);

        statusText.severity = MAV_SEVERITY_INFO;
        mavlink_msg_statustext_encode_chan(message->sysid,
                                           message->compid,
                                           0,                  // Re-encoding uses reserved channel 0
                                           message,
                                           &statusText);
    }
620 621
}

Don Gagne's avatar
Don Gagne committed
622 623 624 625
void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
626 627 628
    // Re-Encoding is always done using mavlink 1.0
    mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0);
    mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
Don Gagne's avatar
Don Gagne committed
629
    statusText.severity = MAV_SEVERITY_INFO;
630
    mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText);
Don Gagne's avatar
Don Gagne committed
631 632
}

633 634
void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
{
635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658
    APMMavlinkStreamRateSettings* streamRates = qgcApp()->toolbox()->settingsManager()->apmMavlinkStreamRateSettings();

    struct StreamInfo_s {
        MAV_DATA_STREAM mavStream;
        int             streamRate;
    };

    StreamInfo_s rgStreamInfo[] = {
        { MAV_DATA_STREAM_RAW_SENSORS,      streamRates->streamRateRawSensors()->rawValue().toInt() },
        { MAV_DATA_STREAM_EXTENDED_STATUS,  streamRates->streamRateExtendedStatus()->rawValue().toInt() },
        { MAV_DATA_STREAM_RC_CHANNELS,      streamRates->streamRateRCChannels()->rawValue().toInt() },
        { MAV_DATA_STREAM_POSITION,         streamRates->streamRatePosition()->rawValue().toInt() },
        { MAV_DATA_STREAM_EXTRA1,           streamRates->streamRateExtra1()->rawValue().toInt() },
        { MAV_DATA_STREAM_EXTRA2,           streamRates->streamRateExtra2()->rawValue().toInt() },
        { MAV_DATA_STREAM_EXTRA3,           streamRates->streamRateExtra3()->rawValue().toInt() },
    };

    for (size_t i=0; i<sizeof(rgStreamInfo)/sizeof(rgStreamInfo[0]); i++) {
        const StreamInfo_s& streamInfo = rgStreamInfo[i];

        if (streamInfo.streamRate >= 0) {
            vehicle->requestDataStream(streamInfo.mavStream, static_cast<uint16_t>(streamInfo.streamRate));
        }
    }
659 660 661
}


662 663
void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
664 665
    vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData);

666 667 668 669 670 671 672 673
    if (vehicle->isOfflineEditingVehicle()) {
        switch (vehicle->vehicleType()) {
        case MAV_TYPE_QUADROTOR:
        case MAV_TYPE_HEXAROTOR:
        case MAV_TYPE_OCTOROTOR:
        case MAV_TYPE_TRICOPTER:
        case MAV_TYPE_COAXIAL:
        case MAV_TYPE_HELICOPTER:
674
            vehicle->setFirmwareVersion(3, 6, 0);
675
            break;
DonLakeFlyer's avatar
DonLakeFlyer committed
676 677 678 679 680 681 682
        case MAV_TYPE_VTOL_DUOROTOR:
        case MAV_TYPE_VTOL_QUADROTOR:
        case MAV_TYPE_VTOL_TILTROTOR:
        case MAV_TYPE_VTOL_RESERVED2:
        case MAV_TYPE_VTOL_RESERVED3:
        case MAV_TYPE_VTOL_RESERVED4:
        case MAV_TYPE_VTOL_RESERVED5:
683
        case MAV_TYPE_FIXED_WING:
684
            vehicle->setFirmwareVersion(3, 9, 0);
685 686 687
            break;
        case MAV_TYPE_GROUND_ROVER:
        case MAV_TYPE_SURFACE_BOAT:
688
            vehicle->setFirmwareVersion(3, 5, 0);
689 690 691 692 693 694 695 696
            break;
        case MAV_TYPE_SUBMARINE:
            vehicle->setFirmwareVersion(3, 4, 0);
            break;
        default:
            // No version set
            break;
        }
697
    } else {
698 699 700 701
        if (qgcApp()->toolbox()->settingsManager()->appSettings()->apmStartMavlinkStreams()->rawValue().toBool()) {
            // Streams are not started automatically on APM stack (sort of)
            initializeStreamRates(vehicle);
        }
702
    }
703
}
704 705 706 707 708

void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)
{
    _supportedModes = supportedModes;
}
709 710 711 712 713 714

bool APMFirmwarePlugin::sendHomePositionToVehicle(void)
{
    // APM stack wants the home position sent in the first position
    return true;
}
715

716
void APMFirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
717
{
718
    APMParameterMetaData* apmMetaData = qobject_cast<APMParameterMetaData*>(parameterMetaData);
Don Gagne's avatar
Don Gagne committed
719

720 721 722 723 724 725
    if (apmMetaData) {
        apmMetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to APMFirmwarePlugin::addMetaDataToFact not APMParameterMetaData";
    }
}
726

Don Gagne's avatar
Don Gagne committed
727 728
QList<MAV_CMD> APMFirmwarePlugin::supportedMissionCommands(void)
{
Tomaz Canabrava's avatar
Tomaz Canabrava committed
729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756
    return {
        MAV_CMD_NAV_WAYPOINT,
        MAV_CMD_NAV_LOITER_UNLIM, MAV_CMD_NAV_LOITER_TURNS, MAV_CMD_NAV_LOITER_TIME,
        MAV_CMD_NAV_RETURN_TO_LAUNCH, MAV_CMD_NAV_LAND, MAV_CMD_NAV_TAKEOFF,
        MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT,
        MAV_CMD_NAV_LOITER_TO_ALT,
        MAV_CMD_NAV_SPLINE_WAYPOINT,
        MAV_CMD_NAV_GUIDED_ENABLE,
        MAV_CMD_NAV_DELAY,
        MAV_CMD_CONDITION_DELAY, MAV_CMD_CONDITION_DISTANCE, MAV_CMD_CONDITION_YAW,
        MAV_CMD_DO_SET_MODE,
        MAV_CMD_DO_JUMP,
        MAV_CMD_DO_CHANGE_SPEED,
        MAV_CMD_DO_SET_HOME,
        MAV_CMD_DO_SET_RELAY, MAV_CMD_DO_REPEAT_RELAY,
        MAV_CMD_DO_SET_SERVO, MAV_CMD_DO_REPEAT_SERVO,
        MAV_CMD_DO_LAND_START,
        MAV_CMD_DO_SET_ROI,
        MAV_CMD_DO_DIGICAM_CONFIGURE, MAV_CMD_DO_DIGICAM_CONTROL,
        MAV_CMD_DO_MOUNT_CONTROL,
        MAV_CMD_DO_SET_CAM_TRIGG_DIST,
        MAV_CMD_DO_FENCE_ENABLE,
        MAV_CMD_DO_PARACHUTE,
        MAV_CMD_DO_INVERTED_FLIGHT,
        MAV_CMD_DO_GRIPPER,
        MAV_CMD_DO_GUIDED_LIMITS,
        MAV_CMD_DO_AUTOTUNE_ENABLE,
        MAV_CMD_NAV_VTOL_TAKEOFF, MAV_CMD_NAV_VTOL_LAND, MAV_CMD_DO_VTOL_TRANSITION,
757
#if 0
758
    // Waiting for module update
Tomaz Canabrava's avatar
Tomaz Canabrava committed
759
        MAV_CMD_DO_SET_REVERSE,
760
#endif
Tomaz Canabrava's avatar
Tomaz Canabrava committed
761
    };
Don Gagne's avatar
Don Gagne committed
762
}
763

764
QString APMFirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
765
{
766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782
    switch (vehicleType) {
    case MAV_TYPE_GENERIC:
        return QStringLiteral(":/json/APM/MavCmdInfoCommon.json");
    case MAV_TYPE_FIXED_WING:
        return QStringLiteral(":/json/APM/MavCmdInfoFixedWing.json");
    case MAV_TYPE_QUADROTOR:
        return QStringLiteral(":/json/APM/MavCmdInfoMultiRotor.json");
    case MAV_TYPE_VTOL_QUADROTOR:
        return QStringLiteral(":/json/APM/MavCmdInfoVTOL.json");
    case MAV_TYPE_SUBMARINE:
        return QStringLiteral(":/json/APM/MavCmdInfoSub.json");
    case MAV_TYPE_GROUND_ROVER:
        return QStringLiteral(":/json/APM/MavCmdInfoRover.json");
    default:
        qWarning() << "APMFirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType;
        return QString();
    }
783
}
784 785 786 787 788

QObject* APMFirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
{
    Q_UNUSED(metaDataFile);

Don Gagne's avatar
Don Gagne committed
789 790
    APMParameterMetaData* metaData = new APMParameterMetaData();
    metaData->loadParameterFactMetaDataFile(metaDataFile);
791 792
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
793 794 795 796 797 798

bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
    return vehicle->flightMode() == "Guided";
}

DonLakeFlyer's avatar
DonLakeFlyer committed
799
void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware)
Don Gagne's avatar
Don Gagne committed
800 801 802 803 804 805
{
    Q_UNUSED(vehicle);

    QTcpSocket* socket = new QTcpSocket();

    socket->connectToHost(_artooIP, _artooVideoHandshakePort);
DonLakeFlyer's avatar
DonLakeFlyer committed
806 807 808
    if (originalSoloFirmware) {
        QObject::connect(socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError);
    }
Don Gagne's avatar
Don Gagne committed
809 810 811 812 813 814
}

void APMFirmwarePlugin::_artooSocketError(QAbstractSocket::SocketError socketError)
{
    qgcApp()->showMessage(tr("Error during Solo video link setup: %1").arg(socketError));
}
Don Gagne's avatar
Don Gagne committed
815

816
QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
817 818 819 820 821 822 823 824
{
    switch (vehicle->vehicleType()) {
    case MAV_TYPE_QUADROTOR:
    case MAV_TYPE_HEXAROTOR:
    case MAV_TYPE_OCTOROTOR:
    case MAV_TYPE_TRICOPTER:
    case MAV_TYPE_COAXIAL:
    case MAV_TYPE_HELICOPTER:
825
        if (vehicle->versionCompare(3, 7, 0) >= 0) {  // 3.7.0 and higher
826
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.7.xml");
827
        }
828
        if (vehicle->versionCompare(3, 6, 0) >= 0) {  // 3.6.0 and higher
829
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml");
Don Gagne's avatar
Don Gagne committed
830
        }
831
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
832

DonLakeFlyer's avatar
DonLakeFlyer committed
833 834 835 836 837 838 839
    case MAV_TYPE_VTOL_DUOROTOR:
    case MAV_TYPE_VTOL_QUADROTOR:
    case MAV_TYPE_VTOL_TILTROTOR:
    case MAV_TYPE_VTOL_RESERVED2:
    case MAV_TYPE_VTOL_RESERVED3:
    case MAV_TYPE_VTOL_RESERVED4:
    case MAV_TYPE_VTOL_RESERVED5:
Don Gagne's avatar
Don Gagne committed
840
    case MAV_TYPE_FIXED_WING:
841 842
        if (vehicle->versionCompare(3, 10, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.10.xml");
843
        }
844 845
        if (vehicle->versionCompare(3, 9, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.9.xml");
Don Gagne's avatar
Don Gagne committed
846
        }
847
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
848

Don Gagne's avatar
Don Gagne committed
849 850
    case MAV_TYPE_GROUND_ROVER:
    case MAV_TYPE_SURFACE_BOAT:
851 852
        if (vehicle->versionCompare(3, 6, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.6.xml");
853
        }
854 855
        if (vehicle->versionCompare(3, 5, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.5.xml");
856
        }
857
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.4.xml");
858

859
    case MAV_TYPE_SUBMARINE:
860
        if (vehicle->versionCompare(3, 6, 0) >= 0) { // 3.5.x
861
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.6dev.xml");
862
        }
863 864 865 866 867 868
        if (vehicle->versionCompare(3, 5, 0) >= 0) { // 3.5.x
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.5.xml");
        }
        // up to 3.4.x
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.4.xml");

Don Gagne's avatar
Don Gagne committed
869 870 871 872
    default:
        return QString();
    }
}
873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902

void APMFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    if (guidedMode) {
        _setFlightModeAndValidate(vehicle, "Guided");
    } else {
        pauseVehicle(vehicle);
    }
}

void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
    _setFlightModeAndValidate(vehicle, pauseFlightMode());
}

void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
        qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
        return;
    }


    setGuidedMode(vehicle, true);

    QGeoCoordinate coordWithAltitude = gotoCoord;
    coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
    vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
}

DonLakeFlyer's avatar
DonLakeFlyer committed
903
void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
904
{
DonLakeFlyer's avatar
DonLakeFlyer committed
905
    _setFlightModeAndValidate(vehicle, smartRTL ? smartRTLFlightMode() : rtlFlightMode());
906 907 908 909 910
}

void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
911
        qgcApp()->showMessage(tr("Unable to change altitude, vehicle altitude not known."));
912 913 914 915 916 917 918 919 920 921
        return;
    }

    setGuidedMode(vehicle, true);

    mavlink_message_t msg;
    mavlink_set_position_target_local_ned_t cmd;

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

922 923
    cmd.target_system    = static_cast<uint8_t>(vehicle->id());
    cmd.target_component = static_cast<uint8_t>(vehicle->defaultComponentId());
924 925 926 927
    cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED;
    cmd.type_mask = 0xFFF8; // Only x/y/z valid
    cmd.x = 0.0f;
    cmd.y = 0.0f;
928
    cmd.z = static_cast<float>(-(altitudeChange));
929 930

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
931
    mavlink_msg_set_position_target_local_ned_encode_chan(
932 933 934 935 936
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                &msg,
                &cmd);
937 938 939

    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}
940

941
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
942
{
943
    _guidedModeTakeoff(vehicle, altitudeRel);
944 945
}

946 947
double APMFirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
948
    double minTakeoffAlt = 0;
949 950 951 952
    QString takeoffAltParam(vehicle->vtol() ? QStringLiteral("Q_RTL_ALT") : QStringLiteral("PILOT_TKOFF_ALT"));
    float paramDivisor = vehicle->vtol() ? 1.0 : 100.0; // PILOT_TAKEOFF_ALT is in centimeters

    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
953
        minTakeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble() / static_cast<double>(paramDivisor);
DonLakeFlyer's avatar
DonLakeFlyer committed
954 955 956 957
    }

    if (minTakeoffAlt == 0) {
        minTakeoffAlt = FirmwarePlugin::minimumTakeoffAltitude(vehicle);
958
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
959 960

    return minTakeoffAlt;
961 962
}

963
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
964
{
DonLakeFlyer's avatar
DonLakeFlyer committed
965 966 967 968 969
    if (!vehicle->multiRotor() && !vehicle->vtol()) {
        qgcApp()->showMessage(tr("Vehicle does not support guided takeoff"));
        return false;
    }

970 971 972 973 974 975
    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
    if (qIsNaN(vehicleAltitudeAMSL)) {
        qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
        return false;
    }

976
    double takeoffAltRel = minimumTakeoffAltitude(vehicle);
977 978
    if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) {
        takeoffAltRel = altitudeRel;
979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994
    }

    if (!_setFlightModeAndValidate(vehicle, "Guided")) {
        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
        return false;
    }

    if (!_armVehicleAndValidate(vehicle)) {
        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
        return false;
    }

    vehicle->sendMavCommand(vehicle->defaultComponentId(),
                            MAV_CMD_NAV_TAKEOFF,
                            true, // show error
                            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
995
                            static_cast<float>(takeoffAltRel));                     // Relative altitude
996 997 998 999 1000 1001

    return true;
}

void APMFirmwarePlugin::startMission(Vehicle* vehicle)
{
1002 1003 1004 1005 1006 1007 1008 1009
    if (vehicle->flying()) {
        // Vehicle already in the air, we just need to switch to auto
        if (!_setFlightModeAndValidate(vehicle, "Auto")) {
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
        }
        return;
    }

1010 1011 1012
    if (!vehicle->armed()) {
        // First switch to flight mode we can arm from
        if (!_setFlightModeAndValidate(vehicle, "Guided")) {
1013 1014
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode."));
            return;
1015
        }
1016

1017 1018 1019 1020
        if (!_armVehicleAndValidate(vehicle)) {
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
            return;
        }
1021 1022
    }

1023 1024 1025 1026 1027 1028 1029
    if (vehicle->fixedWing()) {
        if (!_setFlightModeAndValidate(vehicle, "Auto")) {
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
            return;
        }
    } else {
        vehicle->sendMavCommand(vehicle->defaultComponentId(), MAV_CMD_MISSION_START, true /*show error */);
1030 1031
    }
}
1032 1033 1034 1035

QString APMFirmwarePlugin::_getLatestVersionFileUrl(Vehicle* vehicle)
{
    const static QString baseUrl("http://firmware.ardupilot.org/%1/stable/PX4/git-version.txt");
1036 1037

    if (qobject_cast<ArduPlaneFirmwarePlugin*>(vehicle->firmwarePlugin())) {
1038
        return baseUrl.arg("Plane");
1039
    } else if (qobject_cast<ArduRoverFirmwarePlugin*>(vehicle->firmwarePlugin())) {
1040
        return baseUrl.arg("Rover");
1041
    } else if (qobject_cast<ArduSubFirmwarePlugin*>(vehicle->firmwarePlugin())) {
1042
        return baseUrl.arg("Sub");
1043 1044 1045 1046 1047
    } else if (qobject_cast<ArduCopterFirmwarePlugin*>(vehicle->firmwarePlugin())) {
        return baseUrl.arg("Copter");
    } else {
        qWarning() << "APMFirmwarePlugin::_getLatestVersionFileUrl Unknown vehicle firmware type" << vehicle->vehicleType();
        return QString();
1048 1049 1050 1051 1052 1053
    }
}

QString APMFirmwarePlugin::_versionRegex() {
    return QStringLiteral(" V([0-9,\\.]*)$");
}
1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064

void APMFirmwarePlugin::_handleRCChannels(Vehicle* vehicle, mavlink_message_t* message)
{
    mavlink_rc_channels_t channels;
    mavlink_msg_rc_channels_decode(message, &channels);
    //-- Ardupilot uses 0-255 to indicate 0-100% where QGC expects 0-100
    if(channels.rssi) {
        channels.rssi = static_cast<uint8_t>(static_cast<double>(channels.rssi) / 255.0 * 100.0);
    }
    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
    mavlink_msg_rc_channels_encode_chan(
1065 1066 1067 1068 1069
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                message,
                &channels);
1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081
}

void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t *message)
{
    mavlink_rc_channels_raw_t channels;
    mavlink_msg_rc_channels_raw_decode(message, &channels);
    //-- Ardupilot uses 0-255 to indicate 0-100% where QGC expects 0-100
    if(channels.rssi) {
        channels.rssi = static_cast<uint8_t>(static_cast<double>(channels.rssi) / 255.0 * 100.0);
    }
    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
    mavlink_msg_rc_channels_raw_encode_chan(
1082 1083 1084 1085 1086
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                message,
                &channels);
1087 1088
}