diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc
index 654b15b..165ef96 100644
--- a/src/FactSystem/ParameterLoader.cc
+++ b/src/FactSystem/ParameterLoader.cc
@@ -43,9 +43,9 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, UASInterface* uas,
_uas(uas),
_mavlink(MAVLinkProtocol::instance()),
_parametersReady(false),
+ _initialLoadComplete(false),
_defaultComponentId(FactSystem::defaultComponentId),
- _totalParamCount(0),
- _fullRefresh(false)
+ _totalParamCount(0)
{
Q_ASSERT(_autopilot);
Q_ASSERT(_uas);
@@ -89,6 +89,15 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
"value:" << value <<
")";
+#if 0
+ // Handy for testing retry logic
+ static int counter = 0;
+ if (counter++ & 0x3) {
+ qCDebug(ParameterLoaderLog) << "Artificial discard" << counter;
+ return;
+ }
+#endif
+
_dataMutex.lock();
// Restart our waiting for param timer
@@ -102,25 +111,23 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
// If we've never seen this component id before, setup the wait lists.
if (!_waitingReadParamIndexMap.contains(componentId)) {
- QStringList paramNameList;
- QList<int> paramIndexList;
-
- // Parameter index is 0-based
- for (int i=0; i<parameterCount; i++) {
- paramIndexList << i;
+ // Add all indices to the wait list, parameter index is 0-based
+ for (int waitingIndex=0; waitingIndex<parameterCount; waitingIndex++) {
+ // This will add the new component id, as well as the the new waiting index and set the retry count for that index to 0
+ _waitingReadParamIndexMap[componentId][waitingIndex] = 0;
}
- _waitingReadParamIndexMap[componentId] = paramIndexList;
- _waitingReadParamNameMap[componentId] = paramNameList;
- _waitingWriteParamNameMap[componentId] = paramNameList;
+ // The read and write waiting lists for this component are initialized the empty
+ _waitingReadParamNameMap[componentId] = QMap<QString, int>();
+ _waitingWriteParamNameMap[componentId] = QMap<QString, int>();
qCDebug(ParameterLoaderLog) << "Seeing component for first time, id:" << componentId << "parameter count:" << parameterCount;
}
// Remove this parameter from the waiting lists
- _waitingReadParamIndexMap[componentId].removeOne(parameterId);
- _waitingReadParamNameMap[componentId].removeOne(parameterName);
- _waitingWriteParamNameMap[componentId].removeOne(parameterName);
+ _waitingReadParamIndexMap[componentId].remove(parameterId);
+ _waitingReadParamNameMap[componentId].remove(parameterName);
+ _waitingWriteParamNameMap[componentId].remove(parameterName);
qCDebug(ParameterLoaderLog) << "_waitingReadParamIndexMap:" << _waitingReadParamIndexMap[componentId];
qCDebug(ParameterLoaderLog) << "_waitingReadParamNameMap" << _waitingReadParamNameMap[componentId];
qCDebug(ParameterLoaderLog) << "_waitingWriteParamNameMap" << _waitingWriteParamNameMap[componentId];
@@ -237,18 +244,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
_saveToEEPROM();
}
- // Check to see if we have the full param list for the first time
-
- if (_fullRefresh) {
- if (waitingParamCount == 0) {
- if (!_parametersReady) {
- _parametersReady = true;
- _determineDefaultComponentId();
- _setupGroupMap();
- emit parametersReady();
- }
- }
- }
+ _checkInitialLoadComplete();
}
/// Connected to Fact::valueUpdated
@@ -265,8 +261,8 @@ void ParameterLoader::_valueUpdated(const QVariant& value)
_dataMutex.lock();
Q_ASSERT(_waitingWriteParamNameMap.contains(componentId));
- _waitingWriteParamNameMap[componentId].removeOne(name);
- _waitingWriteParamNameMap[componentId] << name;
+ _waitingWriteParamNameMap[componentId].remove(name); // Remove any old entry
+ _waitingWriteParamNameMap[componentId][name] = 0; // Add new entry and set retry count
_waitingParamTimeoutTimer.start();
_dataMutex.unlock();
@@ -285,18 +281,13 @@ void ParameterLoader::refreshAllParameters(void)
{
_dataMutex.lock();
- _fullRefresh = true;
-
// Reset index wait lists
foreach (int componentId, _paramCountMap.keys()) {
- QList<int> paramIndexList;
-
- // Parameter index is 0-based
- for (int i=0; i<_paramCountMap[componentId]; i++) {
- paramIndexList << i;
+ // Add/Update all indices to the wait list, parameter index is 0-based
+ for (int waitingIndex=0; waitingIndex<_paramCountMap[componentId]; waitingIndex++) {
+ // This will add a new waiting index if needed and set the retry count for that index to 0
+ _waitingReadParamIndexMap[componentId][waitingIndex] = 0;
}
-
- _waitingReadParamIndexMap[componentId] = paramIndexList;
}
_dataMutex.unlock();
@@ -349,8 +340,8 @@ void ParameterLoader::refreshParameter(int componentId, const QString& name)
Q_ASSERT(_waitingReadParamNameMap.contains(componentId));
if (_waitingReadParamNameMap.contains(componentId)) {
- _waitingReadParamNameMap[componentId].removeOne(name);
- _waitingReadParamNameMap[componentId] << name;
+ _waitingReadParamNameMap[componentId].remove(name); // Remove old wait entry if there
+ _waitingReadParamNameMap[componentId][name] = 0; // Add new wait entry and update retry count
emit restartWaitingParamTimer();
}
@@ -431,23 +422,35 @@ void ParameterLoader::_waitingParamTimeout(void)
batchCount = 0;
foreach(int componentId, _waitingReadParamIndexMap.keys()) {
- foreach(int paramIndex, _waitingReadParamIndexMap[componentId]) {
- paramsRequested = true;
- _readParameterRaw(componentId, "", paramIndex);
- qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramIndex:" << paramIndex << ")";
-
- if (++batchCount > maxBatchSize) {
- goto Out;
+ foreach(int paramIndex, _waitingReadParamIndexMap[componentId].keys()) {
+ _waitingReadParamIndexMap[componentId][paramIndex]++; // Bump retry count
+ if (_waitingReadParamIndexMap[componentId][paramIndex] > _maxInitialLoadRetry) {
+ // Give up on this index
+ _failedReadParamIndexMap[componentId] << paramIndex;
+ qCDebug(ParameterLoaderLog) << "Giving up on (componentId:" << componentId << "paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
+ _waitingReadParamIndexMap[componentId].remove(paramIndex);
+ } else {
+ // Retry again
+ paramsRequested = true;
+ _readParameterRaw(componentId, "", paramIndex);
+ qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramIndex:" << paramIndex << "retryCount:" << _waitingReadParamIndexMap[componentId][paramIndex] << ")";
+
+ if (++batchCount > maxBatchSize) {
+ goto Out;
+ }
}
}
}
+ // We need to check for initial load complete here as well, since it could complete on a max retry failure
+ _checkInitialLoadComplete();
if (!paramsRequested) {
foreach(int componentId, _waitingWriteParamNameMap.keys()) {
- foreach(QString paramName, _waitingWriteParamNameMap[componentId]) {
+ foreach(QString paramName, _waitingWriteParamNameMap[componentId].keys()) {
paramsRequested = true;
+ _waitingWriteParamNameMap[componentId][paramName]++; // Bump retry count
_writeParameterRaw(componentId, paramName, _autopilot->getFact(FactSystem::ParameterProvider, componentId, paramName)->value());
- qCDebug(ParameterLoaderLog) << "Write resend for (componentId:" << componentId << "paramName:" << paramName << ")";
+ qCDebug(ParameterLoaderLog) << "Write resend for (componentId:" << componentId << "paramName:" << paramName << "retryCount:" << _waitingWriteParamNameMap[componentId][paramName] << ")";
if (++batchCount > maxBatchSize) {
goto Out;
@@ -458,10 +461,11 @@ void ParameterLoader::_waitingParamTimeout(void)
if (!paramsRequested) {
foreach(int componentId, _waitingReadParamNameMap.keys()) {
- foreach(QString paramName, _waitingReadParamNameMap[componentId]) {
+ foreach(QString paramName, _waitingReadParamNameMap[componentId].keys()) {
paramsRequested = true;
+ _waitingReadParamNameMap[componentId][paramName]++; // Bump retry count
_readParameterRaw(componentId, paramName, -1);
- qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramName:" << paramName << ")";
+ qCDebug(ParameterLoaderLog) << "Read re-request for (componentId:" << componentId << "paramName:" << paramName << "retryCount:" << _waitingReadParamNameMap[componentId][paramName] << ")";
if (++batchCount > maxBatchSize) {
goto Out;
@@ -715,3 +719,52 @@ void ParameterLoader::_restartWaitingParamTimer(void)
{
_waitingParamTimeoutTimer.start();
}
+
+void ParameterLoader::_checkInitialLoadComplete(void)
+{
+ // Already processed?
+ if (_initialLoadComplete) {
+ return;
+ }
+
+ foreach (int componentId, _waitingReadParamIndexMap.keys()) {
+ if (_waitingReadParamIndexMap[componentId].count()) {
+ // We are still waiting on some parameters, not done yet
+ return;
+ }
+ }
+
+
+ // We aren't waiting for any more initial parameter updates, initial parameter loading is complete
+ _initialLoadComplete = true;
+
+ // Check for load failures
+ QString indexList;
+ bool initialLoadFailures = false;
+ foreach (int componentId, _failedReadParamIndexMap.keys()) {
+ foreach (int paramIndex, _failedReadParamIndexMap[componentId]) {
+ if (initialLoadFailures) {
+ indexList += ", ";
+ }
+ indexList += QString("%1").arg(paramIndex);
+ initialLoadFailures = true;
+ qCDebug(ParameterLoaderLog) << "Gave up on initial load after max retries (componentId:" << componentId << "paramIndex:" << paramIndex << ")";
+ }
+ }
+
+ if (initialLoadFailures) {
+ QGCMessageBox::critical("Parameter Load Failure",
+ QString("QGroundControl was unable to retrieve the full set of parameters from the vehicle. "
+ "This will cause QGroundControl to be unable to display it's full user interface. "
+ "This usually indicates an error in the vehicle's firmware. "
+ "Please upgrade your firmware to the latest version if possible. "
+ "If that doesn't work, notify the firmware developers of this error. "
+ "The following parameter indices could not be loaded after the maximum number of retries: %1.").arg(indexList));
+ } else {
+ // No failed parameters, ok to signal ready
+ _parametersReady = true;
+ _determineDefaultComponentId();
+ _setupGroupMap();
+ emit parametersReady();
+ }
+}
\ No newline at end of file
diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h
index 92008ea..1906a96 100644
--- a/src/FactSystem/ParameterLoader.h
+++ b/src/FactSystem/ParameterLoader.h
@@ -120,6 +120,7 @@ private:
MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType);
FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType);
void _saveToEEPROM(void);
+ void _checkInitialLoadComplete(void);
AutoPilotPlugin* _autopilot;
UASInterface* _uas;
@@ -133,21 +134,23 @@ private:
/// Second mapping is group name, to Fact
QMap<int, QMap<QString, QStringList> > _mapGroup2ParameterName;
- bool _parametersReady; ///< All params received from param mgr
+ bool _parametersReady; ///< true: full set of parameters correctly loaded
+ bool _initialLoadComplete; ///< true: Initial load of all parameters complete, whether succesful or not
int _defaultComponentId;
QString _defaultComponentIdParam;
- QMap<int, int> _paramCountMap; ///< Map of total known parameter count, keyed by component id
- QMap<int, QList<int> > _waitingReadParamIndexMap; ///< Map of param indices waiting for initial first time read, keyed by component id
- QMap<int, QStringList> _waitingReadParamNameMap; ///< Map of param names we are waiting to hear a read response from, keyed by component id
- QMap<int, QStringList> _waitingWriteParamNameMap; ///< Map of param names we are waiting to hear a write response from, keyed by component id
+ static const int _maxInitialLoadRetry = 5; ///< Maximum a retries on initial index based load
+
+ QMap<int, int> _paramCountMap; ///< Key: Component id, Value: count of parameters in this component
+ QMap<int, QMap<int, int> > _waitingReadParamIndexMap; ///< Key: Component id, Value: Map { Key: parameter index still waiting for, Value: retry count }
+ QMap<int, QMap<QString, int> > _waitingReadParamNameMap; ///< Key: Component id, Value: Map { Key: parameter name still waiting for, Value: retry count }
+ QMap<int, QMap<QString, int> > _waitingWriteParamNameMap; ///< Key: Component id, Value: Map { Key: parameter name still waiting for, Value: retry count }
+ QMap<int, QList<int> > _failedReadParamIndexMap; ///< Key: Component id, Value: failed parameter index
int _totalParamCount; ///< Number of parameters across all components
QTimer _waitingParamTimeoutTimer;
- bool _fullRefresh;
-
QMutex _dataMutex;
static Fact _defaultFact; ///< Used to return default fact, when parameter not found