APMFirmwarePlugin.cc 44.3 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * 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 "APMFollowComponentController.h"
18
#include "APMSubMotorComponentController.h"
19
#include "MissionManager.h"
20
#include "ParameterManager.h"
21
#include "QGCFileDownload.h"
22 23
#include "SettingsManager.h"
#include "AppSettings.h"
24
#include "APMMavlinkStreamRateSettings.h"
25 26 27 28
#include "ArduPlaneFirmwarePlugin.h"
#include "ArduCopterFirmwarePlugin.h"
#include "ArduRoverFirmwarePlugin.h"
#include "ArduSubFirmwarePlugin.h"
Don Gagne's avatar
Don Gagne committed
29

Don Gagne's avatar
Don Gagne committed
30 31
#include <QTcpSocket>

Pritam Ghanghas's avatar
Pritam Ghanghas committed
32
QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")
Don Gagne's avatar
Don Gagne committed
33

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

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

// minimum firmware versions that don't suffer from mavlink severity inversion bug.
// https://github.com/diydrones/apm_planner/issues/788
48
static const QString MIN_SOLO_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter solo-1.2.0");
49 50
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");
51
static const QString MIN_SUB_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Sub V3.4.0");
52 53
static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0");

Don Gagne's avatar
Don Gagne committed
54 55
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
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 101 102

/*
 * @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
103 104
        qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything"
                                        << "version text to be parsed" << versionText;
105 106 107 108 109 110
        return;
    }

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

    if (capturedTexts.count() < 5) {
Pritam Ghanghas's avatar
Pritam Ghanghas committed
111 112
        qCWarning(APMFirmwarePluginLog) << "something wrong with parsing the version text, not hitting anything"
                                        << VERSION_REXP.captureCount() << VERSION_REXP.capturedTexts();
113 114 115 116 117 118 119 120 121 122 123 124 125
        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();
}

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

/*
 * @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;
}

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

}

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

167 168 169
AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
{
    return new APMAutoPilotPlugin(vehicle, vehicle);
Don Gagne's avatar
Don Gagne committed
170 171
}

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

181
    return (capabilities & available) == capabilities;
Don Gagne's avatar
Don Gagne committed
182 183 184 185 186
}

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

Don Gagne's avatar
Don Gagne committed
188 189 190
    return QList<VehicleComponent*>();
}

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

Don Gagne's avatar
Don Gagne committed
203
QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
204
{
205 206 207 208 209 210 211 212 213 214
    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
215 216 217 218
}

bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237
    *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
238 239
}

240
void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
241
{
Don Gagne's avatar
Don Gagne committed
242 243 244 245 246
    Q_UNUSED(vehicle);

    mavlink_param_value_t paramValue;
    mavlink_param_union_t paramUnion;

Don Gagne's avatar
Don Gagne committed
247 248
    memset(&paramValue, 0, sizeof(paramValue));

Don Gagne's avatar
Don Gagne committed
249 250 251 252 253 254 255
    // 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:
256
        paramUnion.param_uint8 = static_cast<uint8_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
257 258
        break;
    case MAV_PARAM_TYPE_INT8:
259
        paramUnion.param_int8  = static_cast<int8_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
260 261
        break;
    case MAV_PARAM_TYPE_UINT16:
262
        paramUnion.param_uint16 = static_cast<uint16_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
263 264
        break;
    case MAV_PARAM_TYPE_INT16:
265
        paramUnion.param_int16 = static_cast<int16_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
266 267
        break;
    case MAV_PARAM_TYPE_UINT32:
268
        paramUnion.param_uint32 = static_cast<uint32_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
269 270
        break;
    case MAV_PARAM_TYPE_INT32:
271
        paramUnion.param_int32 = static_cast<int32_t>(paramValue.param_value);
Don Gagne's avatar
Don Gagne committed
272 273 274 275 276 277
        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;
278 279
    }

Don Gagne's avatar
Don Gagne committed
280 281
    paramValue.param_value = paramUnion.param_float;

282 283 284
    // 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;
285 286
    mavlink_msg_param_value_encode_chan(message->sysid,
                                        message->compid,
287
                                        0,                  // Re-encoding uses reserved channel 0
288 289
                                        message,
                                        &paramValue);
Don Gagne's avatar
Don Gagne committed
290 291
}

292
void APMFirmwarePlugin::_handleOutgoingParamSetThreadSafe(Vehicle* /*vehicle*/, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
293 294 295 296
{
    mavlink_param_set_t     paramSet;
    mavlink_param_union_t   paramUnion;

Don Gagne's avatar
Don Gagne committed
297 298
    memset(&paramSet, 0, sizeof(paramSet));

Don Gagne's avatar
Don Gagne committed
299 300 301 302 303
    // 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);

304 305 306 307 308
    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
309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334
    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
335 336
    }

337
    _adjustOutgoingMavlinkMutex.lock();
338
    mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, &paramSet);
339
    _adjustOutgoingMavlinkMutex.unlock();
Don Gagne's avatar
Don Gagne committed
340 341
}

342
bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
343 344
{
    QString messageText;
345
    APMFirmwarePluginInstanceData* instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
346

347
    int severity = mavlink_msg_statustext_get_severity(message);
Don Gagne's avatar
Don Gagne committed
348

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

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

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

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

                switch (vehicle->vehicleType()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
366 367 368 369 370 371 372
                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
373 374
                case MAV_TYPE_FIXED_WING:
                    supportedMajorNumber = 3;
375
                    supportedMinorNumber = 8;
Don Gagne's avatar
Don Gagne committed
376 377
                    break;
                case MAV_TYPE_QUADROTOR:
DonLakeFlyer's avatar
DonLakeFlyer committed
378 379
                    // Start TCP video handshake with ARTOO in case it's a Solo running ArduPilot firmware
                    _soloVideoHandshake(vehicle, false /* originalSoloFirmware */);
380
                    [[fallthrough]];
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
                if (supportedMajorNumber != -1) {
399 400
                    if (firmwareVersion.majorNumber() < supportedMajorNumber ||
                            (firmwareVersion.majorNumber() == supportedMajorNumber && firmwareVersion.minorNumber() < supportedMinorNumber)) {
401
                        qgcApp()->showAppMessage(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));
402
                    }
Don Gagne's avatar
Don Gagne committed
403 404
                }
            }
Don Gagne's avatar
Don Gagne committed
405
        }
Don Gagne's avatar
Don Gagne committed
406

Don Gagne's avatar
Don Gagne committed
407 408
        // 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.
409

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

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

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

    // The following messages are incorrectly labeled as warning message.
    // Fixed in newer firmware (unreleased at this point), but still in older firmware.
427
    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
428
            messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
429
        _setInfoSeverity(message);
Don Gagne's avatar
Don Gagne committed
430
    }
431

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

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

        // Start TCP video handshake with ARTOO
DonLakeFlyer's avatar
DonLakeFlyer committed
440
        _soloVideoHandshake(vehicle, true /* originalSoloFirmware */);
Don Gagne's avatar
Don Gagne committed
441 442 443 444 445 446 447 448 449
    } 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
450 451
    }

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

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

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

462 463 464
    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
465

466 467 468 469 470
            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;
            }
471
        }
472
        vehicle->_setFlying(flying);
473
    }
Don Gagne's avatar
Don Gagne committed
474

475 476 477 478 479 480
    // 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
481 482
}

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

491 492 493 494 495 496 497
    // 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:
498
            return _handleIncomingStatusText(vehicle, message);
499 500 501 502 503 504 505
        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
506
    }
507 508

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

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

520
QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const
521 522 523
{
    QByteArray b;

524 525
    b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
    mavlink_msg_statustext_get_text(message, b.data());
526 527 528 529 530 531

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

Pritam Ghanghas's avatar
Pritam Ghanghas committed
532
bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
533
{
Don Gagne's avatar
Don Gagne committed
534 535 536
    if (!firmwareVersion.isValid()) {
        return false;
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
537

Don Gagne's avatar
Don Gagne committed
538 539 540 541 542 543 544 545 546 547 548 549 550
    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;
        }
551 552 553 554
    } 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
555
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
556

Don Gagne's avatar
Don Gagne committed
557
    return adjustmentNeeded;
Pritam Ghanghas's avatar
Pritam Ghanghas committed
558
}
559

Pritam Ghanghas's avatar
Pritam Ghanghas committed
560 561 562
void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
{
    // lets make QGC happy with right severity values
563 564 565
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
    switch(statusText.severity) {
Don Gagne's avatar
Don Gagne committed
566 567 568 569 570 571 572 573 574
    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;
575 576
    }

577 578 579 580 581 582 583 584
    // 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
585
}
586

587
void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const
588
{
589 590 591
    // 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
592

593 594 595 596 597 598 599 600 601
    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);
602 603
}

Don Gagne's avatar
Don Gagne committed
604 605 606 607
void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
608 609 610
    // 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
611
    statusText.severity = MAV_SEVERITY_INFO;
612
    mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText);
Don Gagne's avatar
Don Gagne committed
613 614
}

615 616
void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
{
617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640
    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));
        }
    }
641

642 643 644 645 646 647
    // ArduPilot only sends home position on first boot and then when it arms. It does not stream the position.
    // This means that depending on when QGC connects to the vehicle it may not have home position.
    // This can cause various features to not be available. So we request home position streaming ourselves.
    // The MAV_CMD_SET_MESSAGE_INTERVAL command is only supported on newer firmwares. So we set showError=false.
    // Which also means than on older firmwares you may be left with some missing features.
    vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_SET_MESSAGE_INTERVAL, false /* showError */, MAVLINK_MSG_ID_HOME_POSITION, 1000000 /* 1 second interval in usec */);
648 649 650
}


651 652
void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
653 654
    vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData);

655 656 657 658 659 660 661 662
    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:
663
            vehicle->setFirmwareVersion(3, 6, 0);
664
            break;
DonLakeFlyer's avatar
DonLakeFlyer committed
665 666 667 668 669 670 671
        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:
672
        case MAV_TYPE_FIXED_WING:
673
            vehicle->setFirmwareVersion(3, 9, 0);
674 675 676
            break;
        case MAV_TYPE_GROUND_ROVER:
        case MAV_TYPE_SURFACE_BOAT:
677
            vehicle->setFirmwareVersion(3, 5, 0);
678 679 680 681 682 683 684 685
            break;
        case MAV_TYPE_SUBMARINE:
            vehicle->setFirmwareVersion(3, 4, 0);
            break;
        default:
            // No version set
            break;
        }
686
    } else {
687 688 689 690
        if (qgcApp()->toolbox()->settingsManager()->appSettings()->apmStartMavlinkStreams()->rawValue().toBool()) {
            // Streams are not started automatically on APM stack (sort of)
            initializeStreamRates(vehicle);
        }
691
    }
692
}
693 694 695 696 697

void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)
{
    _supportedModes = supportedModes;
}
698 699 700 701 702 703

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

705
void APMFirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
706
{
707
    APMParameterMetaData* apmMetaData = qobject_cast<APMParameterMetaData*>(parameterMetaData);
Don Gagne's avatar
Don Gagne committed
708

709 710 711 712 713 714
    if (apmMetaData) {
        apmMetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to APMFirmwarePlugin::addMetaDataToFact not APMParameterMetaData";
    }
}
715

Don Gagne's avatar
Don Gagne committed
716 717
QList<MAV_CMD> APMFirmwarePlugin::supportedMissionCommands(void)
{
Tomaz Canabrava's avatar
Tomaz Canabrava committed
718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745
    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,
746
#if 0
747
    // Waiting for module update
Tomaz Canabrava's avatar
Tomaz Canabrava committed
748
        MAV_CMD_DO_SET_REVERSE,
749
#endif
Tomaz Canabrava's avatar
Tomaz Canabrava committed
750
    };
Don Gagne's avatar
Don Gagne committed
751
}
752

753
QString APMFirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
754
{
755 756
    switch (vehicleType) {
    case MAV_TYPE_GENERIC:
757
        return QStringLiteral(":/json/APM-MavCmdInfoCommon.json");
758
    case MAV_TYPE_FIXED_WING:
759
        return QStringLiteral(":/json/APM-MavCmdInfoFixedWing.json");
760
    case MAV_TYPE_QUADROTOR:
761
        return QStringLiteral(":/json/APM-MavCmdInfoMultiRotor.json");
762
    case MAV_TYPE_VTOL_QUADROTOR:
763
        return QStringLiteral(":/json/APM-MavCmdInfoVTOL.json");
764
    case MAV_TYPE_SUBMARINE:
765
        return QStringLiteral(":/json/APM-MavCmdInfoSub.json");
766
    case MAV_TYPE_GROUND_ROVER:
767
        return QStringLiteral(":/json/APM-MavCmdInfoRover.json");
768 769 770 771
    default:
        qWarning() << "APMFirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType;
        return QString();
    }
772
}
773 774 775 776 777

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

Don Gagne's avatar
Don Gagne committed
778 779
    APMParameterMetaData* metaData = new APMParameterMetaData();
    metaData->loadParameterFactMetaDataFile(metaDataFile);
780 781
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
782 783 784 785 786 787

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

DonLakeFlyer's avatar
DonLakeFlyer committed
788
void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware)
Don Gagne's avatar
Don Gagne committed
789 790 791 792 793 794
{
    Q_UNUSED(vehicle);

    QTcpSocket* socket = new QTcpSocket();

    socket->connectToHost(_artooIP, _artooVideoHandshakePort);
DonLakeFlyer's avatar
DonLakeFlyer committed
795
    if (originalSoloFirmware) {
796
#if QT_VERSION < QT_VERSION_CHECK(5, 15, 0)
DonLakeFlyer's avatar
DonLakeFlyer committed
797
        QObject::connect(socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError);
798 799 800
#else
        QObject::connect(socket, &QAbstractSocket::errorOccurred, this, &APMFirmwarePlugin::_artooSocketError);
#endif
DonLakeFlyer's avatar
DonLakeFlyer committed
801
    }
Don Gagne's avatar
Don Gagne committed
802 803 804 805
}

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

809
QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
810 811 812 813 814 815 816 817
{
    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:
DonLakeFlyer's avatar
DonLakeFlyer committed
818 819 820 821
        if (vehicle->versionCompare(4, 0, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.4.0.xml");
        }
        if (vehicle->versionCompare(3, 7, 0) >= 0) {
822
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.7.xml");
823
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
824
        if (vehicle->versionCompare(3, 6, 0) >= 0) {
825
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml");
Don Gagne's avatar
Don Gagne committed
826
        }
827
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
828

DonLakeFlyer's avatar
DonLakeFlyer committed
829 830 831 832 833 834 835
    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
836
    case MAV_TYPE_FIXED_WING:
DonLakeFlyer's avatar
DonLakeFlyer committed
837 838 839
        if (vehicle->versionCompare(4, 0, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.4.0.xml");
        }
840 841
        if (vehicle->versionCompare(3, 10, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.10.xml");
842
        }
843 844
        if (vehicle->versionCompare(3, 9, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.9.xml");
Don Gagne's avatar
Don Gagne committed
845
        }
846
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
847

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

861
    case MAV_TYPE_SUBMARINE:
862 863 864
        if (vehicle->versionCompare(4, 0, 0) >= 0) { // 4.0.x
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.4.0.xml");
        }
865 866
        if (vehicle->versionCompare(3, 6, 0) >= 0) { // 3.6.x
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.6.xml");
867
        }
868 869 870 871 872 873
        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
874 875 876 877
    default:
        return QString();
    }
}
878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895

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())) {
896
        qgcApp()->showAppMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
897 898 899 900 901 902 903 904 905 906 907
        return;
    }


    setGuidedMode(vehicle, true);

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

DonLakeFlyer's avatar
DonLakeFlyer committed
908
void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
909
{
DonLakeFlyer's avatar
DonLakeFlyer committed
910
    _setFlightModeAndValidate(vehicle, smartRTL ? smartRTLFlightMode() : rtlFlightMode());
911 912 913 914 915
}

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

920 921 922 923 924
    if (abs(altitudeChange) < 0.01) {
        // This prevents unecessary changes to Guided mode when the users selects pause and doesn't really touch the altitude slider
        return;
    }

925 926 927 928 929 930 931
    setGuidedMode(vehicle, true);

    mavlink_message_t msg;
    mavlink_set_position_target_local_ned_t cmd;

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

932 933
    cmd.target_system    = static_cast<uint8_t>(vehicle->id());
    cmd.target_component = static_cast<uint8_t>(vehicle->defaultComponentId());
934 935 936 937
    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;
938
    cmd.z = static_cast<float>(-(altitudeChange));
939 940

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
941
    mavlink_msg_set_position_target_local_ned_encode_chan(
942 943 944 945 946
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                &msg,
                &cmd);
947

948
    vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), msg);
949
}
950

951
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
952
{
953
    _guidedModeTakeoff(vehicle, altitudeRel);
954 955
}

956 957
double APMFirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
958
    double minTakeoffAlt = 0;
959 960 961 962
    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)) {
963
        minTakeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble() / static_cast<double>(paramDivisor);
DonLakeFlyer's avatar
DonLakeFlyer committed
964 965 966 967
    }

    if (minTakeoffAlt == 0) {
        minTakeoffAlt = FirmwarePlugin::minimumTakeoffAltitude(vehicle);
968
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
969 970

    return minTakeoffAlt;
971 972
}

973
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
974
{
DonLakeFlyer's avatar
DonLakeFlyer committed
975
    if (!vehicle->multiRotor() && !vehicle->vtol()) {
976
        qgcApp()->showAppMessage(tr("Vehicle does not support guided takeoff"));
DonLakeFlyer's avatar
DonLakeFlyer committed
977 978 979
        return false;
    }

980 981
    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
    if (qIsNaN(vehicleAltitudeAMSL)) {
982
        qgcApp()->showAppMessage(tr("Unable to takeoff, vehicle position not known."));
983 984 985
        return false;
    }

986
    double takeoffAltRel = minimumTakeoffAltitude(vehicle);
987 988
    if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) {
        takeoffAltRel = altitudeRel;
989 990 991
    }

    if (!_setFlightModeAndValidate(vehicle, "Guided")) {
992
        qgcApp()->showAppMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
993 994 995 996
        return false;
    }

    if (!_armVehicleAndValidate(vehicle)) {
997
        qgcApp()->showAppMessage(tr("Unable to takeoff: Vehicle failed to arm."));
998 999 1000 1001 1002 1003 1004
        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,
1005
                            static_cast<float>(takeoffAltRel));                     // Relative altitude
1006 1007 1008 1009 1010 1011

    return true;
}

void APMFirmwarePlugin::startMission(Vehicle* vehicle)
{
1012 1013 1014
    if (vehicle->flying()) {
        // Vehicle already in the air, we just need to switch to auto
        if (!_setFlightModeAndValidate(vehicle, "Auto")) {
1015
            qgcApp()->showAppMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
1016 1017 1018 1019
        }
        return;
    }

1020 1021 1022
    if (!vehicle->armed()) {
        // First switch to flight mode we can arm from
        if (!_setFlightModeAndValidate(vehicle, "Guided")) {
1023
            qgcApp()->showAppMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode."));
1024
            return;
1025
        }
1026

1027
        if (!_armVehicleAndValidate(vehicle)) {
1028
            qgcApp()->showAppMessage(tr("Unable to start mission: Vehicle failed to arm."));
1029 1030
            return;
        }
1031 1032
    }

1033 1034
    if (vehicle->fixedWing()) {
        if (!_setFlightModeAndValidate(vehicle, "Auto")) {
1035
            qgcApp()->showAppMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
1036 1037 1038 1039
            return;
        }
    } else {
        vehicle->sendMavCommand(vehicle->defaultComponentId(), MAV_CMD_MISSION_START, true /*show error */);
1040 1041
    }
}
1042 1043 1044 1045

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

    if (qobject_cast<ArduPlaneFirmwarePlugin*>(vehicle->firmwarePlugin())) {
1048
        return baseUrl.arg("Plane");
1049
    } else if (qobject_cast<ArduRoverFirmwarePlugin*>(vehicle->firmwarePlugin())) {
1050
        return baseUrl.arg("Rover");
1051
    } else if (qobject_cast<ArduSubFirmwarePlugin*>(vehicle->firmwarePlugin())) {
1052
        return baseUrl.arg("Sub");
1053 1054 1055 1056 1057
    } else if (qobject_cast<ArduCopterFirmwarePlugin*>(vehicle->firmwarePlugin())) {
        return baseUrl.arg("Copter");
    } else {
        qWarning() << "APMFirmwarePlugin::_getLatestVersionFileUrl Unknown vehicle firmware type" << vehicle->vehicleType();
        return QString();
1058 1059 1060 1061 1062 1063
    }
}

QString APMFirmwarePlugin::_versionRegex() {
    return QStringLiteral(" V([0-9,\\.]*)$");
}
1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074

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(
1075 1076 1077 1078 1079
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                message,
                &channels);
1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091
}

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(
1092 1093 1094 1095 1096
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                message,
                &channels);
1097 1098
}

1099 1100
void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities)
{
1101
    if (!vehicle->homePosition().isValid()) {
1102 1103 1104
        static bool sentOnce = false;
        if (!sentOnce) {
            sentOnce = true;
1105
            qgcApp()->showAppMessage(tr("Follow failed: Home position not set."));
1106 1107 1108 1109
        }
        return;
    }

1110
    if (!(estimationCapabilities & (FollowMe::POS | FollowMe::VEL | FollowMe::HEADING))) {
1111 1112 1113 1114
        static bool sentOnce = false;
        if (!sentOnce) {
            sentOnce = true;
            qWarning() << "APMFirmwarePlugin::_sendGCSMotionReport estimateCapabilities" << estimationCapabilities;
1115
            qgcApp()->showAppMessage(tr("Follow failed: Ground station cannot provide required position information."));
1116 1117 1118 1119 1120 1121 1122 1123 1124
        }
        return;
    }

    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();

    mavlink_global_position_int_t globalPositionInt;
    memset(&globalPositionInt, 0, sizeof(globalPositionInt));

1125
    // Important note: QGC only supports sending the constant GCS home position altitude for follow me.
1126 1127 1128
    globalPositionInt.time_boot_ms =    static_cast<uint32_t>(qgcApp()->msecsSinceBoot());
    globalPositionInt.lat =             motionReport.lat_int;
    globalPositionInt.lon =             motionReport.lon_int;
1129 1130 1131 1132 1133 1134
    globalPositionInt.alt =             static_cast<int32_t>(vehicle->homePosition().altitude() * 1000);    // mm
    globalPositionInt.relative_alt =    static_cast<int32_t>(0);                                            // mm
    globalPositionInt.vx =              static_cast<int16_t>(motionReport.vxMetersPerSec * 100);            // cm/sec
    globalPositionInt.vy =              static_cast<int16_t>(motionReport.vyMetersPerSec * 100);            // cm/sec
    globalPositionInt.vy =              static_cast<int16_t>(motionReport.vzMetersPerSec * 100);            // cm/sec
    globalPositionInt.hdg =             static_cast<uint16_t>(motionReport.headingDegrees * 100.0);         // centi-degrees
1135 1136 1137 1138 1139 1140 1141

    mavlink_message_t message;
    mavlink_msg_global_position_int_encode_chan(static_cast<uint8_t>(mavlinkProtocol->getSystemId()),
                                          static_cast<uint8_t>(mavlinkProtocol->getComponentId()),
                                          vehicle->priorityLink()->mavlinkChannel(),
                                          &message,
                                          &globalPositionInt);
1142
    vehicle->sendMessageOnLinkThreadSafe(vehicle->priorityLink(), message);
1143
}