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

10 11 12 13 14 15 16

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

#include "ParameterLoader.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
17
#include "QGCApplication.h"
18
#include "UASMessageHandler.h"
Don Gagne's avatar
Don Gagne committed
19
#include "FirmwarePlugin.h"
Don Gagne's avatar
Don Gagne committed
20
#include "APMFirmwarePlugin.h"
21
#include "UAS.h"
22

23
#include <QEasingCurve>
24 25
#include <QFile>
#include <QDebug>
26
#include <QVariantAnimation>
27

28 29 30 31 32
/* types for local parameter cache */
typedef QPair<int, QVariant> ParamTypeVal;
typedef QPair<QString, ParamTypeVal> NamedParam;
typedef QMap<int, NamedParam> MapID2NamedParam;

33
QGC_LOGGING_CATEGORY(ParameterLoaderVerboseLog, "ParameterLoaderVerboseLog")
34

Don Gagne's avatar
Don Gagne committed
35 36
Fact ParameterLoader::_defaultFact;

Don Gagne's avatar
Don Gagne committed
37
const char* ParameterLoader::_cachedMetaDataFilePrefix = "ParameterFactMetaData";
38 39 40

ParameterLoader::ParameterLoader(Vehicle* vehicle)
    : QObject(vehicle)
41 42 43 44
    , _vehicle(vehicle)
    , _mavlink(qgcApp()->toolbox()->mavlinkProtocol())
    , _parametersReady(false)
    , _initialLoadComplete(false)
45
    , _waitingForDefaultComponent(false)
46
    , _saveRequired(false)
47
    , _defaultComponentId(MAV_COMP_ID_ALL)
48 49
    , _parameterSetMajorVersion(-1)
    , _parameterMetaData(NULL)
Don Gagne's avatar
Don Gagne committed
50 51 52
    , _prevWaitingReadParamIndexCount(0)
    , _prevWaitingReadParamNameCount(0)
    , _prevWaitingWriteParamNameCount(0)
53
    , _initialRequestRetryCount(0)
54
    , _totalParamCount(0)
55
{
56
    Q_ASSERT(_vehicle);
57
    Q_ASSERT(_mavlink);
dogmaphobic's avatar
dogmaphobic committed
58

59 60
    // We signal this to ouselves in order to start timer on our thread
    connect(this, &ParameterLoader::restartWaitingParamTimer, this, &ParameterLoader::_restartWaitingParamTimer);
dogmaphobic's avatar
dogmaphobic committed
61

62 63 64 65
    _initialRequestTimeoutTimer.setSingleShot(true);
    _initialRequestTimeoutTimer.setInterval(6000);
    connect(&_initialRequestTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_initialRequestTimeout);

66
    _waitingParamTimeoutTimer.setSingleShot(true);
Don Gagne's avatar
Don Gagne committed
67
    _waitingParamTimeoutTimer.setInterval(1000);
68
    connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterLoader::_waitingParamTimeout);
69

70
    connect(_vehicle->uas(), &UASInterface::parameterUpdate, this, &ParameterLoader::_parameterUpdate);
71

72 73
    _versionParam = vehicle->firmwarePlugin()->getVersionParam();
    _defaultComponentIdParam = vehicle->firmwarePlugin()->getDefaultComponentIdParam();
74
    qCDebug(ParameterLoaderLog) << "Default component param" << _defaultComponentIdParam;
75

76 77 78
    // Ensure the cache directory exists
    QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache");
    refreshAllParameters();
79 80 81 82
}

ParameterLoader::~ParameterLoader()
{
83
    delete _parameterMetaData;
84 85 86
}

/// Called whenever a parameter is updated or first seen.
87
void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value)
88 89
{
    // Is this for our uas?
90
    if (uasId != _vehicle->id()) {
91 92
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
93

94 95
    _initialRequestTimeoutTimer.stop();

Don Gagne's avatar
Don Gagne committed
96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115
    if (_initialLoadComplete) {
        qCDebug(ParameterLoaderLog) << "_parameterUpdate (id:" << uasId <<
                                        "componentId:" << componentId <<
                                        "name:" << parameterName <<
                                        "count:" << parameterCount <<
                                        "index:" << parameterId <<
                                        "mavType:" << mavType <<
                                        "value:" << value <<
                                        ")";
    } else {
        // This is too noisy during initial load
        qCDebug(ParameterLoaderVerboseLog) << "_parameterUpdate (id:" << uasId <<
                                       "componentId:" << componentId <<
                                       "name:" << parameterName <<
                                       "count:" << parameterCount <<
                                       "index:" << parameterId <<
                                       "mavType:" << mavType <<
                                       "value:" << value <<
                                       ")";
    }
dogmaphobic's avatar
dogmaphobic committed
116

117 118 119
#if 0
    // Handy for testing retry logic
    static int counter = 0;
120
    if (counter++ & 0x8) {
121 122 123 124
        qCDebug(ParameterLoaderLog) << "Artificial discard" << counter;
        return;
    }
#endif
125

126 127 128 129 130 131 132
#if 0
    // Use this to test missing default component id
    if (componentId == 50) {
        return;
    }
#endif

Don Gagne's avatar
Don Gagne committed
133
    if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") {
134
        /* we received a cache hash, potentially load from cache */
135
        _tryCacheHashLoad(uasId, componentId, value);
136 137
        return;
    }
138
    _dataMutex.lock();
dogmaphobic's avatar
dogmaphobic committed
139

140 141 142 143 144
    // Update our total parameter counts
    if (!_paramCountMap.contains(componentId)) {
        _paramCountMap[componentId] = parameterCount;
        _totalParamCount += parameterCount;
    }
145 146

    _mapParameterId2Name[componentId][parameterId] = parameterName;
dogmaphobic's avatar
dogmaphobic committed
147

148 149
    // If we've never seen this component id before, setup the wait lists.
    if (!_waitingReadParamIndexMap.contains(componentId)) {
150 151 152 153
        // Add all indices to the wait list, parameter index is 0-based
        for (int waitingIndex=0; waitingIndex<parameterCount; waitingIndex++) {
            // This will add the new component id, as well as the the new waiting index and set the retry count for that index to 0
            _waitingReadParamIndexMap[componentId][waitingIndex] = 0;
154
        }
dogmaphobic's avatar
dogmaphobic committed
155

156 157 158
        // The read and write waiting lists for this component are initialized the empty
        _waitingReadParamNameMap[componentId] = QMap<QString, int>();
        _waitingWriteParamNameMap[componentId] = QMap<QString, int>();
dogmaphobic's avatar
dogmaphobic committed
159

160 161
        qCDebug(ParameterLoaderLog) << "Seeing component for first time, id:" << componentId << "parameter count:" << parameterCount;
    }
dogmaphobic's avatar
dogmaphobic committed
162

163 164 165 166 167 168 169 170 171 172 173 174
    // Determine default component id
    if (!_defaultComponentIdParam.isEmpty() && _defaultComponentIdParam == parameterName) {
        qCDebug(ParameterLoaderLog) << "Default component id determined" << componentId;
        _defaultComponentId = componentId;
    }

    bool componentParamsComplete = false;
    if (_waitingReadParamIndexMap[componentId].count() == 1) {
        // We need to know when we get the last param from a component in order to complete setup
        componentParamsComplete = true;
    }

175 176 177 178 179 180 181 182
    if (_waitingReadParamIndexMap[componentId].contains(parameterId) ||
        _waitingReadParamNameMap[componentId].contains(parameterName) ||
        _waitingWriteParamNameMap[componentId].contains(parameterName)) {
        // We were waiting for this parameter, restart wait timer. Otherwise it is a spurious parameter update which
        // means we should not reset the wait timer.
        _waitingParamTimeoutTimer.start();
    }

183
    // Remove this parameter from the waiting lists
184 185 186
    _waitingReadParamIndexMap[componentId].remove(parameterId);
    _waitingReadParamNameMap[componentId].remove(parameterName);
    _waitingWriteParamNameMap[componentId].remove(parameterName);
187
    qCDebug(ParameterLoaderVerboseLog) << "_waitingReadParamIndexMap:" << _waitingReadParamIndexMap[componentId];
188 189 190 191
    qCDebug(ParameterLoaderLog) << "_waitingReadParamNameMap" << _waitingReadParamNameMap[componentId];
    qCDebug(ParameterLoaderLog) << "_waitingWriteParamNameMap" << _waitingWriteParamNameMap[componentId];

    // Track how many parameters we are still waiting for
dogmaphobic's avatar
dogmaphobic committed
192

193 194 195
    int waitingReadParamIndexCount = 0;
    int waitingReadParamNameCount = 0;
    int waitingWriteParamNameCount = 0;
dogmaphobic's avatar
dogmaphobic committed
196

197 198 199 200 201 202 203
    foreach(int waitingComponentId, _waitingReadParamIndexMap.keys()) {
        waitingReadParamIndexCount += _waitingReadParamIndexMap[waitingComponentId].count();
    }
    if (waitingReadParamIndexCount) {
        qCDebug(ParameterLoaderLog) << "waitingReadParamIndexCount:" << waitingReadParamIndexCount;
    }

dogmaphobic's avatar
dogmaphobic committed
204

205 206 207 208 209 210
    foreach(int waitingComponentId, _waitingReadParamNameMap.keys()) {
        waitingReadParamNameCount += _waitingReadParamNameMap[waitingComponentId].count();
    }
    if (waitingReadParamNameCount) {
        qCDebug(ParameterLoaderLog) << "waitingReadParamNameCount:" << waitingReadParamNameCount;
    }
dogmaphobic's avatar
dogmaphobic committed
211

212 213 214 215 216 217
    foreach(int waitingComponentId, _waitingWriteParamNameMap.keys()) {
        waitingWriteParamNameCount += _waitingWriteParamNameMap[waitingComponentId].count();
    }
    if (waitingWriteParamNameCount) {
        qCDebug(ParameterLoaderLog) << "waitingWriteParamNameCount:" << waitingWriteParamNameCount;
    }
dogmaphobic's avatar
dogmaphobic committed
218

Don Gagne's avatar
Don Gagne committed
219 220 221 222
    int readWaitingParamCount = waitingReadParamIndexCount + waitingReadParamNameCount;
    int totalWaitingParamCount = readWaitingParamCount + waitingWriteParamNameCount;
    if (totalWaitingParamCount) {
        qCDebug(ParameterLoaderLog) << "totalWaitingParamCount:" << totalWaitingParamCount;
223
    } else if (_defaultComponentId != MAV_COMP_ID_ALL) {
224 225
        // No more parameters to wait for, stop the timeout. Be careful to not stop timer if we don't have the default
        // component yet.
226 227 228
        _waitingParamTimeoutTimer.stop();
    }

Don Gagne's avatar
Don Gagne committed
229 230 231 232 233 234 235
    // Update progress bar for waiting reads
    if (readWaitingParamCount == 0) {
        // We are no longer waiting for any reads to complete
        if (_prevWaitingReadParamIndexCount + _prevWaitingReadParamNameCount != 0) {
            // Set progress to 0 if not already there
            emit parameterListProgress(0);
        }
236
    } else {
Don Gagne's avatar
Don Gagne committed
237
        emit parameterListProgress((float)(_totalParamCount - readWaitingParamCount) / (float)_totalParamCount);
238
    }
dogmaphobic's avatar
dogmaphobic committed
239

240 241 242 243 244
    // Get parameter set version
    if (!_versionParam.isEmpty() && _versionParam == parameterName) {
        _parameterSetMajorVersion = value.toInt();
    }

245
    if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(parameterName)) {
246
        qCDebug(ParameterLoaderLog) << "Adding new fact";
dogmaphobic's avatar
dogmaphobic committed
247

248 249 250 251 252 253
        FactMetaData::ValueType_t factType;
        switch (mavType) {
            case MAV_PARAM_TYPE_UINT8:
                factType = FactMetaData::valueTypeUint8;
                break;
            case MAV_PARAM_TYPE_INT8:
254
                factType = FactMetaData::valueTypeInt8;
255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278
                break;
            case MAV_PARAM_TYPE_UINT16:
                factType = FactMetaData::valueTypeUint16;
                break;
            case MAV_PARAM_TYPE_INT16:
                factType = FactMetaData::valueTypeInt16;
                break;
            case MAV_PARAM_TYPE_UINT32:
                factType = FactMetaData::valueTypeUint32;
                break;
            case MAV_PARAM_TYPE_INT32:
                factType = FactMetaData::valueTypeInt32;
                break;
            case MAV_PARAM_TYPE_REAL32:
                factType = FactMetaData::valueTypeFloat;
                break;
            case MAV_PARAM_TYPE_REAL64:
                factType = FactMetaData::valueTypeDouble;
                break;
            default:
                factType = FactMetaData::valueTypeInt32;
                qCritical() << "Unsupported fact type" << mavType;
                break;
        }
dogmaphobic's avatar
dogmaphobic committed
279

280
        Fact* fact = new Fact(componentId, parameterName, factType, this);
dogmaphobic's avatar
dogmaphobic committed
281

282
        _mapParameterName2Variant[componentId][parameterName] = QVariant::fromValue(fact);
dogmaphobic's avatar
dogmaphobic committed
283

284
        // We need to know when the fact changes from QML so that we can send the new value to the parameter manager
Don Gagne's avatar
Don Gagne committed
285
        connect(fact, &Fact::_containerRawValueChanged, this, &ParameterLoader::_valueUpdated);
286
    }
dogmaphobic's avatar
dogmaphobic committed
287

Don Gagne's avatar
Don Gagne committed
288 289
    _dataMutex.unlock();

290
    Q_ASSERT(_mapParameterName2Variant[componentId].contains(parameterName));
dogmaphobic's avatar
dogmaphobic committed
291

292 293
    Fact* fact = _mapParameterName2Variant[componentId][parameterName].value<Fact*>();
    Q_ASSERT(fact);
Don Gagne's avatar
Don Gagne committed
294
    fact->_containerSetRawValue(value);
dogmaphobic's avatar
dogmaphobic committed
295

296 297 298 299 300 301 302 303 304 305 306 307 308
    if (componentParamsComplete) {
        if (componentId == _defaultComponentId) {
            // Add meta data to default component. We need to do this before we setup the group map since group
            // map requires meta data.
            _addMetaDataToDefaultComponent();
        }

        // When we are getting the very last component param index, reset the group maps to update for the
        // new params. By handling this here, we can pick up components which finish up later than the default
        // component param set.
        _setupGroupMap();
    }

Don Gagne's avatar
Don Gagne committed
309 310
    if (_prevWaitingWriteParamNameCount != 0 &&  waitingWriteParamNameCount == 0) {
        // If all the writes just finished the vehicle is up to date, so persist.
311 312
        _saveToEEPROM();
    }
dogmaphobic's avatar
dogmaphobic committed
313

Don Gagne's avatar
Don Gagne committed
314 315 316 317 318 319 320 321 322 323 324 325 326 327
    // Update param cache. The param cache is only used on PX4 Firmware since ArduPilot and Solo have volatile params
    // which invalidate the cache. The Solo also streams param updates in flight for things like gimbal values
    // which in turn causes a perf problem with all the param cache updates.
    if (_vehicle->px4Firmware()) {
        if (_prevWaitingReadParamIndexCount + _prevWaitingReadParamNameCount != 0 && readWaitingParamCount == 0) {
            // All reads just finished, update the cache
            _writeLocalParamCache(uasId, componentId);
        }
    }

    _prevWaitingReadParamIndexCount = waitingReadParamIndexCount;
    _prevWaitingReadParamNameCount = waitingReadParamNameCount;
    _prevWaitingWriteParamNameCount = waitingWriteParamNameCount;

328 329
    // Don't fail initial load complete if default component isn't found yet. That will be handled in wait timeout check.
    _checkInitialLoadComplete(false /* failIfNoDefaultComponent */);
330 331 332 333
}

/// Connected to Fact::valueUpdated
///
334
/// Writes the parameter to mavlink, sets up for write wait
335 336 337 338
void ParameterLoader::_valueUpdated(const QVariant& value)
{
    Fact* fact = qobject_cast<Fact*>(sender());
    Q_ASSERT(fact);
dogmaphobic's avatar
dogmaphobic committed
339

340
    int componentId = fact->componentId();
341
    QString name = fact->name();
dogmaphobic's avatar
dogmaphobic committed
342

343
    _dataMutex.lock();
dogmaphobic's avatar
dogmaphobic committed
344

345
    Q_ASSERT(_waitingWriteParamNameMap.contains(componentId));
346 347
    _waitingWriteParamNameMap[componentId].remove(name);    // Remove any old entry
    _waitingWriteParamNameMap[componentId][name] = 0;       // Add new entry and set retry count
348
    _waitingParamTimeoutTimer.start();
349
    _saveRequired = true;
dogmaphobic's avatar
dogmaphobic committed
350

351
    _dataMutex.unlock();
dogmaphobic's avatar
dogmaphobic committed
352

353 354
    _writeParameterRaw(componentId, fact->name(), value);
    qCDebug(ParameterLoaderLog) << "Set parameter (componentId:" << componentId << "name:" << name << value << ")";
355

356 357
    if (fact->rebootRequired() && !qgcApp()->runningUnitTests()) {
        qgcApp()->showMessage(QStringLiteral("Change of parameter %1 requires a Vehicle reboot to take effect").arg(name));
358
    }
359 360
}

dogmaphobic's avatar
dogmaphobic committed
361
void ParameterLoader::refreshAllParameters(uint8_t componentID)
362
{
363
    _dataMutex.lock();
dogmaphobic's avatar
dogmaphobic committed
364

365 366 367 368
    if (!_initialLoadComplete) {
        _initialRequestTimeoutTimer.start();
    }

369
    // Reset index wait lists
dogmaphobic's avatar
dogmaphobic committed
370
    foreach (int cid, _paramCountMap.keys()) {
371
        // Add/Update all indices to the wait list, parameter index is 0-based
dogmaphobic's avatar
dogmaphobic committed
372 373 374
        if(componentID != MAV_COMP_ID_ALL && componentID != cid)
            continue;
        for (int waitingIndex = 0; waitingIndex < _paramCountMap[cid]; waitingIndex++) {
375
            // This will add a new waiting index if needed and set the retry count for that index to 0
dogmaphobic's avatar
dogmaphobic committed
376
            _waitingReadParamIndexMap[cid][waitingIndex] = 0;
377 378
        }
    }
dogmaphobic's avatar
dogmaphobic committed
379

380
    _dataMutex.unlock();
dogmaphobic's avatar
dogmaphobic committed
381

382
    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
383
    Q_ASSERT(mavlink);
dogmaphobic's avatar
dogmaphobic committed
384

385
    mavlink_message_t msg;
dogmaphobic's avatar
dogmaphobic committed
386
    mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), componentID);
387
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
dogmaphobic's avatar
dogmaphobic committed
388

dogmaphobic's avatar
dogmaphobic committed
389 390
    QString what = (componentID == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentID);
    qCDebug(ParameterLoaderLog) << "Request to refresh all parameters for component ID:" << what;
391 392 393 394
}

void ParameterLoader::_determineDefaultComponentId(void)
{
395
    if (_defaultComponentId == MAV_COMP_ID_ALL) {
396 397 398
        // We don't have a default component id yet. That means the plugin can't provide
        // the param to trigger off of. Instead we use the most prominent component id in
        // the set of parameters. Better than nothing!
dogmaphobic's avatar
dogmaphobic committed
399

400
        int largestCompParamCount = 0;
401
        foreach(int componentId, _mapParameterName2Variant.keys()) {
402 403 404
            int compParamCount = _mapParameterName2Variant[componentId].count();
            if (compParamCount > largestCompParamCount) {
                largestCompParamCount = compParamCount;
405 406 407
                _defaultComponentId = componentId;
            }
        }
408

409
        if (_defaultComponentId == MAV_COMP_ID_ALL) {
410 411
            qWarning() << "All parameters missing, unable to determine default componet id";
        }
412 413 414 415 416 417 418 419
    }
}

/// Translates FactSystem::defaultComponentId to real component id if needed
int ParameterLoader::_actualComponentId(int componentId)
{
    if (componentId == FactSystem::defaultComponentId) {
        componentId = _defaultComponentId;
Don Gagne's avatar
Don Gagne committed
420
        if (componentId == FactSystem::defaultComponentId) {
Don Gagne's avatar
Don Gagne committed
421 422
            qWarning() << "Default component id not set";
        }
423
    }
dogmaphobic's avatar
dogmaphobic committed
424

425 426 427 428 429
    return componentId;
}

void ParameterLoader::refreshParameter(int componentId, const QString& name)
{
430 431
    componentId = _actualComponentId(componentId);
    qCDebug(ParameterLoaderLog) << "refreshParameter (component id:" << componentId << "name:" << name << ")";
dogmaphobic's avatar
dogmaphobic committed
432

433 434 435
    _dataMutex.lock();

    Q_ASSERT(_waitingReadParamNameMap.contains(componentId));
dogmaphobic's avatar
dogmaphobic committed
436

437
    if (_waitingReadParamNameMap.contains(componentId)) {
438 439 440 441
        QString mappedParamName = _remapParamNameToVersion(name);

        _waitingReadParamNameMap[componentId].remove(mappedParamName);  // Remove old wait entry if there
        _waitingReadParamNameMap[componentId][mappedParamName] = 0;     // Add new wait entry and update retry count
442 443
        emit restartWaitingParamTimer();
    }
dogmaphobic's avatar
dogmaphobic committed
444

445 446 447
    _dataMutex.unlock();

    _readParameterRaw(componentId, name, -1);
448 449 450 451 452
}

void ParameterLoader::refreshParametersPrefix(int componentId, const QString& namePrefix)
{
    componentId = _actualComponentId(componentId);
453 454
    qCDebug(ParameterLoaderLog) << "refreshParametersPrefix (component id:" << componentId << "name:" << namePrefix << ")";

455
    foreach(const QString &name, _mapParameterName2Variant[componentId].keys()) {
456 457 458 459 460 461
        if (name.startsWith(namePrefix)) {
            refreshParameter(componentId, name);
        }
    }
}

462
bool ParameterLoader::parameterExists(int componentId, const QString&  name)
463
{
464
    bool ret = false;
dogmaphobic's avatar
dogmaphobic committed
465

466 467
    componentId = _actualComponentId(componentId);
    if (_mapParameterName2Variant.contains(componentId)) {
468
        ret = _mapParameterName2Variant[componentId].contains(_remapParamNameToVersion(name));
469
    }
470 471

    return ret;
472 473 474 475 476
}

Fact* ParameterLoader::getFact(int componentId, const QString& name)
{
    componentId = _actualComponentId(componentId);
dogmaphobic's avatar
dogmaphobic committed
477

478 479 480
    QString mappedParamName = _remapParamNameToVersion(name);
    if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(mappedParamName)) {
        qgcApp()->reportMissingParameter(componentId, mappedParamName);
Don Gagne's avatar
Don Gagne committed
481
        return &_defaultFact;
482
    }
dogmaphobic's avatar
dogmaphobic committed
483

484
    return _mapParameterName2Variant[componentId][mappedParamName].value<Fact*>();
485
}
486

Don Gagne's avatar
Don Gagne committed
487
QStringList ParameterLoader::parameterNames(int componentId)
488
{
dogmaphobic's avatar
dogmaphobic committed
489 490
    QStringList names;

491
    foreach(const QString &paramName, _mapParameterName2Variant[_actualComponentId(componentId)].keys()) {
dogmaphobic's avatar
dogmaphobic committed
492 493 494 495
        names << paramName;
    }

    return names;
496 497 498 499
}

void ParameterLoader::_setupGroupMap(void)
{
500 501 502
    // Must be able to handle being called multiple times
    _mapGroup2ParameterName.clear();

503
    foreach (int componentId, _mapParameterName2Variant.keys()) {
504
        foreach (const QString &name, _mapParameterName2Variant[componentId].keys()) {
505 506 507 508 509 510 511 512 513 514
            Fact* fact = _mapParameterName2Variant[componentId][name].value<Fact*>();
            _mapGroup2ParameterName[componentId][fact->group()] += name;
        }
    }
}

const QMap<int, QMap<QString, QStringList> >& ParameterLoader::getGroupMap(void)
{
    return _mapGroup2ParameterName;
}
515 516 517 518 519 520

void ParameterLoader::_waitingParamTimeout(void)
{
    bool paramsRequested = false;
    const int maxBatchSize = 10;
    int batchCount = 0;
dogmaphobic's avatar
dogmaphobic committed
521

522
    // First check for any missing parameters from the initial index based load
523 524
    batchCount = 0;
    foreach(int componentId, _waitingReadParamIndexMap.keys()) {
525 526
        foreach(int paramIndex, _waitingReadParamIndexMap[componentId].keys()) {
            _waitingReadParamIndexMap[componentId][paramIndex]++;   // Bump retry count
527
            if (_waitingReadParamIndexMap[componentId][paramIndex] > _maxInitialLoadRetrySingleParam) {
528 529 530 531 532 533 534 535 536
                // Give up on this index
                _failedReadParamIndexMap[componentId] << paramIndex;
                qCDebug(ParameterLoaderLog) << "Giving up on (componentId:" << componentId << "paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
                _waitingReadParamIndexMap[componentId].remove(paramIndex);
            } else {
                // Retry again
                paramsRequested = true;
                _readParameterRaw(componentId, "", paramIndex);
                qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
dogmaphobic's avatar
dogmaphobic committed
537

538 539 540
                if (++batchCount > maxBatchSize) {
                    goto Out;
                }
541 542 543
            }
        }
    }
544

545
    if (!paramsRequested && _defaultComponentId == MAV_COMP_ID_ALL && !_waitingForDefaultComponent) {
546 547 548 549 550 551 552 553 554 555
        // Initial load is complete but we still don't have default component params. Wait one more cycle to see if the
        // default component finally shows up.
        _waitingParamTimeoutTimer.start();
        _waitingForDefaultComponent = true;
        return;
    }
    _waitingForDefaultComponent = false;

    // Check for initial load complete success/failure. Fail load if we don't have a default component at this point.
    _checkInitialLoadComplete(true /* failIfNoDefaultComponent */);
dogmaphobic's avatar
dogmaphobic committed
556

557 558
    if (!paramsRequested) {
        foreach(int componentId, _waitingWriteParamNameMap.keys()) {
559
            foreach(const QString &paramName, _waitingWriteParamNameMap[componentId].keys()) {
560
                paramsRequested = true;
561
                _waitingWriteParamNameMap[componentId][paramName]++;   // Bump retry count
562 563 564 565 566 567 568 569 570 571
                if (_waitingWriteParamNameMap[componentId][paramName] <= _maxReadWriteRetry) {
                    _writeParameterRaw(componentId, paramName, _vehicle->autopilotPlugin()->getFact(FactSystem::ParameterProvider, componentId, paramName)->rawValue());
                    qCDebug(ParameterLoaderLog) << "Write resend for (componentId:" << componentId << "paramName:" << paramName << "retryCount:" << _waitingWriteParamNameMap[componentId][paramName] << ")";
                    if (++batchCount > maxBatchSize) {
                        goto Out;
                    }
                } else {
                    // Exceeded max retry count, notify user
                    _waitingWriteParamNameMap[componentId].remove(paramName);
                    qgcApp()->showMessage(tr("Parameter write failed: comp:%1 param:%2").arg(componentId).arg(paramName));
572 573 574 575
                }
            }
        }
    }
dogmaphobic's avatar
dogmaphobic committed
576

577 578
    if (!paramsRequested) {
        foreach(int componentId, _waitingReadParamNameMap.keys()) {
579
            foreach(const QString &paramName, _waitingReadParamNameMap[componentId].keys()) {
580
                paramsRequested = true;
581
                _waitingReadParamNameMap[componentId][paramName]++;   // Bump retry count
582 583 584 585 586 587 588 589 590 591
                if (_waitingReadParamNameMap[componentId][paramName] <= _maxReadWriteRetry) {
                    _readParameterRaw(componentId, paramName, -1);
                    qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramName:" << paramName << "retryCount:" << _waitingReadParamNameMap[componentId][paramName] << ")";
                    if (++batchCount > maxBatchSize) {
                        goto Out;
                    }
                } else {
                    // Exceeded max retry count, notify user
                    _waitingReadParamNameMap[componentId].remove(paramName);
                    qgcApp()->showMessage(tr("Parameter read failed: comp:%1 param:%2").arg(componentId).arg(paramName));
592 593 594 595
                }
            }
        }
    }
dogmaphobic's avatar
dogmaphobic committed
596

597 598 599 600 601 602 603 604 605 606 607 608 609 610 611
Out:
    if (paramsRequested) {
        _waitingParamTimeoutTimer.start();
    }
}

void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex)
{
    mavlink_message_t msg;
    char fixedParamName[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN];

    strncpy(fixedParamName, paramName.toStdString().c_str(), sizeof(fixedParamName));
    mavlink_msg_param_request_read_pack(_mavlink->getSystemId(),    // Our system id
                                        _mavlink->getComponentId(), // Our component id
                                        &msg,                       // Pack into this mavlink_message_t
612
                                        _vehicle->id(),             // Target system id
613 614 615
                                        componentId,                // Target component id
                                        fixedParamName,             // Named parameter being requested
                                        paramIndex);                // Parameter index being requested, -1 for named
616
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
617 618 619 620 621 622
}

void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value)
{
    mavlink_param_set_t     p;
    mavlink_param_union_t   union_value;
dogmaphobic's avatar
dogmaphobic committed
623

624
    FactMetaData::ValueType_t factType = _vehicle->autopilotPlugin()->getFact(FactSystem::ParameterProvider, componentId, paramName)->type();
625
    p.param_type = _factTypeToMavType(factType);
dogmaphobic's avatar
dogmaphobic committed
626

627 628
    switch (factType) {
        case FactMetaData::valueTypeUint8:
Don Gagne's avatar
Don Gagne committed
629
            union_value.param_uint8 = (uint8_t)value.toUInt();
630
            break;
dogmaphobic's avatar
dogmaphobic committed
631

632
        case FactMetaData::valueTypeInt8:
Don Gagne's avatar
Don Gagne committed
633
            union_value.param_int8 = (int8_t)value.toInt();
634
            break;
dogmaphobic's avatar
dogmaphobic committed
635

636
        case FactMetaData::valueTypeUint16:
Don Gagne's avatar
Don Gagne committed
637
            union_value.param_uint16 = (uint16_t)value.toUInt();
638
            break;
dogmaphobic's avatar
dogmaphobic committed
639

640
        case FactMetaData::valueTypeInt16:
Don Gagne's avatar
Don Gagne committed
641
            union_value.param_int16 = (int16_t)value.toInt();
642
            break;
dogmaphobic's avatar
dogmaphobic committed
643

644
        case FactMetaData::valueTypeUint32:
Don Gagne's avatar
Don Gagne committed
645
            union_value.param_uint32 = (uint32_t)value.toUInt();
646
            break;
dogmaphobic's avatar
dogmaphobic committed
647

648 649 650
        case FactMetaData::valueTypeFloat:
            union_value.param_float = value.toFloat();
            break;
dogmaphobic's avatar
dogmaphobic committed
651

652 653 654
        default:
            qCritical() << "Unsupported fact type" << factType;
            // fall through
dogmaphobic's avatar
dogmaphobic committed
655

656
        case FactMetaData::valueTypeInt32:
Don Gagne's avatar
Don Gagne committed
657
            union_value.param_int32 = (int32_t)value.toInt();
658 659
            break;
    }
dogmaphobic's avatar
dogmaphobic committed
660

661
    p.param_value = union_value.param_float;
662
    p.target_system = (uint8_t)_vehicle->id();
663
    p.target_component = (uint8_t)componentId;
dogmaphobic's avatar
dogmaphobic committed
664

665
    strncpy(p.param_id, paramName.toStdString().c_str(), sizeof(p.param_id));
dogmaphobic's avatar
dogmaphobic committed
666

667 668
    mavlink_message_t msg;
    mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p);
669
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
670 671
}

672
void ParameterLoader::_writeLocalParamCache(int uasId, int componentId)
673
{
674
    MapID2NamedParam cache_map;
675

676 677 678 679
    foreach(int id, _mapParameterId2Name[componentId].keys()) {
        const QString name(_mapParameterId2Name[componentId][id]);
        const Fact *fact = _mapParameterName2Variant[componentId][name].value<Fact*>();
        cache_map[id] = NamedParam(name, ParamTypeVal(fact->type(), fact->rawValue()));
680 681
    }

682
    QFile cache_file(parameterCacheFile(uasId, componentId));
683 684 685 686 687 688
    cache_file.open(QIODevice::WriteOnly | QIODevice::Truncate);

    QDataStream ds(&cache_file);
    ds << cache_map;
}

689 690 691 692 693 694 695
QDir ParameterLoader::parameterCacheDir()
{
    const QString spath(QFileInfo(QSettings().fileName()).dir().absolutePath());
    return spath + QDir::separator() + "ParamCache";
}

QString ParameterLoader::parameterCacheFile(int uasId, int componentId)
696
{
697
    return parameterCacheDir().filePath(QString("%1_%2").arg(uasId).arg(componentId));
698 699
}

700
void ParameterLoader::_tryCacheHashLoad(int uasId, int componentId, QVariant hash_value)
701 702 703
{
    uint32_t crc32_value = 0;
    /* The datastructure of the cache table */
704 705
    MapID2NamedParam cache_map;
    QFile cache_file(parameterCacheFile(uasId, componentId));
706
    if (!cache_file.exists()) {
707
        /* no local cache, just wait for them to come in*/
708 709 710 711 712 713 714 715 716
        return;
    }
    cache_file.open(QIODevice::ReadOnly);

    /* Deserialize the parameter cache table */
    QDataStream ds(&cache_file);
    ds >> cache_map;

    /* compute the crc of the local cache to check against the remote */
717 718 719 720 721 722 723

    foreach(int id, cache_map.keys()) {
        const QString name(cache_map[id].first);
        const void *vdat = cache_map[id].second.second.constData();
        const FactMetaData::ValueType_t fact_type = static_cast<FactMetaData::ValueType_t>(cache_map[id].second.first);
        crc32_value = QGC::crc32((const uint8_t *)qPrintable(name), name.length(),  crc32_value);
        crc32_value = QGC::crc32((const uint8_t *)vdat, FactMetaData::typeToSize(fact_type), crc32_value);
724 725 726
    }

    if (crc32_value == hash_value.toUInt()) {
727
        qCInfo(ParameterLoaderLog) << "Parameters loaded from cache" << qPrintable(QFileInfo(cache_file).absoluteFilePath());
728
        /* if the two param set hashes match, just load from the disk */
729 730 731 732 733 734 735
        int count = cache_map.count();
        foreach(int id, cache_map.keys()) {
            const QString &name = cache_map[id].first;
            const QVariant &value = cache_map[id].second.second;
            const FactMetaData::ValueType_t fact_type = static_cast<FactMetaData::ValueType_t>(cache_map[id].second.first);
            const int mavType = _factTypeToMavType(fact_type);
            _parameterUpdate(uasId, componentId, name, count, id, mavType, value);
736
        }
737 738 739 740 741 742 743 744 745 746 747 748
        // Return the hash value to notify we don't want any more updates
        mavlink_param_set_t     p;
        mavlink_param_union_t   union_value;
        p.param_type = MAV_PARAM_TYPE_UINT32;
        strncpy(p.param_id, "_HASH_CHECK", sizeof(p.param_id));
        union_value.param_uint32 = crc32_value;
        p.param_value = union_value.param_float;
        p.target_system = (uint8_t)_vehicle->id();
        p.target_component = (uint8_t)componentId;
        mavlink_message_t msg;
        mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p);
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768

        // Give the user some feedback things loaded properly
        QVariantAnimation *ani = new QVariantAnimation(this);
        ani->setEasingCurve(QEasingCurve::OutCubic);
        ani->setStartValue(0.0);
        ani->setEndValue(1.0);
        ani->setDuration(750);

        connect(ani, &QVariantAnimation::valueChanged, [this](const QVariant &value) {
            emit parameterListProgress(value.toFloat());
        });

        // Hide 500ms after animation finishes
        connect(ani, &QVariantAnimation::finished, [this](){
            QTimer::singleShot(500, [this]() {
                emit parameterListProgress(0);
            });
        });

        ani->start(QAbstractAnimation::DeleteWhenStopped);
769 770 771
    }
}

772 773
void ParameterLoader::_saveToEEPROM(void)
{
774 775
    if (_saveRequired) {
        _saveRequired = false;
776
        if (_vehicle->firmwarePlugin()->isCapable(_vehicle, FirmwarePlugin::MavCmdPreflightStorageCapability)) {
777 778
            mavlink_message_t msg;
            mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
779
            _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
780 781 782 783
            qCDebug(ParameterLoaderLog) << "_saveToEEPROM";
        } else {
            qCDebug(ParameterLoaderLog) << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable";
        }
Don Gagne's avatar
Don Gagne committed
784
    }
785 786
}

787
QString ParameterLoader::readParametersFromStream(QTextStream& stream)
788
{
789
    QString errors;
dogmaphobic's avatar
dogmaphobic committed
790

791 792 793 794 795 796
    while (!stream.atEnd()) {
        QString line = stream.readLine();
        if (!line.startsWith("#")) {
            QStringList wpParams = line.split("\t");
            int lineMavId = wpParams.at(0).toInt();
            if (wpParams.size() == 5) {
Don Gagne's avatar
Don Gagne committed
797 798
                if (_vehicle->id() != lineMavId) {
                    return QString("The parameters in the stream have been saved from System Id %1, but the current vehicle has the System Id %2.").arg(lineMavId).arg(_vehicle->id());
dogmaphobic's avatar
dogmaphobic committed
799 800
                }

801 802 803 804
                int     componentId = wpParams.at(1).toInt();
                QString paramName = wpParams.at(2);
                QString valStr = wpParams.at(3);
                uint    mavType = wpParams.at(4).toUInt();
dogmaphobic's avatar
dogmaphobic committed
805

806
                if (!_vehicle->autopilotPlugin()->factExists(FactSystem::ParameterProvider, componentId, paramName)) {
807 808 809 810
                    QString error;
                    error = QString("Skipped parameter %1:%2 - does not exist on this vehicle\n").arg(componentId).arg(paramName);
                    errors += error;
                    qCDebug(ParameterLoaderLog) << error;
811 812
                    continue;
                }
dogmaphobic's avatar
dogmaphobic committed
813

814
                Fact* fact = _vehicle->autopilotPlugin()->getFact(FactSystem::ParameterProvider, componentId, paramName);
815
                if (fact->type() != _mavTypeToFactType((MAV_PARAM_TYPE)mavType)) {
816 817 818 819
                    QString error;
                    error  = QString("Skipped parameter %1:%2 - type mismatch %3:%4\n").arg(componentId).arg(paramName).arg(fact->type()).arg(_mavTypeToFactType((MAV_PARAM_TYPE)mavType));
                    errors += error;
                    qCDebug(ParameterLoaderLog) << error;
820 821
                    continue;
                }
dogmaphobic's avatar
dogmaphobic committed
822

823
                qCDebug(ParameterLoaderLog) << "Updating parameter" << componentId << paramName << valStr;
Don Gagne's avatar
Don Gagne committed
824
                fact->setRawValue(valStr);
825 826 827
            }
        }
    }
dogmaphobic's avatar
dogmaphobic committed
828

829
    return errors;
830 831
}

832
void ParameterLoader::writeParametersToStream(QTextStream &stream)
833
{
834
    stream << "# Onboard parameters for vehicle " << _vehicle->id() << "\n";
835 836 837 838
    stream << "#\n";
    stream << "# MAV ID  COMPONENT ID  PARAM NAME  VALUE (FLOAT)\n";

    foreach (int componentId, _mapParameterName2Variant.keys()) {
839
        foreach (const QString &paramName, _mapParameterName2Variant[componentId].keys()) {
840
            Fact* fact = _mapParameterName2Variant[componentId][paramName].value<Fact*>();
841 842 843 844 845
            if (fact) {
                stream << _vehicle->id() << "\t" << componentId << "\t" << paramName << "\t" << fact->rawValueStringFullPrecision() << "\t" << QString("%1").arg(_factTypeToMavType(fact->type())) << "\n";
            } else {
                qWarning() << "Internal error: missing fact";
            }
846 847
        }
    }
dogmaphobic's avatar
dogmaphobic committed
848

849 850 851 852 853 854 855 856
    stream.flush();
}

MAV_PARAM_TYPE ParameterLoader::_factTypeToMavType(FactMetaData::ValueType_t factType)
{
    switch (factType) {
        case FactMetaData::valueTypeUint8:
            return MAV_PARAM_TYPE_UINT8;
dogmaphobic's avatar
dogmaphobic committed
857

858 859
        case FactMetaData::valueTypeInt8:
            return MAV_PARAM_TYPE_INT8;
dogmaphobic's avatar
dogmaphobic committed
860

861 862
        case FactMetaData::valueTypeUint16:
            return MAV_PARAM_TYPE_UINT16;
dogmaphobic's avatar
dogmaphobic committed
863

864 865
        case FactMetaData::valueTypeInt16:
            return MAV_PARAM_TYPE_INT16;
dogmaphobic's avatar
dogmaphobic committed
866

867 868
        case FactMetaData::valueTypeUint32:
            return MAV_PARAM_TYPE_UINT32;
dogmaphobic's avatar
dogmaphobic committed
869

870 871
        case FactMetaData::valueTypeFloat:
            return MAV_PARAM_TYPE_REAL32;
dogmaphobic's avatar
dogmaphobic committed
872

873 874 875
        default:
            qWarning() << "Unsupported fact type" << factType;
            // fall through
dogmaphobic's avatar
dogmaphobic committed
876

877 878 879 880 881 882 883 884 885 886
        case FactMetaData::valueTypeInt32:
            return MAV_PARAM_TYPE_INT32;
    }
}

FactMetaData::ValueType_t ParameterLoader::_mavTypeToFactType(MAV_PARAM_TYPE mavType)
{
    switch (mavType) {
        case MAV_PARAM_TYPE_UINT8:
            return FactMetaData::valueTypeUint8;
dogmaphobic's avatar
dogmaphobic committed
887

888 889
        case MAV_PARAM_TYPE_INT8:
            return FactMetaData::valueTypeInt8;
dogmaphobic's avatar
dogmaphobic committed
890

891 892
        case MAV_PARAM_TYPE_UINT16:
            return FactMetaData::valueTypeUint16;
dogmaphobic's avatar
dogmaphobic committed
893

894 895
        case MAV_PARAM_TYPE_INT16:
            return FactMetaData::valueTypeInt16;
dogmaphobic's avatar
dogmaphobic committed
896

897 898
        case MAV_PARAM_TYPE_UINT32:
            return FactMetaData::valueTypeUint32;
dogmaphobic's avatar
dogmaphobic committed
899

900 901
        case MAV_PARAM_TYPE_REAL32:
            return FactMetaData::valueTypeFloat;
dogmaphobic's avatar
dogmaphobic committed
902

903 904 905
        default:
            qWarning() << "Unsupported mav param type" << mavType;
            // fall through
dogmaphobic's avatar
dogmaphobic committed
906

907 908 909 910 911 912 913 914 915
        case MAV_PARAM_TYPE_INT32:
            return FactMetaData::valueTypeInt32;
    }
}

void ParameterLoader::_restartWaitingParamTimer(void)
{
    _waitingParamTimeoutTimer.start();
}
916

917
void ParameterLoader::_addMetaDataToDefaultComponent(void)
918
{
919
     if (_defaultComponentId == MAV_COMP_ID_ALL) {
920 921 922 923 924 925 926 927
         // We don't know what the default component is so we can't support meta data
         return;
     }

     if (_parameterMetaData) {
         return;
     }

Don Gagne's avatar
Don Gagne committed
928
     QString metaDataFile;
929
     int majorVersion, minorVersion;
Don Gagne's avatar
Don Gagne committed
930 931 932 933 934 935 936 937 938 939 940
     if (_vehicle->firmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) {
         // Parameter versioning is still not really figured out correctly. We need to handle ArduPilot specially based on vehicle version.
         // The current three version are hardcoded in.
         metaDataFile = ((APMFirmwarePlugin*)_vehicle->firmwarePlugin())->getParameterMetaDataFile(_vehicle);
         qCDebug(ParameterLoaderLog) << "Adding meta data to Vehicle file:major:minor" << metaDataFile;
     } else {
         // Load best parameter meta data set
         metaDataFile = parameterMetaDataFile(_vehicle->firmwareType(), _parameterSetMajorVersion, majorVersion, minorVersion);
         qCDebug(ParameterLoaderLog) << "Adding meta data to Vehicle file:major:minor" << metaDataFile << majorVersion << minorVersion;
     }

941 942 943 944 945 946 947 948 949
     _parameterMetaData = _vehicle->firmwarePlugin()->loadParameterMetaData(metaDataFile);

    // Loop over all parameters in default component adding meta data
    QVariantMap& factMap = _mapParameterName2Variant[_defaultComponentId];
    foreach (const QString& key, factMap.keys()) {
        _vehicle->firmwarePlugin()->addMetaDataToFact(_parameterMetaData, factMap[key].value<Fact*>(), _vehicle->vehicleType());
    }
}

950 951
/// @param failIfNoDefaultComponent true: Fails parameter load if no default component but we should have one
void ParameterLoader::_checkInitialLoadComplete(bool failIfNoDefaultComponent)
952 953 954 955 956
{
    // Already processed?
    if (_initialLoadComplete) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
957

958 959 960 961 962 963
    foreach (int componentId, _waitingReadParamIndexMap.keys()) {
        if (_waitingReadParamIndexMap[componentId].count()) {
            // We are still waiting on some parameters, not done yet
            return;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
964

965
    if (!failIfNoDefaultComponent && _defaultComponentId == MAV_COMP_ID_ALL) {
966 967 968 969
        // We are still waiting for default component to show up
        return;
    }

970 971
    // We aren't waiting for any more initial parameter updates, initial parameter loading is complete
    _initialLoadComplete = true;
dogmaphobic's avatar
dogmaphobic committed
972

973 974 975 976 977 978 979 980 981 982 983 984 985
    // Check for index based load failures
    QString indexList;
    bool initialLoadFailures = false;
    foreach (int componentId, _failedReadParamIndexMap.keys()) {
        foreach (int paramIndex, _failedReadParamIndexMap[componentId]) {
            if (initialLoadFailures) {
                indexList += ", ";
            }
            indexList += QString("%1").arg(paramIndex);
            initialLoadFailures = true;
            qCDebug(ParameterLoaderLog) << "Gave up on initial load after max retries (componentId:" << componentId << "paramIndex:" << paramIndex << ")";
        }
    }
986
    if (initialLoadFailures) {
987
        qgcApp()->showMessage("QGroundControl was unable to retrieve the full set of parameters from the vehicle. "
dogmaphobic's avatar
dogmaphobic committed
988
                              "This will cause QGroundControl to be unable to display its full user interface. "
989 990
                              "If you are using modified firmware, you may need to resolve any vehicle startup errors to resolve the issue. "
                              "If you are using standard firmware, you may need to upgrade to a newer version to resolve the issue.");
Don Gagne's avatar
Don Gagne committed
991 992 993 994
        if (!qgcApp()->runningUnitTests()) {
            qCWarning(ParameterLoaderLog) << "The following parameter indices could not be loaded after the maximum number of retries: " << indexList;
        }
        emit parametersReady(true /* missingParameters */);
995
        return;
996
    }
997 998 999 1000 1001 1002 1003

    // Check for missing default component when we should have one
    if (_defaultComponentId == FactSystem::defaultComponentId && !_defaultComponentIdParam.isEmpty()) {
        qgcApp()->showMessage("QGroundControl did not receive parameters from the default component. "
                              "This will cause QGroundControl to be unable to display its full user interface. "
                              "If you are using modified firmware, you may need to resolve any vehicle startup errors to resolve the issue. "
                              "If you are using standard firmware, you may need to upgrade to a newer version to resolve the issue.");
Don Gagne's avatar
Don Gagne committed
1004 1005 1006 1007
        if (!qgcApp()->runningUnitTests()) {
            qCWarning(ParameterLoaderLog) << "Default component was never found, param:" << _defaultComponentIdParam;
        }
        emit parametersReady(true /* missingParameters */);
1008 1009 1010 1011 1012 1013
        return;
    }

    // No failures, signal good load
    _parametersReady = true;
    _determineDefaultComponentId();
Don Gagne's avatar
Don Gagne committed
1014
    emit parametersReady(false /* no missingParameters */);
1015
}
1016 1017 1018

void ParameterLoader::_initialRequestTimeout(void)
{
1019 1020 1021 1022 1023 1024 1025 1026
    if (!_vehicle->genericFirmware()) {
        // Generic vehicles (like BeBop) may not have any parameters, so don't annoy the user
        qgcApp()->showMessage("Vehicle did not respond to request for parameters, retrying");
    }
    if (++_initialRequestRetryCount <= _maxInitialRequestListRetry) {
        refreshAllParameters();
        _initialRequestTimeoutTimer.start();
    }
1027
}
1028 1029 1030 1031 1032 1033 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 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165

QString ParameterLoader::parameterMetaDataFile(MAV_AUTOPILOT firmwareType, int wantedMajorVersion, int& majorVersion, int& minorVersion)
{
    bool            cacheHit = false;
    FirmwarePlugin* plugin = qgcApp()->toolbox()->firmwarePluginManager()->firmwarePluginForAutopilot(firmwareType, MAV_TYPE_QUADROTOR);

    // Cached files are stored in settings location
    QSettings settings;
    QDir cacheDir = QFileInfo(settings.fileName()).dir();

    // First look for a direct cache hit
    int cacheMinorVersion, cacheMajorVersion;
    QFile cacheFile(cacheDir.filePath(QString("%1.%2.%3.xml").arg(_cachedMetaDataFilePrefix).arg(firmwareType).arg(wantedMajorVersion)));
    if (cacheFile.exists()) {
        plugin->getParameterMetaDataVersionInfo(cacheFile.fileName(), cacheMajorVersion, cacheMinorVersion);
        if (wantedMajorVersion != cacheMajorVersion) {
            qWarning() << "Parameter meta data cache corruption:" << cacheFile.fileName() << "major version does not match file name" << "actual:excepted" << cacheMajorVersion << wantedMajorVersion;
        } else {
            qCDebug(ParameterLoaderLog) << "Direct cache hit on file:major:minor" << cacheFile.fileName() << cacheMajorVersion << cacheMinorVersion;
            cacheHit = true;
        }
    }

    if (!cacheHit) {
        // No direct hit, look for lower param set version
        QString wildcard = QString("%1.%2.*.xml").arg(_cachedMetaDataFilePrefix).arg(firmwareType);
        QStringList cacheHits = cacheDir.entryList(QStringList(wildcard), QDir::Files, QDir::Name);

        // Find the highest major version number which is below the vehicles major version number
        int cacheHitIndex = -1;
        cacheMajorVersion = -1;
        QRegExp regExp(QString("%1\\.%2\\.(\\d*)\\.xml").arg(_cachedMetaDataFilePrefix).arg(firmwareType));
        for (int i=0; i< cacheHits.count(); i++) {
            if (regExp.exactMatch(cacheHits[i]) && regExp.captureCount() == 1) {
                int majorVersion = regExp.capturedTexts()[0].toInt();
                if (majorVersion > cacheMajorVersion && majorVersion < wantedMajorVersion) {
                    cacheMajorVersion = majorVersion;
                    cacheHitIndex = i;
                }
            }
        }

        if (cacheHitIndex != -1) {
            // We have a cache hit on a lower major version, read minor version as well
            int majorVersion;
            cacheFile.setFileName(cacheDir.filePath(cacheHits[cacheHitIndex]));
            plugin->getParameterMetaDataVersionInfo(cacheFile.fileName(), majorVersion, cacheMinorVersion);
            if (majorVersion != cacheMajorVersion) {
                qWarning() << "Parameter meta data cache corruption:" << cacheFile.fileName() << "major version does not match file name" << "actual:excepted" << majorVersion << cacheMajorVersion;
                cacheHit = false;
            } else {
                qCDebug(ParameterLoaderLog) << "Indirect cache hit on file:major:minor:want" << cacheFile.fileName() << cacheMajorVersion << cacheMinorVersion << wantedMajorVersion;
                cacheHit = true;
            }
        }
    }

    int internalMinorVersion, internalMajorVersion;
    QString internalMetaDataFile = plugin->internalParameterMetaDataFile();
    plugin->getParameterMetaDataVersionInfo(internalMetaDataFile, internalMajorVersion, internalMinorVersion);
    qCDebug(ParameterLoaderLog) << "Internal meta data file:major:minor" << internalMetaDataFile << internalMajorVersion << internalMinorVersion;
    if (cacheHit) {
        // Cache hit is available, we need to check if internal meta data is a better match, if so use internal version
        if (internalMajorVersion == wantedMajorVersion) {
            if (cacheMajorVersion == wantedMajorVersion) {
                // Both internal and cache are direct hit on major version, Use higher minor version number
                cacheHit = cacheMinorVersion > internalMinorVersion;
            } else {
                // Direct internal hit, but not direct hit in cache, use internal
                cacheHit = false;
            }
        } else {
            if (cacheMajorVersion == wantedMajorVersion) {
                // Direct hit on cache, no direct hit on internal, use cache
                cacheHit = true;
            } else {
                // No direct hit anywhere, use internal
                cacheHit = false;
            }
        }
    }

    QString metaDataFile;
    if (cacheHit && !qgcApp()->runningUnitTests()) {
        majorVersion = cacheMajorVersion;
        minorVersion = cacheMinorVersion;
        metaDataFile = cacheFile.fileName();
    } else {
        majorVersion = internalMajorVersion;
        minorVersion = internalMinorVersion;
        metaDataFile = internalMetaDataFile;
    }
    qCDebug(ParameterLoaderLog) << "ParameterLoader::parameterMetaDataFile file:major:minor" << metaDataFile << majorVersion << minorVersion;

    return metaDataFile;
}

void ParameterLoader::cacheMetaDataFile(const QString& metaDataFile, MAV_AUTOPILOT firmwareType)
{
    FirmwarePlugin* plugin = qgcApp()->toolbox()->firmwarePluginManager()->firmwarePluginForAutopilot(firmwareType, MAV_TYPE_QUADROTOR);

    int newMajorVersion, newMinorVersion;
    plugin->getParameterMetaDataVersionInfo(metaDataFile, newMajorVersion, newMinorVersion);
    qCDebug(ParameterLoaderLog) << "ParameterLoader::cacheMetaDataFile file:firmware:major;minor" << metaDataFile << firmwareType << newMajorVersion << newMinorVersion;

    // Find the cache hit closest to this new file
    int cacheMajorVersion, cacheMinorVersion;
    QString cacheHit = ParameterLoader::parameterMetaDataFile(firmwareType, newMajorVersion, cacheMajorVersion, cacheMinorVersion);
    qCDebug(ParameterLoaderLog) << "ParameterLoader::cacheMetaDataFile cacheHit file:firmware:major;minor" << cacheHit << cacheMajorVersion << cacheMinorVersion;

    bool cacheNewFile = false;
    if (cacheHit.isEmpty()) {
        // No cache hits, store the new file
        cacheNewFile = true;
    } else if (cacheMajorVersion == newMajorVersion) {
        // Direct hit on major version in cache:
        //      Cache new file if newer minor version
        //      Also delete older cache file
        if (newMinorVersion > cacheMinorVersion) {
            cacheNewFile = true;
            QFile::remove(cacheHit);
        }
    } else {
        // Indirect hit in cache, store new file
        cacheNewFile = true;
    }

    if (cacheNewFile) {
        // Cached files are stored in settings location. Copy from current file to cache naming.

        QSettings settings;
        QDir cacheDir = QFileInfo(settings.fileName()).dir();
        QFile cacheFile(cacheDir.filePath(QString("%1.%2.%3.xml").arg(_cachedMetaDataFilePrefix).arg(firmwareType).arg(newMajorVersion)));
        qCDebug(ParameterLoaderLog) << "ParameterLoader::cacheMetaDataFile caching file:" << cacheFile.fileName();
        QFile newFile(metaDataFile);
        newFile.copy(cacheFile.fileName());
    }
}
1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198

/// Remap a parameter from one firmware version to another
QString ParameterLoader::_remapParamNameToVersion(const QString& paramName)
{
    QString mappedParamName;

    if (!paramName.startsWith(QStringLiteral("r."))) {
        // No version mapping wanted
        return paramName;
    }

    int majorVersion = _vehicle->firmwareMajorVersion();
    int minorVersion = _vehicle->firmwareMinorVersion();

    qCDebug(ParameterLoaderLog) << "_remapParamNameToVersion" << paramName << majorVersion << minorVersion;

    mappedParamName = paramName.right(paramName.count() - 2);

    if (majorVersion == Vehicle::versionNotSetValue) {
        // Vehicle version unknown
        return mappedParamName;
    }

    const FirmwarePlugin::remapParamNameMajorVersionMap_t& majorVersionRemap = _vehicle->firmwarePlugin()->paramNameRemapMajorVersionMap();

    if (!majorVersionRemap.contains(majorVersion)) {
        // No mapping for this major version
        qCDebug(ParameterLoaderLog) << "_remapParamNameToVersion: no major version mapping";
        return mappedParamName;
    }

    const FirmwarePlugin::remapParamNameMinorVersionRemapMap_t& remapMinorVersion = majorVersionRemap[majorVersion];

1199
    // We must map backwards from the highest known minor version to one above the vehicle's minor version
1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214

    for (int currentMinorVersion=_vehicle->firmwarePlugin()->remapParamNameHigestMinorVersionNumber(majorVersion); currentMinorVersion>minorVersion; currentMinorVersion--) {
        if (remapMinorVersion.contains(currentMinorVersion)) {
            const FirmwarePlugin::remapParamNameMap_t& remap = remapMinorVersion[currentMinorVersion];

            if (remap.contains(mappedParamName)) {
                QString toParamName = remap[mappedParamName];
                qCDebug(ParameterLoaderLog) << "_remapParamNameToVersion: remapped currentMinor:from:to"<< currentMinorVersion << mappedParamName << toParamName;
                mappedParamName = toParamName;
            }
        }
    }

    return mappedParamName;
}