PX4FirmwarePlugin.cc 21.8 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * 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"
25
#include "QGCFileDownload.h"
Don Gagne's avatar
Don Gagne committed
26 27 28

#include <QDebug>

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

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

}

38 39
PX4FirmwarePlugin::PX4FirmwarePlugin()
    : _manualFlightMode     (tr("Manual"))
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
    , _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"))
    , _preclandFlightMode   (tr("Precision Land"))
    , _rtgsFlightMode       (tr("Return to Groundstation"))
    , _followMeFlightMode   (tr("Follow Me"))
    , _simpleFlightMode     (tr("Simple"))
    , _orbitFlightMode      (tr("Orbit"))
57
{
58 59 60 61 62 63
    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");
64 65 66 67 68 69 70 71 72 73

    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[] = {
74 75 76 77 78 79
        //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 },
80 81 82 83
        { PX4_CUSTOM_MAIN_MODE_OFFBOARD,    0,                                      true,   false,  true },
        { PX4_CUSTOM_MAIN_MODE_SIMPLE,      0,                                      false,  false,  true },
        { PX4_CUSTOM_MAIN_MODE_POSCTL,      PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL,      true,   true,   true },
        { PX4_CUSTOM_MAIN_MODE_POSCTL,      PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT,       false,  false,   false },
84 85 86 87 88
        { 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_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,          false,  true,   true },
89
        { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,      false,  false,  true },
90 91 92 93 94 95 96 97 98 99 100 101
        { 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,
102
        &_offboardFlightMode,
103
        &_simpleFlightMode,
104 105
        &_posCtlFlightMode,
        &_orbitFlightMode,
106 107 108 109 110
        &_holdFlightMode,
        &_missionFlightMode,
        &_rtlFlightMode,
        &_followMeFlightMode,
        &_landingFlightMode,
111
        &_preclandFlightMode,
112 113 114
        &_readyFlightMode,
        &_rtgsFlightMode,
        &_takeoffFlightMode,
115 116 117 118 119 120 121 122 123 124
    };

    // 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;
125
        info.name =         rgModeNames[i];
126 127 128 129 130 131
        info.canBeSet =     pModes2Name->canBeSet;
        info.fixedWing =    pModes2Name->fixedWing;
        info.multiRotor =   pModes2Name->multiRotor;

        _flightModeInfoList.append(info);
    }
132
}
133

134 135 136 137
PX4FirmwarePlugin::~PX4FirmwarePlugin()
{
}

138 139 140
AutoPilotPlugin* PX4FirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
{
    return new PX4AutoPilotPlugin(vehicle, vehicle);
141 142
}

Don Gagne's avatar
Don Gagne committed
143 144 145
QList<VehicleComponent*> PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);
dogmaphobic's avatar
dogmaphobic committed
146

Don Gagne's avatar
Don Gagne committed
147 148 149
    return QList<VehicleComponent*>();
}

Daniel Agar's avatar
Daniel Agar committed
150
QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
151 152
{
    QStringList flightModes;
dogmaphobic's avatar
dogmaphobic committed
153

154 155 156 157
    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
158 159 160 161 162

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

            if (fw || mc || other) {
163
                flightModes += *info.name;
Daniel Agar's avatar
Daniel Agar committed
164
            }
Don Gagne's avatar
Don Gagne committed
165 166
        }
    }
dogmaphobic's avatar
dogmaphobic committed
167

Don Gagne's avatar
Don Gagne committed
168 169 170
    return flightModes;
}

Don Gagne's avatar
Don Gagne committed
171
QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
172 173
{
    QString flightMode = "Unknown";
dogmaphobic's avatar
dogmaphobic committed
174

Don Gagne's avatar
Don Gagne committed
175 176 177
    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
178

Don Gagne's avatar
Don Gagne committed
179
        bool found = false;
180 181
        foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
            if (info.main_mode == px4_mode.main_mode && info.sub_mode == px4_mode.sub_mode) {
182
                flightMode = *info.name;
Don Gagne's avatar
Don Gagne committed
183 184 185 186
                found = true;
                break;
            }
        }
dogmaphobic's avatar
dogmaphobic committed
187

Don Gagne's avatar
Don Gagne committed
188 189
        if (!found) {
            qWarning() << "Unknown flight mode" << custom_mode;
190
            return tr("Unknown %1:%2").arg(base_mode).arg(custom_mode);
Don Gagne's avatar
Don Gagne committed
191 192
        }
    }
dogmaphobic's avatar
dogmaphobic committed
193

Don Gagne's avatar
Don Gagne committed
194 195 196 197 198 199 200
    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
201

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

            px4_mode.data = 0;
208 209
            px4_mode.main_mode = info.main_mode;
            px4_mode.sub_mode = info.sub_mode;
dogmaphobic's avatar
dogmaphobic committed
210

Don Gagne's avatar
Don Gagne committed
211 212
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = px4_mode.data;
dogmaphobic's avatar
dogmaphobic committed
213

Don Gagne's avatar
Don Gagne committed
214 215 216 217
            found = true;
            break;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
218

Don Gagne's avatar
Don Gagne committed
219 220 221
    if (!found) {
        qWarning() << "Unknown flight Mode" << flightMode;
    }
dogmaphobic's avatar
dogmaphobic committed
222

Don Gagne's avatar
Don Gagne committed
223 224
    return found;
}
225

226
bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
Don Gagne's avatar
Don Gagne committed
227
{
228
    int available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
Gus Grubba's avatar
Gus Grubba committed
229 230 231 232
    //-- This is arbitrary until I find how to really tell if ROI is avaiable
    if (vehicle->multiRotor()) {
        available |= ROIModeCapability;
    }
233
    if (vehicle->multiRotor() || vehicle->vtol()) {
Don Gagne's avatar
Don Gagne committed
234
        available |= TakeoffVehicleCapability | OrbitModeCapability;
235
    }
236
    return (capabilities & available) == capabilities;
Don Gagne's avatar
Don Gagne committed
237
}
238 239 240

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

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

251 252 253 254 255 256 257 258 259 260
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";
    }

261
    return nullptr;
262 263
}

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

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

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

Don Gagne's avatar
Don Gagne committed
280 281
QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
{
282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299
    return {
        MAV_CMD_NAV_WAYPOINT,
        MAV_CMD_NAV_LOITER_UNLIM, MAV_CMD_NAV_LOITER_TIME, MAV_CMD_NAV_LOITER_TO_ALT,
        MAV_CMD_NAV_LAND, MAV_CMD_NAV_TAKEOFF, MAV_CMD_NAV_RETURN_TO_LAUNCH,
        MAV_CMD_DO_JUMP,
        MAV_CMD_DO_VTOL_TRANSITION, MAV_CMD_NAV_VTOL_TAKEOFF, MAV_CMD_NAV_VTOL_LAND,
        MAV_CMD_DO_DIGICAM_CONTROL,
        MAV_CMD_DO_SET_CAM_TRIGG_DIST,
        MAV_CMD_DO_SET_SERVO,
        MAV_CMD_DO_CHANGE_SPEED,
        MAV_CMD_DO_LAND_START,
        MAV_CMD_DO_SET_ROI_LOCATION, MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET, MAV_CMD_DO_SET_ROI_NONE,
        MAV_CMD_DO_MOUNT_CONFIGURE,
        MAV_CMD_DO_MOUNT_CONTROL,
        MAV_CMD_SET_CAMERA_MODE,
        MAV_CMD_IMAGE_START_CAPTURE, MAV_CMD_IMAGE_STOP_CAPTURE, MAV_CMD_VIDEO_START_CAPTURE, MAV_CMD_VIDEO_STOP_CAPTURE,
        MAV_CMD_NAV_DELAY,
        MAV_CMD_CONDITION_YAW,
Tomaz Canabrava's avatar
Tomaz Canabrava committed
300
    };
Don Gagne's avatar
Don Gagne committed
301
}
302

303
QString PX4FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
304
{
305 306 307 308 309 310 311 312
    switch (vehicleType) {
    case MAV_TYPE_GENERIC:
        return QStringLiteral(":/json/PX4/MavCmdInfoCommon.json");
    case MAV_TYPE_FIXED_WING:
        return QStringLiteral(":/json/PX4/MavCmdInfoFixedWing.json");
    case MAV_TYPE_QUADROTOR:
        return QStringLiteral(":/json/PX4/MavCmdInfoMultiRotor.json");
    case MAV_TYPE_VTOL_QUADROTOR:
Don Gagne's avatar
Don Gagne committed
313
        return QStringLiteral(":/json/PX4/MavCmdInfoVTOL.json");
314 315 316 317 318 319 320 321
    case MAV_TYPE_SUBMARINE:
        return QStringLiteral(":/json/PX4/MavCmdInfoSub.json");
    case MAV_TYPE_GROUND_ROVER:
        return QStringLiteral(":/json/PX4/MavCmdInfoRover.json");
    default:
        qWarning() << "PX4FirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType;
        return QString();
    }
322
}
323 324 325 326

QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
{
    PX4ParameterMetaData* metaData = new PX4ParameterMetaData;
327 328 329
    if (!metaDataFile.isEmpty()) {
        metaData->loadParameterFactMetaDataFile(metaDataFile);
    }
330 331
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
332 333 334

void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
335 336 337 338 339 340 341 342 343 344
    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
345
}
346

DonLakeFlyer's avatar
DonLakeFlyer committed
347
void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
348
{
DonLakeFlyer's avatar
DonLakeFlyer committed
349
    Q_UNUSED(smartRTL);
350
    _setFlightModeAndValidate(vehicle, _rtlFlightMode);
351 352 353 354
}

void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
355
    _setFlightModeAndValidate(vehicle, _landingFlightMode);
356 357
}

358 359 360 361 362 363
void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
{
    Q_UNUSED(vehicleId);
    Q_UNUSED(component);
    Q_UNUSED(noReponseFromVehicle);

364
    auto* vehicle = qobject_cast<Vehicle*>(sender());
365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380
    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);
        }
    }
}

381
double PX4FirmwarePlugin::minimumTakeoffAltitude(Vehicle* vehicle)
382
{
383 384
    QString takeoffAltParam("MIS_TAKEOFF_ALT");

385 386 387 388 389 390 391 392 393
    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)
{
394 395
    double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
    if (qIsNaN(vehicleAltitudeAMSL)) {
396
        qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
397 398 399
        return;
    }

400
    double takeoffAltRelFromVehicle = minimumTakeoffAltitude(vehicle);
401
    double takeoffAltAMSL = qMax(takeoffAltRel, takeoffAltRelFromVehicle) + vehicleAltitudeAMSL;
402

403 404
    qDebug() << takeoffAltRel << takeoffAltRelFromVehicle << takeoffAltAMSL << vehicleAltitudeAMSL;

405
    connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
406 407 408 409 410 411 412 413
    vehicle->sendMavCommand(
        vehicle->defaultComponentId(),
        MAV_CMD_NAV_TAKEOFF,
        true,                                   // show error is fails
        -1,                                     // No pitch requested
        0, 0,                                   // param 2-4 unused
        NAN, NAN, NAN,                          // No yaw, lat, lon
        static_cast<float>(takeoffAltAMSL));    // AMSL altitude
414 415 416 417
}

void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
418
    if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
419
        qgcApp()->showMessage(tr("Unable to go to location, vehicle position not known."));
420 421 422
        return;
    }

Don Gagne's avatar
Don Gagne committed
423
    if (vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442
        vehicle->sendMavCommandInt(vehicle->defaultComponentId(),
                                   MAV_CMD_DO_REPOSITION,
                                   MAV_FRAME_GLOBAL,
                                   true,   // show error is fails
                                   -1.0f,
                                   MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
                                   0.0f,
                                   NAN,
                                   gotoCoord.latitude(),
                                   gotoCoord.longitude(),
                                   vehicle->altitudeAMSL()->rawValue().toFloat());
    } else {
        vehicle->sendMavCommand(vehicle->defaultComponentId(),
                                MAV_CMD_DO_REPOSITION,
                                true,   // show error is fails
                                -1.0f,
                                MAV_DO_REPOSITION_FLAGS_CHANGE_MODE,
                                0.0f,
                                NAN,
443 444
                                static_cast<float>(gotoCoord.latitude()),
                                static_cast<float>(gotoCoord.longitude()),
445 446
                                vehicle->altitudeAMSL()->rawValue().toFloat());
    }
447 448
}

449
void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
450
{
451
    if (!vehicle->homePosition().isValid()) {
452
        qgcApp()->showMessage(tr("Unable to change altitude, home position unknown."));
453 454 455
        return;
    }
    if (qIsNaN(vehicle->homePosition().altitude())) {
456
        qgcApp()->showMessage(tr("Unable to change altitude, home position altitude unknown."));
457 458 459
        return;
    }

460
    double currentAltRel = vehicle->altitudeRelative()->rawValue().toDouble();
Don Gagne's avatar
Don Gagne committed
461
    double newAltRel = currentAltRel + altitudeChange;
462

463 464 465 466 467 468 469 470 471
    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,
472
                            static_cast<float>(vehicle->homePosition().altitude() + newAltRel));
473 474
}

475 476 477
void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
{

478 479 480 481 482 483 484 485
    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."));
    }
486 487
}

488 489 490
void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    if (guidedMode) {
491
        _setFlightModeAndValidate(vehicle, _holdFlightMode);
492 493 494 495
    } else {
        pauseVehicle(vehicle);
    }
}
496 497 498 499

bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
    // Not supported by generic vehicle
500 501
    return (vehicle->flightMode() == _holdFlightMode || vehicle->flightMode() == _takeoffFlightMode
            || vehicle->flightMode() == _landingFlightMode);
502
}
503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523

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

524
    auto* instanceData = qobject_cast<PX4FirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
525
    if (!instanceData->versionNotified) {
526 527 528 529 530 531 532 533 534 535 536 537 538 539 540
        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;

541 542 543 544 545 546 547 548 549
            if (majorVersion < supportedMajorVersion) {
                notifyUser = true;
            } else if (majorVersion == supportedMajorVersion) {
                if (minorVersion < supportedMinorVersion) {
                    notifyUser = true;
                } else if (minorVersion == supportedMinorVersion) {
                    notifyUser = patchVersion < supportedPatchVersion;
                }
            }
550 551 552 553 554
        } else {
            notifyUser = true;
        }

        if (notifyUser) {
555
            instanceData->versionNotified = true;
556
            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));
557 558 559
        }
    }
}
560

561 562 563 564 565 566 567 568
uint32_t PX4FirmwarePlugin::highLatencyCustomModeTo32Bits(uint16_t hlCustomMode)
{
    union px4_custom_mode px4_cm;
    px4_cm.data = 0;
    px4_cm.custom_mode_hl = hlCustomMode;

    return px4_cm.data;
}
569 570 571 572 573 574 575 576 577

QString PX4FirmwarePlugin::_getLatestVersionFileUrl(Vehicle* vehicle){
    Q_UNUSED(vehicle);
    return QStringLiteral("https://api.github.com/repos/PX4/Firmware/releases");
}

QString PX4FirmwarePlugin::_versionRegex() {
    return QStringLiteral("v([0-9,\\.]*) Stable");
}
578

579
bool PX4FirmwarePlugin::supportsNegativeThrust(Vehicle* vehicle)
580
{
581
    return vehicle->vehicleType() == MAV_TYPE_GROUND_ROVER;
582
}