5#include <QtCore/QTextStream>
24#include <QtCore/QEasingCurve>
25#include <QtCore/QFile>
26#include <QtCore/QStandardPaths>
27#include <QtCore/QVariantAnimation>
37 , _logReplay(!vehicle->vehicleLinkManager()->primaryLink().expired() && vehicle->vehicleLinkManager()->primaryLink().lock()->isLogReplay())
38 , _disableAllRetries(_logReplay)
39 , _tryftp(vehicle->apmFirmware())
41 qCDebug(ParameterManagerLog) <<
this;
43 if (_vehicle->isOfflineEditingVehicle()) {
44 _loadOfflineEditingParams();
49 qCDebug(ParameterManagerLog) <<
this <<
"In log replay mode";
52 _hashCheckTimer.setSingleShot(
true);
53 _hashCheckTimer.setInterval(kHashCheckTimeoutMs);
54 (void) connect(&_hashCheckTimer, &QTimer::timeout,
this, &ParameterManager::_hashCheckTimeout);
56 _paramRequestListTimer.setSingleShot(
true);
57 _paramRequestListTimer.setInterval(
QGC::runningUnitTests() ? kTestInitialRequestIntervalMs : kParamRequestListTimeoutMs);
58 (void) connect(&_paramRequestListTimer, &QTimer::timeout,
this, &ParameterManager::_paramRequestListTimeout);
60 _waitingParamTimeoutTimer.setSingleShot(
true);
63 (void) connect(&_waitingParamTimeoutTimer, &QTimer::timeout,
this, &ParameterManager::_waitingParamTimeout);
67 (void) QDir().mkpath(parameterCacheDir().absolutePath());
72 qCDebug(ParameterManagerLog) <<
this;
75void ParameterManager::_updateProgressBar()
77 int waitingReadParamIndexCount = 0;
79 for (
const int compId: _waitingReadParamIndexMap.keys()) {
80 waitingReadParamIndexCount += _waitingReadParamIndexMap[compId].count();
83 if (waitingReadParamIndexCount == 0) {
84 if (_readParamIndexProgressActive) {
85 _readParamIndexProgressActive =
false;
86 _setLoadProgress(0.0);
90 _readParamIndexProgressActive =
true;
91 _setLoadProgress(
static_cast<double>(_totalParamCount - waitingReadParamIndexCount) /
static_cast<double>(_totalParamCount));
98 if (_tryftp && (message.compid == MAV_COMP_ID_AUTOPILOT1) && !_initialLoadComplete)
101 if (message.msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
102 mavlink_param_value_t param_value{};
103 mavlink_msg_param_value_decode(&message, ¶m_value);
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);
111 paramUnion.param_float = param_value.param_value;
112 paramUnion.type = param_value.param_type;
114 QVariant parameterValue;
115 if (!_mavlinkParamUnionToVariant(paramUnion, parameterValue)) {
119 _handleParamValue(message.compid, parameterName, param_value.param_count, param_value.param_index,
static_cast<MAV_PARAM_TYPE
>(param_value.param_type), parameterValue);
123void ParameterManager::_handleParamValue(
int componentId,
const QString ¶meterName,
int parameterCount,
int parameterIndex, MAV_PARAM_TYPE mavParamType,
const QVariant ¶meterValue)
126 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
127 "_parameterUpdate" <<
128 "name:" << parameterName <<
129 "count:" << parameterCount <<
130 "index:" << parameterIndex <<
131 "mavType:" << mavParamType <<
132 "value:" << parameterValue <<
137 if ((parameterIndex == 65535) && (parameterName != QStringLiteral(
"_HASH_CHECK")) && _paramRequestListTimer.isActive()) {
138 qCDebug(ParameterManagerLog) <<
"Disregarding unrequested param prior to initial list response" << parameterName;
142 if (_vehicle->
px4Firmware() && (parameterName ==
"_HASH_CHECK")) {
143 _hashCheckTimer.stop();
144 if (!_initialLoadComplete && !_logReplay) {
146 _tryCacheHashLoad(_vehicle->
id(), componentId, parameterValue);
151 _paramRequestListTimer.stop();
154 if (!_initialLoadComplete && !_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) {
155 if (_debugCacheMap[componentId].contains(parameterName)) {
156 const ParamTypeVal &cacheParamTypeVal = _debugCacheMap[componentId][parameterName];
158 const void *
const cacheData = cacheParamTypeVal.second.constData();
159 const void *
const vehicleData = parameterValue.constData();
161 if (memcmp(cacheData, vehicleData, dataSize) != 0) {
162 qCDebug(ParameterManagerVerbose1Log) <<
"Cache/Vehicle values differ for name:cache:actual" << parameterName << parameterValue << cacheParamTypeVal.second;
164 _debugCacheParamSeen[componentId][parameterName] =
true;
166 qCDebug(ParameterManagerVerbose1Log) <<
"Parameter missing from cache" << parameterName;
170 _waitingParamTimeoutTimer.stop();
173 if (!_paramCountMap.contains(componentId)) {
174 _paramCountMap[componentId] = parameterCount;
175 _totalParamCount += parameterCount;
179 if (!_waitingReadParamIndexMap.contains(componentId)) {
181 for (
int waitingIndex = 0; waitingIndex < parameterCount; waitingIndex++) {
183 _waitingReadParamIndexMap[componentId][waitingIndex] = 0;
186 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) <<
"Seeing component for first time - paramcount:" << parameterCount;
189 if (!_waitingReadParamIndexMap[componentId].contains(parameterIndex)) {
190 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
"Unrequested param update" << parameterName;
194 if (_waitingReadParamIndexMap[componentId].contains(parameterIndex)) {
195 _waitingReadParamIndexMap[componentId].remove(parameterIndex);
196 (void) _indexBatchQueue.removeOne(parameterIndex);
197 _fillIndexBatchQueue(
false );
201 int waitingReadParamIndexCount = 0;
203 for (
const int waitingComponentId: _waitingReadParamIndexMap.keys()) {
204 waitingReadParamIndexCount += _waitingReadParamIndexMap[waitingComponentId].count();
206 if (waitingReadParamIndexCount) {
207 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
"waitingReadParamIndexCount:" << waitingReadParamIndexCount;
210 const int readWaitingParamCount = waitingReadParamIndexCount;
211 const int totalWaitingParamCount = readWaitingParamCount;
212 if (totalWaitingParamCount) {
214 _waitingParamTimeoutTimer.start();
215 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(-1) <<
"Restarting _waitingParamTimeoutTimer: totalWaitingParamCount:" << totalWaitingParamCount;
218 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Restarting _waitingParamTimeoutTimer (still waiting for default component params)";
219 _waitingParamTimeoutTimer.start();
221 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(-1) <<
"Not restarting _waitingParamTimeoutTimer (all requests satisfied)";
224 _updateProgressBar();
226 Fact *fact =
nullptr;
227 if (_mapCompId2FactMap.contains(componentId) && _mapCompId2FactMap[componentId].contains(parameterName)) {
228 fact = _mapCompId2FactMap[componentId][parameterName];
230 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
"Adding new fact" << parameterName;
236 _mapCompId2FactMap[componentId][parameterName] = fact;
250 if (_prevWaitingReadParamIndexCount != 0 && readWaitingParamCount == 0) {
252 _writeLocalParamCache(_vehicle->
id(), componentId);
256 _prevWaitingReadParamIndexCount = waitingReadParamIndexCount;
258 _checkInitialLoadComplete();
260 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
"_parameterUpdate complete";
263QString ParameterManager::_vehicleAndComponentString(
int componentId)
const
266 QString vehicleIdStr;
268 vehicleIdStr = QStringLiteral(
"veh: %1").arg(_vehicle->
id());
272 QString componentIdStr;
273 if (_mapCompId2FactMap.keys().count() > 1) {
274 componentIdStr = QStringLiteral(
"comp: %1").arg(componentId);
277 if (!vehicleIdStr.isEmpty() && !componentIdStr.isEmpty()) {
278 return vehicleIdStr + QStringLiteral(
" ") + componentIdStr;
279 }
else if (!vehicleIdStr.isEmpty()) {
281 }
else if (!componentIdStr.isEmpty()) {
282 return componentIdStr;
288void ParameterManager::_mavlinkParamSet(
int componentId,
const QString ¶mName,
FactMetaData::ValueType_t valueType,
const QVariant &rawValue)
290 auto paramSetEncoder = [
this, componentId, paramName, valueType, rawValue](uint8_t , uint8_t channel,
mavlink_message_t *message) ->
void {
294 if (!_fillMavlinkParamUnion(valueType, rawValue, union_value)) {
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);
301 (void) mavlink_msg_param_set_pack_chan(
306 static_cast<uint8_t
>(_vehicle->
id()),
307 static_cast<uint8_t
>(componentId),
309 union_value.param_float,
310 static_cast<uint8_t
>(paramType));
313 auto checkForCorrectParamValue = [
this, componentId, paramName, rawValue](
const mavlink_message_t &message) ->
bool {
314 if (message.compid != componentId) {
318 mavlink_param_value_t param_value{};
319 mavlink_msg_param_value_decode(&message, ¶m_value);
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);
326 if (parameterName != paramName) {
331 QVariant receivedValue;
333 param_union.param_float = param_value.param_value;
334 param_union.type = param_value.param_type;
335 if (!_mavlinkParamUnionToVariant(param_union, receivedValue)) {
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();
342 if (param_value.param_type == MAV_PARAM_TYPE_REAL32) {
346 return receivedValue == rawValue;
350 auto checkForParamError = [componentId, paramName](
const mavlink_message_t &message) ->
bool {
351 if (message.compid != componentId) {
355 mavlink_param_error_t paramError{};
356 mavlink_msg_param_error_decode(&message, ¶mError);
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);
361 return QString(paramId) == paramName;
381 auto incPendingWriteCountState =
new FunctionState(QStringLiteral(
"ParameterManager increment pending write count"), stateMachine, [
this]() {
382 _incrementPendingWriteCount();
384 auto decPendingWriteCountState =
new FunctionState(QStringLiteral(
"ParameterManager decrement pending write count"), stateMachine, [
this]() {
385 _decrementPendingWriteCount();
387 auto retryDecPendingWriteCountState =
new FunctionState(QStringLiteral(
"ParameterManager retry decrement pending write count"), stateMachine, [
this]() {
388 _decrementPendingWriteCount();
390 auto errorDecPendingWriteCountState =
new FunctionState(QStringLiteral(
"ParameterManager error decrement pending write count"), stateMachine, [
this]() {
391 _decrementPendingWriteCount();
394 auto paramRefreshState =
new FunctionState(QStringLiteral(
"ParameterManager param refresh"), stateMachine, [
this, componentId, paramName]() {
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);
404 auto logSuccessState =
new FunctionState(QStringLiteral(
"ParameterManager log success"), stateMachine, [
this, componentId, paramName]() {
405 qCDebug(ParameterManagerLog) <<
"Parameter write succeeded: param:" << paramName << _vehicleAndComponentString(componentId);
408 auto logFailureState =
new FunctionState(QStringLiteral(
"ParameterManager log failure"), stateMachine, [
this, componentId, paramName]() {
409 qCDebug(ParameterManagerLog) <<
"Parameter write failed: param:" << paramName << _vehicleAndComponentString(componentId);
415 stateMachine->setInitialState(sendParamSetState);
416 sendParamSetState->addThisTransition (&
QGCState::advance, incPendingWriteCountState);
431 sendParamSetState->addThisTransition(&
QGCState::error, logFailureState);
438 qCDebug(ParameterManagerLog) <<
"Starting state machine for PARAM_SET on: " << paramName << _vehicleAndComponentString(componentId);
439 stateMachine->start();
448 paramUnion.param_uint8 =
static_cast<uint8_t
>(rawValue.toUInt(&ok));
451 paramUnion.param_int8 =
static_cast<int8_t
>(rawValue.toInt(&ok));
454 paramUnion.param_uint16 =
static_cast<uint16_t
>(rawValue.toUInt(&ok));
457 paramUnion.param_int16 =
static_cast<int16_t
>(rawValue.toInt(&ok));
460 paramUnion.param_uint32 =
static_cast<uint32_t
>(rawValue.toUInt(&ok));
463 paramUnion.param_float = rawValue.toFloat(&ok);
466 paramUnion.param_int32 =
static_cast<int32_t
>(rawValue.toInt(&ok));
469 qCCritical(ParameterManagerLog) <<
"Internal Error: Unsupported fact value type" << valueType;
470 paramUnion.param_int32 =
static_cast<int32_t
>(rawValue.toInt(&ok));
475 qCCritical(ParameterManagerLog) <<
"Fact Failed to Convert to Param Type:" << valueType;
482bool ParameterManager::_mavlinkParamUnionToVariant(
const mavlink_param_union_t ¶mUnion, QVariant &outValue)
const
484 switch (paramUnion.type) {
485 case MAV_PARAM_TYPE_REAL32:
486 outValue = QVariant(paramUnion.param_float);
488 case MAV_PARAM_TYPE_UINT8:
489 outValue = QVariant(paramUnion.param_uint8);
491 case MAV_PARAM_TYPE_INT8:
492 outValue = QVariant(paramUnion.param_int8);
494 case MAV_PARAM_TYPE_UINT16:
495 outValue = QVariant(paramUnion.param_uint16);
497 case MAV_PARAM_TYPE_INT16:
498 outValue = QVariant(paramUnion.param_int16);
500 case MAV_PARAM_TYPE_UINT32:
501 outValue = QVariant(paramUnion.param_uint32);
503 case MAV_PARAM_TYPE_INT32:
504 outValue = QVariant(paramUnion.param_int32);
507 qCCritical(ParameterManagerLog) <<
"ParameterManager::_mavlinkParamUnionToVariant - unsupported MAV_PARAM_TYPE" << paramUnion.type;
512void ParameterManager::_factRawValueUpdated(
const QVariant &rawValue)
514 Fact *
const fact = qobject_cast<Fact*>(sender());
516 qCWarning(ParameterManagerLog) <<
"Internal error";
523void ParameterManager::_ftpDownloadComplete(
const QString &fileName,
const QString &errorMsg)
525 bool continueWithDefaultParameterdownload =
true;
526 bool immediateRetry =
false;
531 if (errorMsg.isEmpty()) {
532 qCDebug(ParameterManagerLog) <<
"ParameterManager::_ftpDownloadComplete : Parameter file received:" << fileName;
533 if (_parseParamFile(fileName)) {
534 qCDebug(ParameterManagerLog) <<
"ParameterManager::_ftpDownloadComplete : Parsed!";
537 qCDebug(ParameterManagerLog) <<
"ParameterManager::_ftpDownloadComplete : Error in parameter file";
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;
545 }
else if ((_loadProgress > 0.0001) && (_loadProgress < 0.01)) {
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";
550 qCDebug(ParameterManagerLog) <<
"ParameterManager-ftp Retry:" << _initialRequestRetryCount;
551 continueWithDefaultParameterdownload =
false;
554 if (continueWithDefaultParameterdownload) {
556 _initialRequestRetryCount = 0;
561 if (immediateRetry) {
562 _paramRequestListTimeout();
564 _paramRequestListTimer.start();
567 _paramRequestListTimer.start();
571void ParameterManager::_ftpDownloadProgress(
float progress)
573 qCDebug(ParameterManagerVerbose1Log) <<
"ParameterManager::_ftpDownloadProgress:" << progress;
574 _setLoadProgress(
static_cast<double>(progress));
575 if (progress > 0.001) {
576 _paramRequestListTimer.stop();
580void ParameterManager::_resetHashCheck()
582 _hashCheckTimer.stop();
583 _hashCheckDone =
false;
590 _startParameterDownload(componentId);
596 _cacheOnlyHashCheck =
true;
604 if (sharedLink->linkConfiguration()->isHighLatency() || _logReplay) {
605 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Cache-only hash check: high latency or log replay link, signalling failure";
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);
615 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Cache-only hash check: not available, signalling failure";
620void ParameterManager::_startParameterDownload(uint8_t componentId)
627 if (sharedLink->linkConfiguration()->isHighLatency() || _logReplay) {
629 _parametersReady =
true;
630 _missingParameters =
true;
631 _initialLoadComplete =
true;
632 _waitingForDefaultComponent =
false;
638 if (_tryftp && ((componentId == MAV_COMP_ID_ALL) || (componentId == MAV_COMP_ID_AUTOPILOT1))) {
639 if (!_initialLoadComplete) {
640 _paramRequestListTimer.start();
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"),
652 qCWarning(ParameterManagerLog) <<
"ParameterManager::_startParameterDownload FTPManager::download returned failure";
655 }
else if (_vehicle->
px4Firmware() && !_initialLoadComplete && !_hashCheckDone) {
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)
663 _requestHashCheck(hashCheckCompId);
665 if (!_initialLoadComplete) {
666 _paramRequestListTimer.start();
670 for (
int cid: _paramCountMap.keys()) {
672 if ((componentId != MAV_COMP_ID_ALL) && (componentId != cid)) {
675 for (
int waitingIndex = 0; waitingIndex < _paramCountMap[cid]; waitingIndex++) {
677 _waitingReadParamIndexMap[cid][waitingIndex] = 0;
684 sharedLink->mavlinkChannel(),
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;
695int ParameterManager::_actualComponentId(
int componentId)
const
700 qCWarning(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Default component id not set";
709 componentId = _actualComponentId(componentId);
711 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) <<
"refreshParameter - name:" << paramName <<
")";
713 _mavlinkParamRequestRead(componentId, paramName, -1,
true );
718 componentId = _actualComponentId(componentId);
719 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) <<
"refreshParametersPrefix - name:" << namePrefix <<
")";
721 for (
const QString ¶mName: _mapCompId2FactMap[componentId].keys()) {
722 if (paramName.startsWith(namePrefix)) {
732 componentId = _actualComponentId(componentId);
733 if (_mapCompId2FactMap.contains(componentId)) {
734 ret = _mapCompId2FactMap[componentId].contains(_remapParamNameToVersion(paramName));
742 componentId = _actualComponentId(componentId);
744 const QString mappedParamName = _remapParamNameToVersion(paramName);
745 if (!_mapCompId2FactMap.contains(componentId) || !_mapCompId2FactMap[componentId].contains(mappedParamName)) {
746 qgcApp()->reportMissingParameter(componentId, mappedParamName);
747 return &_defaultFact;
750 return _mapCompId2FactMap[componentId][mappedParamName];
757 const int compId = _actualComponentId(componentId);
758 const QMap<QString, Fact*> &factMap = _mapCompId2FactMap[compId];
759 for (
const QString ¶mName: factMap.keys()) {
766bool ParameterManager::_fillIndexBatchQueue(
bool waitingParamTimeout)
768 if (!_indexBatchQueueActive) {
772 constexpr int maxBatchSize = 10;
774 if (waitingParamTimeout) {
776 qCDebug(ParameterManagerLog) <<
"Refilling index based batch queue due to timeout";
777 _indexBatchQueue.clear();
779 qCDebug(ParameterManagerLog) <<
"Refilling index based batch queue due to received parameter";
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];
788 for (
const int paramIndex: _waitingReadParamIndexMap[componentId].keys()) {
789 if (_indexBatchQueue.contains(paramIndex)) {
794 if (_indexBatchQueue.count() > maxBatchSize) {
798 _waitingReadParamIndexMap[componentId][paramIndex]++;
799 if (_disableAllRetries || (_waitingReadParamIndexMap[componentId][paramIndex] > _maxInitialLoadRetrySingleParam)) {
801 _failedReadParamIndexMap[componentId] << paramIndex;
802 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) <<
"Giving up on (paramIndex:" << paramIndex <<
"retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] <<
")";
803 (void) _waitingReadParamIndexMap[componentId].
remove(paramIndex);
806 _indexBatchQueue.append(paramIndex);
807 _mavlinkParamRequestRead(componentId, QString(), paramIndex,
false );
808 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) <<
"Read re-request for (paramIndex:" << paramIndex <<
"retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] <<
")";
813 return (!_indexBatchQueue.isEmpty());
816void ParameterManager::_waitingParamTimeout()
822 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"_waitingParamTimeout";
825 _indexBatchQueueActive =
true;
828 bool paramsRequested = _fillIndexBatchQueue(
true );
829 if (!paramsRequested && !_waitingForDefaultComponent && !_mapCompId2FactMap.contains(_vehicle->
defaultComponentId())) {
832 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Restarting _waitingParamTimeoutTimer - still don't have default component params" << _vehicle->
defaultComponentId();
833 _waitingParamTimeoutTimer.start();
834 _waitingForDefaultComponent =
true;
837 _waitingForDefaultComponent =
false;
839 _checkInitialLoadComplete();
841 if (paramsRequested) {
842 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Restarting _waitingParamTimeoutTimer - re-request";
843 _waitingParamTimeoutTimer.start();
847void ParameterManager::_requestHashCheck(uint8_t componentId)
854 qCDebug(ParameterManagerLog) << _logVehiclePrefix(componentId) <<
"Sending PARAM_REQUEST_READ for _HASH_CHECK";
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);
860 (void) mavlink_msg_param_request_read_pack_chan(
863 sharedLink->mavlinkChannel(),
865 static_cast<uint8_t
>(_vehicle->
id()),
873void ParameterManager::_mavlinkParamRequestRead(
int componentId,
const QString ¶mName,
int paramIndex,
bool notifyFailure)
875 auto paramRequestReadEncoder = [
this, componentId, paramName, paramIndex](uint8_t , 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);
883 static_cast<uint8_t
>(_vehicle->
id()),
884 static_cast<uint8_t
>(componentId),
886 static_cast<int16_t
>(paramIndex));
889 auto checkForCorrectParamValue = [componentId, paramName, paramIndex](
const mavlink_message_t &message) ->
bool {
890 if (message.compid != componentId) {
894 mavlink_param_value_t param_value{};
895 mavlink_msg_param_value_decode(&message, ¶m_value);
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);
903 if (paramIndex != -1) {
905 if (param_value.param_index != paramIndex) {
910 if (parameterName != paramName) {
918 auto checkForParamError = [componentId, paramName, paramIndex](
const mavlink_message_t &message) ->
bool {
919 if (message.compid != componentId) {
923 mavlink_param_error_t paramError{};
924 mavlink_msg_param_error_decode(&message, ¶mError);
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);
929 if (paramIndex != -1) {
930 return paramError.param_index == paramIndex;
932 return QString(paramId) == paramName;
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);
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);
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);
968 stateMachine->setInitialState(sendParamRequestReadState);
980 sendParamRequestReadState->addThisTransition(&
QGCState::error, logFailureState);
990 qCDebug(ParameterManagerLog) <<
"Starting state machine for PARAM_REQUEST_READ on: " << paramName << _vehicleAndComponentString(componentId);
991 stateMachine->start();
994void ParameterManager::_writeLocalParamCache(
int vehicleId,
int componentId)
996 CacheMapName2ParamTypeVal cacheMap;
998 for (
const QString ¶mName: _mapCompId2FactMap[componentId].keys()) {
999 const Fact *
const fact = _mapCompId2FactMap[componentId][paramName];
1000 cacheMap[paramName] = ParamTypeVal(fact->
type(), fact->
rawValue());
1004 if (cacheFile.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
1005 QDataStream ds(&cacheFile);
1008 qCWarning(ParameterManagerLog) <<
"Failed to open cache file for writing" << cacheFile.fileName();
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"));
1023 return parameterCacheDir().filePath(QStringLiteral(
"%1_%2.v2").arg(vehicleId).arg(componentId));
1026void ParameterManager::_tryCacheHashLoad(
int vehicleId,
int componentId,
const QVariant &hashValue)
1028 qCDebug(ParameterManagerLog) <<
"Attemping load from cache";
1031 CacheMapName2ParamTypeVal cacheMap;
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";
1043 _startParameterDownload(MAV_COMP_ID_ALL);
1048 (void) cacheFile.open(QIODevice::ReadOnly);
1051 QDataStream ds(&cacheFile);
1055 uint32_t crc32_value = 0;
1056 for (
const QString &name: cacheMap.keys()) {
1057 const ParamTypeVal ¶mTypeVal = cacheMap[name];
1062 qCDebug(ParameterManagerLog) <<
"Volatile parameter" << name;
1064 const void *
const vdat = paramTypeVal.second.constData();
1066 crc32_value =
QGC::crc32(
reinterpret_cast<const uint8_t *
>(qPrintable(name)), name.length(), crc32_value);
1072 if (crc32_value == hashValue.toUInt()) {
1073 _hashCheckDone =
true;
1074 _paramRequestListTimer.stop();
1075 qCDebug(ParameterManagerLog) <<
"Parameters loaded from cache" << qPrintable(QFileInfo(cacheFile).absoluteFilePath());
1077 const int count = cacheMap.count();
1079 for (
const QString &name: cacheMap.keys()) {
1080 const ParamTypeVal ¶mTypeVal = cacheMap[name];
1083 _handleParamValue(componentId, name, count, index++, mavParamType, paramTypeVal.second);
1088 mavlink_param_set_t p{};
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);
1102 sharedLink->mavlinkChannel(),
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);
1115 (void) connect(ani, &QVariantAnimation::valueChanged,
this, [
this](
const QVariant &value) {
1116 _setLoadProgress(value.toDouble());
1120 connect(ani, &QVariantAnimation::finished,
this, [
this] {
1121 QTimer::singleShot(500,
this, [
this] {
1122 _setLoadProgress(0);
1126 ani->start(QAbstractAnimation::DeleteWhenStopped);
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;
1137 if (!_hashCheckDone) {
1138 _hashCheckDone =
true;
1139 if (_cacheOnlyHashCheck) {
1140 qCDebug(ParameterManagerLog) <<
"Cache-only hash check: CRC mismatch, signalling failure";
1145 _startParameterDownload(MAV_COMP_ID_ALL);
1153 QString missingErrors;
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());
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();
1173 error += QStringLiteral(
"%1:%2").arg(componentId).arg(paramName);
1174 missingErrors +=
error;
1175 qCDebug(ParameterManagerLog) <<
"Skipped due to missing:" <<
error;
1182 error = QStringLiteral(
"%1:%2 ").arg(componentId).arg(paramName);
1183 typeErrors +=
error;
1184 qCDebug(ParameterManagerLog) <<
"Skipped due to type mismatch: %1" <<
error;
1188 qCDebug(ParameterManagerLog) <<
"Updating parameter" << componentId << paramName << valStr;
1196 if (!missingErrors.isEmpty()) {
1197 errors = tr(
"Parameters not loaded since they are not currently on the vehicle: %1\n").arg(missingErrors);
1200 if (!typeErrors.isEmpty()) {
1201 errors += tr(
"Parameters not loaded due to type mismatch: %1").arg(typeErrors);
1209 stream <<
"# Onboard parameters for Vehicle " << _vehicle->
id() <<
"\n";
1214 stream <<
"# Version: "
1219 stream <<
"# Git Revision: " << _vehicle->
gitHash() <<
"\n";
1222 stream <<
"# Vehicle-Id Component-Id Name Value Type\n";
1224 for (
const int componentId: _mapCompId2FactMap.keys()) {
1225 for (
const QString ¶mName: _mapCompId2FactMap[componentId].keys()) {
1226 const Fact *
const fact = _mapCompId2FactMap[componentId][paramName];
1230 qCWarning(ParameterManagerLog) <<
"Internal error: missing fact";
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;
1260 qCWarning(ParameterManagerLog) <<
"Unsupported fact type" << factType;
1263 return MAV_PARAM_TYPE_INT32;
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:
1289 qCWarning(ParameterManagerLog) <<
"Unsupported mav param type" << mavType;
1291 case MAV_PARAM_TYPE_INT32:
1296void ParameterManager::_checkInitialLoadComplete()
1298 if (_initialLoadComplete) {
1302 for (
const int componentId: _waitingReadParamIndexMap.keys()) {
1303 if (!_waitingReadParamIndexMap[componentId].isEmpty()) {
1315 _initialLoadComplete =
true;
1318 for (
const int componentId: _debugCacheParamSeen.keys()) {
1319 if (!_logReplay && _debugCacheCRC.contains(componentId) && _debugCacheCRC[componentId]) {
1320 for (
const QString ¶mName: _debugCacheParamSeen[componentId].keys()) {
1321 if (!_debugCacheParamSeen[componentId][paramName]) {
1322 qCDebug(ParameterManagerLog) <<
"Parameter in cache but not on vehicle componentId:Name" << componentId << paramName;
1327 _debugCacheCRC.clear();
1329 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Initial load complete";
1333 bool initialLoadFailures =
false;
1334 for (
const int componentId: _failedReadParamIndexMap.keys()) {
1335 for (
const int paramIndex: _failedReadParamIndexMap[componentId]) {
1336 if (initialLoadFailures) {
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 <<
")";
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;
1355 qCWarning(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"The following parameter indices could not be loaded after the maximum number of retries:" << indexList;
1360 _parametersReady =
true;
1366void ParameterManager::_hashCheckTimeout()
1368 _hashCheckDone =
true;
1369 if (_cacheOnlyHashCheck) {
1370 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"_HASH_CHECK timed out in cache-only mode, signalling failure";
1374 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"_HASH_CHECK timed out, falling back to PARAM_REQUEST_LIST";
1375 _startParameterDownload(MAV_COMP_ID_ALL);
1378void ParameterManager::_paramRequestListTimeout()
1382 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"_paramRequestListTimeout (log replay): Signalling load complete";
1383 _initialLoadComplete =
true;
1384 _missingParameters =
false;
1385 _parametersReady =
true;
1392 if (!_disableAllRetries && (++_initialRequestRetryCount <= _maxInitialRequestListRetry)) {
1393 qCDebug(ParameterManagerLog) << _logVehiclePrefix(-1) <<
"Retrying initial parameter request list";
1394 _startParameterDownload(MAV_COMP_ID_ALL);
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;
1403QString ParameterManager::_remapParamNameToVersion(
const QString ¶mName)
const
1405 static const QString noRemapPrefix = QStringLiteral(
"noremap.");
1406 if (paramName.startsWith(noRemapPrefix)) {
1407 return paramName.mid(noRemapPrefix.length());
1413 qCDebug(ParameterManagerLog) <<
"_remapParamNameToVersion" << paramName << majorVersion << minorVersion;
1421 if (!majorVersionRemap.contains(majorVersion)) {
1423 qCDebug(ParameterManagerLog) <<
"_remapParamNameToVersion: no major version mapping";
1429 QString mappedParamName = paramName;
1431 if (remapMinorVersion.contains(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;
1441 return mappedParamName;
1444void ParameterManager::_loadOfflineEditingParams()
1447 if (paramFilename.isEmpty()) {
1451 QFile paramFile(paramFilename);
1452 if (!paramFile.open(QFile::ReadOnly)) {
1453 qCWarning(ParameterManagerLog) <<
"Unable to open offline editing params file" << paramFilename;
1456 QTextStream paramStream(¶mFile);
1457 while (!paramStream.atEnd()) {
1458 const QString line = paramStream.readLine();
1459 if (line.startsWith(
"#")) {
1463 const QStringList paramData = line.split(
"\t");
1464 Q_ASSERT(paramData.count() == 5);
1466 const int offlineDefaultComponentId = paramData.at(1).toInt();
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());
1472 QVariant paramValue;
1473 switch (paramType) {
1474 case MAV_PARAM_TYPE_REAL32:
1475 paramValue = QVariant(valStr.toFloat());
1477 case MAV_PARAM_TYPE_UINT32:
1478 paramValue = QVariant(valStr.toUInt());
1480 case MAV_PARAM_TYPE_UINT16:
1481 paramValue = QVariant((quint16)valStr.toUInt());
1483 case MAV_PARAM_TYPE_INT16:
1484 paramValue = QVariant((qint16)valStr.toInt());
1486 case MAV_PARAM_TYPE_UINT8:
1487 paramValue = QVariant((quint8)valStr.toUInt());
1489 case MAV_PARAM_TYPE_INT8:
1490 paramValue = QVariant((qint8)valStr.toUInt());
1493 qCCritical(ParameterManagerLog) <<
"Unknown type" << paramType;
1495 case MAV_PARAM_TYPE_INT32:
1496 paramValue = QVariant(valStr.toInt());
1508 _parametersReady =
true;
1509 _initialLoadComplete =
true;
1510 _debugCacheCRC.clear();
1516 MAV_CMD_PREFLIGHT_STORAGE,
1526 if (sysAutoConfigFact) {
1531QString ParameterManager::_logVehiclePrefix(
int componentId)
const
1533 if (componentId == -1) {
1534 return QStringLiteral(
"V:%1").arg(_vehicle->
id());
1536 return QStringLiteral(
"V:%1 C:%2").arg(_vehicle->
id()).arg(componentId);
1540void ParameterManager::_setLoadProgress(
double loadProgress)
1550 if (_parameterDownloadSkipped != skipped) {
1551 _parameterDownloadSkipped = skipped;
1558 return _paramCountMap.keys();
1563 return _pendingWritesCount > 0;
1572bool ParameterManager::_parseParamFile(
const QString& filename)
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;
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.";
1595 QDataStream in(&file);
1596 in.setByteOrder(QDataStream::LittleEndian);
1598 quint16 magic, num_params, total_params;
1603 if (in.status() != QDataStream::Ok) {
1604 qCDebug(ParameterManagerLog) <<
"_parseParamFile: Error: Could not read Header";
1608 qCDebug(ParameterManagerVerbose2Log) <<
"_parseParamFile: magic: 0x" << Qt::hex << magic;
1609 qCDebug(ParameterManagerVerbose2Log) <<
"_parseParamFile: num_params:" << num_params
1610 <<
"total_params:" << total_params;
1612 if ((magic != magic_standard) && (magic != magic_withdefaults)) {
1613 qCDebug(ParameterManagerLog) <<
"_parseParamFile: Error: File does not start with Magic";
1616 if (num_params > total_params) {
1617 qCDebug(ParameterManagerLog) <<
"_parseParamFile: Error: total_params > num_params";
1620 if (num_params != total_params) {
1622 qCDebug(ParameterManagerLog) <<
"_parseParamFile: Error: total_params != num_params";
1626 while (in.status() == QDataStream::Ok) {
1630 quint8 name_len = 0;
1631 quint8 common_len = 0;
1632 bool withdefault =
false;
1634 char name_buffer[17];
1636 while (
byte == 0x0) {
1638 if (in.status() != QDataStream::Ok) {
1639 if (no_of_parameters_found == num_params) {
1642 qCDebug(ParameterManagerLog) <<
"_parseParamFile: Error: unexpected EOF"
1643 <<
"number of parameters expected:" << num_params
1644 <<
"actual:" << no_of_parameters_found;
1649 ptype =
byte & 0x0F;
1650 flags = (
byte >> 4) & 0x0F;
1651 withdefault = (flags & 0x01) == 0x01;
1653 if (in.status() != QDataStream::Ok) {
1654 qCritical(ParameterManagerLog) <<
"_parseParamFile: Error: Unexpected EOF while reading flags";
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;
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;
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
1678 <<
"flags" << flags;
1680 QVariant parameterValue = 0;
1681 QVariant defaultValue;
1682 switch (
static_cast<ap_var_type
>(ptype)) {
1689 parameterValue = data8;
1692 defaultValue = data8;
1695 case AP_PARAM_INT16:
1697 parameterValue = data16;
1700 defaultValue = data16;
1703 case AP_PARAM_INT32:
1705 parameterValue = data32;
1708 defaultValue = data32;
1711 case AP_PARAM_FLOAT:
1713 (void) memcpy(&dfloat, &data32, 4);
1714 parameterValue = dfloat;
1718 (void) memcpy(&ddefault, &data32, 4);
1719 defaultValue = ddefault;
1723 qCDebug(ParameterManagerLog) <<
"_parseParamFile: Error: type is out of range" << ptype;
1727 qCDebug(ParameterManagerVerbose2Log) <<
"paramValue" << parameterValue;
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;
1737 (ptype == AP_PARAM_INT16) ?
FactMetaData::valueTypeInt16 :
1738 (ptype == AP_PARAM_INT32) ?
FactMetaData::valueTypeInt32 :
1741 Fact *fact =
nullptr;
1742 if (_mapCompId2FactMap.contains(componentId) && _mapCompId2FactMap[componentId].contains(parameterName)) {
1743 fact = _mapCompId2FactMap[componentId][parameterName];
1744 if (withdefault && defaultValue.isValid()) {
1748 qCDebug(ParameterManagerVerbose1Log) << _logVehiclePrefix(componentId) <<
"Adding new fact" << parameterName;
1750 fact =
new Fact(componentId, parameterName, factType,
this);
1754 _mapCompId2FactMap[componentId][parameterName] = fact;
1760 if (withdefault && defaultValue.isValid()) {
1772 _paramCountMap[componentId] = num_params;
1773 _totalParamCount += num_params;
1774 _waitingReadParamIndexMap[componentId] = QMap<int, int>();
1775 _checkInitialLoadComplete();
1776 _setLoadProgress(0.0);
1784void ParameterManager::_incrementPendingWriteCount()
1786 _pendingWritesCount++;
1787 if (_pendingWritesCount == 1) {
1792void ParameterManager::_decrementPendingWriteCount()
1794 if (_pendingWritesCount == 0) {
1795 qCWarning(ParameterManagerLog) <<
"Internal Error: _pendingWriteCount == 0";
1799 _pendingWritesCount--;
1800 if (_pendingWritesCount == 0) {
std::shared_ptr< LinkInterface > SharedLinkInterfacePtr
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)
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)
A Fact is used to hold a single value within the system.
void setMetaData(FactMetaData *metaData, bool setDefaultFromMetaData=false)
void containerSetRawValue(const QVariant &value)
Value coming from Vehicle. This does NOT send a _containerRawValueChanged signal.
FactMetaData * metaData()
FactMetaData::ValueType_t type() const
void setRawValue(const QVariant &value)
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.
QVariant rawValue() const
Value after translation.
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()
static MultiVehicleManager * instance()
bool parameterExists(int componentId, const QString ¶mName) 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 ¶mName)
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
void tryHashCheckCacheLoad()
bool pendingWrites() const
static QDir parameterCacheDir()
void missingParametersChanged(bool missingParameters)
static constexpr int defaultComponentId
void resetAllParametersToDefaults()
QStringList parameterNames(int componentId) const
Returns all parameter names.
void _paramRequestReadFailure(int componentId, const QString ¶mName, int paramIndex)
void _paramRequestReadSuccess(int componentId, const QString ¶mName, int paramIndex)
static constexpr int kParamSetRetryCount
Number of retries for PARAM_SET.
void _paramSetSuccess(int componentId, const QString ¶mName)
void loadProgressChanged(float value)
void cacheCheckOnlyFailed()
void refreshParameter(int componentId, const QString ¶mName)
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 ¶mName)
Final state for a QGCStateMachine with logging support.
static VehicleClass_t vehicleClass(MAV_TYPE mavType)
static const char * vehicleClassToCanonicalString(VehicleClass_t vehicleClass)
static FirmwareClass_t firmwareClass(MAV_AUTOPILOT autopilot)
static const char * firmwareClassToCanonicalString(FirmwareClass_t firmwareClass)
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.
Sends the specified MAVLink message to the vehicle.
WeakLinkInterfacePtr primaryLink() const
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)
QString firmwareVersionTypeString() const
MAV_TYPE vehicleType() const
VehicleLinkManager * vehicleLinkManager()
FirmwarePlugin * firmwarePlugin()
Provides access to the Firmware Plugin for this Vehicle.
ComponentInformationManager * compInfoManager()
int firmwareMinorVersion() const
MAV_AUTOPILOT firmwareType() const
bool sendMessageOnLinkThreadSafe(LinkInterface *link, mavlink_message_t message)
int defaultComponentId() const
bool genericFirmware() const
AutoPilotPlugin * autopilotPlugin()
Provides access to AutoPilotPlugin for this vehicle.
void setOfflineEditingDefaultComponentId(int defaultComponentId)
Sets the default component id for an offline editing vehicle.
int firmwarePatchVersion() const
FTPManager * ftpManager()
int firmwareMajorVersion() const
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)
bool fuzzyCompare(double value1, double value2)
Returns true if the two values are equal or close. Correctly handles 0 and NaN values.
void showAppMessage(const QString &message, const QString &title)
Modal application message. Queued if the UI isn't ready yet.
static const int versionNotSetValue