FirmwarePlugin.cc 44.3 KB
Newer Older
1 2
/****************************************************************************
 *
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.
 *
 ****************************************************************************/
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"
17
#include "QGCCameraManager.h"
Don Gagne's avatar
Don Gagne committed
18

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

22 23
QGC_LOGGING_CATEGORY(FirmwarePluginLog, "FirmwarePluginLog")

24
static FirmwarePluginFactoryRegister* _instance = nullptr;
25

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

28 29
QVariantList FirmwarePlugin::_cameraList;

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

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

37
QList<QGCMAVLink::VehicleClass_t> FirmwarePluginFactory::supportedVehicleClasses(void) const
38
{
39
    return QGCMAVLink::allVehicleClasses();
40 41
}

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

    return _instance;
}

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

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

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

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

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

Don Gagne's avatar
Don Gagne committed
74 75 76 77 78
    struct Bit2Name {
        uint8_t     baseModeBit;
        const char* name;
    };
    static const struct Bit2Name rgBit2Name[] = {
79 80 81 82 83 84
    { 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" },
};
85

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

Don Gagne's avatar
Don Gagne committed
88 89 90 91 92 93 94 95 96 97 98 99 100 101
    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;
            }
        }
    }
102

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

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

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

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

117 118 119 120 121
int FirmwarePlugin::defaultJoystickTXMode(void)
{
    return 2;
}

122 123 124 125 126 127
bool FirmwarePlugin::supportsThrottleModeCenterZero(void)
{
    // By default, this is supported
    return true;
}

128
bool FirmwarePlugin::supportsNegativeThrust(Vehicle* /*vehicle*/)
129 130 131 132 133
{
    // By default, this is not supported
    return false;
}

134 135 136 137 138
bool FirmwarePlugin::supportsRadio(void)
{
    return true;
}

139 140 141 142 143
bool FirmwarePlugin::supportsMotorInterference(void)
{
    return true;
}

144 145 146 147 148
bool FirmwarePlugin::supportsJSButton(void)
{
    return false;
}

149
bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
150 151
{
    Q_UNUSED(vehicle);
152
    Q_UNUSED(message);
Don Gagne's avatar
Don Gagne committed
153
    // Generic plugin does no message adjustment
154
    return true;
Don Gagne's avatar
Don Gagne committed
155 156
}

157
void FirmwarePlugin::adjustOutgoingMavlinkMessageThreadSafe(Vehicle* /*vehicle*/, LinkInterface* /*outgoingLink*/, mavlink_message_t* /*message*/)
Don Gagne's avatar
Don Gagne committed
158 159 160
{
    // Generic plugin does no message adjustment
}
161

162
void FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
163 164
{
    Q_UNUSED(vehicle);
165

166 167
    // Generic Flight Stack is by definition "generic", so no extra work
}
168

169
bool FirmwarePlugin::sendHomePositionToVehicle(void)
170 171 172 173 174 175
{
    // 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;
}
176

177
QList<MAV_CMD> FirmwarePlugin::supportedMissionCommands(QGCMAVLink::VehicleClass_t /* vehicleClass */)
Don Gagne's avatar
Don Gagne committed
178 179 180 181
{
    // Generic supports all commands
    return QList<MAV_CMD>();
}
182

183
QString FirmwarePlugin::missionCommandOverrides(QGCMAVLink::VehicleClass_t vehicleClass) const
184
{
185 186
    switch (vehicleClass) {
    case QGCMAVLink::VehicleClassGeneric:
187
        return QStringLiteral(":/json/MavCmdInfoCommon.json");
188
    case QGCMAVLink::VehicleClassFixedWing:
189
        return QStringLiteral(":/json/MavCmdInfoFixedWing.json");
190
    case QGCMAVLink::VehicleClassMultiRotor:
191
        return QStringLiteral(":/json/MavCmdInfoMultiRotor.json");
192
    case QGCMAVLink::VehicleClassVTOL:
193
        return QStringLiteral(":/json/MavCmdInfoVTOL.json");
194
    case QGCMAVLink::VehicleClassSub:
195
        return QStringLiteral(":/json/MavCmdInfoSub.json");
196
    case QGCMAVLink::VehicleClassRoverBoat:
197 198
        return QStringLiteral(":/json/MavCmdInfoRover.json");
    default:
199
        qWarning() << "FirmwarePlugin::missionCommandOverrides called with bad VehicleClass_t:" << vehicleClass;
200 201
        return QString();
    }
202
}
203

204
void FirmwarePlugin::_getParameterMetaDataVersionInfo(const QString& metaDataFile, int& majorVersion, int& minorVersion)
205 206 207 208 209
{
    Q_UNUSED(metaDataFile);
    majorVersion = -1;
    minorVersion = -1;
}
Don Gagne's avatar
Don Gagne committed
210 211 212 213 214 215 216 217 218 219 220 221

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);
222
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
223 224 225 226 227 228
}

void FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
229
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
230 231
}

232
void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle, bool smartRTL)
Don Gagne's avatar
Don Gagne committed
233 234 235
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
236 237
    Q_UNUSED(smartRTL);
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
238 239 240 241 242 243
}

void FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
244
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
245 246
}

247
void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
Don Gagne's avatar
Don Gagne committed
248 249 250
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
251
    Q_UNUSED(takeoffAltRel);
252
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
253 254 255 256 257 258 259
}

void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
    // Not supported by generic vehicle
    Q_UNUSED(vehicle);
    Q_UNUSED(gotoCoord);
260
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
261 262
}

263
void FirmwarePlugin::guidedModeChangeAltitude(Vehicle*, double)
Don Gagne's avatar
Don Gagne committed
264 265
{
    // Not supported by generic vehicle
266
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
267
}
268

269
void FirmwarePlugin::startMission(Vehicle*)
270 271
{
    // Not supported by generic vehicle
272
    qgcApp()->showAppMessage(guided_mode_not_supported_by_vehicle);
273 274
}

275 276
const FirmwarePlugin::remapParamNameMajorVersionMap_t& FirmwarePlugin::paramNameRemapMajorVersionMap(void) const
{
Don Gagne's avatar
Don Gagne committed
277 278 279
    static const remapParamNameMajorVersionMap_t remap;

    return remap;
280 281
}

282
int FirmwarePlugin::remapParamNameHigestMinorVersionNumber(int) const
283 284 285
{
    return 0;
}
286

287
QString FirmwarePlugin::vehicleImageOpaque(const Vehicle*) const
288 289 290 291
{
    return QStringLiteral("/qmlimages/vehicleArrowOpaque.svg");
}

292
QString FirmwarePlugin::vehicleImageOutline(const Vehicle*) const
293 294 295 296
{
    return QStringLiteral("/qmlimages/vehicleArrowOutline.svg");
}

297
QString FirmwarePlugin::vehicleImageCompass(const Vehicle*) const
298 299 300
{
    return QStringLiteral("/qmlimages/compassInstrumentArrow.svg");
}
301

302
const QVariantList& FirmwarePlugin::toolIndicators(const Vehicle*)
303 304
{
    //-- Default list of indicators for all vehicles.
305 306 307 308 309 310 311 312
    if(_toolIndicatorList.size() == 0) {
        _toolIndicatorList = QVariantList({
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/MessageIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/TelemetryRSSIIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/RCRSSIIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/BatteryIndicator.qml")),
        });
313
    }
314
    return _toolIndicatorList;
315
}
316

317
const QVariantList& FirmwarePlugin::modeIndicators(const Vehicle*)
318
{
319 320 321 322 323 324 325 326 327 328
    //-- Default list of indicators for all vehicles.
    if(_modeIndicatorList.size() == 0) {
        _modeIndicatorList = QVariantList({
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ROIIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/MultiVehicleSelector.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/LinkIndicator.qml")),
        });
    }
    return _modeIndicatorList;
}
329

330 331
const QVariantList& FirmwarePlugin::cameraList(const Vehicle*)
{
332 333 334
    if (_cameraList.size() == 0) {
        CameraMetaData* metaData;

Gus Grubba's avatar
Gus Grubba committed
335
        metaData = new CameraMetaData(
336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 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 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432
                    // Canon S100 @ 5.2mm f/2
                    "Canon S100 PowerShot",     // canonical name saved in plan file
                    tr("Canon"),                // brand
                    tr("S100 PowerShot"),       // model
                    7.6,                        // sensorWidth
                    5.7,                        // sensorHeight
                    4000,                       // imageWidth
                    3000,                       // imageHeight
                    5.2,                        // focalLength
                    true,                       // true: landscape orientation
                    false,                      // true: camera is fixed orientation
                    0,                          // minimum trigger interval
                    tr("Canon S100 PowerShot"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                      // parent
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
                    //tr("Canon EOS-M 22mm f/2"),
                    "Canon EOS-M 22mm",
                    tr("Canon"),
                    tr("EOS-M 22mm"),
                    22.3,                   // sensorWidth
                    14.9,                   // sensorHeight
                    5184,                   // imageWidth
                    3456,                   // imageHeight
                    22,                     // focalLength
                    true,                   // true: landscape orientation
                    false,                  // true: camera is fixed orientation
                    0,                      // minimum trigger interval
                    tr("Canon EOS-M 22mm"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                  // parent
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
                    // Canon G9X @ 10.2mm f/2
                    "Canon G9 X PowerShot",
                    tr("Canon"),
                    tr("G9 X PowerShot"),
                    13.2,                       // sensorWidth
                    8.8,                        // sensorHeight
                    5488,                       // imageWidth
                    3680,                       // imageHeight
                    10.2,                       // focalLength
                    true,                       // true: landscape orientation
                    false,                      // true: camera is fixed orientation
                    0,                          // minimum trigger interval
                    tr("Canon G9 X PowerShot"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                      // parent
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
                    // Canon SX260 HS @ 4.5mm f/3.5
                    "Canon SX260 HS PowerShot",
                    tr("Canon"),
                    tr("SX260 HS PowerShot"),
                    6.17,                           // sensorWidth
                    4.55,                           // sensorHeight
                    4000,                           // imageWidth
                    3000,                           // imageHeight
                    4.5,                            // focalLength
                    true,                           // true: landscape orientation
                    false,                          // true: camera is fixed orientation
                    0,                              // minimum trigger interval
                    tr("Canon SX260 HS PowerShot"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                          // parent
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
                    "GoPro Hero 4",
                    tr("GoPro"),
                    tr("Hero 4"),
                    6.17,               // sensorWidth
                    4.55,               // sendsorHeight
                    4000,               // imageWidth
                    3000,               // imageHeight
                    2.98,               // focalLength
                    true,               // landscape
                    false,              // fixedOrientation
                    0,                  // minTriggerInterval
                    tr("GoPro Hero 4"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
                    "Parrot Sequioa RGB",
                    tr("Parrot"),
                    tr("Sequioa RGB"),
                    6.17,                       // sensorWidth
                    4.63,                       // sendsorHeight
                    4608,                       // imageWidth
                    3456,                       // imageHeight
                    4.9,                        // focalLength
                    true,                       // landscape
                    false,                      // fixedOrientation
                    1,                          // minTriggerInterval
                    tr("Parrot Sequioa RGB"),   // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);
433 434
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
435
        metaData = new CameraMetaData(
436 437 438 439 440 441 442 443 444 445 446 447 448
                    "Parrot Sequioa Monochrome",
                    tr("Parrot"),
                    tr("Sequioa Monochrome"),
                    4.8,                                // sensorWidth
                    3.6,                                // sendsorHeight
                    1280,                               // imageWidth
                    960,                                // imageHeight
                    4.0,                                // focalLength
                    true,                               // landscape
                    false,                              // fixedOrientation
                    0.8,                                // minTriggerInterval
                    tr("Parrot Sequioa Monochrome"),    // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);
449 450
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
451
        metaData = new CameraMetaData(
452 453 454 455 456 457 458 459 460 461 462 463 464
                    "RedEdge",
                    tr("RedEdge"),
                    tr("RedEdge"),
                    4.8,            // sensorWidth
                    3.6,            // sendsorHeight
                    1280,           // imageWidth
                    960,            // imageHeight
                    5.5,            // focalLength
                    true,           // landscape
                    false,          // fixedOrientation
                    0,              // minTriggerInterval
                    tr("RedEdge"),  // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);
465 466
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
467
        metaData = new CameraMetaData(
468 469 470 471 472 473 474 475 476 477 478 479 480 481
                    // Ricoh GR II 18.3mm f/2.8
                    "Ricoh GR II",
                    tr("Ricoh"),
                    tr("GR II"),
                    23.7,               // sensorWidth
                    15.7,               // sendsorHeight
                    4928,               // imageWidth
                    3264,               // imageHeight
                    18.3,               // focalLength
                    true,               // landscape
                    false,              // fixedOrientation
                    0,                  // minTriggerInterval
                    tr("Ricoh GR II"),  // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);
482 483
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
484
        metaData = new CameraMetaData(
485 486 487 488 489 490 491 492 493 494 495 496 497
                    "Sentera Double 4K Sensor",
                    tr("Sentera"),
                    tr("Double 4K Sensor"),
                    6.2,                // sensorWidth
                    4.65,               // sendsorHeight
                    4000,               // imageWidth
                    3000,               // imageHeight
                    5.4,                // focalLength
                    true,               // landscape
                    false,              // fixedOrientation
                    0,                  // minTriggerInterval
                    tr("Sentera Double 4K Sensor"),// SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);
498
        _cameraList.append(QVariant::fromValue(metaData));
499

Gus Grubba's avatar
Gus Grubba committed
500
        metaData = new CameraMetaData(
501 502 503 504 505 506 507 508 509 510 511 512 513
                    "Sentera NDVI Single Sensor",
                    tr("Sentera"),
                    tr("NDVI Single Sensor"),
                    4.68,               // sensorWidth
                    3.56,               // sendsorHeight
                    1248,               // imageWidth
                    952,                // imageHeight
                    4.14,               // focalLength
                    true,               // landscape
                    false,              // fixedOrientation
                    0,                  // minTriggerInterval
                    tr("Sentera NDVI Single Sensor"),// SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);
514 515
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
516
        metaData = new CameraMetaData(
517 518 519 520 521 522 523 524 525 526 527 528 529 530 531
                    //-- http://www.sony.co.uk/electronics/interchangeable-lens-cameras/ilce-6000-body-kit#product_details_default
                    // Sony a6000 Sony 16mm f/2.8"
                    "Sony a6000 16mm",
                    tr("Sony"),
                    tr("a6000 16mm"),
                    23.5,                   // sensorWidth
                    15.6,                   // sensorHeight
                    6000,                   // imageWidth
                    4000,                   // imageHeight
                    16,                     // focalLength
                    true,                   // true: landscape orientation
                    false,                  // true: camera is fixed orientation
                    2.0,                    // minimum trigger interval
                    tr("Sony a6000 16mm"),  // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                  // parent
532
        _cameraList.append(QVariant::fromValue(metaData));
André's avatar
André committed
533

Gus Grubba's avatar
Gus Grubba committed
534
        metaData = new CameraMetaData(
535 536 537 538 539 540 541 542 543 544 545 546 547
                    "Sony a6000 35mm",
                    tr("Sony"),
                    tr("a6000 35mm"),
                    23.5,               // sensorWidth
                    15.6,               // sensorHeight
                    6000,               // imageWidth
                    4000,               // imageHeight
                    35,                 // focalLength
                    true,               // true: landscape orientation
                    false,              // true: camera is fixed orientation
                    2.0,                // minimum trigger interval
                    "",
                    this);              // parent
André's avatar
André committed
548
        _cameraList.append(QVariant::fromValue(metaData));
DonLakeFlyer's avatar
DonLakeFlyer committed
549

Gus Grubba's avatar
Gus Grubba committed
550
        metaData = new CameraMetaData(
551 552 553 554 555 556 557 558 559 560 561 562 563
                    "Sony a6300 Zeiss 21mm f/2.8",
                    tr("Sony"),
                    tr("a6300 Zeiss 21mm f/2.8"),
                    23.5,               // sensorWidth
                    15.6,               // sensorHeight
                    6000,               // imageWidth
                    4000,               // imageHeight
                    21,                 // focalLength
                    true,               // true: landscape orientation
                    false,              // true: camera is fixed orientation
                    2.0,                // minimum trigger interval
                    tr("Sony a6300 Zeiss 21mm f/2.8"),// SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
DonLakeFlyer's avatar
DonLakeFlyer committed
564 565
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
566
        metaData = new CameraMetaData(
567 568 569 570 571 572 573 574 575 576 577 578 579
                    "Sony a6300 Sony 28mm f/2.0",
                    tr("Sony"),
                    tr("a6300 Sony 28mm f/2.0"),
                    23.5,                               // sensorWidth
                    15.6,                               // sensorHeight
                    6000,                               // imageWidth
                    4000,                               // imageHeight
                    28,                                 // focalLength
                    true,                               // true: landscape orientation
                    false,              // true: camera is fixed orientation
                    2.0,                                // minimum trigger interval
                    tr("Sony a6300 Sony 28mm f/2.0"),   // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                              // parent
DonLakeFlyer's avatar
DonLakeFlyer committed
580
        _cameraList.append(QVariant::fromValue(metaData));
581

Gus Grubba's avatar
Gus Grubba committed
582
        metaData = new CameraMetaData(
583 584 585 586 587 588 589 590 591 592 593 594 595
                    "Sony a7R II Zeiss 21mm f/2.8",
                    tr("Sony"),
                    tr("a7R II Zeiss 21mm f/2.8"),
                    35.814,                             // sensorWidth
                    23.876,                             // sensorHeight
                    7952,                               // imageWidth
                    5304,                               // imageHeight
                    21,                                 // focalLength
                    true,                               // true: landscape orientation
                    true,                               // true: camera is fixed orientation
                    2.0,                                // minimum trigger interval
                    tr("Sony a7R II Zeiss 21mm f/2.8"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                              // parent
596 597
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
598
        metaData = new CameraMetaData(
599 600 601 602 603 604 605 606 607 608 609 610 611
                    "Sony a7R II Sony 28mm f/2.0",
                    tr("Sony"),
                    tr("a7R II Sony 28mm f/2.0"),
                    35.814,             // sensorWidth
                    23.876,             // sensorHeight
                    7952,               // imageWidth
                    5304,               // imageHeight
                    28,                 // focalLength
                    true,               // true: landscape orientation
                    true,               // true: camera is fixed orientation
                    2.0,                // minimum trigger interval
                    tr("Sony a7R II Sony 28mm f/2.0"),// SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
612
        _cameraList.append(QVariant::fromValue(metaData));
Jared Szechy's avatar
Jared Szechy committed
613

Gus Grubba's avatar
Gus Grubba committed
614
        metaData = new CameraMetaData(
615 616 617 618 619 620 621 622 623 624 625 626 627
                    "Sony a7r III 35mm",
                    tr("Sony"),
                    tr("a7r III 35mm"),
                    35.9,               // sensorWidth
                    24.0,               // sensorHeight
                    7952,               // imageWidth
                    5304,               // imageHeight
                    35,                 // focalLength
                    true,               // true: landscape orientation
                    false,              // true: camera is fixed orientation
                    2.0,                // minimum trigger interval
                    "",
                    this);              // parent
Jared Szechy's avatar
Jared Szechy committed
628
        _cameraList.append(QVariant::fromValue(metaData));
Don Gagne's avatar
Don Gagne committed
629

Gus Grubba's avatar
Gus Grubba committed
630
        metaData = new CameraMetaData(
631 632 633 634 635 636 637 638 639 640 641 642 643
                    "Sony a7r IV 35mm",
                    tr("Sony"),
                    tr("a7r IV 35mm"),
                    35.7,               // sensorWidth
                    23.8,               // sensorHeight
                    9504,               // imageWidth
                    6336,               // imageHeight
                    35,                 // focalLength
                    true,               // true: landscape orientation
                    false,               // true: camera is fixed orientation
                    2.0,                // minimum trigger interval
                    "",
                    this);              // parent
Don Gagne's avatar
Don Gagne committed
644 645
        _cameraList.append(QVariant::fromValue(metaData));

Gus Grubba's avatar
Gus Grubba committed
646
        metaData = new CameraMetaData(
647 648 649 650 651 652 653 654 655 656 657 658 659
                    "Sony DSC-QX30U @ 4.3mm f/3.5",
                    tr("Sony"),
                    tr("DSC-QX30U @ 4.3mm f/3.5"),
                    7.82,                               // sensorWidth
                    5.865,                              // sensorHeight
                    5184,                               // imageWidth
                    3888,                               // imageHeight
                    4.3,                                // focalLength
                    true,                               // true: landscape orientation
                    false,                              // true: camera is fixed orientation
                    2.0,                                // minimum trigger interval
                    tr("Sony DSC-QX30U @ 4.3mm f/3.5"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                              // parent
Don Gagne's avatar
Don Gagne committed
660
        _cameraList.append(QVariant::fromValue(metaData));
Gus Grubba's avatar
Gus Grubba committed
661 662

        metaData = new CameraMetaData(
663 664 665 666 667 668 669 670 671 672 673 674 675
                    "Sony DSC-RX0",
                    tr("Sony"),
                    tr("DSC-RX0"),
                    13.2,               // sensorWidth
                    8.8,                // sensorHeight
                    4800,               // imageWidth
                    3200,               // imageHeight
                    7.7,                // focalLength
                    true,               // true: landscape orientation
                    false,              // true: camera is fixed orientation
                    0,                  // minimum trigger interval
                    tr("Sony DSC-RX0"),// SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
Gus Grubba's avatar
Gus Grubba committed
676 677 678
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
679 680 681 682 683 684 685 686 687 688 689 690 691
                    "Sony DSC-RX1R II 35mm",
                    tr("Sony"),
                    tr("DSC-RX1R II 35mm"),
                    35.9,             // sensorWidth
                    24.0,             // sensorHeight
                    7952,               // imageWidth
                    5304,               // imageHeight
                    35,                 // focalLength
                    true,               // true: landscape orientation
                    false,              // true: camera is fixed orientation
                    2.0,                // minimum trigger interval
                    "",
                    this);              // parent
Gus Grubba's avatar
Gus Grubba committed
692 693 694
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710
                    //-- http://www.sony.co.uk/electronics/interchangeable-lens-cameras/ilce-qx1-body-kit/specifications
                    //-- http://www.sony.com/electronics/camera-lenses/sel16f28/specifications
                    //tr("Sony ILCE-QX1 Sony 16mm f/2.8"),
                    "Sony ILCE-QX1",
                    tr("Sony"),
                    tr("ILCE-QX1"),
                    23.2,                   // sensorWidth
                    15.4,                   // sensorHeight
                    5456,                   // imageWidth
                    3632,                   // imageHeight
                    16,                     // focalLength
                    true,                   // true: landscape orientation
                    false,                  // true: camera is fixed orientation
                    0,                      // minimum trigger interval
                    tr("Sony ILCE-QX1"),    // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                  // parent
Gus Grubba's avatar
Gus Grubba committed
711 712 713
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
714 715 716 717 718 719 720 721 722 723 724 725 726 727 728
                    //-- http://www.sony.co.uk/electronics/interchangeable-lens-cameras/ilce-qx1-body-kit/specifications
                    // Sony NEX-5R Sony 20mm f/2.8"
                    "Sony NEX-5R 20mm",
                    tr("Sony"),
                    tr("NEX-5R 20mm"),
                    23.2,                   // sensorWidth
                    15.4,                   // sensorHeight
                    4912,                   // imageWidth
                    3264,                   // imageHeight
                    20,                     // focalLength
                    true,                   // true: landscape orientation
                    false,                  // true: camera is fixed orientation
                    1,                      // minimum trigger interval
                    tr("Sony NEX-5R 20mm"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);                  // parent
Gus Grubba's avatar
Gus Grubba committed
729 730 731
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
732 733 734 735 736 737 738 739 740 741 742 743 744 745
                    // Sony RX100 II @ 10.4mm f/1.8
                    "Sony RX100 II 28mm",
                    tr("Sony"),
                    tr("RX100 II 28mm"),
                    13.2,                // sensorWidth
                    8.8,                 // sensorHeight
                    5472,                // imageWidth
                    3648,                // imageHeight
                    10.4,                // focalLength
                    true,                // true: landscape orientation
                    false,               // true: camera is fixed orientation
                    0,                   // minimum trigger interval
                    tr("Sony RX100 II 28mm"),// SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);               // parent
Gus Grubba's avatar
Gus Grubba committed
746 747 748
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
749 750 751 752 753 754 755 756 757 758 759 760 761
                    "Yuneec CGOET",
                    tr("Yuneec"),
                    tr("CGOET"),
                    5.6405,             // sensorWidth
                    3.1813,             // sensorHeight
                    1920,               // imageWidth
                    1080,               // imageHeight
                    3.5,                // focalLength
                    true,               // true: landscape orientation
                    true,               // true: camera is fixed orientation
                    1.3,                // minimum trigger interval
                    tr("Yuneec CGOET"), // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
Gus Grubba's avatar
Gus Grubba committed
762 763 764
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
765 766 767 768 769 770 771 772 773 774 775 776 777
                    "Yuneec E10T",
                    tr("Yuneec"),
                    tr("E10T"),
                    5.6405,             // sensorWidth
                    3.1813,             // sensorHeight
                    1920,               // imageWidth
                    1080,               // imageHeight
                    23,                 // focalLength
                    true,               // true: landscape orientation
                    true,               // true: camera is fixed orientation
                    1.3,                // minimum trigger interval
                    tr("Yuneec E10T"),  // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
Gus Grubba's avatar
Gus Grubba committed
778 779 780
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
781 782 783 784 785 786 787 788 789 790 791 792 793
                    "Yuneec E50",
                    tr("Yuneec"),
                    tr("E50"),
                    6.2372,             // sensorWidth
                    4.7058,             // sensorHeight
                    4000,               // imageWidth
                    3000,               // imageHeight
                    7.2,                // focalLength
                    true,               // true: landscape orientation
                    true,               // true: camera is fixed orientation
                    1.3,                // minimum trigger interval
                    tr("Yuneec E50"),   // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
Gus Grubba's avatar
Gus Grubba committed
794 795 796
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825
                    "Yuneec E90",
                    tr("Yuneec"),
                    tr("E90"),
                    13.3056,            // sensorWidth
                    8.656,              // sensorHeight
                    5472,               // imageWidth
                    3648,               // imageHeight
                    8.29,               // focalLength
                    true,               // true: landscape orientation
                    true,               // true: camera is fixed orientation
                    1.3,                // minimum trigger interval
                    tr("Yuneec E90"),   // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
        _cameraList.append(QVariant::fromValue(metaData));

        metaData = new CameraMetaData(
                    "Flir Duo R",
                    tr("Flir"),
                    tr("Duo R"),
                    160,                // sensorWidth
                    120,                // sensorHeight
                    1920,               // imageWidth
                    1080,               // imageHeight
                    1.9,                // focalLength
                    true,               // true: landscape orientation
                    false,              // true: camera is fixed orientation
                    0,                  // minimum trigger interval
                    tr("Flir Duo R"),   // SHOULD BE BLANK FOR NEWLY ADDED CAMERAS. Deprecated translation from older builds.
                    this);              // parent
Gus Grubba's avatar
Gus Grubba committed
826 827
        _cameraList.append(QVariant::fromValue(metaData));

828 829 830 831
    }

    return _cameraList;
}
832

833 834
QMap<QString, FactGroup*>* FirmwarePlugin::factGroups(void) {
    // Generic plugin has no FactGroups
Gus Grubba's avatar
Gus Grubba committed
835
    return nullptr;
836 837
}

838
bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle)
839
{
840 841 842 843 844 845 846 847
    if (vehicle->armed()) {
        return true;
    }

    bool armedChanged = false;

    // We try arming 3 times
    for (int retries=0; retries<3; retries++) {
848
        vehicle->setArmed(true);
849 850 851 852 853 854 855 856 857 858 859 860 861

        // 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;
        }
862 863
    }

864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879
    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
880
        for (int i=0; i<13; i++) {
881 882 883 884 885 886 887 888
            if (vehicle->flightMode() == flightMode) {
                flightModeChanged = true;
                break;
            }
            QGC::SLEEP::msleep(100);
            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
        }
        if (flightModeChanged) {
889 890 891
            break;
        }
    }
892 893

    return flightModeChanged;
894
}
895

896

Don Gagne's avatar
Don Gagne committed
897
void FirmwarePlugin::batteryConsumptionData(Vehicle* vehicle, int& mAhBattery, double& hoverAmps, double& cruiseAmps) const
898 899
{
    Q_UNUSED(vehicle);
900 901 902
    mAhBattery  = 0;
    hoverAmps   = 0;
    cruiseAmps  = 0;
903
}
904 905 906 907 908 909

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

911 912 913 914 915 916 917 918
bool FirmwarePlugin::hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported)
{
    Q_UNUSED(vehicle);
    rollSupported = false;
    pitchSupported = false;
    yawSupported = false;
    return false;
}
919

920
QGCCameraManager* FirmwarePlugin::createCameraManager(Vehicle* vehicle)
921
{
922
    return new QGCCameraManager(vehicle);
923 924 925 926
}

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

930 931 932 933 934
uint32_t FirmwarePlugin::highLatencyCustomModeTo32Bits(uint16_t hlCustomMode)
{
    // Standard implementation assumes no special handling. Upper part of 32 bit value is not used.
    return hlCustomMode;
}
935

936 937 938 939 940 941 942 943 944 945 946 947
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,
948
        &QGCFileDownload::downloadComplete,
949
        this,
950 951 952 953 954 955
        [vehicle, this](QString remoteFile, QString localFile, QString errorMsg) {
            if (errorMsg.isEmpty()) {
                _versionFileDownloadFinished(remoteFile, localFile, vehicle);
            } else {
                qCDebug(FirmwarePluginLog) << "Failed to download the latest fw version file. Error: " << errorMsg;
            }
956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988
            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();

989
    // Check if lower version than stable or same version but different type
990
    if (currType == FIRMWARE_VERSION_TYPE_OFFICIAL && vehicle->versionCompare(version) < 0) {
991 992 993 994
        QString currentVersionNumber = QString("%1.%2.%3").arg(vehicle->firmwareMajorVersion())
                .arg(vehicle->firmwareMinorVersion())
                .arg(vehicle->firmwarePatchVersion());
        qgcApp()->showAppMessage(tr("Vehicle is not running latest stable firmware! Running %1, latest stable is %2.").arg(currentVersionNumber, version));
995 996
    }
}
997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028

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);
}
Don Gagne's avatar
Don Gagne committed
1029 1030 1031 1032 1033

QString FirmwarePlugin::gotoFlightMode(void) const
{
    return QString();
}
1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058

void FirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities)
{
    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();

    mavlink_follow_target_t follow_target = {};

    follow_target.timestamp =           qgcApp()->msecsSinceBoot();
    follow_target.est_capabilities =    estimationCapabilities;
    follow_target.position_cov[0] =     static_cast<float>(motionReport.pos_std_dev[0]);
    follow_target.position_cov[2] =     static_cast<float>(motionReport.pos_std_dev[2]);
    follow_target.alt =                 static_cast<float>(motionReport.altMetersAMSL);
    follow_target.lat =                 motionReport.lat_int;
    follow_target.lon =                 motionReport.lon_int;
    follow_target.vel[0] =              static_cast<float>(motionReport.vxMetersPerSec);
    follow_target.vel[1] =              static_cast<float>(motionReport.vyMetersPerSec);

    mavlink_message_t message;
    mavlink_msg_follow_target_encode_chan(static_cast<uint8_t>(mavlinkProtocol->getSystemId()),
                                          static_cast<uint8_t>(mavlinkProtocol->getComponentId()),
                                          vehicle->vehicleLinkManager()->primaryLink()->mavlinkChannel(),
                                          &message,
                                          &follow_target);
    vehicle->sendMessageOnLinkThreadSafe(vehicle->vehicleLinkManager()->primaryLink(), message);
}