APMFirmwarePlugin.cc 40.3 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"
Don Gagne's avatar
Don Gagne committed
23

Don Gagne's avatar
Don Gagne committed
24 25
#include <QTcpSocket>

Pritam Ghanghas's avatar
Pritam Ghanghas committed
26
QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")
Don Gagne's avatar
Don Gagne committed
27

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

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

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

Don Gagne's avatar
Don Gagne committed
48 49
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
50 51 52 53 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

/*
 * @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
97 98
        qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything"
                                        << "version text to be parsed" << versionText;
99 100 101 102 103 104
        return;
    }

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

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

120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143

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

144 145 146 147 148 149 150
APMFirmwarePluginInstanceData::APMFirmwarePluginInstanceData(QObject* parent)
    : QObject(parent)
    , textSeverityAdjustmentNeeded(false)
{

}

151
APMFirmwarePlugin::APMFirmwarePlugin(void)
Don Gagne's avatar
Don Gagne committed
152
    : _coaxialMotors(false)
Don Gagne's avatar
Don Gagne committed
153
{
154 155 156 157
    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
158

159 160 161
AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
{
    return new APMAutoPilotPlugin(vehicle, vehicle);
Don Gagne's avatar
Don Gagne committed
162 163
}

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

173
    return (capabilities & available) == capabilities;
Don Gagne's avatar
Don Gagne committed
174 175 176 177 178
}

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

Don Gagne's avatar
Don Gagne committed
180 181 182
    return QList<VehicleComponent*>();
}

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

Don Gagne's avatar
Don Gagne committed
195
QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
196
{
197 198 199 200 201 202 203 204 205 206
    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
207 208 209 210
}

bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229
    *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
230 231
}

232
void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
233
{
Don Gagne's avatar
Don Gagne committed
234 235 236 237 238
    Q_UNUSED(vehicle);

    mavlink_param_value_t paramValue;
    mavlink_param_union_t paramUnion;

Don Gagne's avatar
Don Gagne committed
239 240
    memset(&paramValue, 0, sizeof(paramValue));

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

Don Gagne's avatar
Don Gagne committed
272 273
    paramValue.param_value = paramUnion.param_float;

274 275 276
    // 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;
277 278
    mavlink_msg_param_value_encode_chan(message->sysid,
                                        message->compid,
279
                                        0,                  // Re-encoding uses reserved channel 0
280 281
                                        message,
                                        &paramValue);
Don Gagne's avatar
Don Gagne committed
282 283
}

284
void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
285 286 287 288 289 290
{
    Q_UNUSED(vehicle);

    mavlink_param_set_t     paramSet;
    mavlink_param_union_t   paramUnion;

Don Gagne's avatar
Don Gagne committed
291 292
    memset(&paramSet, 0, sizeof(paramSet));

Don Gagne's avatar
Don Gagne committed
293 294 295 296 297
    // 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);

298 299 300 301 302
    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
303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328
    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
329 330
    }

331
    mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, &paramSet);
Don Gagne's avatar
Don Gagne committed
332 333
}

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

Don Gagne's avatar
Don Gagne committed
339 340 341 342 343 344
    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
345

Don Gagne's avatar
Don Gagne committed
346 347
    if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || severity < MAV_SEVERITY_NOTICE) {
        messageText = _getMessageText(message, longVersion);
Don Gagne's avatar
Don Gagne committed
348 349
        qCDebug(APMFirmwarePluginLog) << messageText;

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

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

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

                switch (vehicle->vehicleType()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
363 364 365 366 367 368 369
                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
370 371
                case MAV_TYPE_FIXED_WING:
                    supportedMajorNumber = 3;
372
                    supportedMinorNumber = 8;
Don Gagne's avatar
Don Gagne committed
373 374
                    break;
                case MAV_TYPE_QUADROTOR:
DonLakeFlyer's avatar
DonLakeFlyer committed
375 376
                    // 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
377 378 379 380 381 382 383
                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;
384
                    supportedMinorNumber = 5;
Don Gagne's avatar
Don Gagne committed
385
                    break;
386 387 388 389
                case MAV_TYPE_GROUND_ROVER:
                case MAV_TYPE_SURFACE_BOAT:
                    supportedMajorNumber = 3;
                    supportedMinorNumber = 4;
Don Gagne's avatar
Don Gagne committed
390 391 392
                default:
                    break;
                }
Don Gagne's avatar
Don Gagne committed
393

Don Gagne's avatar
Don Gagne committed
394 395
                if (supportedMajorNumber != -1) {
                    if (firmwareVersion.majorNumber() < supportedMajorNumber || firmwareVersion.minorNumber() < supportedMinorNumber) {
396
                        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));
397
                    }
Don Gagne's avatar
Don Gagne committed
398 399
                }
            }
Don Gagne's avatar
Don Gagne committed
400
        }
Don Gagne's avatar
Don Gagne committed
401

Don Gagne's avatar
Don Gagne committed
402 403
        // 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.
404

Don Gagne's avatar
Don Gagne committed
405 406
        if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
            _adjustCalibrationMessageSeverity(message);
407
            return true;
408
        }
Don Gagne's avatar
Don Gagne committed
409
    }
410

Don Gagne's avatar
Don Gagne committed
411
    // adjust mesasge if needed
412
    if (instanceData->textSeverityAdjustmentNeeded) {
Don Gagne's avatar
Don Gagne committed
413 414
        _adjustSeverity(message);
    }
415

Don Gagne's avatar
Don Gagne committed
416
    if (messageText.isEmpty()) {
Don Gagne's avatar
Don Gagne committed
417
        messageText = _getMessageText(message, longVersion);
Don Gagne's avatar
Don Gagne committed
418 419 420 421
    }

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

Don Gagne's avatar
Don Gagne committed
427 428
    if (messageText.contains(APM_SOLO_REXP)) {
        qDebug() << "Found Solo";
429
        vehicle->setSoloFirmware(true);
Don Gagne's avatar
Don Gagne committed
430 431

        // Fix up severity
Don Gagne's avatar
Don Gagne committed
432
        _setInfoSeverity(message, longVersion);
Don Gagne's avatar
Don Gagne committed
433 434

        // Start TCP video handshake with ARTOO
DonLakeFlyer's avatar
DonLakeFlyer committed
435
        _soloVideoHandshake(vehicle, true /* originalSoloFirmware */);
Don Gagne's avatar
Don Gagne committed
436 437 438 439 440 441 442 443 444
    } 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
445 446
    }

447
    return true;
Don Gagne's avatar
Don Gagne committed
448 449
}

450
void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
451 452
{
    bool flying = false;
453

454 455
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(message, &heartbeat);
Don Gagne's avatar
Don Gagne committed
456

457 458 459
    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
460

461 462 463 464 465
            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;
            }
466
        }
467
        vehicle->_setFlying(flying);
468
    }
Don Gagne's avatar
Don Gagne committed
469

470 471 472 473 474 475
    // 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
476 477
}

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

486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502
    // 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
503
    }
504 505

    return true;
Don Gagne's avatar
Don Gagne committed
506 507
}

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

Don Gagne's avatar
Don Gagne committed
517
QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message, bool longVersion) const
518 519 520
{
    QByteArray b;

Don Gagne's avatar
Don Gagne committed
521 522 523 524 525 526 527
    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());
    }
528 529 530 531 532 533

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

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

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

Don Gagne's avatar
Don Gagne committed
559
    return adjustmentNeeded;
Pritam Ghanghas's avatar
Pritam Ghanghas committed
560
}
561

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

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

Don Gagne's avatar
Don Gagne committed
589
void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message, bool longVersion) const
590
{
591 592 593
    // 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
594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615

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

Don Gagne's avatar
Don Gagne committed
618 619 620 621
void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
622 623 624
    // 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
625
    statusText.severity = MAV_SEVERITY_INFO;
626
    mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText);
Don Gagne's avatar
Don Gagne committed
627 628
}

629 630
void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
{
631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654
    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));
        }
    }
655 656 657
}


658 659
void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
660 661
    vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData);

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

void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)
{
    _supportedModes = supportedModes;
}
705 706 707 708 709 710

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

712
void APMFirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
713
{
714
    APMParameterMetaData* apmMetaData = qobject_cast<APMParameterMetaData*>(parameterMetaData);
Don Gagne's avatar
Don Gagne committed
715

716 717 718 719 720 721
    if (apmMetaData) {
        apmMetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to APMFirmwarePlugin::addMetaDataToFact not APMParameterMetaData";
    }
}
722

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

760
QString APMFirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
761
{
762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778
    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();
    }
779
}
780 781 782 783 784

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

Don Gagne's avatar
Don Gagne committed
785 786
    APMParameterMetaData* metaData = new APMParameterMetaData();
    metaData->loadParameterFactMetaDataFile(metaDataFile);
787 788
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
789 790 791 792 793 794

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

DonLakeFlyer's avatar
DonLakeFlyer committed
795
void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware)
Don Gagne's avatar
Don Gagne committed
796 797 798 799 800 801
{
    Q_UNUSED(vehicle);

    QTcpSocket* socket = new QTcpSocket();

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

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
811

812
QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
813 814 815 816 817 818 819 820
{
    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:
821
        if (vehicle->versionCompare(3, 7, 0) >= 0) {  // 3.7.0 and higher
822
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.7.xml");
823
        }
824
        if (vehicle->versionCompare(3, 6, 0) >= 0) {  // 3.6.0 and higher
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:
837 838
        if (vehicle->versionCompare(3, 10, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.10.xml");
839
        }
840 841
        if (vehicle->versionCompare(3, 9, 0) >= 0) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.9.xml");
Don Gagne's avatar
Don Gagne committed
842
        }
843
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
844

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

855
    case MAV_TYPE_SUBMARINE:
856
        if (vehicle->versionCompare(3, 6, 0) >= 0) { // 3.5.x
857
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.6dev.xml");
858
        }
859 860 861 862 863 864
        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
865 866 867 868
    default:
        return QString();
    }
}
869 870 871 872 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

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
899
void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
900
{
DonLakeFlyer's avatar
DonLakeFlyer committed
901
    _setFlightModeAndValidate(vehicle, smartRTL ? smartRTLFlightMode() : rtlFlightMode());
902 903 904 905 906
}

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

    setGuidedMode(vehicle, true);

    mavlink_message_t msg;
    mavlink_set_position_target_local_ned_t cmd;

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

918 919
    cmd.target_system    = static_cast<uint8_t>(vehicle->id());
    cmd.target_component = static_cast<uint8_t>(vehicle->defaultComponentId());
920 921 922 923
    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;
924
    cmd.z = static_cast<float>(-(altitudeChange));
925 926

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
927
    mavlink_msg_set_position_target_local_ned_encode_chan(
928 929 930 931 932
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                &msg,
                &cmd);
933 934 935

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

937
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
938
{
939
    _guidedModeTakeoff(vehicle, altitudeRel);
940 941
}

942 943
double APMFirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
944
    double minTakeoffAlt = 0;
945 946 947 948
    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)) {
949
        minTakeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble() / static_cast<double>(paramDivisor);
DonLakeFlyer's avatar
DonLakeFlyer committed
950 951 952 953
    }

    if (minTakeoffAlt == 0) {
        minTakeoffAlt = FirmwarePlugin::minimumTakeoffAltitude(vehicle);
954
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
955 956

    return minTakeoffAlt;
957 958
}

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

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

972
    double takeoffAltRel = minimumTakeoffAltitude(vehicle);
973 974
    if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) {
        takeoffAltRel = altitudeRel;
975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990
    }

    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,
991
                            static_cast<float>(takeoffAltRel));                     // Relative altitude
992 993 994 995 996 997

    return true;
}

void APMFirmwarePlugin::startMission(Vehicle* vehicle)
{
998 999 1000 1001 1002 1003 1004 1005
    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;
    }

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

1013 1014 1015 1016
        if (!_armVehicleAndValidate(vehicle)) {
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
            return;
        }
1017 1018
    }

1019 1020 1021 1022 1023 1024 1025
    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 */);
1026 1027
    }
}
1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049

QString APMFirmwarePlugin::_getLatestVersionFileUrl(Vehicle* vehicle)
{
    const static QString baseUrl("http://firmware.ardupilot.org/%1/stable/PX4/git-version.txt");
    if (vehicle->fixedWing()) {
        return baseUrl.arg("Plane");
    }
    if (vehicle->vtol()) {
        return baseUrl.arg("Plane");
    }
    if (vehicle->rover()) {
        return baseUrl.arg("Rover");
    }
    if (vehicle->sub()) {
        return baseUrl.arg("Sub");
    }
    return baseUrl.arg("Copter");
}

QString APMFirmwarePlugin::_versionRegex() {
    return QStringLiteral(" V([0-9,\\.]*)$");
}
1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060

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(
1061 1062 1063 1064 1065
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                message,
                &channels);
1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077
}

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(
1078 1079 1080 1081 1082
                static_cast<uint8_t>(mavlink->getSystemId()),
                static_cast<uint8_t>(mavlink->getComponentId()),
                vehicle->priorityLink()->mavlinkChannel(),
                message,
                &channels);
1083 1084
}