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

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

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

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

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

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

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

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

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

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

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

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

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

}

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

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

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

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

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

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

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

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

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

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

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

    mavlink_param_value_t paramValue;
    mavlink_param_union_t paramUnion;

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

Don Gagne's avatar
Don Gagne committed
244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272
    // 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;
273 274
    }

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

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

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

    mavlink_param_set_t     paramSet;
    mavlink_param_union_t   paramUnion;

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

Don Gagne's avatar
Don Gagne committed
296 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
    // 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
327 328
    }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

        // Fix up severity
        _setInfoSeverity(message);

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

437
    if (messageText.startsWith("PreArm")) {
Don Gagne's avatar
Don Gagne committed
438 439
        // 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.
440
        if (instanceData->noisyPrearmMap.contains(messageText) && instanceData->noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
441 442
            return false;
        }
443
        instanceData->noisyPrearmMap[messageText] = QTime::currentTime();
Don Gagne's avatar
Don Gagne committed
444 445

        vehicle->setPrearmError(messageText);
446 447 448
    }

    return true;
Don Gagne's avatar
Don Gagne committed
449 450
}

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

Don Gagne's avatar
Don Gagne committed
455 456 457 458 459 460 461 462 463 464
    // 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;
465
        }
466
    }
Don Gagne's avatar
Don Gagne committed
467

468
    vehicle->_setFlying(flying);
Don Gagne's avatar
Don Gagne committed
469 470
}

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

    switch (message->msgid) {
    case MAVLINK_MSG_ID_PARAM_VALUE:
480
        _handleIncomingParamValue(vehicle, message);
Don Gagne's avatar
Don Gagne committed
481 482
        break;
    case MAVLINK_MSG_ID_STATUSTEXT:
483
        return _handleIncomingStatusText(vehicle, message);
Don Gagne's avatar
Don Gagne committed
484
    case MAVLINK_MSG_ID_HEARTBEAT:
485
        _handleIncomingHeartbeat(vehicle, message);
Don Gagne's avatar
Don Gagne committed
486 487
        break;
    }
488 489

    return true;
Don Gagne's avatar
Don Gagne committed
490 491
}

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

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

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

Don Gagne's avatar
Don Gagne committed
543
    return adjustmentNeeded;
Pritam Ghanghas's avatar
Pritam Ghanghas committed
544
}
545

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

563 564 565 566 567 568 569 570
    // 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
571
}
572

573 574 575 576 577
void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);

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

Don Gagne's avatar
Don Gagne committed
589 590 591 592
void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
{
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
593 594 595
    // 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
596
    statusText.severity = MAV_SEVERITY_INFO;
597
    mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText);
Don Gagne's avatar
Don Gagne committed
598 599
}

600 601 602 603 604 605 606 607 608 609 610 611
void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle)
{
    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);
}


612 613
void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
614 615
    vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData);

616 617 618 619 620 621 622 623 624 625
    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
626 627 628 629 630 631 632
        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:
633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648
        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
649
        initializeStreamRates(vehicle);
650
    }
651
}
652 653 654 655 656

void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)
{
    _supportedModes = supportedModes;
}
657 658 659 660 661 662

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

664
void APMFirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
665
{
666
    APMParameterMetaData* apmMetaData = qobject_cast<APMParameterMetaData*>(parameterMetaData);
Don Gagne's avatar
Don Gagne committed
667

668 669 670 671 672 673
    if (apmMetaData) {
        apmMetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to APMFirmwarePlugin::addMetaDataToFact not APMParameterMetaData";
    }
}
674

Don Gagne's avatar
Don Gagne committed
675 676 677 678
QList<MAV_CMD> APMFirmwarePlugin::supportedMissionCommands(void)
{
    QList<MAV_CMD> list;

679 680
    list << MAV_CMD_NAV_WAYPOINT
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TURNS << MAV_CMD_NAV_LOITER_TIME
681
         << MAV_CMD_NAV_RETURN_TO_LAUNCH << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
682 683 684
         << MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT
         << MAV_CMD_NAV_LOITER_TO_ALT
         << MAV_CMD_NAV_SPLINE_WAYPOINT
685
         << MAV_CMD_NAV_GUIDED_ENABLE
686 687 688 689 690 691
         << 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
692 693
         << MAV_CMD_DO_SET_RELAY << MAV_CMD_DO_REPEAT_RELAY
         << MAV_CMD_DO_SET_SERVO << MAV_CMD_DO_REPEAT_SERVO
694 695
         << MAV_CMD_DO_LAND_START
         << MAV_CMD_DO_SET_ROI
696 697
         << MAV_CMD_DO_DIGICAM_CONFIGURE << MAV_CMD_DO_DIGICAM_CONTROL
         << MAV_CMD_DO_MOUNT_CONTROL
698 699 700 701 702 703 704 705 706
         << 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
707 708
    // Waiting for module update
    << MAV_CMD_DO_SET_REVERSE;
709
#endif
Don Gagne's avatar
Don Gagne committed
710

Don Gagne's avatar
Don Gagne committed
711 712
    return list;
}
713

714
QString APMFirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
715
{
716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738
    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();
    }
739
}
740 741 742 743 744

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

Don Gagne's avatar
Don Gagne committed
745 746
    APMParameterMetaData* metaData = new APMParameterMetaData();
    metaData->loadParameterFactMetaDataFile(metaDataFile);
747 748
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
749 750 751 752 753 754

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

DonLakeFlyer's avatar
DonLakeFlyer committed
755
void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware)
Don Gagne's avatar
Don Gagne committed
756 757 758 759 760 761
{
    Q_UNUSED(vehicle);

    QTcpSocket* socket = new QTcpSocket();

    socket->connectToHost(_artooIP, _artooVideoHandshakePort);
DonLakeFlyer's avatar
DonLakeFlyer committed
762 763 764
    if (originalSoloFirmware) {
        QObject::connect(socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError);
    }
Don Gagne's avatar
Don Gagne committed
765 766 767 768 769 770
}

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
771

772
QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
773
{
774 775 776
    int majorVersion = vehicle->firmwareMajorVersion();
    int minorVersion = vehicle->firmwareMinorVersion();

Don Gagne's avatar
Don Gagne committed
777 778 779 780 781 782 783
    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:
784
        if (majorVersion < 3) {
Don Gagne's avatar
Don Gagne committed
785
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.3.xml");
786 787
        } else if (majorVersion == 3) {
            switch (minorVersion) {
788 789 790
            case 0:
            case 1:
            case 2:
791 792 793 794 795 796
            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");
797
            case 6:
798
            default:
799
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.6.xml");
800
            }
Don Gagne's avatar
Don Gagne committed
801
        }
802
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Copter.3.5.xml");
DonLakeFlyer's avatar
DonLakeFlyer committed
803 804 805 806 807 808 809
    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
810
    case MAV_TYPE_FIXED_WING:
811
        if (majorVersion < 3) {
Don Gagne's avatar
Don Gagne committed
812
            return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.3.xml");
813 814
        } else if (majorVersion == 3) {
            switch (minorVersion) {
815 816 817
            case 0:
            case 1:
            case 2:
818 819 820 821 822 823 824 825
            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");
826
            case 8:
827
            default:
828
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
829
            }
Don Gagne's avatar
Don Gagne committed
830
        }
831
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Plane.3.8.xml");
Don Gagne's avatar
Don Gagne committed
832 833
    case MAV_TYPE_GROUND_ROVER:
    case MAV_TYPE_SURFACE_BOAT:
834 835 836 837 838 839 840
        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");
841 842
            case 2:
            case 3:
843
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml");
844 845 846
            case 4:
            default:
                return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.4.xml");
847 848 849
            }
        }
        return QStringLiteral(":/FirmwarePlugin/APM/APMParameterFactMetaData.Rover.3.2.xml");
850
    case MAV_TYPE_SUBMARINE:
851 852 853 854 855
        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
856 857 858 859
    default:
        return QString();
    }
}
860 861 862 863 864 865 866 867 868 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

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())) {
898
        qgcApp()->showMessage(tr("Unable to change altitude, vehicle altitude not known."));
899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925
        return;
    }

    setGuidedMode(vehicle, true);

    mavlink_message_t msg;
    mavlink_set_position_target_local_ned_t cmd;

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

    cmd.target_system = vehicle->id();
    cmd.target_component = vehicle->defaultComponentId();
    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;
    cmd.z = -(altitudeChange);

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
    mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(),
                                                          mavlink->getComponentId(),
                                                          vehicle->priorityLink()->mavlinkChannel(),
                                                          &msg,
                                                          &cmd);

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

927
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
928
{
929
    _guidedModeTakeoff(vehicle, altitudeRel);
930 931
}

932 933
double APMFirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
934
    double minTakeoffAlt = 0;
935 936 937 938
    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)) {
DonLakeFlyer's avatar
DonLakeFlyer committed
939 940 941 942 943
        minTakeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble() / paramDivisor;
    }

    if (minTakeoffAlt == 0) {
        minTakeoffAlt = FirmwarePlugin::minimumTakeoffAltitude(vehicle);
944
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
945 946

    return minTakeoffAlt;
947 948
}

949
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
950
{
DonLakeFlyer's avatar
DonLakeFlyer committed
951 952 953 954 955
    if (!vehicle->multiRotor() && !vehicle->vtol()) {
        qgcApp()->showMessage(tr("Vehicle does not support guided takeoff"));
        return false;
    }

956 957 958 959 960 961
    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
    if (qIsNaN(vehicleAltitudeAMSL)) {
        qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
        return false;
    }

962
    double takeoffAltRel = minimumTakeoffAltitude(vehicle);
963 964
    if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) {
        takeoffAltRel = altitudeRel;
965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980
    }

    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,
981
                            takeoffAltRel);                     // Relative altitude
982 983 984 985 986 987

    return true;
}

void APMFirmwarePlugin::startMission(Vehicle* vehicle)
{
988 989 990 991 992 993 994 995
    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;
    }

996 997 998
    if (!vehicle->armed()) {
        // First switch to flight mode we can arm from
        if (!_setFlightModeAndValidate(vehicle, "Guided")) {
999 1000
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode."));
            return;
1001
        }
1002

1003 1004 1005 1006
        if (!_armVehicleAndValidate(vehicle)) {
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
            return;
        }
1007 1008
    }

1009 1010 1011 1012 1013 1014 1015
    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 */);
1016 1017
    }
}