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

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

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

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

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

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

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

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

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

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

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

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

141 142 143 144 145 146 147
APMFirmwarePluginInstanceData::APMFirmwarePluginInstanceData(QObject* parent)
    : QObject(parent)
    , textSeverityAdjustmentNeeded(false)
{

}

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

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

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

170
    return (capabilities & available) == capabilities;
Don Gagne's avatar
Don Gagne committed
171 172 173 174 175
}

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

Don Gagne's avatar
Don Gagne committed
177 178 179
    return QList<VehicleComponent*>();
}

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

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

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

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;
}

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

    mavlink_param_value_t paramValue;
    mavlink_param_union_t paramUnion;

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

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

    mavlink_msg_param_value_decode(message, &paramValue);

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

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

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

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

    mavlink_param_set_t     paramSet;
    mavlink_param_union_t   paramUnion;

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

Don Gagne's avatar
Don Gagne committed
297 298 299 300 301 302 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
    // 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
328 329
    }

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

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

Don Gagne's avatar
Don Gagne committed
338 339 340
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);

341
    if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || statusText.severity < MAV_SEVERITY_NOTICE) {
Don Gagne's avatar
Don Gagne committed
342 343 344
        messageText = _getMessageText(message);
        qCDebug(APMFirmwarePluginLog) << messageText;

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

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

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

                switch (vehicle->vehicleType()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
358 359 360 361 362 363 364
                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
365 366
                case MAV_TYPE_FIXED_WING:
                    supportedMajorNumber = 3;
DonLakeFlyer's avatar
DonLakeFlyer committed
367
                    supportedMinorNumber = 4;
Don Gagne's avatar
Don Gagne committed
368 369
                    break;
                case MAV_TYPE_QUADROTOR:
DonLakeFlyer's avatar
DonLakeFlyer committed
370 371
                    // 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
372 373 374 375 376 377 378
                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;
DonLakeFlyer's avatar
DonLakeFlyer committed
379
                    supportedMinorNumber = 3;
Don Gagne's avatar
Don Gagne committed
380 381 382 383
                    break;
                default:
                    break;
                }
Don Gagne's avatar
Don Gagne committed
384

Don Gagne's avatar
Don Gagne committed
385 386
                if (supportedMajorNumber != -1) {
                    if (firmwareVersion.majorNumber() < supportedMajorNumber || firmwareVersion.minorNumber() < supportedMinorNumber) {
387
                        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));
388
                    }
Don Gagne's avatar
Don Gagne committed
389 390
                }
            }
Don Gagne's avatar
Don Gagne committed
391
        }
Don Gagne's avatar
Don Gagne committed
392

Don Gagne's avatar
Don Gagne committed
393 394
        // 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.
395

Don Gagne's avatar
Don Gagne committed
396 397
        if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) {
            _adjustCalibrationMessageSeverity(message);
398
            return true;
399
        }
Don Gagne's avatar
Don Gagne committed
400
    }
401

Don Gagne's avatar
Don Gagne committed
402
    // adjust mesasge if needed
403
    if (instanceData->textSeverityAdjustmentNeeded) {
Don Gagne's avatar
Don Gagne committed
404 405
        _adjustSeverity(message);
    }
406

Don Gagne's avatar
Don Gagne committed
407 408 409 410 411 412
    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.
413
    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
414 415 416
            messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) {
        _setInfoSeverity(message);
    }
417

Don Gagne's avatar
Don Gagne committed
418 419
    if (messageText.contains(APM_SOLO_REXP)) {
        qDebug() << "Found Solo";
420
        vehicle->setSoloFirmware(true);
Don Gagne's avatar
Don Gagne committed
421 422 423 424 425

        // Fix up severity
        _setInfoSeverity(message);

        // Start TCP video handshake with ARTOO
DonLakeFlyer's avatar
DonLakeFlyer committed
426
        _soloVideoHandshake(vehicle, true /* originalSoloFirmware */);
Don Gagne's avatar
Don Gagne committed
427 428 429 430 431 432 433 434 435
    } 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
436 437
    }

438
    return true;
Don Gagne's avatar
Don Gagne committed
439 440
}

441
void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
442 443
{
    bool flying = false;
444

Don Gagne's avatar
Don Gagne committed
445 446 447 448 449 450 451 452 453 454
    // 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;
455
        }
456
    }
Don Gagne's avatar
Don Gagne committed
457

458
    vehicle->_setFlying(flying);
Don Gagne's avatar
Don Gagne committed
459 460
}

461
bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
462 463 464
{
    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
465
        return true;
Don Gagne's avatar
Don Gagne committed
466 467 468 469
    }

    switch (message->msgid) {
    case MAVLINK_MSG_ID_PARAM_VALUE:
470
        _handleIncomingParamValue(vehicle, message);
Don Gagne's avatar
Don Gagne committed
471 472
        break;
    case MAVLINK_MSG_ID_STATUSTEXT:
473
        return _handleIncomingStatusText(vehicle, message);
Don Gagne's avatar
Don Gagne committed
474
    case MAVLINK_MSG_ID_HEARTBEAT:
475
        _handleIncomingHeartbeat(vehicle, message);
Don Gagne's avatar
Don Gagne committed
476
        break;
477 478 479 480 481 482
    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
483
    }
484 485

    return true;
Don Gagne's avatar
Don Gagne committed
486 487
}

488
void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
489 490 491 492 493 494 495 496
{
    //-- 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:
497
        _handleOutgoingParamSet(vehicle, outgoingLink, message);
Don Gagne's avatar
Don Gagne committed
498 499
        break;
    }
500 501
}

502 503 504 505 506 507 508 509 510 511 512 513
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
514
bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
515
{
Don Gagne's avatar
Don Gagne committed
516 517 518
    if (!firmwareVersion.isValid()) {
        return false;
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
519

Don Gagne's avatar
Don Gagne committed
520 521 522 523 524 525 526 527 528 529 530 531 532
    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;
        }
533 534 535 536
    } 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
537
    }
Pritam Ghanghas's avatar
Pritam Ghanghas committed
538

Don Gagne's avatar
Don Gagne committed
539
    return adjustmentNeeded;
Pritam Ghanghas's avatar
Pritam Ghanghas committed
540
}
541

Pritam Ghanghas's avatar
Pritam Ghanghas committed
542 543 544
void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
{
    // lets make QGC happy with right severity values
545 546 547
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
    switch(statusText.severity) {
Don Gagne's avatar
Don Gagne committed
548 549 550 551 552 553 554 555 556
    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;
557 558
    }

559 560 561 562 563 564 565 566
    // 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
567
}
568

569 570 571 572 573
void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);

574 575 576
    // 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;
577
    statusText.severity = MAV_SEVERITY_INFO;
578 579 580 581 582
    mavlink_msg_statustext_encode_chan(message->sysid,
                                       message->compid,
                                       0,                  // Re-encoding uses reserved channel 0
                                       message,
                                       &statusText);
583 584
}

Don Gagne's avatar
Don Gagne committed
585 586 587 588
void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
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
    statusText.severity = MAV_SEVERITY_INFO;
593
    mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText);
Don Gagne's avatar
Don Gagne committed
594 595
}

596 597
void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
598 599
    vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData);

600 601 602 603 604 605 606 607 608 609
    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;
DonLakeFlyer's avatar
DonLakeFlyer committed
610 611 612 613 614 615 616
        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:
617 618 619 620 621 622 623 624 625 626 627 628 629 630 631
        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;
        }
    }
632
}
633 634 635 636 637

void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)
{
    _supportedModes = supportedModes;
}
638 639 640 641 642 643

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

645
void APMFirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
646
{
647
    APMParameterMetaData* apmMetaData = qobject_cast<APMParameterMetaData*>(parameterMetaData);
Don Gagne's avatar
Don Gagne committed
648

649 650 651 652 653 654
    if (apmMetaData) {
        apmMetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to APMFirmwarePlugin::addMetaDataToFact not APMParameterMetaData";
    }
}
655

Don Gagne's avatar
Don Gagne committed
656 657 658 659
QList<MAV_CMD> APMFirmwarePlugin::supportedMissionCommands(void)
{
    QList<MAV_CMD> list;

660 661
    list << MAV_CMD_NAV_WAYPOINT
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TURNS << MAV_CMD_NAV_LOITER_TIME
662
         << MAV_CMD_NAV_RETURN_TO_LAUNCH << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
663 664 665
         << MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT
         << MAV_CMD_NAV_LOITER_TO_ALT
         << MAV_CMD_NAV_SPLINE_WAYPOINT
666
         << MAV_CMD_NAV_GUIDED_ENABLE
667 668 669 670 671 672
         << 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
673 674
         << MAV_CMD_DO_SET_RELAY << MAV_CMD_DO_REPEAT_RELAY
         << MAV_CMD_DO_SET_SERVO << MAV_CMD_DO_REPEAT_SERVO
675 676
         << MAV_CMD_DO_LAND_START
         << MAV_CMD_DO_SET_ROI
677 678
         << MAV_CMD_DO_DIGICAM_CONFIGURE << MAV_CMD_DO_DIGICAM_CONTROL
         << MAV_CMD_DO_MOUNT_CONTROL
679 680 681 682 683 684 685 686 687
         << 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
688 689
    // Waiting for module update
    << MAV_CMD_DO_SET_REVERSE;
690
#endif
Don Gagne's avatar
Don Gagne committed
691

Don Gagne's avatar
Don Gagne committed
692 693
    return list;
}
694

695
QString APMFirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
696
{
697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719
    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();
    }
720
}
721 722 723 724 725

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

Don Gagne's avatar
Don Gagne committed
726 727
    APMParameterMetaData* metaData = new APMParameterMetaData();
    metaData->loadParameterFactMetaDataFile(metaDataFile);
728 729
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
730 731 732 733 734 735

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

DonLakeFlyer's avatar
DonLakeFlyer committed
736
void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware)
Don Gagne's avatar
Don Gagne committed
737 738 739 740 741 742
{
    Q_UNUSED(vehicle);

    QTcpSocket* socket = new QTcpSocket();

    socket->connectToHost(_artooIP, _artooVideoHandshakePort);
DonLakeFlyer's avatar
DonLakeFlyer committed
743 744 745
    if (originalSoloFirmware) {
        QObject::connect(socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError);
    }
Don Gagne's avatar
Don Gagne committed
746 747 748 749 750 751
}

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
752

753
QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
754 755 756 757 758 759 760 761
{
    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:
762 763
        if (vehicle->versionCompare(3, 6, 0) >= 0) {  // 3.6.0 and higher
             return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml");
Don Gagne's avatar
Don Gagne committed
764
        }
765
        if (vehicle->versionCompare(3, 5, 0) >= 0) {  // 3.5.x
766 767
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
        }
768 769 770 771 772
        if (vehicle->versionCompare(3, 4, 0) >= 0) {  // 3.4.x
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.4.xml");
        }
        // Up to 3.3.x
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
773

DonLakeFlyer's avatar
DonLakeFlyer committed
774 775 776 777 778 779 780
    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
781
    case MAV_TYPE_FIXED_WING:
782 783
        if (vehicle->versionCompare(3, 8, 0) >= 0) {  // 3.8.0 and higher
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
784
        }
785
        if (vehicle->versionCompare(3, 7, 0) >= 0) {  // 3.7.x
786
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.7.xml");
Don Gagne's avatar
Don Gagne committed
787
        }
788 789 790 791 792
        if (vehicle->versionCompare(3, 5, 0) >= 0) {  // 3.5.x to 3.6.x
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.5.xml");
        }
        // up to 3.4.x
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
793

Don Gagne's avatar
Don Gagne committed
794 795
    case MAV_TYPE_GROUND_ROVER:
    case MAV_TYPE_SURFACE_BOAT:
796 797
        if (vehicle->versionCompare(3, 4, 0) >= 0) {  // 3.4.0 and higher
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.4.xml");
798
        }
799
        if (vehicle->versionCompare(3, 2, 0) >= 0) { // 3.2.x to 3.3.x
800 801
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml");
        }
802 803
        // up to 3.1.x
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.0.xml");
804

805
    case MAV_TYPE_SUBMARINE:
806
        if (vehicle->versionCompare(3, 6, 0) >= 0) { // 3.5.x
807
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Sub.3.6dev.xml");
808
        }
809 810 811 812 813 814
        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
815 816 817 818
    default:
        return QString();
    }
}
819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856

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

void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
    _setFlightModeAndValidate(vehicle, rtlFlightMode());
}

void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
857
        qgcApp()->showMessage(tr("Unable to change altitude, vehicle altitude not known."));
858 859 860 861 862 863 864 865 866 867
        return;
    }

    setGuidedMode(vehicle, true);

    mavlink_message_t msg;
    mavlink_set_position_target_local_ned_t cmd;

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

868 869
    cmd.target_system    = static_cast<uint8_t>(vehicle->id());
    cmd.target_component = static_cast<uint8_t>(vehicle->defaultComponentId());
870 871 872 873
    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;
874
    cmd.z = static_cast<float>(-(altitudeChange));
875 876

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
877 878 879 880 881 882
    mavlink_msg_set_position_target_local_ned_encode_chan(
        static_cast<uint8_t>(mavlink->getSystemId()),
        static_cast<uint8_t>(mavlink->getComponentId()),
        vehicle->priorityLink()->mavlinkChannel(),
        &msg,
        &cmd);
883 884 885

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

887
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
888
{
889
    _guidedModeTakeoff(vehicle, altitudeRel);
890 891
}

892 893
double APMFirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
894
    double minTakeoffAlt = 0;
895 896 897 898
    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)) {
899
        minTakeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble() / static_cast<double>(paramDivisor);
DonLakeFlyer's avatar
DonLakeFlyer committed
900 901 902 903
    }

    if (minTakeoffAlt == 0) {
        minTakeoffAlt = FirmwarePlugin::minimumTakeoffAltitude(vehicle);
904
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
905 906

    return minTakeoffAlt;
907 908
}

909
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
910
{
DonLakeFlyer's avatar
DonLakeFlyer committed
911 912 913 914 915
    if (!vehicle->multiRotor() && !vehicle->vtol()) {
        qgcApp()->showMessage(tr("Vehicle does not support guided takeoff"));
        return false;
    }

916 917 918 919 920 921
    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
    if (qIsNaN(vehicleAltitudeAMSL)) {
        qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
        return false;
    }

922
    double takeoffAltRel = minimumTakeoffAltitude(vehicle);
923 924
    if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) {
        takeoffAltRel = altitudeRel;
925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940
    }

    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,
941
                            static_cast<float>(takeoffAltRel));                     // Relative altitude
942 943 944 945 946 947

    return true;
}

void APMFirmwarePlugin::startMission(Vehicle* vehicle)
{
948 949 950 951 952 953 954 955
    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;
    }

956 957 958
    if (!vehicle->armed()) {
        // First switch to flight mode we can arm from
        if (!_setFlightModeAndValidate(vehicle, "Guided")) {
959 960
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode."));
            return;
961
        }
962

963 964 965 966
        if (!_armVehicleAndValidate(vehicle)) {
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
            return;
        }
967 968
    }

969 970 971 972 973 974 975
    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 */);
976 977
    }
}
978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999

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,\\.]*)$");
}
1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034

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(
        static_cast<uint8_t>(mavlink->getSystemId()),
        static_cast<uint8_t>(mavlink->getComponentId()),
        vehicle->priorityLink()->mavlinkChannel(),
        message,
        &channels);
}

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(
        static_cast<uint8_t>(mavlink->getSystemId()),
        static_cast<uint8_t>(mavlink->getComponentId()),
        vehicle->priorityLink()->mavlinkChannel(),
        message,
        &channels);
}