PX4FirmwarePlugin.cc 22 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 18 19 20 21 22 23
#include "PX4AutoPilotPlugin.h"
#include "PX4AdvancedFlightModesController.h"
#include "PX4SimpleFlightModesController.h"
#include "AirframeComponentController.h"
#include "SensorsComponentController.h"
#include "PowerComponentController.h"
#include "RadioComponentController.h"
24
#include "QGCCameraManager.h"
Don Gagne's avatar
Don Gagne committed
25 26 27

#include <QDebug>

Daniel Agar's avatar
Daniel Agar committed
28
#include "px4_custom_mode.h"
Don Gagne's avatar
Don Gagne committed
29

30 31 32 33 34 35 36
PX4FirmwarePluginInstanceData::PX4FirmwarePluginInstanceData(QObject* parent)
    : QObject(parent)
    , versionNotified(false)
{

}

37
PX4FirmwarePlugin::PX4FirmwarePlugin(void)
38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53
    : _manualFlightMode(tr("Manual"))
    , _acroFlightMode(tr("Acro"))
    , _stabilizedFlightMode(tr("Stabilized"))
    , _rattitudeFlightMode(tr("Rattitude"))
    , _altCtlFlightMode(tr("Altitude"))
    , _posCtlFlightMode(tr("Position"))
    , _offboardFlightMode(tr("Offboard"))
    , _readyFlightMode(tr("Ready"))
    , _takeoffFlightMode(tr("Takeoff"))
    , _holdFlightMode(tr("Hold"))
    , _missionFlightMode(tr("Mission"))
    , _rtlFlightMode(tr("Return"))
    , _landingFlightMode(tr("Land"))
    , _rtgsFlightMode(tr("Return to Groundstation"))
    , _followMeFlightMode(tr("Follow Me"))
    , _simpleFlightMode(tr("Simple"))
54
{
55 56 57 58 59 60
    qmlRegisterType<PX4AdvancedFlightModesController>   ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController");
    qmlRegisterType<PX4SimpleFlightModesController>     ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController");
    qmlRegisterType<AirframeComponentController>        ("QGroundControl.Controllers", 1, 0, "AirframeComponentController");
    qmlRegisterType<SensorsComponentController>         ("QGroundControl.Controllers", 1, 0, "SensorsComponentController");
    qmlRegisterType<PowerComponentController>           ("QGroundControl.Controllers", 1, 0, "PowerComponentController");
    qmlRegisterType<RadioComponentController>           ("QGroundControl.Controllers", 1, 0, "RadioComponentController");
61 62 63 64 65 66 67 68 69 70

    struct Modes2Name {
        uint8_t     main_mode;
        uint8_t     sub_mode;
        bool        canBeSet;   ///< true: Vehicle can be set to this flight mode
        bool        fixedWing;  /// fixed wing compatible
        bool        multiRotor;  /// multi rotor compatible
    };

    static const struct Modes2Name rgModes2Name[] = {
71 72 73 74 75 76 77
        //main_mode                         sub_mode                                canBeSet  FW      MC
        { PX4_CUSTOM_MAIN_MODE_MANUAL,      0,                                      true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_STABILIZED,  0,                                      true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_ACRO,        0,                                      true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_RATTITUDE,   0,                                      true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_ALTCTL,      0,                                      true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_POSCTL,      0,                                      true,   true,   true },
78 79
        // simple can't be set by the user right now
        { PX4_CUSTOM_MAIN_MODE_SIMPLE,      0,                                      false,   false,  true },
80 81 82 83 84
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LOITER,        true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_MISSION,       true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,           true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, true,   false,  true },
        { PX4_CUSTOM_MAIN_MODE_OFFBOARD,    0,                                      true,   false,  true },
85
        // modes that can't be directly set by the user
86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,          false,  true,   true },
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_READY,         false,  true,   true },
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTGS,          false,  true,   true },
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,       false,  true,   true },
    };

    // Must be in same order as above structure
    const QString* rgModeNames[] = {
        &_manualFlightMode,
        &_stabilizedFlightMode,
        &_acroFlightMode,
        &_rattitudeFlightMode,
        &_altCtlFlightMode,
        &_posCtlFlightMode,
        &_simpleFlightMode,
        &_holdFlightMode,
        &_missionFlightMode,
        &_rtlFlightMode,
        &_followMeFlightMode,
        &_offboardFlightMode,
        &_landingFlightMode,
        &_readyFlightMode,
        &_rtgsFlightMode,
        &_takeoffFlightMode,
110 111 112 113 114 115 116 117 118 119
    };

    // Convert static information to dynamic list. This allows for plugin override class to manipulate list.
    for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
        const struct Modes2Name* pModes2Name = &rgModes2Name[i];

        FlightModeInfo_t info;

        info.main_mode =    pModes2Name->main_mode;
        info.sub_mode =     pModes2Name->sub_mode;
120
        info.name =         rgModeNames[i];
121 122 123 124 125 126
        info.canBeSet =     pModes2Name->canBeSet;
        info.fixedWing =    pModes2Name->fixedWing;
        info.multiRotor =   pModes2Name->multiRotor;

        _flightModeInfoList.append(info);
    }
127
}
128

129 130 131 132
PX4FirmwarePlugin::~PX4FirmwarePlugin()
{
}

133 134 135
AutoPilotPlugin* PX4FirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
{
    return new PX4AutoPilotPlugin(vehicle, vehicle);
136 137
}

Don Gagne's avatar
Don Gagne committed
138 139 140
QList<VehicleComponent*> PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);
dogmaphobic's avatar
dogmaphobic committed
141

Don Gagne's avatar
Don Gagne committed
142 143 144
    return QList<VehicleComponent*>();
}

Daniel Agar's avatar
Daniel Agar committed
145
QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
146 147
{
    QStringList flightModes;
dogmaphobic's avatar
dogmaphobic committed
148

149 150 151 152
    foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
        if (info.canBeSet) {
            bool fw = (vehicle->fixedWing() && info.fixedWing);
            bool mc = (vehicle->multiRotor() && info.multiRotor);
Daniel Agar's avatar
Daniel Agar committed
153 154 155 156 157

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

            if (fw || mc || other) {
158
                flightModes += *info.name;
Daniel Agar's avatar
Daniel Agar committed
159
            }
Don Gagne's avatar
Don Gagne committed
160 161
        }
    }
dogmaphobic's avatar
dogmaphobic committed
162

Don Gagne's avatar
Don Gagne committed
163 164 165
    return flightModes;
}

Don Gagne's avatar
Don Gagne committed
166
QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
167 168
{
    QString flightMode = "Unknown";
dogmaphobic's avatar
dogmaphobic committed
169

Don Gagne's avatar
Don Gagne committed
170 171 172
    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
173

Don Gagne's avatar
Don Gagne committed
174
        bool found = false;
175 176
        foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
            if (info.main_mode == px4_mode.main_mode && info.sub_mode == px4_mode.sub_mode) {
177
                flightMode = *info.name;
Don Gagne's avatar
Don Gagne committed
178 179 180 181
                found = true;
                break;
            }
        }
dogmaphobic's avatar
dogmaphobic committed
182

Don Gagne's avatar
Don Gagne committed
183 184
        if (!found) {
            qWarning() << "Unknown flight mode" << custom_mode;
185
            return tr("Unknown %1:%2").arg(base_mode).arg(custom_mode);
Don Gagne's avatar
Don Gagne committed
186 187 188 189
        }
    } else {
        qWarning() << "PX4 Flight Stack flight mode without custom mode enabled?";
    }
dogmaphobic's avatar
dogmaphobic committed
190

Don Gagne's avatar
Don Gagne committed
191 192 193 194 195 196 197
    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
198

Don Gagne's avatar
Don Gagne committed
199
    bool found = false;
200 201
    foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
        if (flightMode.compare(info.name, Qt::CaseInsensitive) == 0) {
Don Gagne's avatar
Don Gagne committed
202 203 204
            union px4_custom_mode px4_mode;

            px4_mode.data = 0;
205 206
            px4_mode.main_mode = info.main_mode;
            px4_mode.sub_mode = info.sub_mode;
dogmaphobic's avatar
dogmaphobic committed
207

Don Gagne's avatar
Don Gagne committed
208 209
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = px4_mode.data;
dogmaphobic's avatar
dogmaphobic committed
210

Don Gagne's avatar
Don Gagne committed
211 212 213 214
            found = true;
            break;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
215

Don Gagne's avatar
Don Gagne committed
216 217 218
    if (!found) {
        qWarning() << "Unknown flight Mode" << flightMode;
    }
dogmaphobic's avatar
dogmaphobic committed
219

Don Gagne's avatar
Don Gagne committed
220 221
    return found;
}
222 223 224

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

228
bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
Don Gagne's avatar
Don Gagne committed
229
{
230 231 232
    int available = MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
    if (vehicle->multiRotor() || vehicle->vtol()) {
        available |= TakeoffVehicleCapability;
233
    }
234 235

    return (capabilities & available) == capabilities;
Don Gagne's avatar
Don Gagne committed
236
}
237 238 239

void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
240
    vehicle->setFirmwarePluginInstanceData(new PX4FirmwarePluginInstanceData);
241
}
242 243 244 245 246 247 248

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

250 251 252 253 254 255 256 257 258 259 260 261 262
FactMetaData* PX4FirmwarePlugin::getMetaDataForFact(QObject* parameterMetaData, const QString& name, MAV_TYPE vehicleType)
{
    PX4ParameterMetaData* px4MetaData = qobject_cast<PX4ParameterMetaData*>(parameterMetaData);

    if (px4MetaData) {
        return px4MetaData->getMetaDataForFact(name, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to PX4FirmwarePlugin::getMetaDataForFact not PX4ParameterMetaData";
    }

    return NULL;
}

263
void PX4FirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
264
{
265 266 267 268 269 270 271
    PX4ParameterMetaData* px4MetaData = qobject_cast<PX4ParameterMetaData*>(parameterMetaData);

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

274 275 276 277 278
void PX4FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataFile, int& majorVersion, int& minorVersion)
{
    return PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion);
}

Don Gagne's avatar
Don Gagne committed
279 280 281 282
QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
{
    QList<MAV_CMD> list;

283
    list << MAV_CMD_NAV_WAYPOINT
284
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TIME << MAV_CMD_NAV_LOITER_TO_ALT
Don Gagne's avatar
Don Gagne committed
285
         << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
286
         << MAV_CMD_DO_JUMP
Don Gagne's avatar
Don Gagne committed
287
         << MAV_CMD_DO_VTOL_TRANSITION << MAV_CMD_NAV_VTOL_TAKEOFF << MAV_CMD_NAV_VTOL_LAND
288
         << MAV_CMD_DO_DIGICAM_CONTROL
289
         << MAV_CMD_DO_SET_CAM_TRIGG_DIST
290
         << MAV_CMD_DO_SET_SERVO
Don Gagne's avatar
Don Gagne committed
291
         << MAV_CMD_DO_CHANGE_SPEED
292
         << MAV_CMD_DO_LAND_START
Don Gagne's avatar
Don Gagne committed
293
         << MAV_CMD_DO_SET_ROI_LOCATION << MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET << MAV_CMD_DO_SET_ROI_NONE
294
         << MAV_CMD_DO_MOUNT_CONFIGURE
295
         << MAV_CMD_DO_MOUNT_CONTROL
296
         << MAV_CMD_SET_CAMERA_MODE
DonLakeFlyer's avatar
DonLakeFlyer committed
297
         << MAV_CMD_IMAGE_START_CAPTURE << MAV_CMD_IMAGE_STOP_CAPTURE << MAV_CMD_VIDEO_START_CAPTURE << MAV_CMD_VIDEO_STOP_CAPTURE
298 299
         << MAV_CMD_NAV_DELAY
         << MAV_CMD_CONDITION_YAW;
Jimmy Johnson's avatar
Jimmy Johnson committed
300

Don Gagne's avatar
Don Gagne committed
301 302
    return list;
}
303

304
QString PX4FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
305
{
306 307 308 309 310 311 312 313 314 315 316
    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:
Don Gagne's avatar
Don Gagne committed
317
        return QStringLiteral(":/json/PX4/MavCmdInfoVTOL.json");
318 319 320 321 322 323 324 325 326 327 328
        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();
    }
329
}
330 331 332 333

QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
{
    PX4ParameterMetaData* metaData = new PX4ParameterMetaData;
334 335 336
    if (!metaDataFile.isEmpty()) {
        metaData->loadParameterFactMetaDataFile(metaDataFile);
    }
337 338
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
339 340 341

void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
342 343 344 345 346 347 348 349 350 351
    vehicle->sendMavCommand(vehicle->defaultComponentId(),
                            MAV_CMD_DO_REPOSITION,
                            true,   // show error if failed
                            -1.0f,
                            MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
                            0.0f,
                            NAN,
                            NAN,
                            NAN,
                            NAN);
Don Gagne's avatar
Don Gagne committed
352
}
353 354 355

void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
356
    _setFlightModeAndValidate(vehicle, _rtlFlightMode);
357 358 359 360
}

void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
361
    _setFlightModeAndValidate(vehicle, _landingFlightMode);
362 363
}

364 365
void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
{
366
    if (!isGuidedMode(vehicle)) {
367
        setGuidedMode(vehicle, true);
368 369 370 371 372 373 374 375 376 377 378 379
    }

    vehicle->sendMavCommand(vehicle->defaultComponentId(),
                            MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE,
                            true,   // show error if fails
                            radius,
                            velocity,
                            altitude,
                            NAN,
                            centerCoord.isValid() ? centerCoord.latitude()  : NAN,
                            centerCoord.isValid() ? centerCoord.longitude() : NAN,
                            centerCoord.isValid() ? centerCoord.altitude()  : NAN);
380 381
}

382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404
void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
{
    Q_UNUSED(vehicleId);
    Q_UNUSED(component);
    Q_UNUSED(noReponseFromVehicle);

    Vehicle* vehicle = dynamic_cast<Vehicle*>(sender());
    if (!vehicle) {
        qWarning() << "Dynamic cast failed!";
        return;
    }

    if (command == MAV_CMD_NAV_TAKEOFF && result == MAV_RESULT_ACCEPTED) {
        // Now that we are in takeoff mode we can arm the vehicle which will cause it to takeoff.
        // We specifically don't retry arming if it fails. This way we don't fight with the user if
        // They are trying to disarm.
        disconnect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
        if (!vehicle->armed()) {
            vehicle->setArmed(true);
        }
    }
}

405
double PX4FirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle)
406
{
407 408
    QString takeoffAltParam("MIS_TAKEOFF_ALT");

409 410 411 412 413 414 415 416 417
    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
        return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble();
    } else {
        return FirmwarePlugin::minimumTakeoffAltitude(vehicle);
    }
}

void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
{
418 419
    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
    if (qIsNaN(vehicleAltitudeAMSL)) {
420
        qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
421 422 423
        return;
    }

424
    double takeoffAltRelFromVehicle = minimumTakeoffAltitude(vehicle);
425
    double takeoffAltAMSL = qMax(takeoffAltRel, takeoffAltRelFromVehicle) + vehicleAltitudeAMSL;
426

427 428
    qDebug() << takeoffAltRel << takeoffAltRelFromVehicle << takeoffAltAMSL << vehicleAltitudeAMSL;

429
    connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
430 431
    vehicle->sendMavCommand(vehicle->defaultComponentId(),
                            MAV_CMD_NAV_TAKEOFF,
432 433 434 435
                            true,                           // show error is fails
                            -1,                             // No pitch requested
                            0, 0,                           // param 2-4 unused
                            NAN, NAN, NAN,                  // No yaw, lat, lon
436
                            takeoffAltAMSL);                // AMSL altitude
437 438 439 440
}

void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
441
    if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
442
        qgcApp()->showMessage(tr("Unable to go to location, vehicle position not known."));
443 444 445
        return;
    }

446 447 448 449 450 451 452 453 454 455
    vehicle->sendMavCommand(vehicle->defaultComponentId(),
                            MAV_CMD_DO_REPOSITION,
                            true,   // show error is fails
                            -1.0f,
                            MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
                            0.0f,
                            NAN,
                            gotoCoord.latitude(),
                            gotoCoord.longitude(),
                            vehicle->altitudeAMSL()->rawValue().toFloat());
456 457
}

458
void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
459
{
460
    if (!vehicle->homePosition().isValid()) {
461
        qgcApp()->showMessage(tr("Unable to change altitude, home position unknown."));
462 463 464
        return;
    }
    if (qIsNaN(vehicle->homePosition().altitude())) {
465
        qgcApp()->showMessage(tr("Unable to change altitude, home position altitude unknown."));
466 467 468
        return;
    }

469
    double currentAltRel = vehicle->altitudeRelative()->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
470
    double newAltRel = currentAltRel + altitudeChange;
471

472 473 474 475 476 477 478 479 480
    vehicle->sendMavCommand(vehicle->defaultComponentId(),
                            MAV_CMD_DO_REPOSITION,
                            true,   // show error is fails
                            -1.0f,
                            MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
                            0.0f,
                            NAN,
                            NAN,
                            NAN,
481
                            vehicle->homePosition().altitude() + newAltRel);
482 483
}

484 485 486
void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
{

487 488 489 490 491 492 493 494
    if (_setFlightModeAndValidate(vehicle, missionFlightMode())) {
        if (!_armVehicleAndValidate(vehicle)) {
            qgcApp()->showMessage(tr("Unable to start mission: Vehicle rejected arming."));
            return;
        }
    } else {
        qgcApp()->showMessage(tr("Unable to start mission: Vehicle not ready."));
    }
495 496
}

497 498 499
void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    if (guidedMode) {
500
        _setFlightModeAndValidate(vehicle, _holdFlightMode);
501 502 503 504
    } else {
        pauseVehicle(vehicle);
    }
}
505 506 507 508

bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
    // Not supported by generic vehicle
509 510
    return (vehicle->flightMode() == _holdFlightMode || vehicle->flightMode() == _takeoffFlightMode
            || vehicle->flightMode() == _landingFlightMode);
511
}
512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532

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

533 534
    PX4FirmwarePluginInstanceData* instanceData = qobject_cast<PX4FirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
    if (!instanceData->versionNotified) {
535 536 537 538 539 540 541 542 543 544 545 546 547 548 549
        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;

550 551 552 553 554 555 556 557 558
            if (majorVersion < supportedMajorVersion) {
                notifyUser = true;
            } else if (majorVersion == supportedMajorVersion) {
                if (minorVersion < supportedMinorVersion) {
                    notifyUser = true;
                } else if (minorVersion == supportedMinorVersion) {
                    notifyUser = patchVersion < supportedPatchVersion;
                }
            }
559 560 561 562 563
        } else {
            notifyUser = true;
        }

        if (notifyUser) {
564
            instanceData->versionNotified = true;
565
            qgcApp()->showMessage(tr("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));
566 567 568
        }
    }
}
569

570 571
bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
572 573 574 575 576 577 578
    if (vehicle->isOfflineEditingVehicle()) {
        return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle);
    } else {
        if (vehicle->multiRotor() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) {
            Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"));
            return yawMode && yawMode->rawValue().toInt() == 1;
        }
579
    }
580
    return true;
581
}
582

583
QGCCameraManager* PX4FirmwarePlugin::createCameraManager(Vehicle* vehicle)
584
{
585
    return new QGCCameraManager(vehicle);
586 587 588 589 590 591
}

QGCCameraControl* PX4FirmwarePlugin::createCameraControl(const mavlink_camera_information_t* info, Vehicle *vehicle, int compID, QObject* parent)
{
    return new QGCCameraControl(info, vehicle, compID, parent);
}