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

10
#include "FirmwarePlugin.h"
Don Gagne's avatar
Don Gagne committed
11
#include "QGCApplication.h"
12
#include "Generic/GenericAutoPilotPlugin.h"
13
#include "CameraMetaData.h"
14 15
#include "SettingsManager.h"
#include "AppSettings.h"
16
#include "QGCFileDownload.h"
Don Gagne's avatar
Don Gagne committed
17

18
#include <QRegularExpression>
Don Gagne's avatar
Don Gagne committed
19 20
#include <QDebug>

21 22
QGC_LOGGING_CATEGORY(FirmwarePluginLog, "FirmwarePluginLog")

23 24
static FirmwarePluginFactoryRegister* _instance = NULL;

25
const QString guided_mode_not_supported_by_vehicle = QObject::tr("Guided mode not supported by Vehicle.");
26

27 28
QVariantList FirmwarePlugin::_cameraList;

29
const QString FirmwarePlugin::px4FollowMeFlightMode(QObject::tr("Follow Me"));
30 31 32 33 34 35

FirmwarePluginFactory::FirmwarePluginFactory(void)
{
    FirmwarePluginFactoryRegister::instance()->registerPluginFactory(this);
}

36 37 38 39 40 41 42
QList<MAV_TYPE> FirmwarePluginFactory::supportedVehicleTypes(void) const
{
    QList<MAV_TYPE> vehicleTypes;
    vehicleTypes << MAV_TYPE_FIXED_WING << MAV_TYPE_QUADROTOR << MAV_TYPE_VTOL_QUADROTOR << MAV_TYPE_GROUND_ROVER << MAV_TYPE_SUBMARINE;
    return vehicleTypes;
}

43 44 45 46 47 48 49 50 51 52 53 54 55 56
FirmwarePluginFactoryRegister* FirmwarePluginFactoryRegister::instance(void)
{
    if (!_instance) {
        _instance = new FirmwarePluginFactoryRegister;
    }

    return _instance;
}

AutoPilotPlugin* FirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
{
    return new GenericAutoPilotPlugin(vehicle, vehicle);
}

57
bool FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
Don Gagne's avatar
Don Gagne committed
58
{
59
    Q_UNUSED(vehicle);
Don Gagne's avatar
Don Gagne committed
60 61 62 63
    Q_UNUSED(capabilities);
    return false;
}

64
QList<VehicleComponent*> FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
Don Gagne's avatar
Don Gagne committed
65 66
{
    Q_UNUSED(vehicle);
67

Don Gagne's avatar
Don Gagne committed
68 69 70
    return QList<VehicleComponent*>();
}

Don Gagne's avatar
Don Gagne committed
71
QString FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
72 73
{
    QString flightMode;
74

Don Gagne's avatar
Don Gagne committed
75 76 77 78 79
    struct Bit2Name {
        uint8_t     baseModeBit;
        const char* name;
    };
    static const struct Bit2Name rgBit2Name[] = {
80 81 82 83 84 85
    { MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,   "Manual" },
    { MAV_MODE_FLAG_STABILIZE_ENABLED,      "Stabilize" },
    { MAV_MODE_FLAG_GUIDED_ENABLED,         "Guided" },
    { MAV_MODE_FLAG_AUTO_ENABLED,           "Auto" },
    { MAV_MODE_FLAG_TEST_ENABLED,           "Test" },
};
86

Don Gagne's avatar
Don Gagne committed
87
    Q_UNUSED(custom_mode);
88

Don Gagne's avatar
Don Gagne committed
89 90 91 92 93 94 95 96 97 98 99 100 101 102
    if (base_mode == 0) {
        flightMode = "PreFlight";
    } else if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        flightMode = QString("Custom:0x%1").arg(custom_mode, 0, 16);
    } else {
        for (size_t i=0; i<sizeof(rgBit2Name)/sizeof(rgBit2Name[0]); i++) {
            if (base_mode & rgBit2Name[i].baseModeBit) {
                if (i != 0) {
                    flightMode += " ";
                }
                flightMode += rgBit2Name[i].name;
            }
        }
    }
103

Don Gagne's avatar
Don Gagne committed
104 105 106
    return flightMode;
}

107
bool FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
Don Gagne's avatar
Don Gagne committed
108 109 110 111
{
    Q_UNUSED(flightMode);
    Q_UNUSED(base_mode);
    Q_UNUSED(custom_mode);
112

113
    qWarning() << "FirmwarePlugin::setFlightMode called on base class, not supported";
114

Don Gagne's avatar
Don Gagne committed
115 116
    return false;
}
117

118
int FirmwarePlugin::manualControlReservedButtonCount(void)
119 120 121 122 123
{
    // We don't know whether the firmware is going to used any of these buttons.
    // So reserve them all.
    return -1;
}
Don Gagne's avatar
Don Gagne committed
124

125 126 127 128 129
int FirmwarePlugin::defaultJoystickTXMode(void)
{
    return 2;
}

130 131 132 133 134 135
bool FirmwarePlugin::supportsThrottleModeCenterZero(void)
{
    // By default, this is supported
    return true;
}

136 137 138 139 140 141
bool FirmwarePlugin::supportsNegativeThrust(void)
{
    // By default, this is not supported
    return false;
}

142 143 144 145 146
bool FirmwarePlugin::supportsRadio(void)
{
    return true;
}

147 148 149 150 151
bool FirmwarePlugin::supportsMotorInterference(void)
{
    return true;
}

152 153 154 155 156
bool FirmwarePlugin::supportsJSButton(void)
{
    return false;
}

157 158 159 160 161 162
bool FirmwarePlugin::supportsTerrainFrame(void) const
{
    // Generic firmware supports this since we don't know
    return true;
}

163
bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
164 165
{
    Q_UNUSED(vehicle);
166
    Q_UNUSED(message);
Don Gagne's avatar
Don Gagne committed
167
    // Generic plugin does no message adjustment
168
    return true;
Don Gagne's avatar
Don Gagne committed
169 170
}

171
void FirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
172
{
173
    Q_UNUSED(vehicle);
174
    Q_UNUSED(outgoingLink);
Don Gagne's avatar
Don Gagne committed
175 176 177
    Q_UNUSED(message);
    // Generic plugin does no message adjustment
}
178

179
void FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
180 181
{
    Q_UNUSED(vehicle);
182

183 184
    // Generic Flight Stack is by definition "generic", so no extra work
}
185

186
bool FirmwarePlugin::sendHomePositionToVehicle(void)
187 188 189 190 191 192
{
    // Generic stack does not want home position sent in the first position.
    // Subsequent sequence numbers must be adjusted.
    // This is the mavlink spec default.
    return false;
}
193

194
QList<MAV_CMD> FirmwarePlugin::supportedMissionCommands(void)
Don Gagne's avatar
Don Gagne committed
195 196 197 198
{
    // Generic supports all commands
    return QList<MAV_CMD>();
}
199

200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224
QString FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const
{
    switch (vehicleType) {
    case MAV_TYPE_GENERIC:
        return QStringLiteral(":/json/MavCmdInfoCommon.json");
        break;
    case MAV_TYPE_FIXED_WING:
        return QStringLiteral(":/json/MavCmdInfoFixedWing.json");
        break;
    case MAV_TYPE_QUADROTOR:
        return QStringLiteral(":/json/MavCmdInfoMultiRotor.json");
        break;
    case MAV_TYPE_VTOL_QUADROTOR:
        return QStringLiteral(":/json/MavCmdInfoVTOL.json");
        break;
    case MAV_TYPE_SUBMARINE:
        return QStringLiteral(":/json/MavCmdInfoSub.json");
        break;
    case MAV_TYPE_GROUND_ROVER:
        return QStringLiteral(":/json/MavCmdInfoRover.json");
        break;
    default:
        qWarning() << "FirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType;
        return QString();
    }
225
}
226 227 228 229 230 231 232

void FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataFile, int& majorVersion, int& minorVersion)
{
    Q_UNUSED(metaDataFile);
    majorVersion = -1;
    minorVersion = -1;
}
Don Gagne's avatar
Don Gagne committed
233 234 235 236 237 238 239 240 241 242 243 244

bool FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
    return false;
}

void FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    Q_UNUSED(vehicle);
    Q_UNUSED(guidedMode);
245
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
246 247 248 249 250 251
}

void FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
252
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
253 254 255 256 257 258
}

void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
259
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
260 261 262 263 264 265
}

void FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
266
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
267 268
}

269
void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
Don Gagne's avatar
Don Gagne committed
270 271 272
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
273
    Q_UNUSED(takeoffAltRel);
274
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
275 276 277 278 279 280 281
}

void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
    Q_UNUSED(gotoCoord);
282
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
283 284 285 286 287 288 289
}

void FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
    Q_UNUSED(altitudeRel);
290
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
291
}
292

293 294 295 296 297 298 299
void FirmwarePlugin::startMission(Vehicle* vehicle)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
    qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}

300 301
const FirmwarePlugin::remapParamNameMajorVersionMap_t& FirmwarePlugin::paramNameRemapMajorVersionMap(void) const
{
Don Gagne's avatar
Don Gagne committed
302 303 304
    static const remapParamNameMajorVersionMap_t remap;

    return remap;
305 306 307 308 309 310 311
}

int FirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
{
    Q_UNUSED(majorVersionNumber);
    return 0;
}
312

313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329
QString FirmwarePlugin::vehicleImageOpaque(const Vehicle* vehicle) const
{
    Q_UNUSED(vehicle);
    return QStringLiteral("/qmlimages/vehicleArrowOpaque.svg");
}

QString FirmwarePlugin::vehicleImageOutline(const Vehicle* vehicle) const
{
    Q_UNUSED(vehicle);
    return QStringLiteral("/qmlimages/vehicleArrowOutline.svg");
}

QString FirmwarePlugin::vehicleImageCompass(const Vehicle* vehicle) const
{
    Q_UNUSED(vehicle);
    return QStringLiteral("/qmlimages/compassInstrumentArrow.svg");
}
330

331
const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle* vehicle)
332 333 334 335 336 337 338 339 340 341
{
    Q_UNUSED(vehicle);
    //-- Default list of indicators for all vehicles.
    if(_toolBarIndicatorList.size() == 0) {
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/MessageIndicator.qml")));
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSIndicator.qml")));
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/TelemetryRSSIIndicator.qml")));
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/RCRSSIIndicator.qml")));
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/BatteryIndicator.qml")));
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ModeIndicator.qml")));
342
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/VTOLModeIndicator.qml")));
343
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ArmedIndicator.qml")));
344
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml")));
345
        _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/LinkIndicator.qml")));
346 347 348
    }
    return _toolBarIndicatorList;
}
349 350 351 352 353 354 355 356

const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle)
{
    Q_UNUSED(vehicle);

    if (_cameraList.size() == 0) {
        CameraMetaData* metaData;

357 358 359 360 361 362 363 364 365 366 367 368
        metaData = new CameraMetaData(tr("Sony NEX-5R 20mm"),  //http://www.sony.co.uk/electronics/interchangeable-lens-cameras/ilce-qx1-body-kit/specifications
                                      23.2,                 //http://www.sony.com/electronics/camera-lenses/sel16f28/specifications
                                      15.4,
                                      4912,
                                      3264,
                                      20,
                                      true,
                                      false,
                                      1,
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

369 370
        metaData = new CameraMetaData(tr("Sony ILCE-QX1"),  //http://www.sony.co.uk/electronics/interchangeable-lens-cameras/ilce-qx1-body-kit/specifications
                                      23.2,                 //http://www.sony.com/electronics/camera-lenses/sel16f28/specifications
371 372 373 374
                                      15.4,
                                      5456,
                                      3632,
                                      16,
375 376
                                      true,
                                      false,
377
                                      0,
378 379 380 381 382 383 384 385 386
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(tr("Canon S100 PowerShot"),
                                      7.6,
                                      5.7,
                                      4000,
                                      3000,
                                      5.2,
387 388
                                      true,
                                      false,
389
                                      0,
390 391 392
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

393 394 395 396 397 398 399 400
        metaData = new CameraMetaData(tr("Canon G9 X PowerShot"),
                                      13.2,
                                      8.8,
                                      5488,
                                      3680,
                                      10.2,
                                      true,
                                      false,
401
                                      0,
402 403 404
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

405 406 407 408 409 410
        metaData = new CameraMetaData(tr("Canon SX260 HS PowerShot"),
                                      6.17,
                                      4.55,
                                      4000,
                                      3000,
                                      4.5,
411 412
                                      true,
                                      false,
413
                                      0,
414
                                      this);
415
        _cameraList.append(QVariant::fromValue(metaData));
416 417 418 419 420 421 422

        metaData = new CameraMetaData(tr("Canon EOS-M 22mm"),
                                      22.3,
                                      14.9,
                                      5184,
                                      3456,
                                      22,
423 424
                                      true,
                                      false,
425
                                      0,
426 427 428 429 430 431 432 433 434
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(tr("Sony a6000 16mm"),    //http://www.sony.co.uk/electronics/interchangeable-lens-cameras/ilce-6000-body-kit#product_details_default
                                      23.5,
                                      15.6,
                                      6000,
                                      4000,
                                      16,
435 436
                                      true,
                                      false,
437
                                      0,
438 439
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));
André's avatar
André committed
440 441 442 443 444 445 446 447 448

        metaData = new CameraMetaData(tr("Sony RX100 II 28mm"),
                                      13.2,
                                      8.8,
                                      5472,
                                      3648,
                                      10.4,
                                      true,
                                      false,
449
                                      0,
André's avatar
André committed
450 451
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));
DonLakeFlyer's avatar
DonLakeFlyer committed
452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475

        metaData = new CameraMetaData(tr("Ricoh GR II"),
                                      23.7,     // sensorWidth
                                      15.7,     // sendsorHeight
                                      4928,     // imageWidth
                                      3264,     // imageHeight
                                      18.3,     // focalLength
                                      true,     // landscape
                                      false,    // fixedOrientation
                                      0,        // minTriggerInterval
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(tr("RedEdge"),
                                      4.8,      // sensorWidth
                                      3.6,      // sendsorHeight
                                      1280,     // imageWidth
                                      960,      // imageHeight
                                      5.5,      // focalLength
                                      true,     // landscape
                                      false,    // fixedOrientation
                                      0,        // minTriggerInterval
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));
476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499

        metaData = new CameraMetaData(tr("Parrot Sequioa RGB"),
                                      6.17,     // sensorWidth
                                      4.63,     // sendsorHeight
                                      4608,     // imageWidth
                                      3456,     // imageHeight
                                      4.9,      // focalLength
                                      true,     // landscape
                                      false,    // fixedOrientation
                                      1,        // minTriggerInterval
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(tr("Parrot Sequioa Monochrome"),
                                      4.8,      // sensorWidth
                                      3.6,      // sendsorHeight
                                      1280,     // imageWidth
                                      960,      // imageHeight
                                      4.0,      // focalLength
                                      true,     // landscape
                                      false,    // fixedOrientation
                                      0.8,      // minTriggerInterval
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));
Jared Szechy's avatar
Jared Szechy committed
500 501 502 503 504 505 506 507 508 509 510 511

        metaData = new CameraMetaData(tr("GoPro Hero 4"),
                                      6.17,     // sensorWidth
                                      4.55,     // sendsorHeight
                                      4000,     // imageWidth
                                      3000,     // imageHeight
                                      2.98,     // focalLength
                                      true,     // landscape
                                      false,    // fixedOrientation
                                      0,        // minTriggerInterval
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));
Don Gagne's avatar
Don Gagne committed
512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535

        metaData = new CameraMetaData(tr("Sentera NDVI Single Sensor"),
                                      4.68,     // sensorWidth
                                      3.56,     // sendsorHeight
                                      1248,     // imageWidth
                                      952,      // imageHeight
                                      4.14,     // focalLength
                                      true,     // landscape
                                      false,    // fixedOrientation
                                      0,        // minTriggerInterval
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(tr("Sentera Double 4K Sensor"),
                                      6.2,      // sensorWidth
                                      4.65,     // sendsorHeight
                                      4000,     // imageWidth
                                      3000,     // imageHeight
                                      5.4,      // focalLength
                                      true,     // landscape
                                      false,    // fixedOrientation
                                      0,        // minTriggerInterval
                                      this);
        _cameraList.append(QVariant::fromValue(metaData));
536 537 538 539
    }

    return _cameraList;
}
540

541 542 543
QMap<QString, FactGroup*>* FirmwarePlugin::factGroups(void) {
    // Generic plugin has no FactGroups
    return NULL;
544 545
}

546 547 548 549
bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
    return vehicle->multiRotor() ? false : true;
}
550

551
bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle)
552
{
553 554 555 556 557 558 559 560
    if (vehicle->armed()) {
        return true;
    }

    bool armedChanged = false;

    // We try arming 3 times
    for (int retries=0; retries<3; retries++) {
561
        vehicle->setArmed(true);
562 563 564 565 566 567 568 569 570 571 572 573 574

        // Wait for vehicle to return armed state for 3 seconds
        for (int i=0; i<30; i++) {
            if (vehicle->armed()) {
                armedChanged = true;
                break;
            }
            QGC::SLEEP::msleep(100);
            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
        }
        if (armedChanged) {
            break;
        }
575 576
    }

577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592
    return armedChanged;
}

bool FirmwarePlugin::_setFlightModeAndValidate(Vehicle* vehicle, const QString& flightMode)
{
    if (vehicle->flightMode() == flightMode) {
        return true;
    }

    bool flightModeChanged = false;

    // We try 3 times
    for (int retries=0; retries<3; retries++) {
        vehicle->setFlightMode(flightMode);

        // Wait for vehicle to return flight mode
593
        for (int i=0; i<13; i++) {
594 595 596 597 598 599 600 601
            if (vehicle->flightMode() == flightMode) {
                flightModeChanged = true;
                break;
            }
            QGC::SLEEP::msleep(100);
            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
        }
        if (flightModeChanged) {
602 603 604
            break;
        }
    }
605 606

    return flightModeChanged;
607
}
608

609

Don Gagne's avatar
Don Gagne committed
610
void FirmwarePlugin::batteryConsumptionData(Vehicle* vehicle, int& mAhBattery, double& hoverAmps, double& cruiseAmps) const
611 612 613 614 615 616
{
    Q_UNUSED(vehicle);
    mAhBattery = 0;
    hoverAmps = 0;
    cruiseAmps = 0;
}
617 618 619 620 621 622

QString FirmwarePlugin::autoDisarmParameter(Vehicle* vehicle)
{
    Q_UNUSED(vehicle);
    return QString();
}
623

624 625 626 627 628 629 630 631
bool FirmwarePlugin::hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported)
{
    Q_UNUSED(vehicle);
    rollSupported = false;
    pitchSupported = false;
    yawSupported = false;
    return false;
}
632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647

bool FirmwarePlugin::isVtol(const Vehicle* vehicle) const
{
    switch (vehicle->vehicleType()) {
    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:
        return true;
    default:
        return false;
    }
}
648

649
QGCCameraManager* FirmwarePlugin::createCameraManager(Vehicle* vehicle)
650 651 652 653 654 655 656 657 658 659 660 661 662 663
{
    Q_UNUSED(vehicle);
    return NULL;
}

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

664 665 666 667 668
uint32_t FirmwarePlugin::highLatencyCustomModeTo32Bits(uint16_t hlCustomMode)
{
    // Standard implementation assumes no special handling. Upper part of 32 bit value is not used.
    return hlCustomMode;
}
669

670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718
void FirmwarePlugin::checkIfIsLatestStable(Vehicle* vehicle)
{
    // This is required as mocklink uses a hardcoded firmware version
    if (qgcApp()->runningUnitTests()) {
        qCDebug(FirmwarePluginLog) << "Skipping version check";
        return;
    }
    QString versionFile = _getLatestVersionFileUrl(vehicle);
    qCDebug(FirmwarePluginLog) << "Downloading" << versionFile;
    QGCFileDownload* downloader = new QGCFileDownload(this);
    connect(
        downloader,
        &QGCFileDownload::downloadFinished,
        this,
        [vehicle, this](QString remoteFile, QString localFile) {
            _versionFileDownloadFinished(remoteFile, localFile, vehicle);
            sender()->deleteLater();
        });
    downloader->download(versionFile);
}

void FirmwarePlugin::_versionFileDownloadFinished(QString& remoteFile, QString& localFile, Vehicle* vehicle)
{
    qCDebug(FirmwarePluginLog) << "Download complete" << remoteFile << localFile;
    // Now read the version file and pull out the version string
    QFile versionFile(localFile);
    if (!versionFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qCWarning(FirmwarePluginLog) << "Error opening downloaded version file.";
        return;
    }

    QTextStream stream(&versionFile);
    QString versionFileContents = stream.readAll();
    QString version;
    QRegularExpressionMatch match = QRegularExpression(_versionRegex()).match(versionFileContents);

    qCDebug(FirmwarePluginLog) << "Looking for version number...";

    if (match.hasMatch()) {
        version = match.captured(1);
    } else {
        qCWarning(FirmwarePluginLog) << "Unable to parse version info from file" << remoteFile;
        return;
    }

    qCDebug(FirmwarePluginLog) << "Latest stable version = "  << version;

    int currType = vehicle->firmwareVersionType();

719
    // Check if lower version than stable or same version but different type
720
    if (currType == FIRMWARE_VERSION_TYPE_OFFICIAL && vehicle->versionCompare(version) < 0) {
721 722 723 724 725 726 727
        const static QString currentVersion = QString("%1.%2.%3").arg(vehicle->firmwareMajorVersion())
                                                                 .arg(vehicle->firmwareMinorVersion())
                                                                 .arg(vehicle->firmwarePatchVersion());
        const static QString message = tr("Vehicle is not running latest stable firmware! Running %2-%1, latest stable is %3.");
        qgcApp()->showMessage(message.arg(vehicle->firmwareVersionTypeString(), currentVersion, version));
    }
}
728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759

int FirmwarePlugin::versionCompare(Vehicle* vehicle, int major, int minor, int patch)
{
    int currMajor = vehicle->firmwareMajorVersion();
    int currMinor = vehicle->firmwareMinorVersion();
    int currPatch = vehicle->firmwarePatchVersion();

    if (currMajor == major && currMinor == minor && currPatch == patch) {
        return 0;
    }

    if (currMajor > major
       || (currMajor == major && currMinor > minor)
       || (currMajor == major && currMinor == minor && currPatch > patch))
    {
        return 1;
    }
    return -1;
}

int FirmwarePlugin::versionCompare(Vehicle* vehicle, QString& compare)
{
    QStringList versionNumbers = compare.split(".");
    if(versionNumbers.size() != 3) {
        qCWarning(FirmwarePluginLog) << "Error parsing version number: wrong format";
        return -1;
    }
    int major = versionNumbers[0].toInt();
    int minor = versionNumbers[1].toInt();
    int patch = versionNumbers[2].toInt();
    return versionCompare(vehicle, major, minor, patch);
}