PX4FirmwarePlugin.cc 18.7 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
dogmaphobic's avatar
dogmaphobic committed
9

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14

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

#include "PX4FirmwarePlugin.h"
15
#include "PX4ParameterMetaData.h"
16
#include "QGCApplication.h"
17
#include "AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h"    // FIXME: Hack
Don Gagne's avatar
Don Gagne committed
18 19 20

#include <QDebug>

Daniel Agar's avatar
Daniel Agar committed
21
#include "px4_custom_mode.h"
Don Gagne's avatar
Don Gagne committed
22 23 24 25 26 27

struct Modes2Name {
    uint8_t     main_mode;
    uint8_t     sub_mode;
    const char* name;       ///< Name for flight mode
    bool        canBeSet;   ///< true: Vehicle can be set to this flight mode
Daniel Agar's avatar
Daniel Agar committed
28 29
    bool        fixedWing;  /// fixed wing compatible
    bool        multiRotor;  /// multi rotor compatible
Don Gagne's avatar
Don Gagne committed
30 31
};

Don Gagne's avatar
Don Gagne committed
32
const char* PX4FirmwarePlugin::manualFlightMode =       "Manual";
Daniel Agar's avatar
Daniel Agar committed
33 34 35 36 37 38 39
const char* PX4FirmwarePlugin::altCtlFlightMode =       "Altitude";
const char* PX4FirmwarePlugin::posCtlFlightMode =       "Position";
const char* PX4FirmwarePlugin::missionFlightMode =      "Mission";
const char* PX4FirmwarePlugin::holdFlightMode =         "Hold";
const char* PX4FirmwarePlugin::takeoffFlightMode =      "Takeoff";
const char* PX4FirmwarePlugin::landingFlightMode =      "Land";
const char* PX4FirmwarePlugin::rtlFlightMode =          "Return";
Don Gagne's avatar
Don Gagne committed
40
const char* PX4FirmwarePlugin::acroFlightMode =         "Acro";
Daniel Agar's avatar
Daniel Agar committed
41
const char* PX4FirmwarePlugin::offboardFlightMode =     "Offboard";
Don Gagne's avatar
Don Gagne committed
42 43
const char* PX4FirmwarePlugin::stabilizedFlightMode =   "Stabilized";
const char* PX4FirmwarePlugin::rattitudeFlightMode =    "Rattitude";
Jimmy Johnson's avatar
Jimmy Johnson committed
44
const char* PX4FirmwarePlugin::followMeFlightMode =     "Follow Me";
Don Gagne's avatar
Don Gagne committed
45

Daniel Agar's avatar
Daniel Agar committed
46 47 48 49
const char* PX4FirmwarePlugin::rtgsFlightMode =         "Return to Groundstation";

const char* PX4FirmwarePlugin::readyFlightMode =        "Ready"; // unused

Don Gagne's avatar
Don Gagne committed
50
/// Tranlates from PX4 custom modes to flight mode names
51

Don Gagne's avatar
Don Gagne committed
52
static const struct Modes2Name rgModes2Name[] = {
Daniel Agar's avatar
Daniel Agar committed
53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
    //main_mode                         sub_mode                                name                                      canBeSet  FW      MC
    { PX4_CUSTOM_MAIN_MODE_MANUAL,      0,                                      PX4FirmwarePlugin::manualFlightMode,        true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_STABILIZED,  0,                                      PX4FirmwarePlugin::stabilizedFlightMode,    true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_ACRO,        0,                                      PX4FirmwarePlugin::acroFlightMode,          true,   false,  true },
    { PX4_CUSTOM_MAIN_MODE_RATTITUDE,   0,                                      PX4FirmwarePlugin::rattitudeFlightMode,     true,   false,  true },
    { PX4_CUSTOM_MAIN_MODE_ALTCTL,      0,                                      PX4FirmwarePlugin::altCtlFlightMode,        true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_POSCTL,      0,                                      PX4FirmwarePlugin::posCtlFlightMode,        true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LOITER,        PX4FirmwarePlugin::holdFlightMode,          true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_MISSION,       PX4FirmwarePlugin::missionFlightMode,       true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,           PX4FirmwarePlugin::rtlFlightMode,           true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4FirmwarePlugin::followMeFlightMode,      true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_OFFBOARD,    0,                                      PX4FirmwarePlugin::offboardFlightMode,      true,   true,   true },
    // modes that can't be directly set by the user
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,          PX4FirmwarePlugin::landingFlightMode,       false,  true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_READY,         PX4FirmwarePlugin::readyFlightMode,         false,  true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTGS,          PX4FirmwarePlugin::rtgsFlightMode,          false,  true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,       PX4FirmwarePlugin::takeoffFlightMode,       false,  true,   true },
Don Gagne's avatar
Don Gagne committed
70 71
};

72 73 74 75 76 77
PX4FirmwarePlugin::PX4FirmwarePlugin(void)
    : _versionNotified(false)
{

}

Don Gagne's avatar
Don Gagne committed
78 79 80
QList<VehicleComponent*> PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);
dogmaphobic's avatar
dogmaphobic committed
81

Don Gagne's avatar
Don Gagne committed
82 83 84
    return QList<VehicleComponent*>();
}

Daniel Agar's avatar
Daniel Agar committed
85
QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
86 87
{
    QStringList flightModes;
dogmaphobic's avatar
dogmaphobic committed
88

Don Gagne's avatar
Don Gagne committed
89 90
    for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
        const struct Modes2Name* pModes2Name = &rgModes2Name[i];
dogmaphobic's avatar
dogmaphobic committed
91

Don Gagne's avatar
Don Gagne committed
92
        if (pModes2Name->canBeSet) {
Daniel Agar's avatar
Daniel Agar committed
93 94 95 96 97 98 99 100 101
            bool fw = (vehicle->fixedWing() && pModes2Name->fixedWing);
            bool mc = (vehicle->multiRotor() && pModes2Name->multiRotor);

            // show all modes for generic, vtol, etc
            bool other = !vehicle->fixedWing() && !vehicle->multiRotor();

            if (fw || mc || other) {
                flightModes += pModes2Name->name;
            }
Don Gagne's avatar
Don Gagne committed
102 103
        }
    }
dogmaphobic's avatar
dogmaphobic committed
104

Don Gagne's avatar
Don Gagne committed
105 106 107
    return flightModes;
}

Don Gagne's avatar
Don Gagne committed
108
QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
109 110
{
    QString flightMode = "Unknown";
dogmaphobic's avatar
dogmaphobic committed
111

Don Gagne's avatar
Don Gagne committed
112 113 114
    if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        union px4_custom_mode px4_mode;
        px4_mode.data = custom_mode;
dogmaphobic's avatar
dogmaphobic committed
115

Don Gagne's avatar
Don Gagne committed
116 117 118
        bool found = false;
        for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
            const struct Modes2Name* pModes2Name = &rgModes2Name[i];
dogmaphobic's avatar
dogmaphobic committed
119

Don Gagne's avatar
Don Gagne committed
120 121 122 123 124 125
            if (pModes2Name->main_mode == px4_mode.main_mode && pModes2Name->sub_mode == px4_mode.sub_mode) {
                flightMode = pModes2Name->name;
                found = true;
                break;
            }
        }
dogmaphobic's avatar
dogmaphobic committed
126

Don Gagne's avatar
Don Gagne committed
127 128 129 130 131 132
        if (!found) {
            qWarning() << "Unknown flight mode" << custom_mode;
        }
    } else {
        qWarning() << "PX4 Flight Stack flight mode without custom mode enabled?";
    }
dogmaphobic's avatar
dogmaphobic committed
133

Don Gagne's avatar
Don Gagne committed
134 135 136 137 138 139 140
    return flightMode;
}

bool PX4FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
    *base_mode = 0;
    *custom_mode = 0;
dogmaphobic's avatar
dogmaphobic committed
141

Don Gagne's avatar
Don Gagne committed
142 143 144
    bool found = false;
    for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
        const struct Modes2Name* pModes2Name = &rgModes2Name[i];
dogmaphobic's avatar
dogmaphobic committed
145

Don Gagne's avatar
Don Gagne committed
146 147 148 149 150 151
        if (flightMode.compare(pModes2Name->name, Qt::CaseInsensitive) == 0) {
            union px4_custom_mode px4_mode;

            px4_mode.data = 0;
            px4_mode.main_mode = pModes2Name->main_mode;
            px4_mode.sub_mode = pModes2Name->sub_mode;
dogmaphobic's avatar
dogmaphobic committed
152

Don Gagne's avatar
Don Gagne committed
153 154
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = px4_mode.data;
dogmaphobic's avatar
dogmaphobic committed
155

Don Gagne's avatar
Don Gagne committed
156 157 158 159
            found = true;
            break;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
160

Don Gagne's avatar
Don Gagne committed
161 162 163
    if (!found) {
        qWarning() << "Unknown flight Mode" << flightMode;
    }
dogmaphobic's avatar
dogmaphobic committed
164

Don Gagne's avatar
Don Gagne committed
165 166
    return found;
}
167 168 169

int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
{
170
    return 0;   // 0 buttons reserved for rc switch simulation
171
}
Don Gagne's avatar
Don Gagne committed
172

173 174 175 176 177
bool PX4FirmwarePlugin::supportsManualControl(void)
{
    return true;
}

178
bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
Don Gagne's avatar
Don Gagne committed
179
{
180 181 182
    if(vehicle->multiRotor()) {
        return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability | OrbitModeCapability)) == capabilities;
    }
183
    return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
Don Gagne's avatar
Don Gagne committed
184
}
185 186 187 188

void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
    Q_UNUSED(vehicle);
dogmaphobic's avatar
dogmaphobic committed
189

190 191
    // PX4 Flight Stack doesn't need to do any extra work
}
192 193 194 195 196 197 198

bool PX4FirmwarePlugin::sendHomePositionToVehicle(void)
{
    // PX4 stack does not want home position sent in the first position.
    // Subsequent sequence numbers must be adjusted.
    return false;
}
199

200
void PX4FirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
201
{
202 203 204 205 206 207 208
    PX4ParameterMetaData* px4MetaData = qobject_cast<PX4ParameterMetaData*>(parameterMetaData);

    if (px4MetaData) {
        px4MetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to PX4FirmwarePlugin::addMetaDataToFact not PX4ParameterMetaData";
    }
209
}
Don Gagne's avatar
Don Gagne committed
210 211 212 213 214

QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
{
    QList<MAV_CMD> list;

215
    list << MAV_CMD_NAV_WAYPOINT
216
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TIME << MAV_CMD_NAV_LOITER_TO_ALT
Don Gagne's avatar
Don Gagne committed
217
         << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
218
         << MAV_CMD_DO_JUMP
Don Gagne's avatar
Don Gagne committed
219
         << MAV_CMD_DO_VTOL_TRANSITION << MAV_CMD_NAV_VTOL_TAKEOFF << MAV_CMD_NAV_VTOL_LAND
220
         << MAV_CMD_DO_DIGICAM_CONTROL
221
         << MAV_CMD_DO_SET_CAM_TRIGG_DIST
222
         << MAV_CMD_DO_SET_SERVO
Don Gagne's avatar
Don Gagne committed
223
         << MAV_CMD_DO_CHANGE_SPEED
224
         << MAV_CMD_DO_LAND_START
225
         << MAV_CMD_DO_MOUNT_CONFIGURE
226
         << MAV_CMD_DO_MOUNT_CONTROL;
Jimmy Johnson's avatar
Jimmy Johnson committed
227

Don Gagne's avatar
Don Gagne committed
228 229
    return list;
}
230

231
QString PX4FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
232
{
233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255
    switch (vehicleType) {
    case MAV_TYPE_GENERIC:
        return QStringLiteral(":/json/PX4/MavCmdInfoCommon.json");
        break;
    case MAV_TYPE_FIXED_WING:
        return QStringLiteral(":/json/PX4/MavCmdInfoFixedWing.json");
        break;
    case MAV_TYPE_QUADROTOR:
        return QStringLiteral(":/json/PX4/MavCmdInfoMultiRotor.json");
        break;
    case MAV_TYPE_VTOL_QUADROTOR:
        return QStringLiteral(":/json/APM/MavCmdInfoVTOL.json");
        break;
    case MAV_TYPE_SUBMARINE:
        return QStringLiteral(":/json/PX4/MavCmdInfoSub.json");
        break;
    case MAV_TYPE_GROUND_ROVER:
        return QStringLiteral(":/json/PX4/MavCmdInfoRover.json");
        break;
    default:
        qWarning() << "PX4FirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType;
        return QString();
    }
256
}
257 258 259 260

QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
{
    PX4ParameterMetaData* metaData = new PX4ParameterMetaData;
261 262 263
    if (!metaDataFile.isEmpty()) {
        metaData->loadParameterFactMetaDataFile(metaDataFile);
    }
264 265
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
266 267 268

void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
269 270
    // then tell it to loiter at the current position
    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
271
    mavlink_command_long_t cmd;
272 273 274 275

    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
    cmd.confirmation = 0;
    cmd.param1 = -1.0f;
276
    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
277 278 279 280 281 282
    cmd.param3 = 0.0f;
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = NAN;
    cmd.target_system = vehicle->id();
283
    cmd.target_component = vehicle->defaultComponentId();
284 285

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
286 287 288 289 290
    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
                                         mavlink->getComponentId(),
                                         vehicle->priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
291

292
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
Don Gagne's avatar
Don Gagne committed
293
}
294 295 296 297 298 299 300 301 302 303 304

void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
    vehicle->setFlightMode(rtlFlightMode);
}

void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
    vehicle->setFlightMode(landingFlightMode);
}

305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323
void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
{
    //-- If not in "guided" mode, make it so.
    if(!isGuidedMode(vehicle))
        setGuidedMode(vehicle, true);
    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
    mavlink_message_t msg;
    mavlink_command_long_t cmd;
    cmd.command = (uint16_t)MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE;
    cmd.confirmation = 0;
    cmd.param1 = radius;
    cmd.param2 = velocity;
    cmd.param3 = altitude;
    cmd.param4 = NAN;
    cmd.param5 = centerCoord.isValid() ? centerCoord.latitude()  : NAN;
    cmd.param6 = centerCoord.isValid() ? centerCoord.longitude() : NAN;
    cmd.param7 = centerCoord.isValid() ? centerCoord.altitude()  : NAN;
    cmd.target_system = vehicle->id();
    cmd.target_component = vehicle->defaultComponentId();
324 325 326 327 328 329
    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
                                         mavlink->getComponentId(),
                                         vehicle->priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
330 331
}

332 333 334 335 336 337 338 339
void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{
    Q_UNUSED(altitudeRel);
    if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
        qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
        return;
    }

340
    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
341

342
    // Set destination altitude
343
    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
344
    mavlink_command_long_t cmd;
345

Lorenz Meier's avatar
Lorenz Meier committed
346
    cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF;
347
    cmd.confirmation = 0;
348
    cmd.param1 = -1.0f;
349
    cmd.param2 = 0.0f;
350
    cmd.param3 = 0.0f;
351 352 353 354
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel;
355
    cmd.target_system = vehicle->id();
356
    cmd.target_component = vehicle->defaultComponentId();
357

358 359 360 361 362 363
    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
                                         mavlink->getComponentId(),
                                         vehicle->priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
364 365 366 367 368 369 370 371 372 373
}

void PX4FirmwarePlugin::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;
    }

    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
374
    mavlink_command_long_t cmd;
375 376 377 378 379 380 381

    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
    cmd.confirmation = 0;
    cmd.param1 = -1.0f;
    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
    cmd.param3 = 0.0f;
    cmd.param4 = NAN;
Don Gagne's avatar
Don Gagne committed
382 383
    cmd.param5 = gotoCoord.latitude();
    cmd.param6 = gotoCoord.longitude();
384 385
    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble();
    cmd.target_system = vehicle->id();
386
    cmd.target_component = vehicle->defaultComponentId();
387 388

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
389 390 391 392 393
    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
                                         mavlink->getComponentId(),
                                         vehicle->priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
394

395
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
396 397 398 399 400 401 402 403 404 405
}

void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
{
    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
        return;
    }

    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
406
    mavlink_command_long_t cmd;
407 408 409 410 411 412 413 414 415 416 417

    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
    cmd.confirmation = 0;
    cmd.param1 = -1.0f;
    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
    cmd.param3 = 0.0f;
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel;
    cmd.target_system = vehicle->id();
418
    cmd.target_component = vehicle->defaultComponentId();
419 420

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
421 422 423 424 425
    mavlink_msg_command_long_encode_chan(mavlink->getSystemId(),
                                         mavlink->getComponentId(),
                                         vehicle->priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
426

427
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
428 429 430 431 432
}

void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    if (guidedMode) {
Daniel Agar's avatar
Daniel Agar committed
433
        vehicle->setFlightMode(holdFlightMode);
434 435 436 437
    } else {
        pauseVehicle(vehicle);
    }
}
438 439 440 441

bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
    // Not supported by generic vehicle
Daniel Agar's avatar
Daniel Agar committed
442
    return (vehicle->flightMode() == holdFlightMode || vehicle->flightMode() == takeoffFlightMode
443
            || vehicle->flightMode() == landingFlightMode);
444
}
445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481

bool PX4FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
{
    //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues
    if (message->compid == MAV_COMP_ID_UDP_BRIDGE) {
        return true;
    }

    switch (message->msgid) {
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
        _handleAutopilotVersion(vehicle, message);
        break;
    }

    return true;
}

void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message)
{
    Q_UNUSED(vehicle);

    if (!_versionNotified) {
        bool notifyUser = false;
        int supportedMajorVersion = 1;
        int supportedMinorVersion = 4;
        int supportedPatchVersion = 1;

        mavlink_autopilot_version_t version;
        mavlink_msg_autopilot_version_decode(message, &version);

        if (version.flight_sw_version != 0) {
            int majorVersion, minorVersion, patchVersion;

            majorVersion = (version.flight_sw_version >> (8*3)) & 0xFF;
            minorVersion = (version.flight_sw_version >> (8*2)) & 0xFF;
            patchVersion = (version.flight_sw_version >> (8*1)) & 0xFF;

482 483 484 485 486 487 488 489 490
            if (majorVersion < supportedMajorVersion) {
                notifyUser = true;
            } else if (majorVersion == supportedMajorVersion) {
                if (minorVersion < supportedMinorVersion) {
                    notifyUser = true;
                } else if (minorVersion == supportedMinorVersion) {
                    notifyUser = patchVersion < supportedPatchVersion;
                }
            }
491 492 493 494 495 496 497 498 499 500
        } else {
            notifyUser = true;
        }

        if (notifyUser) {
            _versionNotified = true;
            qgcApp()->showMessage(QString("QGroundControl supports PX4 Pro firmware Version %1.%2.%3 and above. You are using a version prior to that which will lead to unpredictable results. Please upgrade your firmware.").arg(supportedMajorVersion).arg(supportedMinorVersion).arg(supportedPatchVersion));
        }
    }
}