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

10
#include "ParameterManager.h"
11 12
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
13
#include "QGCApplication.h"
14
#include "UASMessageHandler.h"
Don Gagne's avatar
Don Gagne committed
15
#include "FirmwarePlugin.h"
16
#include "UAS.h"
17
#include "JsonHelper.h"
18

19
#include <QEasingCurve>
20 21
#include <QFile>
#include <QDebug>
22
#include <QVariantAnimation>
23
#include <QJsonArray>
24

25 26 27
QGC_LOGGING_CATEGORY(ParameterManagerVerbose1Log,           "ParameterManagerVerbose1Log")
QGC_LOGGING_CATEGORY(ParameterManagerVerbose2Log,           "ParameterManagerVerbose2Log")
QGC_LOGGING_CATEGORY(ParameterManagerDebugCacheFailureLog,  "ParameterManagerDebugCacheFailureLog") // Turn on to debug parameter cache crc misses
28

29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53
const QHash<int, QString> _mavlinkCompIdHash {
    { MAV_COMP_ID_CAMERA,   "Camera1" },
    { MAV_COMP_ID_CAMERA2,  "Camera2" },
    { MAV_COMP_ID_CAMERA3,  "Camera3" },
    { MAV_COMP_ID_CAMERA4,  "Camera4" },
    { MAV_COMP_ID_CAMERA5,  "Camera5" },
    { MAV_COMP_ID_CAMERA6,  "Camera6" },
    { MAV_COMP_ID_SERVO1,   "Servo1" },
    { MAV_COMP_ID_SERVO2,   "Servo2" },
    { MAV_COMP_ID_SERVO3,   "Servo3" },
    { MAV_COMP_ID_SERVO4,   "Servo4" },
    { MAV_COMP_ID_SERVO5,   "Servo5" },
    { MAV_COMP_ID_SERVO6,   "Servo6" },
    { MAV_COMP_ID_SERVO7,   "Servo7" },
    { MAV_COMP_ID_SERVO8,   "Servo8" },
    { MAV_COMP_ID_SERVO9,   "Servo9" },
    { MAV_COMP_ID_SERVO10,  "Servo10" },
    { MAV_COMP_ID_SERVO11,  "Servo11" },
    { MAV_COMP_ID_SERVO12,  "Servo12" },
    { MAV_COMP_ID_SERVO13,  "Servo13" },
    { MAV_COMP_ID_SERVO14,  "Servo14" },
    { MAV_COMP_ID_GIMBAL,   "Gimbal1" },
    { MAV_COMP_ID_ADSB,     "ADSB" },
    { MAV_COMP_ID_OSD,      "OSD" },
    { MAV_COMP_ID_FLARM,    "FLARM" },
54 55 56 57 58
    { MAV_COMP_ID_GIMBAL2,  "Gimbal2" },
    { MAV_COMP_ID_GIMBAL3,  "Gimbal3" },
    { MAV_COMP_ID_GIMBAL4,  "Gimbal4" },
    { MAV_COMP_ID_GIMBAL5,  "Gimbal5" },
    { MAV_COMP_ID_GIMBAL6,  "Gimbal6" },
59 60 61 62 63 64 65
    { MAV_COMP_ID_IMU,      "IMU1" },
    { MAV_COMP_ID_IMU_2,    "IMU2" },
    { MAV_COMP_ID_IMU_3,    "IMU3" },
    { MAV_COMP_ID_GPS,      "GPS1" },
    { MAV_COMP_ID_GPS2,     "GPS2" }
};

66 67 68 69 70
const char* ParameterManager::_cachedMetaDataFilePrefix =   "ParameterFactMetaData";
const char* ParameterManager::_jsonParametersKey =          "parameters";
const char* ParameterManager::_jsonCompIdKey =              "compId";
const char* ParameterManager::_jsonParamNameKey =           "name";
const char* ParameterManager::_jsonParamValueKey =          "value";
71

72
ParameterManager::ParameterManager(Vehicle* vehicle)
73 74
    : QObject                           (vehicle)
    , _vehicle                          (vehicle)
75
    , _mavlink                          (nullptr)
76 77 78 79 80 81 82 83 84
    , _loadProgress                     (0.0)
    , _parametersReady                  (false)
    , _missingParameters                (false)
    , _initialLoadComplete              (false)
    , _waitingForDefaultComponent       (false)
    , _saveRequired                     (false)
    , _metaDataAddedToFacts             (false)
    , _logReplay                        (vehicle->priorityLink() && vehicle->priorityLink()->isLogReplay())
    , _parameterSetMajorVersion         (-1)
85
    , _parameterMetaData                (nullptr)
86 87 88 89 90 91 92
    , _prevWaitingReadParamIndexCount   (0)
    , _prevWaitingReadParamNameCount    (0)
    , _prevWaitingWriteParamNameCount   (0)
    , _initialRequestRetryCount         (0)
    , _disableAllRetries                (false)
    , _indexBatchQueueActive            (false)
    , _totalParamCount                  (0)
93
{
94 95
    _versionParam = vehicle->firmwarePlugin()->getVersionParam();

96 97 98 99 100 101
    if (_vehicle->isOfflineEditingVehicle()) {
        _loadOfflineEditingParams();
        return;
    }

    _mavlink = qgcApp()->toolbox()->mavlinkProtocol();
dogmaphobic's avatar
dogmaphobic committed
102

103
    _initialRequestTimeoutTimer.setSingleShot(true);
104
    _initialRequestTimeoutTimer.setInterval(5000);
105
    connect(&_initialRequestTimeoutTimer, &QTimer::timeout, this, &ParameterManager::_initialRequestTimeout);
106

107
    _waitingParamTimeoutTimer.setSingleShot(true);
108
    _waitingParamTimeoutTimer.setInterval(3000);
109
    connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterManager::_waitingParamTimeout);
110

111
    connect(_vehicle->uas(), &UASInterface::parameterUpdate, this, &ParameterManager::_parameterUpdate);
112

113 114
    // Ensure the cache directory exists
    QFileInfo(QSettings().fileName()).dir().mkdir("ParamCache");
Don Gagne's avatar
Don Gagne committed
115

116 117 118 119 120 121 122 123
    if (_vehicle->highLatencyLink()) {
        // High latency links don't load parameters
        _parametersReady = true;
        _missingParameters = true;
        _initialLoadComplete = true;
        _waitingForDefaultComponent = false;
        emit parametersReadyChanged(_parametersReady);
        emit missingParametersChanged(_missingParameters);
124
    } else if (!_logReplay){
125 126
        refreshAllParameters();
    }
127 128
}

129
ParameterManager::~ParameterManager()
130
{
131
    delete _parameterMetaData;
132 133
}

134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190
void ParameterManager::_updateProgressBar(void)
{
    int waitingReadParamIndexCount = 0;
    int waitingReadParamNameCount = 0;
    int waitingWriteParamCount = 0;

    for (int compId: _waitingReadParamIndexMap.keys()) {
        waitingReadParamIndexCount += _waitingReadParamIndexMap[compId].count();
    }
    for(int compId: _waitingReadParamNameMap.keys()) {
        waitingReadParamNameCount += _waitingReadParamNameMap[compId].count();
    }
    for(int compId: _waitingWriteParamNameMap.keys()) {
        waitingWriteParamCount += _waitingWriteParamNameMap[compId].count();
    }

    if (waitingReadParamIndexCount == 0) {
        if (_readParamIndexProgressActive) {
            _readParamIndexProgressActive = false;
            _setLoadProgress(0.0);
            return;
        }
    } else {
        _readParamIndexProgressActive = true;
        _setLoadProgress((double)(_totalParamCount - waitingReadParamIndexCount) / (double)_totalParamCount);
        return;
    }

    if (waitingWriteParamCount == 0) {
        if (_writeParamProgressActive) {
            _writeParamProgressActive = false;
            _waitingWriteParamBatchCount = 0;
            _setLoadProgress(0.0);
            emit pendingWritesChanged(false);
            return;
        }
    } else {
        _writeParamProgressActive = true;
        _setLoadProgress((double)(qMax(_waitingWriteParamBatchCount - waitingWriteParamCount, 1)) / (double)(_waitingWriteParamBatchCount + 1));
        emit pendingWritesChanged(true);
        return;
    }

    if (waitingReadParamNameCount == 0) {
        if (_readParamNameProgressActive) {
            _readParamNameProgressActive = false;
            _waitingReadParamNameBatchCount = 0;
            _setLoadProgress(0.0);
            return;
        }
    } else {
        _readParamNameProgressActive = true;
        _setLoadProgress((double)(qMax(_waitingReadParamNameBatchCount - waitingReadParamNameCount, 1)) / (double)(_waitingReadParamNameBatchCount + 1));
        return;
    }
}

191
/// Called whenever a parameter is updated or first seen.
192
void ParameterManager::_parameterUpdate(int vehicleId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value)
193 194
{
    // Is this for our uas?
195
    if (vehicleId != _vehicle->id()) {
196 197
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
198

199 200 201 202 203 204 205 206
    qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
                                            "_parameterUpdate" <<
                                            "name:" << parameterName <<
                                            "count:" << parameterCount <<
                                            "index:" << parameterId <<
                                            "mavType:" << mavType <<
                                            "value:" << value <<
                                            ")";
207

208 209
    // ArduPilot has this strange behavior of streaming parameters that we didn't ask for. This even happens before it responds to the
    // PARAM_REQUEST_LIST. We disregard any of this until the initial request is responded to.
210
    if (parameterId == 65535 && parameterName != "_HASH_CHECK" && _initialRequestTimeoutTimer.isActive()) {
Patrick José Pereira's avatar
Patrick José Pereira committed
211
        qCDebug(ParameterManagerVerbose1Log) << "Disregarding unrequested param prior to initial list response" << parameterName;
212 213 214
        return;
    }

215
    _initialRequestTimeoutTimer.stop();
dogmaphobic's avatar
dogmaphobic committed
216

217
#if 0
218 219 220 221 222 223 224
    if (!_initialLoadComplete && !_indexBatchQueueActive) {
        // Handy for testing retry logic
        static int counter = 0;
        if (counter++ & 0x8) {
            qCDebug(ParameterManagerLog) << "Artificial discard" << counter;
            return;
        }
225 226
    }
#endif
227

228 229 230 231 232 233 234
#if 0
    // Use this to test missing default component id
    if (componentId == 50) {
        return;
    }
#endif

235 236 237 238 239 240
    if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") {
        if (!_initialLoadComplete && !_logReplay) {
            /* we received a cache hash, potentially load from cache */
            _tryCacheHashLoad(vehicleId, componentId, value);
        }
        return;
241
    }
242

243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260
    // Used to debug cache crc misses (turn on ParameterManagerDebugCacheFailureLog)
    if (!_initialLoadComplete && !_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) {
        if (_debugCacheMap[componentId].contains(parameterName)) {
            const ParamTypeVal& cacheParamTypeVal = _debugCacheMap[componentId][parameterName];
            size_t dataSize = FactMetaData::typeToSize(static_cast<FactMetaData::ValueType_t>(cacheParamTypeVal.first));
            const void *cacheData = cacheParamTypeVal.second.constData();

            const void *vehicleData = value.constData();

            if (memcmp(cacheData, vehicleData, dataSize)) {
                qDebug() << "Cache/Vehicle values differ for name:cache:actual" << parameterName << value << cacheParamTypeVal.second;
            }
            _debugCacheParamSeen[componentId][parameterName] = true;
        } else {
            qDebug() << "Parameter missing from cache" << parameterName;
        }
    }

261
    _initialRequestTimeoutTimer.stop();
262
    _waitingParamTimeoutTimer.stop();
263

264
    _dataMutex.lock();
dogmaphobic's avatar
dogmaphobic committed
265

266 267 268 269 270
    // Update our total parameter counts
    if (!_paramCountMap.contains(componentId)) {
        _paramCountMap[componentId] = parameterCount;
        _totalParamCount += parameterCount;
    }
271

272 273
    // If we've never seen this component id before, setup the wait lists.
    if (!_waitingReadParamIndexMap.contains(componentId)) {
274 275 276 277
        // 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;
278
        }
dogmaphobic's avatar
dogmaphobic committed
279

280 281 282
        // 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
283

284
        qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Seeing component for first time - paramcount:" << parameterCount;
285
    }
dogmaphobic's avatar
dogmaphobic committed
286

287 288 289 290 291 292
    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;
    }

293
    if (!_waitingReadParamIndexMap[componentId].contains(parameterId) &&
294 295
            !_waitingReadParamNameMap[componentId].contains(parameterName) &&
            !_waitingWriteParamNameMap[componentId].contains(parameterName)) {
296
        qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "Unrequested param update" << parameterName;
297 298
    }

299
    // Remove this parameter from the waiting lists
300 301 302 303 304
    if (_waitingReadParamIndexMap[componentId].contains(parameterId)) {
        _waitingReadParamIndexMap[componentId].remove(parameterId);
        _indexBatchQueue.removeOne(parameterId);
        _fillIndexBatchQueue(false /* waitingParamTimeout */);
    }
305 306
    _waitingReadParamNameMap[componentId].remove(parameterName);
    _waitingWriteParamNameMap[componentId].remove(parameterName);
307 308 309 310 311 312 313 314 315
    if (_waitingReadParamIndexMap[componentId].count()) {
        qCDebug(ParameterManagerVerbose2Log) << _logVehiclePrefix(componentId) << "_waitingReadParamIndexMap:" << _waitingReadParamIndexMap[componentId];
    }
    if (_waitingReadParamNameMap[componentId].count()) {
        qCDebug(ParameterManagerVerbose2Log) << _logVehiclePrefix(componentId) << "_waitingReadParamNameMap" << _waitingReadParamNameMap[componentId];
    }
    if (_waitingWriteParamNameMap[componentId].count()) {
        qCDebug(ParameterManagerVerbose2Log) << _logVehiclePrefix(componentId) << "_waitingWriteParamNameMap" << _waitingWriteParamNameMap[componentId];
    }
316 317

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

319 320 321
    int waitingReadParamIndexCount = 0;
    int waitingReadParamNameCount = 0;
    int waitingWriteParamNameCount = 0;
dogmaphobic's avatar
dogmaphobic committed
322

323
    for(int waitingComponentId: _waitingReadParamIndexMap.keys()) {
324 325 326
        waitingReadParamIndexCount += _waitingReadParamIndexMap[waitingComponentId].count();
    }
    if (waitingReadParamIndexCount) {
327
        qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "waitingReadParamIndexCount:" << waitingReadParamIndexCount;
328 329
    }

330
    for(int waitingComponentId: _waitingReadParamNameMap.keys()) {
331 332 333
        waitingReadParamNameCount += _waitingReadParamNameMap[waitingComponentId].count();
    }
    if (waitingReadParamNameCount) {
334
        qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "waitingReadParamNameCount:" << waitingReadParamNameCount;
335
    }
dogmaphobic's avatar
dogmaphobic committed
336

337
    for(int waitingComponentId: _waitingWriteParamNameMap.keys()) {
338 339 340
        waitingWriteParamNameCount += _waitingWriteParamNameMap[waitingComponentId].count();
    }
    if (waitingWriteParamNameCount) {
341
        qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "waitingWriteParamNameCount:" << waitingWriteParamNameCount;
342
    }
dogmaphobic's avatar
dogmaphobic committed
343

Don Gagne's avatar
Don Gagne committed
344 345 346
    int readWaitingParamCount = waitingReadParamIndexCount + waitingReadParamNameCount;
    int totalWaitingParamCount = readWaitingParamCount + waitingWriteParamNameCount;
    if (totalWaitingParamCount) {
347 348
        // More params to wait for, restart timer
        _waitingParamTimeoutTimer.start();
349
        qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer: totalWaitingParamCount:" << totalWaitingParamCount;
350
    } else {
351 352
        if (!_mapParameterName2Variant.contains(_vehicle->defaultComponentId())) {
            // Still waiting for parameters from default component
353
            qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer (still waiting for default component params)";
354 355
            _waitingParamTimeoutTimer.start();
        } else {
356
            qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(-1) << "Not restarting _waitingParamTimeoutTimer (all requests satisfied)";
357
        }
358
    }
359 360
\
    _updateProgressBar();
dogmaphobic's avatar
dogmaphobic committed
361

362 363 364 365 366
    // Get parameter set version
    if (!_versionParam.isEmpty() && _versionParam == parameterName) {
        _parameterSetMajorVersion = value.toInt();
    }

367
    if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(parameterName)) {
368
        qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "Adding new fact" << parameterName;
dogmaphobic's avatar
dogmaphobic committed
369

370 371
        FactMetaData::ValueType_t factType;
        switch (mavType) {
372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389
        case MAV_PARAM_TYPE_UINT8:
            factType = FactMetaData::valueTypeUint8;
            break;
        case MAV_PARAM_TYPE_INT8:
            factType = FactMetaData::valueTypeInt8;
            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;
390 391 392 393 394 395
        case MAV_PARAM_TYPE_UINT64:
            factType = FactMetaData::valueTypeUint64;
            break;
        case MAV_PARAM_TYPE_INT64:
            factType = FactMetaData::valueTypeInt64;
            break;
396 397 398 399 400 401 402 403 404 405
        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;
406
        }
dogmaphobic's avatar
dogmaphobic committed
407

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

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

412
        // We need to know when the fact changes from QML so that we can send the new value to the parameter manager
413
        connect(fact, &Fact::_containerRawValueChanged, this, &ParameterManager::_valueUpdated);
414
    }
dogmaphobic's avatar
dogmaphobic committed
415

Don Gagne's avatar
Don Gagne committed
416 417
    _dataMutex.unlock();

418
    Fact* fact = nullptr;
DonLakeFlyer's avatar
DonLakeFlyer committed
419 420 421 422 423 424 425 426
    if (_mapParameterName2Variant[componentId].contains(parameterName)) {
        fact = _mapParameterName2Variant[componentId][parameterName].value<Fact*>();
    }
    if (fact) {
        fact->_containerSetRawValue(value);
    } else {
        qWarning() << "Internal error";
    }
dogmaphobic's avatar
dogmaphobic committed
427

428
    if (componentParamsComplete) {
429
        if (componentId == _vehicle->defaultComponentId()) {
430 431 432
            // Add meta data to default component. We need to do this before we setup the group map since group
            // map requires meta data.
            _addMetaDataToDefaultComponent();
433
        }
434 435 436 437
        // 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.
        _setupComponentCategoryMap(componentId);
438 439
    }

Don Gagne's avatar
Don Gagne committed
440 441 442
    // 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.
443
    if (!_logReplay && _vehicle->px4Firmware()) {
Don Gagne's avatar
Don Gagne committed
444 445
        if (_prevWaitingReadParamIndexCount + _prevWaitingReadParamNameCount != 0 && readWaitingParamCount == 0) {
            // All reads just finished, update the cache
446
            _writeLocalParamCache(vehicleId, componentId);
Don Gagne's avatar
Don Gagne committed
447 448 449 450 451 452 453
        }
    }

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

454
    _checkInitialLoadComplete();
455 456

    qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "_parameterUpdate complete";
457 458 459 460
}

/// Connected to Fact::valueUpdated
///
461
/// Writes the parameter to mavlink, sets up for write wait
462
void ParameterManager::_valueUpdated(const QVariant& value)
463 464
{
    Fact* fact = qobject_cast<Fact*>(sender());
DonLakeFlyer's avatar
DonLakeFlyer committed
465 466 467 468
    if (!fact) {
        qWarning() << "Internal error";
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
469

470
    int componentId = fact->componentId();
471
    QString name = fact->name();
dogmaphobic's avatar
dogmaphobic committed
472

473
    _dataMutex.lock();
dogmaphobic's avatar
dogmaphobic committed
474

DonLakeFlyer's avatar
DonLakeFlyer committed
475
    if (_waitingWriteParamNameMap.contains(componentId)) {
476 477 478 479 480 481 482
        if (_waitingWriteParamNameMap[componentId].contains(name)) {
            _waitingWriteParamNameMap[componentId].remove(name);
        } else {
            _waitingWriteParamBatchCount++;
        }
        _waitingWriteParamNameMap[componentId][name] = 0; // Add new entry and set retry count
        _updateProgressBar();
DonLakeFlyer's avatar
DonLakeFlyer committed
483 484 485 486 487
        _waitingParamTimeoutTimer.start();
        _saveRequired = true;
    } else {
        qWarning() << "Internal error";
    }
dogmaphobic's avatar
dogmaphobic committed
488

489
    _dataMutex.unlock();
dogmaphobic's avatar
dogmaphobic committed
490

491
    _writeParameterRaw(componentId, fact->name(), value);
492
    qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Set parameter - name:" << name << value << "(_waitingParamTimeoutTimer started)";
493 494
}

495
void ParameterManager::refreshAllParameters(uint8_t componentId)
496
{
Don Gagne's avatar
Don Gagne committed
497 498 499 500
    if (_logReplay) {
        return;
    }

501
    _dataMutex.lock();
dogmaphobic's avatar
dogmaphobic committed
502

503 504 505 506
    if (!_initialLoadComplete) {
        _initialRequestTimeoutTimer.start();
    }

507
    // Reset index wait lists
508
    for (int cid: _paramCountMap.keys()) {
509
        // Add/Update all indices to the wait list, parameter index is 0-based
510
        if(componentId != MAV_COMP_ID_ALL && componentId != cid)
dogmaphobic's avatar
dogmaphobic committed
511 512
            continue;
        for (int waitingIndex = 0; waitingIndex < _paramCountMap[cid]; waitingIndex++) {
513
            // This will add a new waiting index if needed and set the retry count for that index to 0
dogmaphobic's avatar
dogmaphobic committed
514
            _waitingReadParamIndexMap[cid][waitingIndex] = 0;
515 516
        }
    }
dogmaphobic's avatar
dogmaphobic committed
517

518
    _dataMutex.unlock();
dogmaphobic's avatar
dogmaphobic committed
519

520
    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
dogmaphobic's avatar
dogmaphobic committed
521

522
    mavlink_message_t msg;
523 524 525 526 527
    mavlink_msg_param_request_list_pack_chan(mavlink->getSystemId(),
                                             mavlink->getComponentId(),
                                             _vehicle->priorityLink()->mavlinkChannel(),
                                             &msg,
                                             _vehicle->id(),
528
                                             componentId);
529
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
dogmaphobic's avatar
dogmaphobic committed
530

531
    QString what = (componentId == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentId);
532
    qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Request to refresh all parameters for component ID:" << what;
533 534 535
}

/// Translates FactSystem::defaultComponentId to real component id if needed
536
int ParameterManager::_actualComponentId(int componentId)
537 538
{
    if (componentId == FactSystem::defaultComponentId) {
539
        componentId = _vehicle->defaultComponentId();
Don Gagne's avatar
Don Gagne committed
540
        if (componentId == FactSystem::defaultComponentId) {
541
            qWarning() << _logVehiclePrefix(-1) << "Default component id not set";
Don Gagne's avatar
Don Gagne committed
542
        }
543
    }
dogmaphobic's avatar
dogmaphobic committed
544

545 546 547
    return componentId;
}

548
void ParameterManager::refreshParameter(int componentId, const QString& paramName)
549
{
550
    componentId = _actualComponentId(componentId);
551
    qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParameter - name:" << paramName << ")";
dogmaphobic's avatar
dogmaphobic committed
552

553 554 555
    _dataMutex.lock();

    if (_waitingReadParamNameMap.contains(componentId)) {
556
        QString mappedParamName = _remapParamNameToVersion(paramName);
557

558 559 560 561 562
        if (_waitingReadParamNameMap[componentId].contains(mappedParamName)) {
            _waitingReadParamNameMap[componentId].remove(mappedParamName);
        } else {
            _waitingReadParamNameBatchCount++;
        }
563
        _waitingReadParamNameMap[componentId][mappedParamName] = 0;     // Add new wait entry and update retry count
564
        _updateProgressBar();
565 566
        qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "restarting _waitingParamTimeout";
        _waitingParamTimeoutTimer.start();
DonLakeFlyer's avatar
DonLakeFlyer committed
567 568
    } else {
        qWarning() << "Internal error";
569
    }
dogmaphobic's avatar
dogmaphobic committed
570

571 572
    _dataMutex.unlock();

573
    _readParameterRaw(componentId, paramName, -1);
574 575
}

576
void ParameterManager::refreshParametersPrefix(int componentId, const QString& namePrefix)
577 578
{
    componentId = _actualComponentId(componentId);
579
    qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParametersPrefix - name:" << namePrefix << ")";
580

581 582 583
    for(const QString &paramName: _mapParameterName2Variant[componentId].keys()) {
        if (paramName.startsWith(namePrefix)) {
            refreshParameter(componentId, paramName);
584 585 586 587
        }
    }
}

588
bool ParameterManager::parameterExists(int componentId, const QString& paramName)
589
{
590
    bool ret = false;
dogmaphobic's avatar
dogmaphobic committed
591

592 593
    componentId = _actualComponentId(componentId);
    if (_mapParameterName2Variant.contains(componentId)) {
594
        ret = _mapParameterName2Variant[componentId].contains(_remapParamNameToVersion(paramName));
595
    }
596 597

    return ret;
598 599
}

600
Fact* ParameterManager::getParameter(int componentId, const QString& paramName)
601 602
{
    componentId = _actualComponentId(componentId);
dogmaphobic's avatar
dogmaphobic committed
603

604
    QString mappedParamName = _remapParamNameToVersion(paramName);
605 606
    if (!_mapParameterName2Variant.contains(componentId) || !_mapParameterName2Variant[componentId].contains(mappedParamName)) {
        qgcApp()->reportMissingParameter(componentId, mappedParamName);
Don Gagne's avatar
Don Gagne committed
607
        return &_defaultFact;
608
    }
dogmaphobic's avatar
dogmaphobic committed
609

610
    return _mapParameterName2Variant[componentId][mappedParamName].value<Fact*>();
611
}
612

613
QStringList ParameterManager::parameterNames(int componentId)
614
{
dogmaphobic's avatar
dogmaphobic committed
615 616
    QStringList names;

617
    for(const QString &paramName: _mapParameterName2Variant[_actualComponentId(componentId)].keys()) {
dogmaphobic's avatar
dogmaphobic committed
618 619 620 621
        names << paramName;
    }

    return names;
622 623
}

624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643
void ParameterManager::_setupComponentCategoryMap(int componentId)
{
    if (componentId == _vehicle->defaultComponentId()) {
        _setupDefaultComponentCategoryMap();
        return;
    }

    ComponentCategoryMapType& componentCategoryMap = _componentCategoryMaps[componentId];

    QString category = getComponentCategory(componentId);

    // Must be able to handle being called multiple times
    componentCategoryMap.clear();

    // Fill parameters into the group determined by param name
    for (const QString &paramName: _mapParameterName2Variant[componentId].keys()) {
        int i = paramName.indexOf("_");
        if (i > 0) {
            componentCategoryMap[category][paramName.left(i)] += paramName;
        } else {
644
            componentCategoryMap[category][tr("Misc")] += paramName;
645 646 647 648 649 650 651 652 653
        }
    }

    // Memorize category for component ID
    if (!_componentCategoryHash.contains(category)) {
        _componentCategoryHash.insert(category, componentId);
    }
}

Don Gagne's avatar
Don Gagne committed
654
void ParameterManager::_setupDefaultComponentCategoryMap(void)
655
{
656 657
    ComponentCategoryMapType& defaultComponentCategoryMap = _componentCategoryMaps[_vehicle->defaultComponentId()];

658
    // Must be able to handle being called multiple times
659 660 661 662 663 664 665
    defaultComponentCategoryMap.clear();

    for (const QString &paramName: _mapParameterName2Variant[_vehicle->defaultComponentId()].keys()) {
        Fact* fact = _mapParameterName2Variant[_vehicle->defaultComponentId()][paramName].value<Fact*>();
        defaultComponentCategoryMap[fact->category()][fact->group()] += paramName;
    }
}
666

667 668 669
QString ParameterManager::getComponentCategory(int componentId)
{
    if (_mavlinkCompIdHash.contains(componentId)) {
670
        return tr("Component %1  (%2)").arg(_mavlinkCompIdHash.value(componentId)).arg(componentId);
671
    }
672 673 674 675 676 677 678
    QString componentCategoryPrefix = tr("Component ");
    return QString("%1%2").arg(componentCategoryPrefix).arg(componentId);
}

const QMap<QString, QMap<QString, QStringList> >& ParameterManager::getComponentCategoryMap(int componentId)
{
    return _componentCategoryMaps[componentId];
679 680
}

681
int  ParameterManager::getComponentId(const QString& category)
682
{
683
    return (_componentCategoryHash.contains(category)) ? _componentCategoryHash.value(category) : _vehicle->defaultComponentId();
684
}
685

686 687 688 689
/// Requests missing index based parameters from the vehicle.
///     @param waitingParamTimeout: true: being called due to timeout, false: being called to re-fill the batch queue
/// return true: Parameters were requested, false: No more requests needed
bool ParameterManager::_fillIndexBatchQueue(bool waitingParamTimeout)
690
{
691 692 693
    if (!_indexBatchQueueActive) {
        return false;
    }
dogmaphobic's avatar
dogmaphobic committed
694

695
    const int maxBatchSize = 10;
696

697 698 699 700 701 702 703
    if (waitingParamTimeout) {
        // We timed out, clear the queue and try again
        qCDebug(ParameterManagerLog) << "Refilling index based batch queue due to timeout";
        _indexBatchQueue.clear();
    } else {
        qCDebug(ParameterManagerLog) << "Refilling index based batch queue due to received parameter";
    }
704

705
    for(int componentId: _waitingReadParamIndexMap.keys()) {
706
        if (_waitingReadParamIndexMap[componentId].count()) {
707 708
            qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "_waitingReadParamIndexMap count" << _waitingReadParamIndexMap[componentId].count();
            qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "_waitingReadParamIndexMap" << _waitingReadParamIndexMap[componentId];
709 710
        }

711
        for(int paramIndex: _waitingReadParamIndexMap[componentId].keys()) {
712 713 714 715 716 717 718
            if (_indexBatchQueue.contains(paramIndex)) {
                // Don't add more than once
                continue;
            }

            if (_indexBatchQueue.count() > maxBatchSize) {
                break;
719 720
            }

721
            _waitingReadParamIndexMap[componentId][paramIndex]++;   // Bump retry count
722
            if (_disableAllRetries || _waitingReadParamIndexMap[componentId][paramIndex] > _maxInitialLoadRetrySingleParam) {
723 724
                // Give up on this index
                _failedReadParamIndexMap[componentId] << paramIndex;
725
                qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Giving up on (paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
726 727 728
                _waitingReadParamIndexMap[componentId].remove(paramIndex);
            } else {
                // Retry again
729
                _indexBatchQueue.append(paramIndex);
730
                _readParameterRaw(componentId, "", paramIndex);
731
                qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Read re-request for (paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
732 733 734
            }
        }
    }
735

736 737 738 739 740
    return _indexBatchQueue.count() != 0;
}

void ParameterManager::_waitingParamTimeout(void)
{
741 742 743 744
    if (_logReplay) {
        return;
    }

745 746 747 748
    bool paramsRequested = false;
    const int maxBatchSize = 10;
    int batchCount = 0;

749
    qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "_waitingParamTimeout";
750 751 752 753 754 755 756

    // Now that we have timed out for possibly the first time we can activate the index batch queue
    _indexBatchQueueActive = true;

    // First check for any missing parameters from the initial index based load
    paramsRequested = _fillIndexBatchQueue(true /* waitingParamTimeout */);

757 758 759
    if (!paramsRequested && !_waitingForDefaultComponent && !_mapParameterName2Variant.contains(_vehicle->defaultComponentId())) {
        // Initial load is complete but we still don't have any default component params. Wait one more cycle to see if the
        // any show up.
760
        qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer - still don't have default component params" << _vehicle->defaultComponentId() << _mapParameterName2Variant.keys();
761 762 763 764 765 766
        _waitingParamTimeoutTimer.start();
        _waitingForDefaultComponent = true;
        return;
    }
    _waitingForDefaultComponent = false;

767
    _checkInitialLoadComplete();
dogmaphobic's avatar
dogmaphobic committed
768

769
    if (!paramsRequested) {
770 771
        for(int componentId: _waitingWriteParamNameMap.keys()) {
            for(const QString &paramName: _waitingWriteParamNameMap[componentId].keys()) {
772
                paramsRequested = true;
773
                _waitingWriteParamNameMap[componentId][paramName]++;   // Bump retry count
774
                if (_waitingWriteParamNameMap[componentId][paramName] <= _maxReadWriteRetry) {
775
                    _writeParameterRaw(componentId, paramName, getParameter(componentId, paramName)->rawValue());
776
                    qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Write resend for (paramName:" << paramName << "retryCount:" << _waitingWriteParamNameMap[componentId][paramName] << ")";
777
                    if (++batchCount > maxBatchSize) {
778 779 780 781 782
                        goto Out;
                    }
                } else {
                    // Exceeded max retry count, notify user
                    _waitingWriteParamNameMap[componentId].remove(paramName);
783 784 785
                    QString errorMsg = tr("Parameter write failed: veh:%1 comp:%2 param:%3").arg(_vehicle->id()).arg(componentId).arg(paramName);
                    qCDebug(ParameterManagerLog) << errorMsg;
                    qgcApp()->showMessage(errorMsg);
786 787 788 789
                }
            }
        }
    }
dogmaphobic's avatar
dogmaphobic committed
790

791
    if (!paramsRequested) {
792 793
        for(int componentId: _waitingReadParamNameMap.keys()) {
            for(const QString &paramName: _waitingReadParamNameMap[componentId].keys()) {
794
                paramsRequested = true;
795
                _waitingReadParamNameMap[componentId][paramName]++;   // Bump retry count
796 797
                if (_waitingReadParamNameMap[componentId][paramName] <= _maxReadWriteRetry) {
                    _readParameterRaw(componentId, paramName, -1);
798
                    qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Read re-request for (paramName:" << paramName << "retryCount:" << _waitingReadParamNameMap[componentId][paramName] << ")";
799
                    if (++batchCount > maxBatchSize) {
800 801 802 803 804
                        goto Out;
                    }
                } else {
                    // Exceeded max retry count, notify user
                    _waitingReadParamNameMap[componentId].remove(paramName);
805 806 807
                    QString errorMsg = tr("Parameter read failed: veh:%1 comp:%2 param:%3").arg(_vehicle->id()).arg(componentId).arg(paramName);
                    qCDebug(ParameterManagerLog) << errorMsg;
                    qgcApp()->showMessage(errorMsg);
808 809 810 811
                }
            }
        }
    }
dogmaphobic's avatar
dogmaphobic committed
812

813 814
Out:
    if (paramsRequested) {
815
        qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer - re-request";
816 817 818 819
        _waitingParamTimeoutTimer.start();
    }
}

820
void ParameterManager::_readParameterRaw(int componentId, const QString& paramName, int paramIndex)
821 822 823 824 825
{
    mavlink_message_t msg;
    char fixedParamName[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN];

    strncpy(fixedParamName, paramName.toStdString().c_str(), sizeof(fixedParamName));
826 827 828 829 830 831 832 833
    mavlink_msg_param_request_read_pack_chan(_mavlink->getSystemId(),   // QGC system id
                                             _mavlink->getComponentId(),     // QGC component id
                                             _vehicle->priorityLink()->mavlinkChannel(),
                                             &msg,                           // Pack into this mavlink_message_t
                                             _vehicle->id(),                 // Target system id
                                             componentId,                    // Target component id
                                             fixedParamName,                 // Named parameter being requested
                                             paramIndex);                    // Parameter index being requested, -1 for named
834
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
835 836
}

837
void ParameterManager::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value)
838 839 840
{
    mavlink_param_set_t     p;
    mavlink_param_union_t   union_value;
dogmaphobic's avatar
dogmaphobic committed
841

Don Gagne's avatar
Don Gagne committed
842 843
    memset(&p, 0, sizeof(p));

844
    FactMetaData::ValueType_t factType = getParameter(componentId, paramName)->type();
845
    p.param_type = _factTypeToMavType(factType);
dogmaphobic's avatar
dogmaphobic committed
846

847
    switch (factType) {
848 849 850
    case FactMetaData::valueTypeUint8:
        union_value.param_uint8 = (uint8_t)value.toUInt();
        break;
dogmaphobic's avatar
dogmaphobic committed
851

852 853 854
    case FactMetaData::valueTypeInt8:
        union_value.param_int8 = (int8_t)value.toInt();
        break;
dogmaphobic's avatar
dogmaphobic committed
855

856 857 858
    case FactMetaData::valueTypeUint16:
        union_value.param_uint16 = (uint16_t)value.toUInt();
        break;
dogmaphobic's avatar
dogmaphobic committed
859

860 861 862
    case FactMetaData::valueTypeInt16:
        union_value.param_int16 = (int16_t)value.toInt();
        break;
dogmaphobic's avatar
dogmaphobic committed
863

864 865 866
    case FactMetaData::valueTypeUint32:
        union_value.param_uint32 = (uint32_t)value.toUInt();
        break;
dogmaphobic's avatar
dogmaphobic committed
867

868 869 870
    case FactMetaData::valueTypeFloat:
        union_value.param_float = value.toFloat();
        break;
dogmaphobic's avatar
dogmaphobic committed
871

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

876 877 878
    case FactMetaData::valueTypeInt32:
        union_value.param_int32 = (int32_t)value.toInt();
        break;
879
    }
dogmaphobic's avatar
dogmaphobic committed
880

881
    p.param_value = union_value.param_float;
882
    p.target_system = (uint8_t)_vehicle->id();
883
    p.target_component = (uint8_t)componentId;
dogmaphobic's avatar
dogmaphobic committed
884

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

887
    mavlink_message_t msg;
888 889 890 891 892
    mavlink_msg_param_set_encode_chan(_mavlink->getSystemId(),
                                      _mavlink->getComponentId(),
                                      _vehicle->priorityLink()->mavlinkChannel(),
                                      &msg,
                                      &p);
893
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
894 895
}

896
void ParameterManager::_writeLocalParamCache(int vehicleId, int componentId)
897
{
898
    CacheMapName2ParamTypeVal cacheMap;
899

900 901 902
    for(const QString& paramName: _mapParameterName2Variant[componentId].keys()) {
        const Fact *fact = _mapParameterName2Variant[componentId][paramName].value<Fact*>();
        cacheMap[paramName] = ParamTypeVal(fact->type(), fact->rawValue());
903 904
    }

905 906
    QFile cacheFile(parameterCacheFile(vehicleId, componentId));
    cacheFile.open(QIODevice::WriteOnly | QIODevice::Truncate);
907

908 909
    QDataStream ds(&cacheFile);
    ds << cacheMap;
910 911
}

912
QDir ParameterManager::parameterCacheDir()
913 914 915 916 917
{
    const QString spath(QFileInfo(QSettings().fileName()).dir().absolutePath());
    return spath + QDir::separator() + "ParamCache";
}

918
QString ParameterManager::parameterCacheFile(int vehicleId, int componentId)
919
{
920
    return parameterCacheDir().filePath(QString("%1_%2.v2").arg(vehicleId).arg(componentId));
921 922
}

923
void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVariant hash_value)
924
{
925 926
    qCInfo(ParameterManagerLog) << "Attemping load from cache";

927 928
    uint32_t crc32_value = 0;
    /* The datastructure of the cache table */
929 930 931
    CacheMapName2ParamTypeVal cacheMap;
    QFile cacheFile(parameterCacheFile(vehicleId, componentId));
    if (!cacheFile.exists()) {
932
        /* no local cache, just wait for them to come in*/
933 934
        return;
    }
935
    cacheFile.open(QIODevice::ReadOnly);
936 937

    /* Deserialize the parameter cache table */
938 939 940 941 942 943 944 945 946
    QDataStream ds(&cacheFile);
    ds >> cacheMap;

    // Load parameter meta data for the version number stored in cache.
    // We need meta data so we have access to the volatile bit
    if (cacheMap.contains(_versionParam)) {
        _parameterSetMajorVersion = cacheMap[_versionParam].second.toInt();
    }
    _loadMetaData();
947 948

    /* compute the crc of the local cache to check against the remote */
949

950
    FirmwarePlugin* firmwarePlugin = _vehicle->firmwarePlugin();
951
    for (const QString& name: cacheMap.keys()) {
952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968
        bool volatileValue = false;

        FactMetaData* metaData = firmwarePlugin->getMetaDataForFact(_parameterMetaData, name, _vehicle->vehicleType());
        if (metaData) {
            volatileValue = metaData->volatileValue();
        }

        if (volatileValue) {
            // Does not take part in CRC
            qCDebug(ParameterManagerLog) << "Volatile parameter" << name;
        } else {
            const ParamTypeVal& paramTypeVal = cacheMap[name];
            const void *vdat = paramTypeVal.second.constData();
            const FactMetaData::ValueType_t fact_type = static_cast<FactMetaData::ValueType_t>(paramTypeVal.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);
        }
969 970
    }

971
    /* if the two param set hashes match, just load from the disk */
972
    if (crc32_value == hash_value.toUInt()) {
973 974 975 976
        qCInfo(ParameterManagerLog) << "Parameters loaded from cache" << qPrintable(QFileInfo(cacheFile).absoluteFilePath());

        int count = cacheMap.count();
        int index = 0;
977
        for (const QString& name: cacheMap.keys()) {
978 979
            const ParamTypeVal& paramTypeVal = cacheMap[name];
            const FactMetaData::ValueType_t fact_type = static_cast<FactMetaData::ValueType_t>(paramTypeVal.first);
980
            const int mavType = _factTypeToMavType(fact_type);
981
            _parameterUpdate(vehicleId, componentId, name, count, index++, mavType, paramTypeVal.second);
982
        }
983

984 985 986
        // Return the hash value to notify we don't want any more updates
        mavlink_param_set_t     p;
        mavlink_param_union_t   union_value;
Don Gagne's avatar
Don Gagne committed
987
        memset(&p, 0, sizeof(p));
988 989 990 991 992 993 994
        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;
995 996 997 998 999
        mavlink_msg_param_set_encode_chan(_mavlink->getSystemId(),
                                          _mavlink->getComponentId(),
                                          _vehicle->priorityLink()->mavlinkChannel(),
                                          &msg,
                                          &p);
1000
        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
1001 1002 1003 1004 1005 1006 1007 1008

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

1009
        connect(ani, &QVariantAnimation::valueChanged, this, [this](const QVariant &value) {
1010
            _setLoadProgress(value.toDouble());
1011 1012 1013
        });

        // Hide 500ms after animation finishes
1014 1015
        connect(ani, &QVariantAnimation::finished, this, [this] {
            QTimer::singleShot(500, [this] {
1016
                _setLoadProgress(0);
1017 1018 1019 1020
            });
        });

        ani->start(QAbstractAnimation::DeleteWhenStopped);
DonLakeFlyer's avatar
DonLakeFlyer committed
1021
    } else {
1022 1023 1024 1025
        // Cache parameter version may differ from vehicle parameter version so we can't trust information loaded from cache parameter version number
        _parameterSetMajorVersion = -1;
        _clearMetaData();
        qCInfo(ParameterManagerLog) << "Parameters cache match failed" << qPrintable(QFileInfo(cacheFile).absoluteFilePath());
1026 1027 1028
        if (ParameterManagerDebugCacheFailureLog().isDebugEnabled()) {
            _debugCacheCRC[componentId] = true;
            _debugCacheMap[componentId] = cacheMap;
1029
            for (const QString& name: cacheMap.keys()) {
1030 1031 1032
                _debugCacheParamSeen[componentId][name] = false;
            }
            qgcApp()->showMessage(tr("Parameter cache CRC match failed"));
1033
        }
Don Gagne's avatar
Don Gagne committed
1034
    }
1035 1036
}

1037
QString ParameterManager::readParametersFromStream(QTextStream& stream)
1038
{
1039 1040
    QString missingErrors;
    QString typeErrors;
dogmaphobic's avatar
dogmaphobic committed
1041

1042 1043 1044 1045 1046 1047
    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
1048 1049
                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
1050 1051
                }

1052 1053 1054 1055
                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
1056

1057
                if (!parameterExists(componentId, paramName)) {
1058
                    QString error;
1059 1060 1061
                    error += QStringLiteral("%1:%2 ").arg(componentId).arg(paramName);
                    missingErrors += error;
                    qCDebug(ParameterManagerLog) << QStringLiteral("Skipped due to missing: %1").arg(error);
1062 1063
                    continue;
                }
dogmaphobic's avatar
dogmaphobic committed
1064

1065
                Fact* fact = getParameter(componentId, paramName);
1066
                if (fact->type() != _mavTypeToFactType((MAV_PARAM_TYPE)mavType)) {
1067
                    QString error;
1068 1069 1070
                    error  = QStringLiteral("%1:%2 ").arg(componentId).arg(paramName);
                    typeErrors += error;
                    qCDebug(ParameterManagerLog) << QStringLiteral("Skipped due to type mismatch: %1").arg(error);
1071 1072
                    continue;
                }
dogmaphobic's avatar
dogmaphobic committed
1073

1074
                qCDebug(ParameterManagerLog) << "Updating parameter" << componentId << paramName << valStr;
Don Gagne's avatar
Don Gagne committed
1075
                fact->setRawValue(valStr);
1076 1077 1078
            }
        }
    }
dogmaphobic's avatar
dogmaphobic committed
1079

1080 1081 1082 1083 1084 1085 1086 1087 1088 1089
    QString errors;

    if (!missingErrors.isEmpty()) {
        errors = tr("Parameters not loaded since they are not currently on the vehicle: %1\n").arg(missingErrors);
    }

    if (!typeErrors.isEmpty()) {
        errors += tr("Parameters not loaded due to type mismatch: %1").arg(typeErrors);
    }

1090
    return errors;
1091 1092
}

1093
void ParameterManager::writeParametersToStream(QTextStream& stream)
1094
{
Don Gagne's avatar
Don Gagne committed
1095
    stream << "# Onboard parameters for Vehicle " << _vehicle->id() << "\n";
1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106
    stream << "#\n";

    stream << "# Stack: " << _vehicle->firmwareTypeString() << "\n";
    stream << "# Vehicle: " << _vehicle->vehicleTypeString() << "\n";
    stream << "# Version: "
           << _vehicle->firmwareMajorVersion() << "."
           << _vehicle->firmwareMinorVersion() << "."
           << _vehicle->firmwarePatchVersion() << " "
           << _vehicle->firmwareVersionTypeString() << "\n";
    stream << "# Git Revision: " << _vehicle->gitHash() << "\n";

1107
    stream << "#\n";
Don Gagne's avatar
Don Gagne committed
1108
    stream << "# Vehicle-Id Component-Id Name Value Type\n";
1109

1110 1111
    for (int componentId: _mapParameterName2Variant.keys()) {
        for (const QString &paramName: _mapParameterName2Variant[componentId].keys()) {
1112
            Fact* fact = _mapParameterName2Variant[componentId][paramName].value<Fact*>();
1113 1114 1115 1116 1117
            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";
            }
1118 1119
        }
    }
dogmaphobic's avatar
dogmaphobic committed
1120

1121 1122 1123
    stream.flush();
}

1124
MAV_PARAM_TYPE ParameterManager::_factTypeToMavType(FactMetaData::ValueType_t factType)
1125 1126
{
    switch (factType) {
1127 1128
    case FactMetaData::valueTypeUint8:
        return MAV_PARAM_TYPE_UINT8;
dogmaphobic's avatar
dogmaphobic committed
1129

1130 1131
    case FactMetaData::valueTypeInt8:
        return MAV_PARAM_TYPE_INT8;
dogmaphobic's avatar
dogmaphobic committed
1132

1133 1134
    case FactMetaData::valueTypeUint16:
        return MAV_PARAM_TYPE_UINT16;
dogmaphobic's avatar
dogmaphobic committed
1135

1136 1137
    case FactMetaData::valueTypeInt16:
        return MAV_PARAM_TYPE_INT16;
dogmaphobic's avatar
dogmaphobic committed
1138

1139 1140
    case FactMetaData::valueTypeUint32:
        return MAV_PARAM_TYPE_UINT32;
dogmaphobic's avatar
dogmaphobic committed
1141

1142 1143 1144 1145 1146 1147
    case FactMetaData::valueTypeUint64:
        return MAV_PARAM_TYPE_UINT64;

    case FactMetaData::valueTypeInt64:
        return MAV_PARAM_TYPE_INT64;

1148 1149
    case FactMetaData::valueTypeFloat:
        return MAV_PARAM_TYPE_REAL32;
dogmaphobic's avatar
dogmaphobic committed
1150

1151 1152 1153
    case FactMetaData::valueTypeDouble:
        return MAV_PARAM_TYPE_REAL64;

1154 1155 1156
    default:
        qWarning() << "Unsupported fact type" << factType;
        // fall through
dogmaphobic's avatar
dogmaphobic committed
1157

1158 1159
    case FactMetaData::valueTypeInt32:
        return MAV_PARAM_TYPE_INT32;
1160 1161 1162
    }
}

1163
FactMetaData::ValueType_t ParameterManager::_mavTypeToFactType(MAV_PARAM_TYPE mavType)
1164 1165
{
    switch (mavType) {
1166 1167
    case MAV_PARAM_TYPE_UINT8:
        return FactMetaData::valueTypeUint8;
dogmaphobic's avatar
dogmaphobic committed
1168

1169 1170
    case MAV_PARAM_TYPE_INT8:
        return FactMetaData::valueTypeInt8;
dogmaphobic's avatar
dogmaphobic committed
1171

1172 1173
    case MAV_PARAM_TYPE_UINT16:
        return FactMetaData::valueTypeUint16;
dogmaphobic's avatar
dogmaphobic committed
1174

1175 1176
    case MAV_PARAM_TYPE_INT16:
        return FactMetaData::valueTypeInt16;
dogmaphobic's avatar
dogmaphobic committed
1177

1178 1179
    case MAV_PARAM_TYPE_UINT32:
        return FactMetaData::valueTypeUint32;
dogmaphobic's avatar
dogmaphobic committed
1180

1181 1182 1183 1184 1185 1186
    case MAV_PARAM_TYPE_UINT64:
        return FactMetaData::valueTypeUint64;

    case MAV_PARAM_TYPE_INT64:
        return FactMetaData::valueTypeInt64;

1187 1188
    case MAV_PARAM_TYPE_REAL32:
        return FactMetaData::valueTypeFloat;
dogmaphobic's avatar
dogmaphobic committed
1189

1190 1191 1192
    case MAV_PARAM_TYPE_REAL64:
        return FactMetaData::valueTypeDouble;

1193 1194 1195
    default:
        qWarning() << "Unsupported mav param type" << mavType;
        // fall through
dogmaphobic's avatar
dogmaphobic committed
1196

1197 1198
    case MAV_PARAM_TYPE_INT32:
        return FactMetaData::valueTypeInt32;
1199 1200 1201
    }
}

1202 1203 1204 1205
void ParameterManager::_clearMetaData(void)
{
    if (_parameterMetaData) {
        _parameterMetaData->deleteLater();
1206
        _parameterMetaData = nullptr;
1207 1208 1209 1210
    }
}

void ParameterManager::_loadMetaData(void)
1211
{
1212 1213 1214
    if (_parameterMetaData) {
        return;
    }
1215

1216 1217
    QString metaDataFile;
    int majorVersion, minorVersion;
1218

1219 1220
    // Load best parameter meta data set
    metaDataFile = parameterMetaDataFile(_vehicle, _vehicle->firmwareType(), _parameterSetMajorVersion, majorVersion, minorVersion);
1221
    qCDebug(ParameterManagerLog) << "Loading meta data file:major:minor" << metaDataFile << majorVersion << minorVersion;
1222
    _parameterMetaData = _vehicle->firmwarePlugin()->loadParameterMetaData(metaDataFile);
1223 1224 1225 1226 1227 1228 1229 1230 1231 1232
}

void ParameterManager::_addMetaDataToDefaultComponent(void)
{
    _loadMetaData();

    if (_metaDataAddedToFacts) {
        return;
    }
    _metaDataAddedToFacts = true;
1233 1234

    // Loop over all parameters in default component adding meta data
1235
    QVariantMap& factMap = _mapParameterName2Variant[_vehicle->defaultComponentId()];
1236
    for (const QString& key: factMap.keys()) {
1237 1238 1239 1240
        _vehicle->firmwarePlugin()->addMetaDataToFact(_parameterMetaData, factMap[key].value<Fact*>(), _vehicle->vehicleType());
    }
}

1241
void ParameterManager::_checkInitialLoadComplete(void)
1242 1243 1244 1245 1246
{
    // Already processed?
    if (_initialLoadComplete) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1247

1248
    for (int componentId: _waitingReadParamIndexMap.keys()) {
1249 1250 1251 1252 1253
        if (_waitingReadParamIndexMap[componentId].count()) {
            // We are still waiting on some parameters, not done yet
            return;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
1254

1255 1256 1257 1258 1259
    if (!_mapParameterName2Variant.contains(_vehicle->defaultComponentId())) {
        // No default component params yet, not done yet
        return;
    }

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

1263 1264
    // Parameter cache crc failure debugging
    for (int componentId: _debugCacheParamSeen.keys()) {
1265
        if (!_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) {
1266
            for (const QString& paramName: _debugCacheParamSeen[componentId].keys()) {
1267 1268 1269 1270 1271 1272 1273 1274
                if (!_debugCacheParamSeen[componentId][paramName]) {
                    qDebug() << "Parameter in cache but not on vehicle componentId:Name" << componentId << paramName;
                }
            }
        }
    }
    _debugCacheCRC.clear();

1275
    qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Initial load complete";
1276

1277 1278 1279
    // Check for index based load failures
    QString indexList;
    bool initialLoadFailures = false;
1280 1281
    for (int componentId: _failedReadParamIndexMap.keys()) {
        for (int paramIndex: _failedReadParamIndexMap[componentId]) {
1282 1283 1284
            if (initialLoadFailures) {
                indexList += ", ";
            }
1285
            indexList += QString("%1:%2").arg(componentId).arg(paramIndex);
1286
            initialLoadFailures = true;
1287
            qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Gave up on initial load after max retries (paramIndex:" << paramIndex << ")";
1288 1289
        }
    }
1290 1291

    _missingParameters = false;
1292
    if (initialLoadFailures) {
1293
        _missingParameters = true;
1294 1295
        QString errorMsg = tr("%1 was unable to retrieve the full set of parameters from vehicle %2. "
                              "This will cause %1 to be unable to display its full user interface. "
1296
                              "If you are using modified firmware, you may need to resolve any vehicle startup errors to resolve the issue. "
1297
                              "If you are using standard firmware, you may need to upgrade to a newer version to resolve the issue.").arg(qgcApp()->applicationName()).arg(_vehicle->id());
1298 1299
        qCDebug(ParameterManagerLog) << errorMsg;
        qgcApp()->showMessage(errorMsg);
Don Gagne's avatar
Don Gagne committed
1300
        if (!qgcApp()->runningUnitTests()) {
1301
            qCWarning(ParameterManagerLog) << _logVehiclePrefix(-1) << "The following parameter indices could not be loaded after the maximum number of retries: " << indexList;
Don Gagne's avatar
Don Gagne committed
1302
        }
1303 1304
    }

1305
    // Signal load complete
1306
    _parametersReady = true;
1307 1308 1309
    _vehicle->autopilotPlugin()->parametersReadyPreChecks();
    emit parametersReadyChanged(true);
    emit missingParametersChanged(_missingParameters);
1310
}
1311

1312
void ParameterManager::_initialRequestTimeout(void)
1313
{
1314
    if (!_disableAllRetries && ++_initialRequestRetryCount <= _maxInitialRequestListRetry) {
1315
        qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Retrying initial parameter request list";
1316 1317
        refreshAllParameters();
        _initialRequestTimeoutTimer.start();
1318 1319
    } else {
        if (!_vehicle->genericFirmware()) {
Don Gagne's avatar
Don Gagne committed
1320
            QString errorMsg = tr("Vehicle %1 did not respond to request for parameters. "
1321
                                  "This will cause %2 to be unable to display its full user interface.").arg(_vehicle->id()).arg(qgcApp()->applicationName());
1322 1323 1324
            qCDebug(ParameterManagerLog) << errorMsg;
            qgcApp()->showMessage(errorMsg);
        }
1325
    }
1326
}
1327

1328
QString ParameterManager::parameterMetaDataFile(Vehicle* vehicle, MAV_AUTOPILOT firmwareType, int wantedMajorVersion, int& majorVersion, int& minorVersion)
1329 1330
{
    bool            cacheHit = false;
1331
    FirmwarePlugin* plugin = _anyVehicleTypeFirmwarePlugin(firmwareType);
1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344

    // 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 {
1345
            qCDebug(ParameterManagerLog) << "Direct cache hit on file:major:minor" << cacheFile.fileName() << cacheMajorVersion << cacheMinorVersion;
1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377
            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 {
1378
                qCDebug(ParameterManagerLog) << "Indirect cache hit on file:major:minor:want" << cacheFile.fileName() << cacheMajorVersion << cacheMinorVersion << wantedMajorVersion;
1379 1380 1381 1382 1383 1384
                cacheHit = true;
            }
        }
    }

    int internalMinorVersion, internalMajorVersion;
1385
    QString internalMetaDataFile = plugin->internalParameterMetaDataFile(vehicle);
1386
    plugin->getParameterMetaDataVersionInfo(internalMetaDataFile, internalMajorVersion, internalMinorVersion);
1387
    qCDebug(ParameterManagerLog) << "Internal meta data file:major:minor" << internalMetaDataFile << internalMajorVersion << internalMinorVersion;
1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418
    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;
    }
1419
    qCDebug(ParameterManagerLog) << "ParameterManager::parameterMetaDataFile file:major:minor" << metaDataFile << majorVersion << minorVersion;
1420 1421 1422 1423

    return metaDataFile;
}

1424
FirmwarePlugin* ParameterManager::_anyVehicleTypeFirmwarePlugin(MAV_AUTOPILOT firmwareType)
1425
{
1426
    // There are cases where we need a FirmwarePlugin but we don't have a vehicle. In those specified case the plugin for any of the supported vehicle types will do.
1427 1428
    MAV_TYPE anySupportedVehicleType = qgcApp()->toolbox()->firmwarePluginManager()->supportedVehicleTypes(firmwareType)[0];

1429 1430 1431 1432 1433 1434
    return qgcApp()->toolbox()->firmwarePluginManager()->firmwarePluginForAutopilot(firmwareType, anySupportedVehicleType);
}

void ParameterManager::cacheMetaDataFile(const QString& metaDataFile, MAV_AUTOPILOT firmwareType)
{
    FirmwarePlugin* plugin = _anyVehicleTypeFirmwarePlugin(firmwareType);
1435 1436 1437

    int newMajorVersion, newMinorVersion;
    plugin->getParameterMetaDataVersionInfo(metaDataFile, newMajorVersion, newMinorVersion);
1438
    qCDebug(ParameterManagerLog) << "ParameterManager::cacheMetaDataFile file:firmware:major;minor" << metaDataFile << firmwareType << newMajorVersion << newMinorVersion;
1439 1440 1441

    // Find the cache hit closest to this new file
    int cacheMajorVersion, cacheMinorVersion;
1442
    QString cacheHit = ParameterManager::parameterMetaDataFile(nullptr, firmwareType, newMajorVersion, cacheMajorVersion, cacheMinorVersion);
1443
    qCDebug(ParameterManagerLog) << "ParameterManager::cacheMetaDataFile cacheHit file:firmware:major;minor" << cacheHit << cacheMajorVersion << cacheMinorVersion;
1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465 1466 1467

    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)));
1468
        qCDebug(ParameterManagerLog) << "ParameterManager::cacheMetaDataFile caching file:" << cacheFile.fileName();
1469 1470 1471 1472
        QFile newFile(metaDataFile);
        newFile.copy(cacheFile.fileName());
    }
}
1473 1474

/// Remap a parameter from one firmware version to another
1475
QString ParameterManager::_remapParamNameToVersion(const QString& paramName)
1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486
{
    QString mappedParamName;

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

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

1487
    qCDebug(ParameterManagerLog) << "_remapParamNameToVersion" << paramName << majorVersion << minorVersion;
1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498 1499

    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
1500
        qCDebug(ParameterManagerLog) << "_remapParamNameToVersion: no major version mapping";
1501 1502 1503 1504 1505
        return mappedParamName;
    }

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

1506
    // We must map backwards from the highest known minor version to one above the vehicle's minor version
1507 1508 1509 1510 1511 1512 1513

    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];
1514
                qCDebug(ParameterManagerLog) << "_remapParamNameToVersion: remapped currentMinor:from:to"<< currentMinorVersion << mappedParamName << toParamName;
1515 1516 1517 1518 1519 1520 1521
                mappedParamName = toParamName;
            }
        }
    }

    return mappedParamName;
}
1522 1523

/// The offline editing vehicle can have custom loaded params bolted into it.
1524
void ParameterManager::_loadOfflineEditingParams(void)
1525
{
1526 1527 1528 1529 1530 1531 1532
    QString paramFilename = _vehicle->firmwarePlugin()->offlineEditingParamFile(_vehicle);
    if (paramFilename.isEmpty()) {
        return;
    }

    QFile paramFile(paramFilename);
    if (!paramFile.open(QFile::ReadOnly)) {
1533
        qCWarning(ParameterManagerLog) << "Unable to open offline editing params file" << paramFilename;
1534 1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547
    }

    QTextStream paramStream(&paramFile);

    while (!paramStream.atEnd()) {
        QString line = paramStream.readLine();

        if (line.startsWith("#")) {
            continue;
        }

        QStringList paramData = line.split("\t");
        Q_ASSERT(paramData.count() == 5);

1548 1549
        int defaultComponentId = paramData.at(1).toInt();
        _vehicle->setOfflineEditingDefaultComponentId(defaultComponentId);
1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564 1565 1566 1567 1568 1569 1570 1571 1572 1573 1574 1575 1576 1577 1578 1579 1580 1581 1582
        QString paramName = paramData.at(2);
        QString valStr = paramData.at(3);
        MAV_PARAM_TYPE paramType = static_cast<MAV_PARAM_TYPE>(paramData.at(4).toUInt());

        QVariant paramValue;
        switch (paramType) {
        case MAV_PARAM_TYPE_REAL32:
            paramValue = QVariant(valStr.toFloat());
            break;
        case MAV_PARAM_TYPE_UINT32:
            paramValue = QVariant(valStr.toUInt());
            break;
        case MAV_PARAM_TYPE_INT32:
            paramValue = QVariant(valStr.toInt());
            break;
        case MAV_PARAM_TYPE_UINT16:
            paramValue = QVariant((quint16)valStr.toUInt());
            break;
        case MAV_PARAM_TYPE_INT16:
            paramValue = QVariant((qint16)valStr.toInt());
            break;
        case MAV_PARAM_TYPE_UINT8:
            paramValue = QVariant((quint8)valStr.toUInt());
            break;
        case MAV_PARAM_TYPE_INT8:
            paramValue = QVariant((qint8)valStr.toUInt());
            break;
        default:
            qCritical() << "Unknown type" << paramType;
            paramValue = QVariant(valStr.toInt());
            break;
        }

1583 1584 1585 1586 1587
        // Get parameter set version
        if (!_versionParam.isEmpty() && _versionParam == paramName) {
            _parameterSetMajorVersion = paramValue.toInt();
        }

1588 1589
        Fact* fact = new Fact(defaultComponentId, paramName, _mavTypeToFactType(paramType), this);
        _mapParameterName2Variant[defaultComponentId][paramName] = QVariant::fromValue(fact);
1590 1591 1592
    }

    _addMetaDataToDefaultComponent();
1593
    _setupDefaultComponentCategoryMap();
1594 1595
    _parametersReady = true;
    _initialLoadComplete = true;
1596
    _debugCacheCRC.clear();
1597
}
1598 1599 1600 1601 1602 1603 1604 1605 1606 1607 1608 1609 1610 1611 1612 1613 1614 1615 1616 1617 1618 1619 1620 1621 1622 1623 1624 1625 1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637

void ParameterManager::saveToJson(int componentId, const QStringList& paramsToSave, QJsonObject& saveObject)
{
    QList<int>  rgCompIds;
    QStringList rgParamNames;

    if (componentId == MAV_COMP_ID_ALL) {
        rgCompIds = _mapParameterName2Variant.keys();
    } else {
        rgCompIds.append(_actualComponentId(componentId));
    }

    QJsonArray rgParams;

    // Loop over all requested component ids
    for (int i=0; i<rgCompIds.count(); i++) {
        int compId = rgCompIds[i];

        if (!_mapParameterName2Variant.contains(compId)) {
            qCDebug(ParameterManagerLog) << "ParameterManager::saveToJson no params for compId" << compId;
            continue;
        }

        // Build list of parameter names if not specified
        if (paramsToSave.count() == 0) {
            rgParamNames = 	parameterNames(compId);
        } else {
            rgParamNames = paramsToSave;
        }

        // Loop over parameter names adding each to json array
        for (int j=0; j<rgParamNames.count(); j++) {
            QString paramName = rgParamNames[j];

            if (!parameterExists(compId, paramName)) {
                qCDebug(ParameterManagerLog) << "ParameterManager::saveToJson param not found compId:param" << compId << paramName;
                continue;
            }

            QJsonObject paramJson;
1638
            Fact* fact = getParameter(compId, paramName);
1639 1640 1641 1642 1643 1644 1645 1646 1647 1648 1649 1650 1651 1652 1653 1654 1655 1656 1657 1658 1659 1660 1661 1662 1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673 1674 1675 1676 1677 1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691 1692 1693 1694 1695 1696
            paramJson.insert(_jsonCompIdKey, QJsonValue(compId));
            paramJson.insert(_jsonParamNameKey, QJsonValue(fact->name()));
            paramJson.insert(_jsonParamValueKey, QJsonValue(fact->rawValue().toDouble()));

            rgParams.append(QJsonValue(paramJson));
        }
    }

    saveObject.insert(_jsonParametersKey, QJsonValue(rgParams));
}

bool ParameterManager::loadFromJson(const QJsonObject& json, bool required, QString& errorString)
{
    QList<QJsonValue::Type> keyTypes;

    errorString.clear();

    if (required) {
        if (!JsonHelper::validateRequiredKeys(json, QStringList(_jsonParametersKey), errorString)) {
            return false;
        }
    } else if (!json.contains(_jsonParametersKey)) {
        return true;
    }

    keyTypes = { QJsonValue::Array };
    if (!JsonHelper::validateKeyTypes(json, QStringList(_jsonParametersKey), keyTypes, errorString)) {
        return false;
    }

    QJsonArray rgParams = json[_jsonParametersKey].toArray();
    for (int i=0; i<rgParams.count(); i++) {
        QJsonValueRef paramValue = rgParams[i];

        if (!paramValue.isObject()) {
            errorString = tr("%1 key is not a json object").arg(_jsonParametersKey);
            return false;
        }
        QJsonObject param = paramValue.toObject();

        QStringList requiredKeys = { _jsonCompIdKey, _jsonParamNameKey, _jsonParamValueKey };
        if (!JsonHelper::validateRequiredKeys(param, requiredKeys, errorString)) {
            return false;
        }
        keyTypes = { QJsonValue::Double, QJsonValue::String, QJsonValue::Double };
        if (!JsonHelper::validateKeyTypes(param, requiredKeys, keyTypes, errorString)) {
            return false;
        }

        int compId = param[_jsonCompIdKey].toInt();
        QString name = param[_jsonParamNameKey].toString();
        double value = param[_jsonParamValueKey].toDouble();

        if (!parameterExists(compId, name)) {
            qCDebug(ParameterManagerLog) << "ParameterManager::loadFromJson param not found compId:param" << compId << name;
            continue;
        }

1697
        Fact* fact = getParameter(compId, name);
1698 1699 1700 1701 1702 1703
        fact->setRawValue(value);
    }

    return true;
}

1704
void ParameterManager::resetAllParametersToDefaults()
1705
{
1706 1707 1708 1709 1710
    _vehicle->sendMavCommand(MAV_COMP_ID_ALL,
                             MAV_CMD_PREFLIGHT_STORAGE,
                             true,  // showError
                             2,     // Reset params to default
                             -1);   // Don't do anything with mission storage
1711 1712
}

1713 1714 1715 1716 1717 1718 1719 1720 1721
void ParameterManager::resetAllToVehicleConfiguration()
{
    //-- https://github.com/PX4/Firmware/pull/11760
    Fact* sysAutoConfigFact = getParameter(-1, "SYS_AUTOCONFIG");
    if(sysAutoConfigFact) {
        sysAutoConfigFact->setRawValue(2);
    }
}

1722 1723 1724 1725 1726 1727 1728 1729
QString ParameterManager::_logVehiclePrefix(int componentId)
{
    if (componentId == -1) {
        return QString("V:%1").arg(_vehicle->id());
    } else {
        return QString("V:%1 C:%2").arg(_vehicle->id()).arg(componentId);
    }
}
1730 1731 1732

void ParameterManager::_setLoadProgress(double loadProgress)
{
1733 1734 1735 1736
    if (_loadProgress != loadProgress) {
        _loadProgress = loadProgress;
        emit loadProgressChanged(static_cast<float>(loadProgress));
    }
1737
}
Don Gagne's avatar
Don Gagne committed
1738 1739 1740 1741 1742

QList<int> ParameterManager::componentIds(void)
{
    return _paramCountMap.keys();
}
1743 1744 1745 1746 1747 1748 1749 1750 1751 1752 1753

bool ParameterManager::pendingWrites(void)
{
    for (int compId: _waitingWriteParamNameMap.keys()) {
        if (_waitingWriteParamNameMap[compId].count()) {
            return true;
        }
    }

    return false;
}