APMFirmwarePlugin.cc 29.7 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 11 12 13 14

/// @file
///     @author Don Gagne <don@thegagnes.com>

#include "APMFirmwarePlugin.h"
15
#include "APMAutoPilotPlugin.h"
16
#include "QGCMAVLink.h"
17
#include "QGCApplication.h"
18 19 20
#include "APMFlightModesComponentController.h"
#include "APMAirframeComponentController.h"
#include "APMSensorsComponentController.h"
Don Gagne's avatar
Don Gagne committed
21

Don Gagne's avatar
Don Gagne committed
22 23
#include <QTcpSocket>

Pritam Ghanghas's avatar
Pritam Ghanghas committed
24
QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")
Don Gagne's avatar
Don Gagne committed
25

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

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

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

Don Gagne's avatar
Don Gagne committed
46 47
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
48 49 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

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

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

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

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

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

142
APMFirmwarePlugin::APMFirmwarePlugin(void)
Don Gagne's avatar
Don Gagne committed
143 144
    : _coaxialMotors(false)
    , _textSeverityAdjustmentNeeded(false)
Don Gagne's avatar
Don Gagne committed
145
{
146 147 148 149
    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
150

151 152 153
AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
{
    return new APMAutoPilotPlugin(vehicle, vehicle);
Don Gagne's avatar
Don Gagne committed
154 155
}

156
bool APMFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
Don Gagne's avatar
Don Gagne committed
157
{
Don Gagne's avatar
Don Gagne committed
158
    return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
Don Gagne's avatar
Don Gagne committed
159 160 161 162 163
}

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

Don Gagne's avatar
Don Gagne committed
165 166 167
    return QList<VehicleComponent*>();
}

Daniel Agar's avatar
Daniel Agar committed
168
QStringList APMFirmwarePlugin::flightModes(Vehicle* vehicle)
169
{
Daniel Agar's avatar
Daniel Agar committed
170
    Q_UNUSED(vehicle)
171 172 173 174 175 176 177
    QStringList flightModesList;
    foreach (const APMCustomMode& customMode, _supportedModes) {
        if (customMode.canBeSet()) {
            flightModesList << customMode.modeString();
        }
    }
    return flightModesList;
Don Gagne's avatar
Don Gagne committed
178 179
}

Don Gagne's avatar
Don Gagne committed
180
QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
181
{
182 183 184 185 186 187 188 189 190 191
    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
192 193 194 195
}

bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214
    *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
215 216 217 218 219 220 221 222 223
}

int APMFirmwarePlugin::manualControlReservedButtonCount(void)
{
    // We don't know whether the firmware is going to used any of these buttons.
    // So reserve them all.
    return -1;
}

224
void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
225
{
Don Gagne's avatar
Don Gagne committed
226 227 228 229 230
    Q_UNUSED(vehicle);

    mavlink_param_value_t paramValue;
    mavlink_param_union_t paramUnion;

Don Gagne's avatar
Don Gagne committed
231 232
    memset(&paramValue, 0, sizeof(paramValue));

Don Gagne's avatar
Don Gagne committed
233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261
    // 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:
        paramUnion.param_uint8 = (uint8_t)paramValue.param_value;
        break;
    case MAV_PARAM_TYPE_INT8:
        paramUnion.param_int8 = (int8_t)paramValue.param_value;
        break;
    case MAV_PARAM_TYPE_UINT16:
        paramUnion.param_uint16 = (uint16_t)paramValue.param_value;
        break;
    case MAV_PARAM_TYPE_INT16:
        paramUnion.param_int16 = (int16_t)paramValue.param_value;
        break;
    case MAV_PARAM_TYPE_UINT32:
        paramUnion.param_uint32 = (uint32_t)paramValue.param_value;
        break;
    case MAV_PARAM_TYPE_INT32:
        paramUnion.param_int32 = (int32_t)paramValue.param_value;
        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;
262 263
    }

Don Gagne's avatar
Don Gagne committed
264 265
    paramValue.param_value = paramUnion.param_float;

266 267 268
    // 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;
269 270
    mavlink_msg_param_value_encode_chan(message->sysid,
                                        message->compid,
271
                                        0,                  // Re-encoding uses reserved channel 0
272 273
                                        message,
                                        &paramValue);
Don Gagne's avatar
Don Gagne committed
274 275
}

276
void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
277 278 279 280 281 282
{
    Q_UNUSED(vehicle);

    mavlink_param_set_t     paramSet;
    mavlink_param_union_t   paramUnion;

Don Gagne's avatar
Don Gagne committed
283 284
    memset(&paramSet, 0, sizeof(paramSet));

Don Gagne's avatar
Don Gagne committed
285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315
    // 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);

    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
316 317
    }

318
    mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, &paramSet);
Don Gagne's avatar
Don Gagne committed
319 320
}

321
bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
322 323
{
    QString messageText;
324

Don Gagne's avatar
Don Gagne committed
325 326 327
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);

328
    if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || statusText.severity < MAV_SEVERITY_NOTICE) {
Don Gagne's avatar
Don Gagne committed
329 330 331
        messageText = _getMessageText(message);
        qCDebug(APMFirmwarePluginLog) << messageText;

Don Gagne's avatar
Don Gagne committed
332
        if (!messageText.contains(APM_SOLO_REXP)) {
Don Gagne's avatar
Don Gagne committed
333
            // if don't know firmwareVersion yet, try and see if this message contains it
334
            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
335
                // found version string
Don Gagne's avatar
Don Gagne committed
336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361
                APMFirmwareVersion firmwareVersion(messageText);
                _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(firmwareVersion);

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

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

                switch (vehicle->vehicleType()) {
                case MAV_TYPE_FIXED_WING:
                    supportedMajorNumber = 3;
                    supportedMinorNumber = 2;
                    break;
                case MAV_TYPE_QUADROTOR:
                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;
                    supportedMinorNumber = 2;
                    break;
                default:
                    break;
                }
Don Gagne's avatar
Don Gagne committed
362

Don Gagne's avatar
Don Gagne committed
363 364 365
                if (supportedMajorNumber != -1) {
                    if (firmwareVersion.majorNumber() < supportedMajorNumber || firmwareVersion.minorNumber() < supportedMinorNumber) {
                        qgcApp()->showMessage(QString("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));
366
                    }
Don Gagne's avatar
Don Gagne committed
367 368
                }
            }
Don Gagne's avatar
Don Gagne committed
369
        }
Don Gagne's avatar
Don Gagne committed
370

Don Gagne's avatar
Don Gagne committed
371 372
        // 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.
373

Don Gagne's avatar
Don Gagne committed
374 375
        if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
            _adjustCalibrationMessageSeverity(message);
376
            return true;
377
        }
Don Gagne's avatar
Don Gagne committed
378
    }
379

Don Gagne's avatar
Don Gagne committed
380 381 382 383
    // adjust mesasge if needed
    if (_textSeverityAdjustmentNeeded) {
        _adjustSeverity(message);
    }
384

Don Gagne's avatar
Don Gagne committed
385 386 387 388 389 390
    if (messageText.isEmpty()) {
        messageText = _getMessageText(message);
    }

    // The following messages are incorrectly labeled as warning message.
    // Fixed in newer firmware (unreleased at this point), but still in older firmware.
391
    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
392 393 394
            messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
        _setInfoSeverity(message);
    }
395

Don Gagne's avatar
Don Gagne committed
396 397
    if (messageText.contains(APM_SOLO_REXP)) {
        qDebug() << "Found Solo";
398
        vehicle->setSoloFirmware(true);
Don Gagne's avatar
Don Gagne committed
399 400 401 402 403 404

        // Fix up severity
        _setInfoSeverity(message);

        // Start TCP video handshake with ARTOO
        _soloVideoHandshake(vehicle);
Don Gagne's avatar
Don Gagne committed
405 406 407 408 409 410 411 412 413
    } 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
414 415
    }

416
    if (messageText.startsWith("PreArm")) {
Don Gagne's avatar
Don Gagne committed
417 418
        // ArduPilot PreArm messages can come across very frequently especially on Solo, which seems to send them once a second.
        // Filter them out if they come too quickly.
419 420 421 422
        if (_noisyPrearmMap.contains(messageText) && _noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
            return false;
        }
        _noisyPrearmMap[messageText] = QTime::currentTime();
Don Gagne's avatar
Don Gagne committed
423 424

        vehicle->setPrearmError(messageText);
425 426 427
    }

    return true;
Don Gagne's avatar
Don Gagne committed
428 429
}

430
void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
431 432
{
    bool flying = false;
433

Don Gagne's avatar
Don Gagne committed
434 435 436 437 438 439 440 441 442 443
    // We pull Vehicle::flying state from HEARTBEAT on ArduPilot. This is a firmware specific test.

    if (vehicle->armed()) {
        mavlink_heartbeat_t heartbeat;
        mavlink_msg_heartbeat_decode(message, &heartbeat);

        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;
444
        }
445
    }
Don Gagne's avatar
Don Gagne committed
446 447 448 449

    vehicle->setFlying(flying);
}

450
bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
451 452 453
{
    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
454
        return true;
Don Gagne's avatar
Don Gagne committed
455 456 457 458
    }

    switch (message->msgid) {
    case MAVLINK_MSG_ID_PARAM_VALUE:
459
        _handleIncomingParamValue(vehicle, message);
Don Gagne's avatar
Don Gagne committed
460 461
        break;
    case MAVLINK_MSG_ID_STATUSTEXT:
462
        return _handleIncomingStatusText(vehicle, message);
Don Gagne's avatar
Don Gagne committed
463
    case MAVLINK_MSG_ID_HEARTBEAT:
464
        _handleIncomingHeartbeat(vehicle, message);
Don Gagne's avatar
Don Gagne committed
465 466
        break;
    }
467 468

    return true;
Don Gagne's avatar
Don Gagne committed
469 470
}

471
void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
472 473 474 475 476 477 478 479
{
    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
        return;
    }

    switch (message->msgid) {
    case MAVLINK_MSG_ID_PARAM_SET:
480
        _handleOutgoingParamSet(vehicle, outgoingLink, message);
Don Gagne's avatar
Don Gagne committed
481 482
        break;
    }
483 484
}

485 486 487 488 489 490 491 492 493 494 495 496
QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const
{
    QByteArray b;

    b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
    mavlink_msg_statustext_get_text(message, b.data());

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

Pritam Ghanghas's avatar
Pritam Ghanghas committed
497
bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
498
{
Don Gagne's avatar
Don Gagne committed
499 500 501
    if (!firmwareVersion.isValid()) {
        return false;
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
502

Don Gagne's avatar
Don Gagne committed
503 504 505 506 507 508 509 510 511 512 513 514 515
    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;
        }
516 517 518 519
    } 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
520
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
521

Don Gagne's avatar
Don Gagne committed
522
    return adjustmentNeeded;
Pritam Ghanghas's avatar
Pritam Ghanghas committed
523
}
524

Pritam Ghanghas's avatar
Pritam Ghanghas committed
525 526 527
void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
{
    // lets make QGC happy with right severity values
528 529 530
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
    switch(statusText.severity) {
Don Gagne's avatar
Don Gagne committed
531 532 533 534 535 536 537 538 539
    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;
540 541
    }

542 543 544 545 546 547 548 549
    // 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
550
}
551

552 553 554 555 556
void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);

557 558 559
    // 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;
560
    statusText.severity = MAV_SEVERITY_INFO;
561 562 563 564 565
    mavlink_msg_statustext_encode_chan(message->sysid,
                                       message->compid,
                                       0,                  // Re-encoding uses reserved channel 0
                                       message,
                                       &statusText);
566 567
}

Don Gagne's avatar
Don Gagne committed
568 569 570 571
void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
572 573 574
    // 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
575
    statusText.severity = MAV_SEVERITY_INFO;
576
    mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText);
Don Gagne's avatar
Don Gagne committed
577 578
}

579 580
void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614
    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:
            vehicle->setFirmwareVersion(3, 4, 0);
            break;
        case MAV_TYPE_FIXED_WING:
            vehicle->setFirmwareVersion(3, 5, 0);
            break;
        case MAV_TYPE_GROUND_ROVER:
        case MAV_TYPE_SURFACE_BOAT:
            vehicle->setFirmwareVersion(3, 0, 0);
            break;
        case MAV_TYPE_SUBMARINE:
            vehicle->setFirmwareVersion(3, 4, 0);
            break;
        default:
            // No version set
            break;
        }
    } else {
        // Streams are not started automatically on APM stack
        vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,     2);
        vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2);
        vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,     2);
        vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,        3);
        vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,          10);
        vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,          10);
        vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,          3);
    }
615
}
616 617 618 619 620

void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)
{
    _supportedModes = supportedModes;
}
621 622 623 624 625 626

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

628
void APMFirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
629
{
630
    APMParameterMetaData* apmMetaData = qobject_cast<APMParameterMetaData*>(parameterMetaData);
Don Gagne's avatar
Don Gagne committed
631

632 633 634 635 636 637
    if (apmMetaData) {
        apmMetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to APMFirmwarePlugin::addMetaDataToFact not APMParameterMetaData";
    }
}
638

Don Gagne's avatar
Don Gagne committed
639 640 641 642
QList<MAV_CMD> APMFirmwarePlugin::supportedMissionCommands(void)
{
    QList<MAV_CMD> list;

643 644
    list << MAV_CMD_NAV_WAYPOINT
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TURNS << MAV_CMD_NAV_LOITER_TIME
645
         << MAV_CMD_NAV_RETURN_TO_LAUNCH << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
646 647 648
         << MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT
         << MAV_CMD_NAV_LOITER_TO_ALT
         << MAV_CMD_NAV_SPLINE_WAYPOINT
649
         << MAV_CMD_NAV_GUIDED_ENABLE
650 651 652 653 654 655
         << 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
656 657
         << MAV_CMD_DO_SET_RELAY << MAV_CMD_DO_REPEAT_RELAY
         << MAV_CMD_DO_SET_SERVO << MAV_CMD_DO_REPEAT_SERVO
658 659
         << MAV_CMD_DO_LAND_START
         << MAV_CMD_DO_SET_ROI
660 661
         << MAV_CMD_DO_DIGICAM_CONFIGURE << MAV_CMD_DO_DIGICAM_CONTROL
         << MAV_CMD_DO_MOUNT_CONTROL
662 663 664 665 666 667 668 669 670
         << 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;
#if 0
671 672
    // Waiting for module update
    << MAV_CMD_DO_SET_REVERSE;
673
#endif
Don Gagne's avatar
Don Gagne committed
674

Don Gagne's avatar
Don Gagne committed
675 676
    return list;
}
677

678
QString APMFirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
679
{
680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702
    switch (vehicleType) {
    case MAV_TYPE_GENERIC:
        return QStringLiteral(":/json/APM/MavCmdInfoCommon.json");
        break;
    case MAV_TYPE_FIXED_WING:
        return QStringLiteral(":/json/APM/MavCmdInfoFixedWing.json");
        break;
    case MAV_TYPE_QUADROTOR:
        return QStringLiteral(":/json/APM/MavCmdInfoMultiRotor.json");
        break;
    case MAV_TYPE_VTOL_QUADROTOR:
        return QStringLiteral(":/json/APM/MavCmdInfoVTOL.json");
        break;
    case MAV_TYPE_SUBMARINE:
        return QStringLiteral(":/json/APM/MavCmdInfoSub.json");
        break;
    case MAV_TYPE_GROUND_ROVER:
        return QStringLiteral(":/json/APM/MavCmdInfoRover.json");
        break;
    default:
        qWarning() << "APMFirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType;
        return QString();
    }
703
}
704 705 706 707 708

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

Don Gagne's avatar
Don Gagne committed
709 710
    APMParameterMetaData* metaData = new APMParameterMetaData();
    metaData->loadParameterFactMetaDataFile(metaDataFile);
711 712
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
713 714 715 716 717 718 719 720 721 722 723

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

void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
    // Best we can do in this case
    vehicle->setFlightMode("Loiter");
}
Don Gagne's avatar
Don Gagne committed
724 725 726 727 728 729 730 731 732 733 734 735 736 737 738

void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle)
{
    Q_UNUSED(vehicle);

    QTcpSocket* socket = new QTcpSocket();

    socket->connectToHost(_artooIP, _artooVideoHandshakePort);
    QObject::connect(socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError);
}

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
739

740
QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
741
{
742 743 744
    int majorVersion = vehicle->firmwareMajorVersion();
    int minorVersion = vehicle->firmwareMinorVersion();

Don Gagne's avatar
Don Gagne committed
745 746 747 748 749 750 751
    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:
752
        if (majorVersion < 3) {
Don Gagne's avatar
Don Gagne committed
753
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
754 755 756 757 758 759 760 761 762 763 764 765 766
        } else if (majorVersion == 3) {
            switch (minorVersion) {
            case 3:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
            case 4:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml");
            case 5:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
            default:
                if (minorVersion < 3) {
                    return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
                }
            }
Don Gagne's avatar
Don Gagne committed
767
        }
768
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
Don Gagne's avatar
Don Gagne committed
769
    case MAV_TYPE_FIXED_WING:
770
        if (majorVersion < 3) {
Don Gagne's avatar
Don Gagne committed
771
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
772 773 774 775 776 777 778 779 780 781 782 783 784 785 786
        } else if (majorVersion == 3) {
            switch (minorVersion) {
            case 3:
            case 4:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
            case 5:
            case 6:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.5.xml");
            case 7:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.7.xml");
            default:
                if (minorVersion < 3) {
                    return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
                }
            }
Don Gagne's avatar
Don Gagne committed
787
        }
788
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
Don Gagne's avatar
Don Gagne committed
789 790
    case MAV_TYPE_GROUND_ROVER:
    case MAV_TYPE_SURFACE_BOAT:
791 792 793 794 795 796 797 798 799 800 801 802
        if (majorVersion < 3) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.0.xml");
        } else if (majorVersion == 3) {
            switch (minorVersion) {
            case 0:
            case 1:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.0.xml");
            default:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml");
            }
        }
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml");
803
    case MAV_TYPE_SUBMARINE:
804 805 806 807 808
        if (vehicle->firmwareMajorVersion() < 3 || (vehicle->firmwareMajorVersion() == 3 && vehicle->firmwareMinorVersion() <= 4)) {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.4.xml");
        } else {
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.5.xml");
        }
Don Gagne's avatar
Don Gagne committed
809 810 811 812
    default:
        return QString();
    }
}