1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#ifndef PARAMETERLOADER_H
#define PARAMETERLOADER_H
#include <QObject>
#include <QMap>
#include <QXmlStreamReader>
#include <QLoggingCategory>
#include <QMutex>
#include <QDir>
#include "FactSystem.h"
#include "MAVLinkProtocol.h"
#include "AutoPilotPlugin.h"
#include "QGCMAVLink.h"
#include "Vehicle.h"
/// @file
/// @author Don Gagne <don@thegagnes.com>
Q_DECLARE_LOGGING_CATEGORY(ParameterLoaderVerboseLog)
/// Connects to Parameter Manager to load/update Facts
class ParameterLoader : public QObject
{
Q_OBJECT
public:
/// @param uas Uas which this set of facts is associated with
ParameterLoader(Vehicle* vehicle);
~ParameterLoader();
/// @return Directory of parameter caches
static QDir parameterCacheDir();
/// @return Location of parameter cache file
static QString parameterCacheFile(int uasId, int componentId);
/// Returns true if the full set of facts are ready
bool parametersAreReady(void) { return _parametersReady; }
/// Re-request the full set of parameters from the autopilot
void refreshAllParameters(uint8_t componentID = MAV_COMP_ID_ALL);
/// Request a refresh on the specific parameter
void refreshParameter(int componentId, const QString& name);
/// Request a refresh on all parameters that begin with the specified prefix
void refreshParametersPrefix(int componentId, const QString& namePrefix);
/// Returns true if the specifed parameter exists
bool parameterExists(int componentId, ///< fact component, -1=default component
const QString& name); ///< fact name
/// Returns all parameter names
QStringList parameterNames(int componentId);
/// Returns the specified Fact.
/// WARNING: Will assert if parameter does not exists. If that possibily exists, check for existence first with
/// parameterExists.
Fact* getFact(int componentId, ///< fact component, -1=default component
const QString& name); ///< fact name
const QMap<int, QMap<QString, QStringList> >& getGroupMap(void);
/// Returns error messages from loading
QString readParametersFromStream(QTextStream& stream);
void writeParametersToStream(QTextStream &stream);
/// Returns the version number for the parameter set, -1 if not known
int parameterSetVersion(void) { return _parameterSetMajorVersion; }
/// Returns the newest available parameter meta data file (from cache or internal) for the specified information.
/// @param wantedMajorVersion Major version you are looking for
/// @param[out] majorVersion Major version for found meta data
/// @param[out] minorVersion Minor version for found meta data
/// @return Meta data file name of best match, emptyString is none found
static QString parameterMetaDataFile(MAV_AUTOPILOT firmwareType, int wantedMajorVersion, int& majorVersion, int& minorVersion);
/// If this file is newer than anything in the cache, cache it as the latest version
static void cacheMetaDataFile(const QString& metaDataFile, MAV_AUTOPILOT firmwareType);
int defaultComponenentId(void) { return _defaultComponentId; }
signals:
/// Signalled when the full set of facts are ready
void parametersReady(bool missingParameters);
/// Signalled to update progress of full parameter list request
void parameterListProgress(float value);
/// Signalled to ourselves in order to get call on our own thread
void restartWaitingParamTimer(void);
protected:
Vehicle* _vehicle;
MAVLinkProtocol* _mavlink;
void _parameterUpdate(int uasId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value);
void _valueUpdated(const QVariant& value);
void _restartWaitingParamTimer(void);
void _waitingParamTimeout(void);
void _tryCacheLookup(void);
void _initialRequestTimeout(void);
private:
static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false);
int _actualComponentId(int componentId);
void _determineDefaultComponentId(void);
void _setupGroupMap(void);
void _readParameterRaw(int componentId, const QString& paramName, int paramIndex);
void _writeParameterRaw(int componentId, const QString& paramName, const QVariant& value);
void _writeLocalParamCache(int uasId, int componentId);
void _tryCacheHashLoad(int uasId, int componentId, QVariant hash_value);
void _addMetaDataToDefaultComponent(void);
QString _remapParamNameToVersion(const QString& paramName);
MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType);
FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType);
void _saveToEEPROM(void);
void _checkInitialLoadComplete(bool failIfNoDefaultComponent);
/// First mapping is by component id
/// Second mapping is parameter name, to Fact* in QVariant
QMap<int, QVariantMap> _mapParameterName2Variant;
QMap<int, QMap<int, QString> > _mapParameterId2Name;
/// First mapping is by component id
/// Second mapping is group name, to Fact
QMap<int, QMap<QString, QStringList> > _mapGroup2ParameterName;
bool _parametersReady; ///< true: full set of parameters correctly loaded
bool _initialLoadComplete; ///< true: Initial load of all parameters complete, whether succesful or not
bool _waitingForDefaultComponent; ///< true: last chance wait for default component params
bool _saveRequired; ///< true: _saveToEEPROM should be called
int _defaultComponentId;
QString _defaultComponentIdParam; ///< Parameter which identifies default component
QString _versionParam; ///< Parameter which contains parameter set version
int _parameterSetMajorVersion; ///< Version for parameter set, -1 if not known
QObject* _parameterMetaData; ///< Opaque data from FirmwarePlugin::loadParameterMetaDataCall
static const int _maxInitialRequestListRetry = 5; ///< Maximum retries for request list
int _initialRequestRetryCount; ///< Current retry count for request list
static const int _maxInitialLoadRetrySingleParam = 10; ///< Maximum retries for initial index based load of a single param
static const int _maxReadWriteRetry = 5; ///< Maximum retries read/write
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 _initialRequestTimeoutTimer;
QTimer _waitingParamTimeoutTimer;
QMutex _dataMutex;
static Fact _defaultFact; ///< Used to return default fact, when parameter not found
static const char* _cachedMetaDataFilePrefix;
};
#endif