QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
ParameterManager.cc
Go to the documentation of this file.
2#include "ParameterManager.h"
3
4#include <QtCore/QDir>
5#include <QtCore/QTextStream>
6
7#include "AutoPilotPlugin.h"
8#include "CompInfoParam.h"
10#include "FactGroup.h"
11#include "FirmwarePlugin.h"
12#include "FTPManager.h"
13#include "MAVLinkProtocol.h"
14#include "AppMessages.h"
15#include "QGCMath.h"
16#include "QGCApplication.h"
17#include "QGCLoggingCategory.h"
18#include "QGCMAVLink.h"
19#include "Vehicle.h"
20#include "VehicleLinkManager.h"
21#include "QGCStateMachine.h"
22#include "MultiVehicleManager.h"
23
24#include <QtCore/QEasingCurve>
25#include <QtCore/QFile>
26#include <QtCore/QStandardPaths>
27#include <QtCore/QVariantAnimation>
28
29QGC_LOGGING_CATEGORY(ParameterManagerLog, "FactSystem.ParameterManager")
30QGC_LOGGING_CATEGORY(ParameterManagerVerbose1Log, "FactSystem.ParameterManager:verbose1")
31QGC_LOGGING_CATEGORY(ParameterManagerVerbose2Log, "FactSystem.ParameterManager:verbose2")
32QGC_LOGGING_CATEGORY(ParameterManagerDebugCacheFailureLog, "FactSystem.ParameterManager:debugCacheFailure") // Turn on to debug parameter cache crc misses
33
35 : QObject(vehicle)
36 , _vehicle(vehicle)
37 , _logReplay(!vehicle->vehicleLinkManager()->primaryLink().expired() && vehicle->vehicleLinkManager()->primaryLink().lock()->isLogReplay())
38 , _disableAllRetries(_logReplay)
39 , _tryftp(vehicle->apmFirmware())
40{
41 qCDebug(ParameterManagerLog) << this;
42
43 if (_vehicle->isOfflineEditingVehicle()) {
44 _loadOfflineEditingParams();
45 return;
46 }
47
48 if (_logReplay) {
49 qCDebug(ParameterManagerLog) << this << "In log replay mode";
50 }
51
52 _hashCheckTimer.setSingleShot(true);
53 _hashCheckTimer.setInterval(kHashCheckTimeoutMs);
54 (void) connect(&_hashCheckTimer, &QTimer::timeout, this, &ParameterManager::_hashCheckTimeout);
55
56 _paramRequestListTimer.setSingleShot(true);
57 _paramRequestListTimer.setInterval(QGC::runningUnitTests() ? kTestInitialRequestIntervalMs : kParamRequestListTimeoutMs);
58 (void) connect(&_paramRequestListTimer, &QTimer::timeout, this, &ParameterManager::_paramRequestListTimeout);
59
60 _waitingParamTimeoutTimer.setSingleShot(true);
61 _waitingParamTimeoutTimer.setInterval(QGC::runningUnitTests() ? 500 : 3000);
62 if (!_logReplay) {
63 (void) connect(&_waitingParamTimeoutTimer, &QTimer::timeout, this, &ParameterManager::_waitingParamTimeout);
64 }
65
66 // Ensure the cache directory exists
67 (void) QDir().mkpath(parameterCacheDir().absolutePath());
68}
69
71{
72 qCDebug(ParameterManagerLog) << this;
73}
74
75void ParameterManager::_updateProgressBar()
76{
77 int waitingReadParamIndexCount = 0;
78
79 for (const int compId: _waitingReadParamIndexMap.keys()) {
80 waitingReadParamIndexCount += _waitingReadParamIndexMap[compId].count();
81 }
82
83 if (waitingReadParamIndexCount == 0) {
84 if (_readParamIndexProgressActive) {
85 _readParamIndexProgressActive = false;
86 _setLoadProgress(0.0);
87 return;
88 }
89 } else {
90 _readParamIndexProgressActive = true;
91 _setLoadProgress(static_cast<double>(_totalParamCount - waitingReadParamIndexCount) / static_cast<double>(_totalParamCount));
92 return;
93 }
94}
95
97{
98 if (_tryftp && (message.compid == MAV_COMP_ID_AUTOPILOT1) && !_initialLoadComplete)
99 return;
100
101 if (message.msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
102 mavlink_param_value_t param_value{};
103 mavlink_msg_param_value_decode(&message, &param_value);
104
105 // This will null terminate the name string
106 char parameterNameWithNull[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1] = {};
107 (void) strncpy(parameterNameWithNull, param_value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
108 const QString parameterName(parameterNameWithNull);
109
110 mavlink_param_union_t paramUnion{};
111 paramUnion.param_float = param_value.param_value;
112 paramUnion.type = param_value.param_type;
113
114 QVariant parameterValue;
115 if (!_mavlinkParamUnionToVariant(paramUnion, parameterValue)) {
116 return;
117 }
118
119 _handleParamValue(message.compid, parameterName, param_value.param_count, param_value.param_index, static_cast<MAV_PARAM_TYPE>(param_value.param_type), parameterValue);
120 }
121}
122
123void ParameterManager::_handleParamValue(int componentId, const QString &parameterName, int parameterCount, int parameterIndex, MAV_PARAM_TYPE mavParamType, const QVariant &parameterValue)
124{
125
126 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
127 "_parameterUpdate" <<
128 "name:" << parameterName <<
129 "count:" << parameterCount <<
130 "index:" << parameterIndex <<
131 "mavType:" << mavParamType <<
132 "value:" << parameterValue <<
133 ")";
134
135 // ArduPilot has this strange behavior of streaming parameters that we didn't ask for. This even happens before it responds to the
136 // PARAM_REQUEST_LIST. We disregard any of this until the initial request is responded to.
137 if ((parameterIndex == 65535) && (parameterName != QStringLiteral("_HASH_CHECK")) && _paramRequestListTimer.isActive()) {
138 qCDebug(ParameterManagerLog) << "Disregarding unrequested param prior to initial list response" << parameterName;
139 return;
140 }
141
142 if (_vehicle->px4Firmware() && (parameterName == "_HASH_CHECK")) {
143 _hashCheckTimer.stop();
144 if (!_initialLoadComplete && !_logReplay) {
145 /* we received a cache hash, potentially load from cache */
146 _tryCacheHashLoad(_vehicle->id(), componentId, parameterValue);
147 }
148 return;
149 }
150
151 _paramRequestListTimer.stop();
152
153 // Used to debug cache crc misses (turn on ParameterManagerDebugCacheFailureLog)
154 if (!_initialLoadComplete && !_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) {
155 if (_debugCacheMap[componentId].contains(parameterName)) {
156 const ParamTypeVal &cacheParamTypeVal = _debugCacheMap[componentId][parameterName];
157 const size_t dataSize = FactMetaData::typeToSize(static_cast<FactMetaData::ValueType_t>(cacheParamTypeVal.first));
158 const void *const cacheData = cacheParamTypeVal.second.constData();
159 const void *const vehicleData = parameterValue.constData();
160
161 if (memcmp(cacheData, vehicleData, dataSize) != 0) {
162 qCDebug(ParameterManagerVerbose1Log) << "Cache/Vehicle values differ for name:cache:actual" << parameterName << parameterValue << cacheParamTypeVal.second;
163 }
164 _debugCacheParamSeen[componentId][parameterName] = true;
165 } else {
166 qCDebug(ParameterManagerVerbose1Log) << "Parameter missing from cache" << parameterName;
167 }
168 }
169
170 _waitingParamTimeoutTimer.stop();
171
172 // Update our total parameter counts
173 if (!_paramCountMap.contains(componentId)) {
174 _paramCountMap[componentId] = parameterCount;
175 _totalParamCount += parameterCount;
176 }
177
178 // If we've never seen this component id before, setup the index wait lists.
179 if (!_waitingReadParamIndexMap.contains(componentId)) {
180 // Add all indices to the wait list, parameter index is 0-based
181 for (int waitingIndex = 0; waitingIndex < parameterCount; waitingIndex++) {
182 // 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
183 _waitingReadParamIndexMap[componentId][waitingIndex] = 0;
184 }
185
186 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Seeing component for first time - paramcount:" << parameterCount;
187 }
188
189 if (!_waitingReadParamIndexMap[componentId].contains(parameterIndex)) {
190 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "Unrequested param update" << parameterName;
191 }
192
193 // Remove this parameter from the waiting lists
194 if (_waitingReadParamIndexMap[componentId].contains(parameterIndex)) {
195 _waitingReadParamIndexMap[componentId].remove(parameterIndex);
196 (void) _indexBatchQueue.removeOne(parameterIndex);
197 _fillIndexBatchQueue(false /* waitingParamTimeout */);
198 }
199
200 // Track how many parameters we are still waiting for
201 int waitingReadParamIndexCount = 0;
202
203 for (const int waitingComponentId: _waitingReadParamIndexMap.keys()) {
204 waitingReadParamIndexCount += _waitingReadParamIndexMap[waitingComponentId].count();
205 }
206 if (waitingReadParamIndexCount) {
207 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "waitingReadParamIndexCount:" << waitingReadParamIndexCount;
208 }
209
210 const int readWaitingParamCount = waitingReadParamIndexCount;
211 const int totalWaitingParamCount = readWaitingParamCount;
212 if (totalWaitingParamCount) {
213 // More params to wait for, restart timer
214 _waitingParamTimeoutTimer.start();
215 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer: totalWaitingParamCount:" << totalWaitingParamCount;
216 } else if (!_mapCompId2FactMap.contains(_vehicle->defaultComponentId())) {
217 // Still waiting for parameters from default component
218 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer (still waiting for default component params)";
219 _waitingParamTimeoutTimer.start();
220 } else {
221 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(-1) << "Not restarting _waitingParamTimeoutTimer (all requests satisfied)";
222 }
223
224 _updateProgressBar();
225
226 Fact *fact = nullptr;
227 if (_mapCompId2FactMap.contains(componentId) && _mapCompId2FactMap[componentId].contains(parameterName)) {
228 fact = _mapCompId2FactMap[componentId][parameterName];
229 } else {
230 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "Adding new fact" << parameterName;
231
232 fact = new Fact(componentId, parameterName, mavTypeToFactType(mavParamType), this);
233 FactMetaData *const factMetaData = _vehicle->compInfoManager()->compInfoParam(componentId)->factMetaDataForName(parameterName, fact->type());
234 fact->setMetaData(factMetaData);
235
236 _mapCompId2FactMap[componentId][parameterName] = fact;
237
238 // We need to know when the fact value changes so we can update the vehicle
239 (void) connect(fact, &Fact::containerRawValueChanged, this, &ParameterManager::_factRawValueUpdated);
240
241 emit factAdded(componentId, fact);
242 }
243
244 fact->containerSetRawValue(parameterValue);
245
246 // Update param cache. The param cache is only used on PX4 Firmware since ArduPilot and Solo have volatile params
247 // which invalidate the cache. The Solo also streams param updates in flight for things like gimbal values
248 // which in turn causes a perf problem with all the param cache updates.
249 if (!_logReplay && _vehicle->px4Firmware()) {
250 if (_prevWaitingReadParamIndexCount != 0 && readWaitingParamCount == 0) {
251 // All reads just finished, update the cache
252 _writeLocalParamCache(_vehicle->id(), componentId);
253 }
254 }
255
256 _prevWaitingReadParamIndexCount = waitingReadParamIndexCount;
257
258 _checkInitialLoadComplete();
259
260 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "_parameterUpdate complete";
261}
262
263QString ParameterManager::_vehicleAndComponentString(int componentId) const
264{
265 // If there are multiple vehicles include the vehicle id for disambiguation
266 QString vehicleIdStr;
267 if (MultiVehicleManager::instance()->vehicles()->count() > 1) {
268 vehicleIdStr = QStringLiteral("veh: %1").arg(_vehicle->id());
269 }
270
271 // IF we have parameters for multiple components include the component id for disambiguation
272 QString componentIdStr;
273 if (_mapCompId2FactMap.keys().count() > 1) {
274 componentIdStr = QStringLiteral("comp: %1").arg(componentId);
275 }
276
277 if (!vehicleIdStr.isEmpty() && !componentIdStr.isEmpty()) {
278 return vehicleIdStr + QStringLiteral(" ") + componentIdStr;
279 } else if (!vehicleIdStr.isEmpty()) {
280 return vehicleIdStr;
281 } else if (!componentIdStr.isEmpty()) {
282 return componentIdStr;
283 } else {
284 return QString();
285 }
286}
287
288void ParameterManager::_mavlinkParamSet(int componentId, const QString &paramName, FactMetaData::ValueType_t valueType, const QVariant &rawValue)
289{
290 auto paramSetEncoder = [this, componentId, paramName, valueType, rawValue](uint8_t /*systemId*/, uint8_t channel, mavlink_message_t *message) -> void {
291 const MAV_PARAM_TYPE paramType = factTypeToMavType(valueType);
292
293 mavlink_param_union_t union_value{};
294 if (!_fillMavlinkParamUnion(valueType, rawValue, union_value)) {
295 return;
296 }
297
298 char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1] = {};
299 (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
300
301 (void) mavlink_msg_param_set_pack_chan(
304 channel,
305 message,
306 static_cast<uint8_t>(_vehicle->id()),
307 static_cast<uint8_t>(componentId),
308 paramId,
309 union_value.param_float,
310 static_cast<uint8_t>(paramType));
311 };
312
313 auto checkForCorrectParamValue = [this, componentId, paramName, rawValue](const mavlink_message_t &message) -> bool {
314 if (message.compid != componentId) {
315 return false;
316 }
317
318 mavlink_param_value_t param_value{};
319 mavlink_msg_param_value_decode(&message, &param_value);
320
321 // This will null terminate the name string
322 char parameterNameWithNull[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1] = {};
323 (void) strncpy(parameterNameWithNull, param_value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
324 const QString parameterName(parameterNameWithNull);
325
326 if (parameterName != paramName) {
327 return false;
328 }
329
330 // Check that the value matches what we expect within tolerance, if it doesn't match then this message is not for us
331 QVariant receivedValue;
332 mavlink_param_union_t param_union;
333 param_union.param_float = param_value.param_value;
334 param_union.type = param_value.param_type;
335 if (!_mavlinkParamUnionToVariant(param_union, receivedValue)) {
336 return false;
337 }
338 if (rawValue.typeId() != receivedValue.typeId()) {
339 qCWarning(ParameterManagerLog) << "QVariant type mismatch on PARAM_VALUE ack for" << paramName << ": expected type" << rawValue.typeId() << "got type" << receivedValue.typeId();
340 return false;
341 }
342 if (param_value.param_type == MAV_PARAM_TYPE_REAL32) {
343 // Float comparison must be fuzzy
344 return QGC::fuzzyCompare(rawValue.toFloat(), receivedValue.toFloat());
345 } else {
346 return receivedValue == rawValue;
347 }
348 };
349
350 auto checkForParamError = [componentId, paramName](const mavlink_message_t &message) -> bool {
351 if (message.compid != componentId) {
352 return false;
353 }
354
355 mavlink_param_error_t paramError{};
356 mavlink_msg_param_error_decode(&message, &paramError);
357
358 char paramId[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN + 1] = {};
359 (void) strncpy(paramId, paramError.param_id, MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN);
360
361 return QString(paramId) == paramName;
362 };
363
364 // State Machine:
365 // Send PARAM_SET - 2 retries after initial attempt
366 // Increment pending write count
367 // Wait for PARAM_VALUE ack or PARAM_ERROR rejection
368 // Decrement pending write count
369 //
370 // timeout:
371 // Decrement pending write count
372 // Back up to PARAM_SET for retries
373 //
374 // error (PARAM_ERROR or retries exhausted):
375 // Refresh parameter from vehicle
376 // Notify user of failure
377
378 // Create states
379 auto stateMachine = new QGCStateMachine(QStringLiteral("ParameterManager PARAM_SET"), vehicle(), this);
380 auto sendParamSetState = new SendMavlinkMessageState(stateMachine, paramSetEncoder, kParamSetRetryCount);
381 auto incPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager increment pending write count"), stateMachine, [this]() {
382 _incrementPendingWriteCount();
383 });
384 auto decPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager decrement pending write count"), stateMachine, [this]() {
385 _decrementPendingWriteCount();
386 });
387 auto retryDecPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager retry decrement pending write count"), stateMachine, [this]() {
388 _decrementPendingWriteCount();
389 });
390 auto errorDecPendingWriteCountState = new FunctionState(QStringLiteral("ParameterManager error decrement pending write count"), stateMachine, [this]() {
391 _decrementPendingWriteCount();
392 });
393 auto waitAckState = new WaitForParamResponseState(stateMachine, kWaitForParamValueAckMs, checkForCorrectParamValue, checkForParamError);
394 auto paramRefreshState = new FunctionState(QStringLiteral("ParameterManager param refresh"), stateMachine, [this, componentId, paramName]() {
395 refreshParameter(componentId, paramName);
396 });
397 auto userNotifyState = new FunctionState(QStringLiteral("ParameterManager user notify"), stateMachine, [waitAckState, paramName, this, componentId]() {
398 const QString errorDetail = waitAckState->lastParamErrorString();
399 const QString msg = errorDetail.isEmpty()
400 ? QStringLiteral("Parameter write failed: param: %1 %2").arg(paramName, _vehicleAndComponentString(componentId))
401 : QStringLiteral("Parameter write failed: param: %1 %2 - %3").arg(paramName, _vehicleAndComponentString(componentId), errorDetail);
403 });
404 auto logSuccessState = new FunctionState(QStringLiteral("ParameterManager log success"), stateMachine, [this, componentId, paramName]() {
405 qCDebug(ParameterManagerLog) << "Parameter write succeeded: param:" << paramName << _vehicleAndComponentString(componentId);
406 emit _paramSetSuccess(componentId, paramName);
407 });
408 auto logFailureState = new FunctionState(QStringLiteral("ParameterManager log failure"), stateMachine, [this, componentId, paramName]() {
409 qCDebug(ParameterManagerLog) << "Parameter write failed: param:" << paramName << _vehicleAndComponentString(componentId);
410 emit _paramSetFailure(componentId, paramName);
411 });
412 auto finalState = new QGCFinalState(stateMachine);
413
414 // Successful state machine transitions
415 stateMachine->setInitialState(sendParamSetState);
416 sendParamSetState->addThisTransition (&QGCState::advance, incPendingWriteCountState);
417 incPendingWriteCountState->addThisTransition(&QGCState::advance, waitAckState);
418 waitAckState->addThisTransition (&QGCState::advance, decPendingWriteCountState);
419 decPendingWriteCountState->addThisTransition(&QGCState::advance, logSuccessState);
420 logSuccessState->addThisTransition (&QGCState::advance, finalState);
421
422 // Retry transitions (timeout path)
423 waitAckState->addTransition(waitAckState, &WaitStateBase::timeout, retryDecPendingWriteCountState);
424 retryDecPendingWriteCountState->addThisTransition(&QGCState::advance, sendParamSetState);
425
426 // PARAM_ERROR path (definitive rejection - no retries)
427 waitAckState->addThisTransition (&QGCState::error, errorDecPendingWriteCountState);
428 errorDecPendingWriteCountState->addThisTransition (&QGCState::advance, logFailureState);
429
430 // Error transitions (retries exhausted)
431 sendParamSetState->addThisTransition(&QGCState::error, logFailureState);
432
433 // Error state branching transitions
434 logFailureState->addThisTransition (&QGCState::advance, userNotifyState);
435 userNotifyState->addThisTransition (&QGCState::advance, paramRefreshState);
436 paramRefreshState->addThisTransition(&QGCState::advance, finalState);
437
438 qCDebug(ParameterManagerLog) << "Starting state machine for PARAM_SET on: " << paramName << _vehicleAndComponentString(componentId);
439 stateMachine->start();
440}
441
442bool ParameterManager::_fillMavlinkParamUnion(FactMetaData::ValueType_t valueType, const QVariant &rawValue, mavlink_param_union_t &paramUnion) const
443{
444 bool ok = false;
445
446 switch (valueType) {
448 paramUnion.param_uint8 = static_cast<uint8_t>(rawValue.toUInt(&ok));
449 break;
451 paramUnion.param_int8 = static_cast<int8_t>(rawValue.toInt(&ok));
452 break;
454 paramUnion.param_uint16 = static_cast<uint16_t>(rawValue.toUInt(&ok));
455 break;
457 paramUnion.param_int16 = static_cast<int16_t>(rawValue.toInt(&ok));
458 break;
460 paramUnion.param_uint32 = static_cast<uint32_t>(rawValue.toUInt(&ok));
461 break;
463 paramUnion.param_float = rawValue.toFloat(&ok);
464 break;
466 paramUnion.param_int32 = static_cast<int32_t>(rawValue.toInt(&ok));
467 break;
468 default:
469 qCCritical(ParameterManagerLog) << "Internal Error: Unsupported fact value type" << valueType;
470 paramUnion.param_int32 = static_cast<int32_t>(rawValue.toInt(&ok));
471 break;
472 }
473
474 if (!ok) {
475 qCCritical(ParameterManagerLog) << "Fact Failed to Convert to Param Type:" << valueType;
476 return false;
477 }
478
479 return true;
480}
481
482bool ParameterManager::_mavlinkParamUnionToVariant(const mavlink_param_union_t &paramUnion, QVariant &outValue) const
483{
484 switch (paramUnion.type) {
485 case MAV_PARAM_TYPE_REAL32:
486 outValue = QVariant(paramUnion.param_float);
487 return true;
488 case MAV_PARAM_TYPE_UINT8:
489 outValue = QVariant(paramUnion.param_uint8);
490 return true;
491 case MAV_PARAM_TYPE_INT8:
492 outValue = QVariant(paramUnion.param_int8);
493 return true;
494 case MAV_PARAM_TYPE_UINT16:
495 outValue = QVariant(paramUnion.param_uint16);
496 return true;
497 case MAV_PARAM_TYPE_INT16:
498 outValue = QVariant(paramUnion.param_int16);
499 return true;
500 case MAV_PARAM_TYPE_UINT32:
501 outValue = QVariant(paramUnion.param_uint32);
502 return true;
503 case MAV_PARAM_TYPE_INT32:
504 outValue = QVariant(paramUnion.param_int32);
505 return true;
506 default:
507 qCCritical(ParameterManagerLog) << "ParameterManager::_mavlinkParamUnionToVariant - unsupported MAV_PARAM_TYPE" << paramUnion.type;
508 return false;
509 }
510}
511
512void ParameterManager::_factRawValueUpdated(const QVariant &rawValue)
513{
514 Fact *const fact = qobject_cast<Fact*>(sender());
515 if (!fact) {
516 qCWarning(ParameterManagerLog) << "Internal error";
517 return;
518 }
519
520 _mavlinkParamSet(fact->componentId(), fact->name(), fact->type(), rawValue);
521}
522
523void ParameterManager::_ftpDownloadComplete(const QString &fileName, const QString &errorMsg)
524{
525 bool continueWithDefaultParameterdownload = true;
526 bool immediateRetry = false;
527
528 (void) disconnect(_vehicle->ftpManager(), &FTPManager::downloadComplete, this, &ParameterManager::_ftpDownloadComplete);
529 (void) disconnect(_vehicle->ftpManager(), &FTPManager::commandProgress, this, &ParameterManager::_ftpDownloadProgress);
530
531 if (errorMsg.isEmpty()) {
532 qCDebug(ParameterManagerLog) << "ParameterManager::_ftpDownloadComplete : Parameter file received:" << fileName;
533 if (_parseParamFile(fileName)) {
534 qCDebug(ParameterManagerLog) << "ParameterManager::_ftpDownloadComplete : Parsed!";
535 return;
536 } else {
537 qCDebug(ParameterManagerLog) << "ParameterManager::_ftpDownloadComplete : Error in parameter file";
538 /* This should not happen... */
539 }
540 } else if (errorMsg.contains("File Not Found")) {
541 qCDebug(ParameterManagerLog) << "ParameterManager-ftp: No Parameterfile on vehicle - Start Conventional Parameter Download";
542 if (_initialRequestRetryCount == 0) {
543 immediateRetry = true;
544 }
545 } else if ((_loadProgress > 0.0001) && (_loadProgress < 0.01)) { /* FTP supported but too slow */
546 qCDebug(ParameterManagerLog) << "ParameterManager-ftp progress too slow - Start Conventional Parameter Download";
547 } else if (_initialRequestRetryCount == 1) {
548 qCDebug(ParameterManagerLog) << "ParameterManager-ftp: Too many retries - Start Conventional Parameter Download";
549 } else {
550 qCDebug(ParameterManagerLog) << "ParameterManager-ftp Retry:" << _initialRequestRetryCount;
551 continueWithDefaultParameterdownload = false;
552 }
553
554 if (continueWithDefaultParameterdownload) {
555 _tryftp = false;
556 _initialRequestRetryCount = 0;
557 /* If we receive "File not Found" this indicates that the vehicle does not support
558 * the parameter download via ftp. If we received this without retry, then we
559 * can immediately response with the conventional parameter download request, because
560 * we have no indication of communication link congestion.*/
561 if (immediateRetry) {
562 _paramRequestListTimeout();
563 } else {
564 _paramRequestListTimer.start();
565 }
566 } else {
567 _paramRequestListTimer.start();
568 }
569}
570
571void ParameterManager::_ftpDownloadProgress(float progress)
572{
573 qCDebug(ParameterManagerVerbose1Log) << "ParameterManager::_ftpDownloadProgress:" << progress;
574 _setLoadProgress(static_cast<double>(progress));
575 if (progress > 0.001) {
576 _paramRequestListTimer.stop();
577 }
578}
579
580void ParameterManager::_resetHashCheck()
581{
582 _hashCheckTimer.stop();
583 _hashCheckDone = false;
584}
585
587{
588 _resetHashCheck();
590 _startParameterDownload(componentId);
591}
592
594{
595 _resetHashCheck();
596 _cacheOnlyHashCheck = true;
597
598 const SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock();
599 if (!sharedLink) {
601 return;
602 }
603
604 if (sharedLink->linkConfiguration()->isHighLatency() || _logReplay) {
605 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Cache-only hash check: high latency or log replay link, signalling failure";
607 return;
608 }
609
610 if (_vehicle->px4Firmware() && !_initialLoadComplete) {
611 _hashCheckTimer.start();
612 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Cache-only hash check: requesting _HASH_CHECK";
613 _requestHashCheck(MAV_COMP_ID_AUTOPILOT1);
614 } else {
615 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Cache-only hash check: not available, signalling failure";
617 }
618}
619
620void ParameterManager::_startParameterDownload(uint8_t componentId)
621{
622 const SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock();
623 if (!sharedLink) {
624 return;
625 }
626
627 if (sharedLink->linkConfiguration()->isHighLatency() || _logReplay) {
628 // These links don't load params
629 _parametersReady = true;
630 _missingParameters = true;
631 _initialLoadComplete = true;
632 _waitingForDefaultComponent = false;
633 emit parametersReadyChanged(_parametersReady);
634 emit missingParametersChanged(_missingParameters);
635 return;
636 }
637
638 if (_tryftp && ((componentId == MAV_COMP_ID_ALL) || (componentId == MAV_COMP_ID_AUTOPILOT1))) {
639 if (!_initialLoadComplete) {
640 _paramRequestListTimer.start();
641 }
642 FTPManager *const ftpManager = _vehicle->ftpManager();
643 (void) connect(ftpManager, &FTPManager::downloadComplete, this, &ParameterManager::_ftpDownloadComplete);
644 _waitingParamTimeoutTimer.stop();
645 if (ftpManager->download(MAV_COMP_ID_AUTOPILOT1,
646 QStringLiteral("@PARAM/param.pck?withdefaults=1"),
647 QStandardPaths::writableLocation(QStandardPaths::TempLocation),
648 QStringLiteral("param.pck"),
649 false /* No filesize check */)) {
650 (void) connect(ftpManager, &FTPManager::commandProgress, this, &ParameterManager::_ftpDownloadProgress);
651 } else {
652 qCWarning(ParameterManagerLog) << "ParameterManager::_startParameterDownload FTPManager::download returned failure";
653 (void) disconnect(ftpManager, &FTPManager::downloadComplete, this, &ParameterManager::_ftpDownloadComplete);
654 }
655 } else if (_vehicle->px4Firmware() && !_initialLoadComplete && !_hashCheckDone) {
656 // PX4: Try _HASH_CHECK first to see if we can load from cache without a full parameter stream
657 _cacheOnlyHashCheck = false;
658 _hashCheckTimer.start();
659 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Requesting _HASH_CHECK before full parameter list";
660 const uint8_t hashCheckCompId = (componentId == MAV_COMP_ID_ALL)
661 ? static_cast<uint8_t>(MAV_COMP_ID_AUTOPILOT1)
662 : componentId;
663 _requestHashCheck(hashCheckCompId);
664 } else {
665 if (!_initialLoadComplete) {
666 _paramRequestListTimer.start();
667 }
668
669 // Reset index wait lists
670 for (int cid: _paramCountMap.keys()) {
671 // Add/Update all indices to the wait list, parameter index is 0-based
672 if ((componentId != MAV_COMP_ID_ALL) && (componentId != cid)) {
673 continue;
674 }
675 for (int waitingIndex = 0; waitingIndex < _paramCountMap[cid]; waitingIndex++) {
676 // This will add a new waiting index if needed and set the retry count for that index to 0
677 _waitingReadParamIndexMap[cid][waitingIndex] = 0;
678 }
679 }
680
681 mavlink_message_t msg{};
682 mavlink_msg_param_request_list_pack_chan(MAVLinkProtocol::instance()->getSystemId(),
684 sharedLink->mavlinkChannel(),
685 &msg,
686 _vehicle->id(),
687 componentId);
688 (void) _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
689 }
690
691 const QString what = (componentId == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentId);
692 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Request to refresh all parameters for component ID:" << what;
693}
694
695int ParameterManager::_actualComponentId(int componentId) const
696{
697 if (componentId == defaultComponentId) {
698 componentId = _vehicle->defaultComponentId();
699 if (componentId == defaultComponentId) {
700 qCWarning(ParameterManagerLog) << _logVehiclePrefix(-1) << "Default component id not set";
701 }
702 }
703
704 return componentId;
705}
706
707void ParameterManager::refreshParameter(int componentId, const QString &paramName)
708{
709 componentId = _actualComponentId(componentId);
710
711 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParameter - name:" << paramName << ")";
712
713 _mavlinkParamRequestRead(componentId, paramName, -1, true /* notifyFailure */);
714}
715
716void ParameterManager::refreshParametersPrefix(int componentId, const QString &namePrefix)
717{
718 componentId = _actualComponentId(componentId);
719 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "refreshParametersPrefix - name:" << namePrefix << ")";
720
721 for (const QString &paramName: _mapCompId2FactMap[componentId].keys()) {
722 if (paramName.startsWith(namePrefix)) {
723 refreshParameter(componentId, paramName);
724 }
725 }
726}
727
728bool ParameterManager::parameterExists(int componentId, const QString &paramName) const
729{
730 bool ret = false;
731
732 componentId = _actualComponentId(componentId);
733 if (_mapCompId2FactMap.contains(componentId)) {
734 ret = _mapCompId2FactMap[componentId].contains(_remapParamNameToVersion(paramName));
735 }
736
737 return ret;
738}
739
740Fact *ParameterManager::getParameter(int componentId, const QString &paramName)
741{
742 componentId = _actualComponentId(componentId);
743
744 const QString mappedParamName = _remapParamNameToVersion(paramName);
745 if (!_mapCompId2FactMap.contains(componentId) || !_mapCompId2FactMap[componentId].contains(mappedParamName)) {
746 qgcApp()->reportMissingParameter(componentId, mappedParamName);
747 return &_defaultFact;
748 }
749
750 return _mapCompId2FactMap[componentId][mappedParamName];
751}
752
753QStringList ParameterManager::parameterNames(int componentId) const
754{
755 QStringList names;
756
757 const int compId = _actualComponentId(componentId);
758 const QMap<QString, Fact*> &factMap = _mapCompId2FactMap[compId];
759 for (const QString &paramName: factMap.keys()) {
760 names << paramName;
761 }
762
763 return names;
764}
765
766bool ParameterManager::_fillIndexBatchQueue(bool waitingParamTimeout)
767{
768 if (!_indexBatchQueueActive) {
769 return false;
770 }
771
772 constexpr int maxBatchSize = 10;
773
774 if (waitingParamTimeout) {
775 // We timed out, clear the queue and try again
776 qCDebug(ParameterManagerLog) << "Refilling index based batch queue due to timeout";
777 _indexBatchQueue.clear();
778 } else {
779 qCDebug(ParameterManagerLog) << "Refilling index based batch queue due to received parameter";
780 }
781
782 for (const int componentId: _waitingReadParamIndexMap.keys()) {
783 if (_waitingReadParamIndexMap[componentId].count()) {
784 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "_waitingReadParamIndexMap count" << _waitingReadParamIndexMap[componentId].count();
785 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "_waitingReadParamIndexMap" << _waitingReadParamIndexMap[componentId];
786 }
787
788 for (const int paramIndex: _waitingReadParamIndexMap[componentId].keys()) {
789 if (_indexBatchQueue.contains(paramIndex)) {
790 // Don't add more than once
791 continue;
792 }
793
794 if (_indexBatchQueue.count() > maxBatchSize) {
795 break;
796 }
797
798 _waitingReadParamIndexMap[componentId][paramIndex]++; // Bump retry count
799 if (_disableAllRetries || (_waitingReadParamIndexMap[componentId][paramIndex] > _maxInitialLoadRetrySingleParam)) {
800 // Give up on this index
801 _failedReadParamIndexMap[componentId] << paramIndex;
802 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Giving up on (paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
803 (void) _waitingReadParamIndexMap[componentId].remove(paramIndex);
804 } else {
805 // Retry again
806 _indexBatchQueue.append(paramIndex);
807 _mavlinkParamRequestRead(componentId, QString(), paramIndex, false /* notifyFailure */);
808 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Read re-request for (paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
809 }
810 }
811 }
812
813 return (!_indexBatchQueue.isEmpty());
814}
815
816void ParameterManager::_waitingParamTimeout()
817{
818 if (_logReplay) {
819 return;
820 }
821
822 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "_waitingParamTimeout";
823
824 // Now that we have timed out for possibly the first time we can activate the index batch queue
825 _indexBatchQueueActive = true;
826
827 // First check for any missing parameters from the initial index based load
828 bool paramsRequested = _fillIndexBatchQueue(true /* waitingParamTimeout */);
829 if (!paramsRequested && !_waitingForDefaultComponent && !_mapCompId2FactMap.contains(_vehicle->defaultComponentId())) {
830 // Initial load is complete but we still don't have any default component params. Wait one more cycle to see if the
831 // any show up.
832 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer - still don't have default component params" << _vehicle->defaultComponentId();
833 _waitingParamTimeoutTimer.start();
834 _waitingForDefaultComponent = true;
835 return;
836 }
837 _waitingForDefaultComponent = false;
838
839 _checkInitialLoadComplete();
840
841 if (paramsRequested) {
842 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Restarting _waitingParamTimeoutTimer - re-request";
843 _waitingParamTimeoutTimer.start();
844 }
845}
846
847void ParameterManager::_requestHashCheck(uint8_t componentId)
848{
849 const SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock();
850 if (!sharedLink) {
851 return;
852 }
853
854 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Sending PARAM_REQUEST_READ for _HASH_CHECK";
855
856 char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1] = {};
857 (void) strncpy(paramId, "_HASH_CHECK", MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
858
859 mavlink_message_t msg{};
860 (void) mavlink_msg_param_request_read_pack_chan(
863 sharedLink->mavlinkChannel(),
864 &msg,
865 static_cast<uint8_t>(_vehicle->id()),
866 componentId,
867 paramId,
868 -1);
869
870 (void) _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
871}
872
873void ParameterManager::_mavlinkParamRequestRead(int componentId, const QString &paramName, int paramIndex, bool notifyFailure)
874{
875 auto paramRequestReadEncoder = [this, componentId, paramName, paramIndex](uint8_t /*systemId*/, uint8_t channel, mavlink_message_t *message) -> void {
876 char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1] = {};
877 (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
878
879 (void) mavlink_msg_param_request_read_pack_chan(MAVLinkProtocol::instance()->getSystemId(), // QGC system id
880 MAVLinkProtocol::getComponentId(), // QGC component id
881 channel,
882 message,
883 static_cast<uint8_t>(_vehicle->id()),
884 static_cast<uint8_t>(componentId),
885 paramId,
886 static_cast<int16_t>(paramIndex));
887 };
888
889 auto checkForCorrectParamValue = [componentId, paramName, paramIndex](const mavlink_message_t &message) -> bool {
890 if (message.compid != componentId) {
891 return false;
892 }
893
894 mavlink_param_value_t param_value{};
895 mavlink_msg_param_value_decode(&message, &param_value);
896
897 // This will null terminate the name string
898 char parameterNameWithNull[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1] = {};
899 (void) strncpy(parameterNameWithNull, param_value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
900 const QString parameterName(parameterNameWithNull);
901
902 // Check that this is for the parameter we requested
903 if (paramIndex != -1) {
904 // Index based request
905 if (param_value.param_index != paramIndex) {
906 return false;
907 }
908 } else {
909 // Name based request
910 if (parameterName != paramName) {
911 return false;
912 }
913 }
914
915 return true;
916 };
917
918 auto checkForParamError = [componentId, paramName, paramIndex](const mavlink_message_t &message) -> bool {
919 if (message.compid != componentId) {
920 return false;
921 }
922
923 mavlink_param_error_t paramError{};
924 mavlink_msg_param_error_decode(&message, &paramError);
925
926 char paramId[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN + 1] = {};
927 (void) strncpy(paramId, paramError.param_id, MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN);
928
929 if (paramIndex != -1) {
930 return paramError.param_index == paramIndex;
931 } else {
932 return QString(paramId) == paramName;
933 }
934 };
935
936 // State Machine:
937 // Send PARAM_REQUEST_READ - 2 retries after initial attempt
938 // Wait for PARAM_VALUE ack or PARAM_ERROR rejection
939 //
940 // timeout:
941 // Back up to PARAM_REQUEST_READ for retries
942 //
943 // error (PARAM_ERROR or retries exhausted):
944 // Notify user of failure
945
946 // Create states
947 auto stateMachine = new QGCStateMachine(QStringLiteral("PARAM_REQUEST_READ"), vehicle(), this);
948 auto sendParamRequestReadState = new SendMavlinkMessageState(stateMachine, paramRequestReadEncoder, kParamRequestReadRetryCount);
949 auto waitAckState = new WaitForParamResponseState(stateMachine, kWaitForParamValueAckMs, checkForCorrectParamValue, checkForParamError);
950 auto userNotifyState = new FunctionState(QStringLiteral("User notify"), stateMachine, [waitAckState, paramName, this, componentId]() {
951 const QString errorDetail = waitAckState->lastParamErrorString();
952 const QString msg = errorDetail.isEmpty()
953 ? QStringLiteral("Parameter read failed: param: %1 %2").arg(paramName, _vehicleAndComponentString(componentId))
954 : QStringLiteral("Parameter read failed: param: %1 %2 - %3").arg(paramName, _vehicleAndComponentString(componentId), errorDetail);
956 });
957 auto logSuccessState = new FunctionState(QStringLiteral("Log success"), stateMachine, [this, componentId, paramName, paramIndex]() {
958 qCDebug(ParameterManagerLog) << "PARAM_REQUEST_READ succeeded: name:" << paramName << "index" << paramIndex << _vehicleAndComponentString(componentId);
959 emit _paramRequestReadSuccess(componentId, paramName, paramIndex);
960 });
961 auto logFailureState = new FunctionState(QStringLiteral("Log failure"), stateMachine, [this, componentId, paramName, paramIndex]() {
962 qCDebug(ParameterManagerLog) << "PARAM_REQUEST_READ failed: param:" << paramName << "index" << paramIndex << _vehicleAndComponentString(componentId);
963 emit _paramRequestReadFailure(componentId, paramName, paramIndex);
964 });
965 auto finalState = new QGCFinalState(stateMachine);
966
967 // Successful state machine transitions
968 stateMachine->setInitialState(sendParamRequestReadState);
969 sendParamRequestReadState->addThisTransition(&QGCState::advance, waitAckState);
970 waitAckState->addThisTransition (&QGCState::advance, logSuccessState);
971 logSuccessState->addThisTransition (&QGCState::advance, finalState);
972
973 // Retry transitions (timeout path)
974 waitAckState->addTransition(waitAckState, &WaitStateBase::timeout, sendParamRequestReadState);
975
976 // PARAM_ERROR path (definitive rejection - no retries)
977 waitAckState->addThisTransition(&QGCState::error, logFailureState);
978
979 // Error transitions (retries exhausted)
980 sendParamRequestReadState->addThisTransition(&QGCState::error, logFailureState);
981
982 // Error state branching transitions
983 if (notifyFailure) {
984 logFailureState->addThisTransition (&QGCState::advance, userNotifyState);
985 } else {
986 logFailureState->addThisTransition (&QGCState::advance, finalState);
987 }
988 userNotifyState->addThisTransition (&QGCState::advance, finalState);
989
990 qCDebug(ParameterManagerLog) << "Starting state machine for PARAM_REQUEST_READ on: " << paramName << _vehicleAndComponentString(componentId);
991 stateMachine->start();
992}
993
994void ParameterManager::_writeLocalParamCache(int vehicleId, int componentId)
995{
996 CacheMapName2ParamTypeVal cacheMap;
997
998 for (const QString &paramName: _mapCompId2FactMap[componentId].keys()) {
999 const Fact *const fact = _mapCompId2FactMap[componentId][paramName];
1000 cacheMap[paramName] = ParamTypeVal(fact->type(), fact->rawValue());
1001 }
1002
1003 QFile cacheFile(parameterCacheFile(vehicleId, componentId));
1004 if (cacheFile.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
1005 QDataStream ds(&cacheFile);
1006 ds << cacheMap;
1007 } else {
1008 qCWarning(ParameterManagerLog) << "Failed to open cache file for writing" << cacheFile.fileName();
1009 }
1010}
1011
1013{
1014 // Use application-specific subdirectory to isolate parallel test runs
1015 const QFileInfo settingsFile(QSettings().fileName());
1016 const QString basePath = settingsFile.dir().absolutePath();
1017 const QString appName = settingsFile.completeBaseName();
1018 return QDir(basePath + QDir::separator() + appName + QDir::separator() + QStringLiteral("ParamCache"));
1019}
1020
1021QString ParameterManager::parameterCacheFile(int vehicleId, int componentId)
1022{
1023 return parameterCacheDir().filePath(QStringLiteral("%1_%2.v2").arg(vehicleId).arg(componentId));
1024}
1025
1026void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, const QVariant &hashValue)
1027{
1028 qCDebug(ParameterManagerLog) << "Attemping load from cache";
1029
1030 /* The datastructure of the cache table */
1031 CacheMapName2ParamTypeVal cacheMap;
1032 QFile cacheFile(parameterCacheFile(vehicleId, componentId));
1033 if (!cacheFile.exists()) {
1034 qCDebug(ParameterManagerLog) << "No parameter cache file";
1035 if (!_hashCheckDone) {
1036 _hashCheckDone = true;
1037 if (_cacheOnlyHashCheck) {
1038 qCDebug(ParameterManagerLog) << "Cache-only hash check: no cache file, signalling failure";
1039 emit cacheCheckOnlyFailed();
1040 return;
1041 }
1042 // Standalone hash check path — fall back to PARAM_REQUEST_LIST
1043 _startParameterDownload(MAV_COMP_ID_ALL);
1044 }
1045 // If already in PARAM_REQUEST_LIST flow, just let the stream continue
1046 return;
1047 }
1048 (void) cacheFile.open(QIODevice::ReadOnly);
1049
1050 /* Deserialize the parameter cache table */
1051 QDataStream ds(&cacheFile);
1052 ds >> cacheMap;
1053
1054 /* compute the crc of the local cache to check against the remote */
1055 uint32_t crc32_value = 0;
1056 for (const QString &name: cacheMap.keys()) {
1057 const ParamTypeVal &paramTypeVal = cacheMap[name];
1058 const FactMetaData::ValueType_t factType = static_cast<FactMetaData::ValueType_t>(paramTypeVal.first);
1059
1060 if (_vehicle->compInfoManager()->compInfoParam(MAV_COMP_ID_AUTOPILOT1)->factMetaDataForName(name, factType)->volatileValue()) {
1061 // Does not take part in CRC
1062 qCDebug(ParameterManagerLog) << "Volatile parameter" << name;
1063 } else {
1064 const void *const vdat = paramTypeVal.second.constData();
1065 const FactMetaData::ValueType_t cacheFactType = static_cast<FactMetaData::ValueType_t>(paramTypeVal.first);
1066 crc32_value = QGC::crc32(reinterpret_cast<const uint8_t *>(qPrintable(name)), name.length(), crc32_value);
1067 crc32_value = QGC::crc32(static_cast<const uint8_t *>(vdat), FactMetaData::typeToSize(cacheFactType), crc32_value);
1068 }
1069 }
1070
1071 /* if the two param set hashes match, just load from the disk */
1072 if (crc32_value == hashValue.toUInt()) {
1073 _hashCheckDone = true;
1074 _paramRequestListTimer.stop();
1075 qCDebug(ParameterManagerLog) << "Parameters loaded from cache" << qPrintable(QFileInfo(cacheFile).absoluteFilePath());
1076
1077 const int count = cacheMap.count();
1078 int index = 0;
1079 for (const QString &name: cacheMap.keys()) {
1080 const ParamTypeVal &paramTypeVal = cacheMap[name];
1081 const FactMetaData::ValueType_t factType = static_cast<FactMetaData::ValueType_t>(paramTypeVal.first);
1082 const MAV_PARAM_TYPE mavParamType = factTypeToMavType(factType);
1083 _handleParamValue(componentId, name, count, index++, mavParamType, paramTypeVal.second);
1084 }
1085
1086 const SharedLinkInterfacePtr sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock();
1087 if (sharedLink) {
1088 mavlink_param_set_t p{};
1089 mavlink_param_union_t union_value{};
1090
1091 // Return the hash value to notify we don't want any more updates
1092 p.param_type = MAV_PARAM_TYPE_UINT32;
1093 (void) strncpy(p.param_id, "_HASH_CHECK", sizeof(p.param_id));
1094 union_value.param_uint32 = crc32_value;
1095 p.param_value = union_value.param_float;
1096 p.target_system = static_cast<uint8_t>(_vehicle->id());
1097 p.target_component = static_cast<uint8_t>(componentId);
1098
1099 mavlink_message_t msg{};
1100 (void) mavlink_msg_param_set_encode_chan(MAVLinkProtocol::instance()->getSystemId(),
1102 sharedLink->mavlinkChannel(),
1103 &msg,
1104 &p);
1105 (void) _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
1106 }
1107
1108 // Give the user some feedback things loaded properly
1109 QVariantAnimation *const ani = new QVariantAnimation(this);
1110 ani->setEasingCurve(QEasingCurve::OutCubic);
1111 ani->setStartValue(0.0);
1112 ani->setEndValue(1.0);
1113 ani->setDuration(750);
1114
1115 (void) connect(ani, &QVariantAnimation::valueChanged, this, [this](const QVariant &value) {
1116 _setLoadProgress(value.toDouble());
1117 });
1118
1119 // Hide 500ms after animation finishes
1120 connect(ani, &QVariantAnimation::finished, this, [this] {
1121 QTimer::singleShot(500, this, [this] {
1122 _setLoadProgress(0);
1123 });
1124 });
1125
1126 ani->start(QAbstractAnimation::DeleteWhenStopped);
1127 } else {
1128 qCDebug(ParameterManagerLog) << "Parameters cache match failed" << qPrintable(QFileInfo(cacheFile).absoluteFilePath());
1129 if (ParameterManagerDebugCacheFailureLog().isDebugEnabled()) {
1130 _debugCacheCRC[componentId] = true;
1131 _debugCacheMap[componentId] = cacheMap;
1132 for (const QString &name: cacheMap.keys()) {
1133 _debugCacheParamSeen[componentId][name] = false;
1134 }
1135 QGC::showAppMessage(tr("Parameter cache CRC match failed"));
1136 }
1137 if (!_hashCheckDone) {
1138 _hashCheckDone = true;
1139 if (_cacheOnlyHashCheck) {
1140 qCDebug(ParameterManagerLog) << "Cache-only hash check: CRC mismatch, signalling failure";
1141 emit cacheCheckOnlyFailed();
1142 return;
1143 }
1144 // Standalone hash check path — fall back to PARAM_REQUEST_LIST
1145 _startParameterDownload(MAV_COMP_ID_ALL);
1146 }
1147 // If already in PARAM_REQUEST_LIST flow, just let the stream continue
1148 }
1149}
1150
1152{
1153 QString missingErrors;
1154 QString typeErrors;
1155
1156 while (!stream.atEnd()) {
1157 const QString line = stream.readLine();
1158 if (!line.startsWith("#")) {
1159 const QStringList wpParams = line.split("\t");
1160 const int lineMavId = wpParams.at(0).toInt();
1161 if (wpParams.size() == 5) {
1162 if (_vehicle->id() != lineMavId) {
1163 return QStringLiteral("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());
1164 }
1165
1166 const int componentId = wpParams.at(1).toInt();
1167 const QString paramName = wpParams.at(2);
1168 const QString valStr = wpParams.at(3);
1169 const uint mavType = wpParams.at(4).toUInt();
1170
1171 if (!parameterExists(componentId, paramName)) {
1172 QString error;
1173 error += QStringLiteral("%1:%2").arg(componentId).arg(paramName);
1174 missingErrors += error;
1175 qCDebug(ParameterManagerLog) << "Skipped due to missing:" << error;
1176 continue;
1177 }
1178
1179 Fact *const fact = getParameter(componentId, paramName);
1180 if (fact->type() != mavTypeToFactType(static_cast<MAV_PARAM_TYPE>(mavType))) {
1181 QString error;
1182 error = QStringLiteral("%1:%2 ").arg(componentId).arg(paramName);
1183 typeErrors += error;
1184 qCDebug(ParameterManagerLog) << "Skipped due to type mismatch: %1" << error;
1185 continue;
1186 }
1187
1188 qCDebug(ParameterManagerLog) << "Updating parameter" << componentId << paramName << valStr;
1189 fact->setRawValue(valStr);
1190 }
1191 }
1192 }
1193
1194 QString errors;
1195
1196 if (!missingErrors.isEmpty()) {
1197 errors = tr("Parameters not loaded since they are not currently on the vehicle: %1\n").arg(missingErrors);
1198 }
1199
1200 if (!typeErrors.isEmpty()) {
1201 errors += tr("Parameters not loaded due to type mismatch: %1").arg(typeErrors);
1202 }
1203
1204 return errors;
1205}
1206
1207void ParameterManager::writeParametersToStream(QTextStream &stream) const
1208{
1209 stream << "# Onboard parameters for Vehicle " << _vehicle->id() << "\n";
1210 stream << "#\n";
1211
1212 stream << "# Stack: " << QGCMAVLink::firmwareClassToCanonicalString(QGCMAVLink::firmwareClass(_vehicle->firmwareType())) << "\n";
1213 stream << "# Vehicle: " << QGCMAVLink::vehicleClassToCanonicalString(QGCMAVLink::vehicleClass(_vehicle->vehicleType())) << "\n";
1214 stream << "# Version: "
1215 << _vehicle->firmwareMajorVersion() << "."
1216 << _vehicle->firmwareMinorVersion() << "."
1217 << _vehicle->firmwarePatchVersion() << " "
1218 << _vehicle->firmwareVersionTypeString() << "\n";
1219 stream << "# Git Revision: " << _vehicle->gitHash() << "\n";
1220
1221 stream << "#\n";
1222 stream << "# Vehicle-Id Component-Id Name Value Type\n";
1223
1224 for (const int componentId: _mapCompId2FactMap.keys()) {
1225 for (const QString &paramName: _mapCompId2FactMap[componentId].keys()) {
1226 const Fact *const fact = _mapCompId2FactMap[componentId][paramName];
1227 if (fact) {
1228 stream << _vehicle->id() << "\t" << componentId << "\t" << paramName << "\t" << fact->rawValueStringFullPrecision() << "\t" << QStringLiteral("%1").arg(factTypeToMavType(fact->type())) << "\n";
1229 } else {
1230 qCWarning(ParameterManagerLog) << "Internal error: missing fact";
1231 }
1232 }
1233 }
1234
1235 stream.flush();
1236}
1237
1239{
1240 switch (factType) {
1242 return MAV_PARAM_TYPE_UINT8;
1244 return MAV_PARAM_TYPE_INT8;
1246 return MAV_PARAM_TYPE_UINT16;
1248 return MAV_PARAM_TYPE_INT16;
1250 return MAV_PARAM_TYPE_UINT32;
1252 return MAV_PARAM_TYPE_UINT64;
1254 return MAV_PARAM_TYPE_INT64;
1256 return MAV_PARAM_TYPE_REAL32;
1258 return MAV_PARAM_TYPE_REAL64;
1259 default:
1260 qCWarning(ParameterManagerLog) << "Unsupported fact type" << factType;
1261 [[fallthrough]];
1263 return MAV_PARAM_TYPE_INT32;
1264 }
1265}
1266
1268{
1269 switch (mavType) {
1270 case MAV_PARAM_TYPE_UINT8:
1272 case MAV_PARAM_TYPE_INT8:
1274 case MAV_PARAM_TYPE_UINT16:
1276 case MAV_PARAM_TYPE_INT16:
1278 case MAV_PARAM_TYPE_UINT32:
1280 case MAV_PARAM_TYPE_UINT64:
1282 case MAV_PARAM_TYPE_INT64:
1284 case MAV_PARAM_TYPE_REAL32:
1286 case MAV_PARAM_TYPE_REAL64:
1288 default:
1289 qCWarning(ParameterManagerLog) << "Unsupported mav param type" << mavType;
1290 [[fallthrough]];
1291 case MAV_PARAM_TYPE_INT32:
1293 }
1294}
1295
1296void ParameterManager::_checkInitialLoadComplete()
1297{
1298 if (_initialLoadComplete) {
1299 return;
1300 }
1301
1302 for (const int componentId: _waitingReadParamIndexMap.keys()) {
1303 if (!_waitingReadParamIndexMap[componentId].isEmpty()) {
1304 // We are still waiting on some parameters, not done yet
1305 return;
1306 }
1307 }
1308
1309 if (!_mapCompId2FactMap.contains(_vehicle->defaultComponentId())) {
1310 // No default component params yet, not done yet
1311 return;
1312 }
1313
1314 // We aren't waiting for any more initial parameter updates, initial parameter loading is complete
1315 _initialLoadComplete = true;
1316
1317 // Parameter cache crc failure debugging
1318 for (const int componentId: _debugCacheParamSeen.keys()) {
1319 if (!_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) {
1320 for (const QString &paramName: _debugCacheParamSeen[componentId].keys()) {
1321 if (!_debugCacheParamSeen[componentId][paramName]) {
1322 qCDebug(ParameterManagerLog) << "Parameter in cache but not on vehicle componentId:Name" << componentId << paramName;
1323 }
1324 }
1325 }
1326 }
1327 _debugCacheCRC.clear();
1328
1329 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Initial load complete";
1330
1331 // Check for index based load failures
1332 QString indexList;
1333 bool initialLoadFailures = false;
1334 for (const int componentId: _failedReadParamIndexMap.keys()) {
1335 for (const int paramIndex: _failedReadParamIndexMap[componentId]) {
1336 if (initialLoadFailures) {
1337 indexList += ", ";
1338 }
1339 indexList += QStringLiteral("%1:%2").arg(componentId).arg(paramIndex);
1340 initialLoadFailures = true;
1341 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) << "Gave up on initial load after max retries (paramIndex:" << paramIndex << ")";
1342 }
1343 }
1344
1345 _missingParameters = false;
1346 if (initialLoadFailures) {
1347 _missingParameters = true;
1348 const QString errorMsg = tr("%1 was unable to retrieve the full set of parameters from vehicle %2. "
1349 "This will cause %1 to be unable to display its full user interface. "
1350 "If you are using modified firmware, you may need to resolve any vehicle startup errors to resolve the issue. "
1351 "If you are using standard firmware, you may need to upgrade to a newer version to resolve the issue.").arg(QCoreApplication::applicationName()).arg(_vehicle->id());
1352 qCDebug(ParameterManagerLog) << errorMsg;
1353 QGC::showAppMessage(errorMsg);
1354 if (!QGC::runningUnitTests()) {
1355 qCWarning(ParameterManagerLog) << _logVehiclePrefix(-1) << "The following parameter indices could not be loaded after the maximum number of retries:" << indexList;
1356 }
1357 }
1358
1359 // Signal load complete
1360 _parametersReady = true;
1362 emit parametersReadyChanged(true);
1363 emit missingParametersChanged(_missingParameters);
1364}
1365
1366void ParameterManager::_hashCheckTimeout()
1367{
1368 _hashCheckDone = true;
1369 if (_cacheOnlyHashCheck) {
1370 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "_HASH_CHECK timed out in cache-only mode, signalling failure";
1371 emit cacheCheckOnlyFailed();
1372 return;
1373 }
1374 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "_HASH_CHECK timed out, falling back to PARAM_REQUEST_LIST";
1375 _startParameterDownload(MAV_COMP_ID_ALL);
1376}
1377
1378void ParameterManager::_paramRequestListTimeout()
1379{
1380 if (_logReplay) {
1381 // Signal load complete
1382 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "_paramRequestListTimeout (log replay): Signalling load complete";
1383 _initialLoadComplete = true;
1384 _missingParameters = false;
1385 _parametersReady = true;
1387 emit parametersReadyChanged(true);
1388 emit missingParametersChanged(_missingParameters);
1389 return;
1390 }
1391
1392 if (!_disableAllRetries && (++_initialRequestRetryCount <= _maxInitialRequestListRetry)) {
1393 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) << "Retrying initial parameter request list";
1394 _startParameterDownload(MAV_COMP_ID_ALL);
1395 } else if (!_vehicle->genericFirmware()) {
1396 const QString errorMsg = tr("Vehicle %1 did not respond to request for parameters. "
1397 "This will cause %2 to be unable to display its full user interface.").arg(_vehicle->id()).arg(QCoreApplication::applicationName());
1398 qCDebug(ParameterManagerLog) << errorMsg;
1399 QGC::showAppMessage(errorMsg);
1400 }
1401}
1402
1403QString ParameterManager::_remapParamNameToVersion(const QString &paramName) const
1404{
1405 static const QString noRemapPrefix = QStringLiteral("noremap.");
1406 if (paramName.startsWith(noRemapPrefix)) {
1407 return paramName.mid(noRemapPrefix.length());
1408 }
1409
1410 const int majorVersion = _vehicle->firmwareMajorVersion();
1411 const int minorVersion = _vehicle->firmwareMinorVersion();
1412
1413 qCDebug(ParameterManagerLog) << "_remapParamNameToVersion" << paramName << majorVersion << minorVersion;
1414
1415 if (majorVersion == Vehicle::versionNotSetValue) {
1416 // Vehicle version unknown
1417 return paramName;
1418 }
1419
1421 if (!majorVersionRemap.contains(majorVersion)) {
1422 // No mapping for this major version
1423 qCDebug(ParameterManagerLog) << "_remapParamNameToVersion: no major version mapping";
1424 return paramName;
1425 }
1426
1427 const FirmwarePlugin::remapParamNameMinorVersionRemapMap_t &remapMinorVersion = majorVersionRemap[majorVersion];
1428 // We must map backwards from the highest known minor version to one above the vehicle's minor version
1429 QString mappedParamName = paramName;
1430 for (int currentMinorVersion = _vehicle->firmwarePlugin()->remapParamNameHigestMinorVersionNumber(majorVersion); currentMinorVersion>minorVersion; currentMinorVersion--) {
1431 if (remapMinorVersion.contains(currentMinorVersion)) {
1432 const FirmwarePlugin::remapParamNameMap_t &remap = remapMinorVersion[currentMinorVersion];
1433 if (remap.contains(mappedParamName)) {
1434 const QString toParamName = remap[mappedParamName];
1435 qCDebug(ParameterManagerLog) << "_remapParamNameToVersion: remapped currentMinor:from:to" << currentMinorVersion << mappedParamName << toParamName;
1436 mappedParamName = toParamName;
1437 }
1438 }
1439 }
1440
1441 return mappedParamName;
1442}
1443
1444void ParameterManager::_loadOfflineEditingParams()
1445{
1446 const QString paramFilename = _vehicle->firmwarePlugin()->offlineEditingParamFile(_vehicle);
1447 if (paramFilename.isEmpty()) {
1448 return;
1449 }
1450
1451 QFile paramFile(paramFilename);
1452 if (!paramFile.open(QFile::ReadOnly)) {
1453 qCWarning(ParameterManagerLog) << "Unable to open offline editing params file" << paramFilename;
1454 }
1455
1456 QTextStream paramStream(&paramFile);
1457 while (!paramStream.atEnd()) {
1458 const QString line = paramStream.readLine();
1459 if (line.startsWith("#")) {
1460 continue;
1461 }
1462
1463 const QStringList paramData = line.split("\t");
1464 Q_ASSERT(paramData.count() == 5);
1465
1466 const int offlineDefaultComponentId = paramData.at(1).toInt();
1467 _vehicle->setOfflineEditingDefaultComponentId(offlineDefaultComponentId);
1468 const QString paramName = paramData.at(2);
1469 const QString valStr = paramData.at(3);
1470 const MAV_PARAM_TYPE paramType = static_cast<MAV_PARAM_TYPE>(paramData.at(4).toUInt());
1471
1472 QVariant paramValue;
1473 switch (paramType) {
1474 case MAV_PARAM_TYPE_REAL32:
1475 paramValue = QVariant(valStr.toFloat());
1476 break;
1477 case MAV_PARAM_TYPE_UINT32:
1478 paramValue = QVariant(valStr.toUInt());
1479 break;
1480 case MAV_PARAM_TYPE_UINT16:
1481 paramValue = QVariant((quint16)valStr.toUInt());
1482 break;
1483 case MAV_PARAM_TYPE_INT16:
1484 paramValue = QVariant((qint16)valStr.toInt());
1485 break;
1486 case MAV_PARAM_TYPE_UINT8:
1487 paramValue = QVariant((quint8)valStr.toUInt());
1488 break;
1489 case MAV_PARAM_TYPE_INT8:
1490 paramValue = QVariant((qint8)valStr.toUInt());
1491 break;
1492 default:
1493 qCCritical(ParameterManagerLog) << "Unknown type" << paramType;
1494 [[fallthrough]];
1495 case MAV_PARAM_TYPE_INT32:
1496 paramValue = QVariant(valStr.toInt());
1497 break;
1498 }
1499
1500 Fact *const fact = new Fact(offlineDefaultComponentId, paramName, mavTypeToFactType(paramType), this);
1501
1502 FactMetaData *const factMetaData = _vehicle->compInfoManager()->compInfoParam(offlineDefaultComponentId)->factMetaDataForName(paramName, fact->type());
1503 fact->setMetaData(factMetaData);
1504
1505 _mapCompId2FactMap[defaultComponentId][paramName] = fact;
1506 }
1507
1508 _parametersReady = true;
1509 _initialLoadComplete = true;
1510 _debugCacheCRC.clear();
1511}
1512
1514{
1515 _vehicle->sendMavCommand(MAV_COMP_ID_AUTOPILOT1,
1516 MAV_CMD_PREFLIGHT_STORAGE,
1517 true, // showError
1518 2, // Reset params to default
1519 -1); // Don't do anything with mission storage
1520}
1521
1523{
1524 //-- https://github.com/PX4/Firmware/pull/11760
1525 Fact *const sysAutoConfigFact = getParameter(defaultComponentId, "SYS_AUTOCONFIG");
1526 if (sysAutoConfigFact) {
1527 sysAutoConfigFact->setRawValue(2);
1528 }
1529}
1530
1531QString ParameterManager::_logVehiclePrefix(int componentId) const
1532{
1533 if (componentId == -1) {
1534 return QStringLiteral("V:%1").arg(_vehicle->id());
1535 } else {
1536 return QStringLiteral("V:%1 C:%2").arg(_vehicle->id()).arg(componentId);
1537 }
1538}
1539
1540void ParameterManager::_setLoadProgress(double loadProgress)
1541{
1542 if (_loadProgress != loadProgress) {
1543 _loadProgress = loadProgress;
1544 emit loadProgressChanged(static_cast<float>(loadProgress));
1545 }
1546}
1547
1549{
1550 if (_parameterDownloadSkipped != skipped) {
1551 _parameterDownloadSkipped = skipped;
1553 }
1554}
1555
1557{
1558 return _paramCountMap.keys();
1559}
1560
1562{
1563 return _pendingWritesCount > 0;
1564}
1565
1567{
1568 return _vehicle;
1569}
1570
1571
1572bool ParameterManager::_parseParamFile(const QString& filename)
1573{
1574 constexpr quint16 magic_standard = 0x671B;
1575 constexpr quint16 magic_withdefaults = 0x671C;
1576 quint32 no_of_parameters_found = 0;
1577 constexpr int componentId = MAV_COMP_ID_AUTOPILOT1;
1578 enum ap_var_type {
1579 AP_PARAM_NONE = 0,
1580 AP_PARAM_INT8,
1581 AP_PARAM_INT16,
1582 AP_PARAM_INT32,
1583 AP_PARAM_FLOAT,
1584 AP_PARAM_VECTOR3F,
1585 AP_PARAM_GROUP
1586 };
1587
1588 qCDebug(ParameterManagerLog) << "_parseParamFile:" << filename;
1589 QFile file(filename);
1590 if (!file.open(QIODevice::ReadOnly)) {
1591 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: Could not open downloaded parameter file.";
1592 return false;
1593 }
1594
1595 QDataStream in(&file);
1596 in.setByteOrder(QDataStream::LittleEndian);
1597
1598 quint16 magic, num_params, total_params;
1599 in >> magic;
1600 in >> num_params;
1601 in >> total_params;
1602
1603 if (in.status() != QDataStream::Ok) {
1604 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: Could not read Header";
1605 goto Error;
1606 }
1607
1608 qCDebug(ParameterManagerVerbose2Log) << "_parseParamFile: magic: 0x" << Qt::hex << magic;
1609 qCDebug(ParameterManagerVerbose2Log) << "_parseParamFile: num_params:" << num_params
1610 << "total_params:" << total_params;
1611
1612 if ((magic != magic_standard) && (magic != magic_withdefaults)) {
1613 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: File does not start with Magic";
1614 goto Error;
1615 }
1616 if (num_params > total_params) {
1617 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: total_params > num_params";
1618 goto Error;
1619 }
1620 if (num_params != total_params) {
1621 /* We requested all parameters, so this is an error here */
1622 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: total_params != num_params";
1623 goto Error;
1624 }
1625
1626 while (in.status() == QDataStream::Ok) {
1627 quint8 byte = 0;
1628 quint8 flags = 0;
1629 quint8 ptype = 0;
1630 quint8 name_len = 0;
1631 quint8 common_len = 0;
1632 bool withdefault = false;
1633 int no_read = 0;
1634 char name_buffer[17];
1635
1636 while (byte == 0x0) { // Eat padding bytes
1637 in >> byte;
1638 if (in.status() != QDataStream::Ok) {
1639 if (no_of_parameters_found == num_params) {
1640 goto Success;
1641 } else {
1642 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: unexpected EOF"
1643 << "number of parameters expected:" << num_params
1644 << "actual:" << no_of_parameters_found;
1645 goto Error;
1646 }
1647 }
1648 }
1649 ptype = byte & 0x0F;
1650 flags = (byte >> 4) & 0x0F;
1651 withdefault = (flags & 0x01) == 0x01;
1652 in >> byte;
1653 if (in.status() != QDataStream::Ok) {
1654 qCritical(ParameterManagerLog) << "_parseParamFile: Error: Unexpected EOF while reading flags";
1655 goto Error;
1656 }
1657 name_len = ((byte >> 4) & 0x0F) + 1;
1658 common_len = byte & 0x0F;
1659 if ((name_len + common_len) > 16) {
1660 qCritical(ParameterManagerLog) << "_parseParamFile: Error: common_len + name_len > 16"
1661 << "name_len" << name_len
1662 << "common_len" << common_len;
1663 goto Error;
1664 }
1665 no_read = in.readRawData(&name_buffer[common_len], static_cast<int>(name_len));
1666 if (no_read != name_len) {
1667 qCritical(ParameterManagerLog) << "_parseParamFile: Error: Unexpected EOF while reading parameterName"
1668 << "Expected:" << name_len
1669 << "Actual:" << no_read;
1670 goto Error;
1671 }
1672 name_buffer[common_len + name_len] = '\0';
1673 const QString parameterName(name_buffer);
1674 qCDebug(ParameterManagerVerbose2Log) << "_parseParamFile: parameter" << parameterName
1675 << "name_len" << name_len
1676 << "common_len" << common_len
1677 << "ptype" << ptype
1678 << "flags" << flags;
1679
1680 QVariant parameterValue = 0;
1681 QVariant defaultValue;
1682 switch (static_cast<ap_var_type>(ptype)) {
1683 qint8 data8;
1684 qint16 data16;
1685 qint32 data32;
1686 float dfloat;
1687 case AP_PARAM_INT8:
1688 in >> data8;
1689 parameterValue = data8;
1690 if (withdefault) {
1691 in >> data8;
1692 defaultValue = data8;
1693 }
1694 break;
1695 case AP_PARAM_INT16:
1696 in >> data16;
1697 parameterValue = data16;
1698 if (withdefault) {
1699 in >> data16;
1700 defaultValue = data16;
1701 }
1702 break;
1703 case AP_PARAM_INT32:
1704 in >> data32;
1705 parameterValue = data32;
1706 if (withdefault) {
1707 in >> data32;
1708 defaultValue = data32;
1709 }
1710 break;
1711 case AP_PARAM_FLOAT:
1712 in >> data32;
1713 (void) memcpy(&dfloat, &data32, 4);
1714 parameterValue = dfloat;
1715 if (withdefault) {
1716 in >> data32;
1717 float ddefault;
1718 (void) memcpy(&ddefault, &data32, 4);
1719 defaultValue = ddefault;
1720 }
1721 break;
1722 default:
1723 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: type is out of range" << ptype;
1724 goto Error;
1725 break;
1726 }
1727 qCDebug(ParameterManagerVerbose2Log) << "paramValue" << parameterValue;
1728
1729 if (++no_of_parameters_found > num_params) {
1730 qCDebug(ParameterManagerLog) << "_parseParamFile: Error: more parameters in file than expected."
1731 << "Expected:" << num_params
1732 << "Actual:" << no_of_parameters_found;
1733 goto Error;
1734 }
1735
1736 const FactMetaData::ValueType_t factType = ((ptype == AP_PARAM_INT8) ? FactMetaData::valueTypeInt8 :
1737 (ptype == AP_PARAM_INT16) ? FactMetaData::valueTypeInt16 :
1738 (ptype == AP_PARAM_INT32) ? FactMetaData::valueTypeInt32 :
1739 FactMetaData::valueTypeFloat);
1740
1741 Fact *fact = nullptr;
1742 if (_mapCompId2FactMap.contains(componentId) && _mapCompId2FactMap[componentId].contains(parameterName)) {
1743 fact = _mapCompId2FactMap[componentId][parameterName];
1744 if (withdefault && defaultValue.isValid()) {
1745 fact->metaData()->setRawDefaultValue(defaultValue);
1746 }
1747 } else {
1748 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) << "Adding new fact" << parameterName;
1749
1750 fact = new Fact(componentId, parameterName, factType, this);
1751 FactMetaData *const factMetaData = _vehicle->compInfoManager()->compInfoParam(componentId)->factMetaDataForName(parameterName, fact->type());
1752 fact->setMetaData(factMetaData);
1753
1754 _mapCompId2FactMap[componentId][parameterName] = fact;
1755
1756 // We need to know when the fact value changes so we can update the vehicle
1757 (void) connect(fact, &Fact::containerRawValueChanged, this, &ParameterManager::_factRawValueUpdated);
1758
1759 // Set default before emitting factAdded so QML sees defaultValueAvailable from the start
1760 if (withdefault && defaultValue.isValid()) {
1761 fact->metaData()->setRawDefaultValue(defaultValue);
1762 }
1763
1764 emit factAdded(componentId, fact);
1765 }
1766 fact->containerSetRawValue(parameterValue);
1767 }
1768
1769Success:
1770 file.close();
1771 /* Create empty waiting lists as we have all parameters */
1772 _paramCountMap[componentId] = num_params;
1773 _totalParamCount += num_params;
1774 _waitingReadParamIndexMap[componentId] = QMap<int, int>();
1775 _checkInitialLoadComplete();
1776 _setLoadProgress(0.0);
1777 return true;
1778
1779Error:
1780 file.close();
1781 return false;
1782}
1783
1784void ParameterManager::_incrementPendingWriteCount()
1785{
1786 _pendingWritesCount++;
1787 if (_pendingWritesCount == 1) {
1788 emit pendingWritesChanged(true);
1789 }
1790}
1791
1792void ParameterManager::_decrementPendingWriteCount()
1793{
1794 if (_pendingWritesCount == 0) {
1795 qCWarning(ParameterManagerLog) << "Internal Error: _pendingWriteCount == 0";
1796 return;
1797 }
1798
1799 _pendingWritesCount--;
1800 if (_pendingWritesCount == 0) {
1801 emit pendingWritesChanged(false);
1802 }
1803}
std::shared_ptr< LinkInterface > SharedLinkInterfacePtr
#define qgcApp()
Error error
struct __mavlink_message mavlink_message_t
#define QGC_LOGGING_CATEGORY(name, categoryStr)
struct param_union mavlink_param_union_t
virtual void parametersReadyPreChecks()
FactMetaData * factMetaDataForName(const QString &name, FactMetaData::ValueType_t valueType)
CompInfoParam * compInfoParam(uint8_t compId)
void commandProgress(float value)
void downloadComplete(const QString &file, const QString &errorMsg)
bool download(uint8_t fromCompId, const QString &fromURI, const QString &toDir, const QString &fileName="", bool checksize=true)
Definition FTPManager.cc:30
Holds the meta data associated with a Fact.
static size_t typeToSize(ValueType_t type)
void setRawDefaultValue(const QVariant &rawDefaultValue)
bool volatileValue() const
A Fact is used to hold a single value within the system.
Definition Fact.h:17
void setMetaData(FactMetaData *metaData, bool setDefaultFromMetaData=false)
Definition Fact.cc:674
void containerSetRawValue(const QVariant &value)
Value coming from Vehicle. This does NOT send a _containerRawValueChanged signal.
Definition Fact.cc:192
FactMetaData * metaData()
Definition Fact.h:171
int componentId() const
Definition Fact.h:90
FactMetaData::ValueType_t type() const
Definition Fact.h:124
void setRawValue(const QVariant &value)
Definition Fact.cc:128
QString name() const
Definition Fact.h:121
void containerRawValueChanged(const QVariant &value)
This signal is meant for use by Fact container implementations. Used to send changed values to vehicl...
QString rawValueStringFullPrecision() const
Returns the values as a string with full 18 digit precision if float/double.
Definition Fact.cc:428
QVariant rawValue() const
Value after translation.
Definition Fact.h:85
QMap< int, remapParamNameMap_t > remapParamNameMinorVersionRemapMap_t
virtual int remapParamNameHigestMinorVersionNumber(int) const
QMap< QString, QString > remapParamNameMap_t
virtual QString offlineEditingParamFile(Vehicle *) const
Return the resource file which contains the set of params loaded for offline editing.
QMap< int, remapParamNameMinorVersionRemapMap_t > remapParamNameMajorVersionMap_t
virtual const remapParamNameMajorVersionMap_t & paramNameRemapMajorVersionMap() const
static int getComponentId()
static MAVLinkProtocol * instance()
int getSystemId() const
static MultiVehicleManager * instance()
bool parameterExists(int componentId, const QString &paramName) const
void parameterDownloadSkippedChanged()
static constexpr int kWaitForParamValueAckMs
Time to wait for param value ack after set param.
void mavlinkMessageReceived(const mavlink_message_t &message)
Fact * getParameter(int componentId, const QString &paramName)
void factAdded(int componentId, Fact *fact)
static FactMetaData::ValueType_t mavTypeToFactType(MAV_PARAM_TYPE mavType)
void setParameterDownloadSkipped(bool skipped)
QList< int > componentIds() const
void refreshParametersPrefix(int componentId, const QString &namePrefix)
Request a refresh on all parameters that begin with the specified prefix.
void parametersReadyChanged(bool parametersReady)
QString readParametersFromStream(QTextStream &stream)
Returns error messages from loading.
double loadProgress() const
bool pendingWrites() const
static QDir parameterCacheDir()
void missingParametersChanged(bool missingParameters)
static constexpr int defaultComponentId
QStringList parameterNames(int componentId) const
Returns all parameter names.
void _paramRequestReadFailure(int componentId, const QString &paramName, int paramIndex)
void _paramRequestReadSuccess(int componentId, const QString &paramName, int paramIndex)
static constexpr int kParamSetRetryCount
Number of retries for PARAM_SET.
void _paramSetSuccess(int componentId, const QString &paramName)
void loadProgressChanged(float value)
void cacheCheckOnlyFailed()
void refreshParameter(int componentId, const QString &paramName)
Request a refresh on the specific parameter.
void pendingWritesChanged(bool pendingWrites)
static MAV_PARAM_TYPE factTypeToMavType(FactMetaData::ValueType_t factType)
static constexpr int kParamRequestReadRetryCount
Number of retries for PARAM_REQUEST_READ.
void resetAllToVehicleConfiguration()
Q_INVOKABLE void refreshAllParameters()
static QString parameterCacheFile(int vehicleId, int componentId)
void writeParametersToStream(QTextStream &stream) const
void _paramSetFailure(int componentId, const QString &paramName)
Final state for a QGCStateMachine with logging support.
QGroundControl specific state machine with enhanced error handling.
QSignalTransition * addThisTransition(PointerToMemberFunction signal, QAbstractState *target)
Simpler version of QState::addTransition which assumes the sender is this.
Definition QGCState.h:31
Sends the specified MAVLink message to the vehicle.
WeakLinkInterfacePtr primaryLink() const
bool px4Firmware() const
Definition Vehicle.h:494
void sendMavCommand(int compId, MAV_CMD command, bool showError, float param1=0.0f, float param2=0.0f, float param3=0.0f, float param4=0.0f, float param5=0.0f, float param6=0.0f, float param7=0.0f)
Definition Vehicle.cc:2146
QString firmwareVersionTypeString() const
Definition Vehicle.cc:2289
MAV_TYPE vehicleType() const
Definition Vehicle.h:428
VehicleLinkManager * vehicleLinkManager()
Definition Vehicle.h:575
FirmwarePlugin * firmwarePlugin()
Provides access to the Firmware Plugin for this Vehicle.
Definition Vehicle.h:444
ComponentInformationManager * compInfoManager()
Definition Vehicle.h:577
int firmwareMinorVersion() const
Definition Vehicle.h:650
MAV_AUTOPILOT firmwareType() const
Definition Vehicle.h:427
int id() const
Definition Vehicle.h:425
bool sendMessageOnLinkThreadSafe(LinkInterface *link, mavlink_message_t message)
Definition Vehicle.cc:1389
QString gitHash() const
Definition Vehicle.h:663
int defaultComponentId() const
Definition Vehicle.h:670
bool genericFirmware() const
Definition Vehicle.h:496
AutoPilotPlugin * autopilotPlugin()
Provides access to AutoPilotPlugin for this vehicle.
Definition Vehicle.h:441
void setOfflineEditingDefaultComponentId(int defaultComponentId)
Sets the default component id for an offline editing vehicle.
Definition Vehicle.cc:2448
int firmwarePatchVersion() const
Definition Vehicle.h:651
FTPManager * ftpManager()
Definition Vehicle.h:576
int firmwareMajorVersion() const
Definition Vehicle.h:649
Waits for either PARAM_VALUE (success) or PARAM_ERROR (rejection) from the vehicle.
Error
Error codes for decompression operations.
bool remove(const QString &key)
quint32 crc32(const quint8 *src, unsigned len, unsigned state)
Definition QGCMath.cc:100
bool runningUnitTests()
bool fuzzyCompare(double value1, double value2)
Returns true if the two values are equal or close. Correctly handles 0 and NaN values.
Definition QGCMath.cc:109
void showAppMessage(const QString &message, const QString &title)
Modal application message. Queued if the UI isn't ready yet.
Definition AppMessages.cc:9
static const int versionNotSetValue