ParameterLoader.h 8.06 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

10 11 12 13 14 15 16 17

#ifndef PARAMETERLOADER_H
#define PARAMETERLOADER_H

#include <QObject>
#include <QMap>
#include <QXmlStreamReader>
#include <QLoggingCategory>
18
#include <QMutex>
Don Gagne's avatar
Don Gagne committed
19
#include <QDir>
20 21

#include "FactSystem.h"
22 23
#include "MAVLinkProtocol.h"
#include "AutoPilotPlugin.h"
24
#include "QGCMAVLink.h"
25
#include "Vehicle.h"
26 27 28 29

/// @file
///     @author Don Gagne <don@thegagnes.com>

30
Q_DECLARE_LOGGING_CATEGORY(ParameterLoaderVerboseLog)
31 32 33 34 35 36 37 38

/// 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
39
    ParameterLoader(Vehicle* vehicle);
40 41
    
    ~ParameterLoader();
42

43 44 45
    /// @return Directory of parameter caches
    static QDir parameterCacheDir();

46
    /// @return Location of parameter cache file
47
    static QString parameterCacheFile(int uasId, int componentId);
48 49 50
    
    /// Returns true if the full set of facts are ready
    bool parametersAreReady(void) { return _parametersReady; }
51

52
    /// Re-request the full set of parameters from the autopilot
dogmaphobic's avatar
dogmaphobic committed
53
    void refreshAllParameters(uint8_t componentID = MAV_COMP_ID_ALL);
54

55 56 57 58 59 60
    /// 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);
    
61 62 63 64 65
    /// 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
Don Gagne's avatar
Don Gagne committed
66
	QStringList parameterNames(int componentId);
67 68
    
    /// Returns the specified Fact.
nopeppermint's avatar
nopeppermint committed
69
    /// WARNING: Will assert if parameter does not exists. If that possibily exists, check for existence first with
70
    /// parameterExists.
71 72 73
    Fact* getFact(int               componentId,    ///< fact component, -1=default component
                  const QString&    name);          ///< fact name
    
74 75
    const QMap<int, QMap<QString, QStringList> >& getGroupMap(void);
    
76 77 78
    /// Returns error messages from loading
    QString readParametersFromStream(QTextStream& stream);
    
79
    void writeParametersToStream(QTextStream &stream);
80 81 82 83 84 85 86 87 88 89 90 91 92

    /// 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);
Don Gagne's avatar
Don Gagne committed
93 94

    int defaultComponenentId(void) { return _defaultComponentId; }
95 96 97
    
signals:
    /// Signalled when the full set of facts are ready
98
    void parametersReady(bool missingParameters);
99 100 101 102 103 104

    /// 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);
105 106
    
protected:
107 108
    Vehicle*            _vehicle;
    MAVLinkProtocol*    _mavlink;
109
    
110
    void _parameterUpdate(int uasId, int componentId, QString parameterName, int parameterCount, int parameterId, int mavType, QVariant value);
111
    void _valueUpdated(const QVariant& value);
112 113
    void _restartWaitingParamTimer(void);
    void _waitingParamTimeout(void);
114
    void _tryCacheLookup(void);
115
    void _initialRequestTimeout(void);
dogmaphobic's avatar
dogmaphobic committed
116

117 118 119 120
private:
    static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false);
    int _actualComponentId(int componentId);
    void _determineDefaultComponentId(void);
121
    void _setupGroupMap(void);
122 123
    void _readParameterRaw(int componentId, const QString& paramName, int paramIndex);
    void _writeParameterRaw(int componentId, const QString& paramName, const QVariant& value);
124 125
    void _writeLocalParamCache(int uasId, int componentId);
    void _tryCacheHashLoad(int uasId, int componentId, QVariant hash_value);
126
    void _addMetaDataToDefaultComponent(void);
127
    QString _remapParamNameToVersion(const QString& paramName);
128

129 130 131
    MAV_PARAM_TYPE _factTypeToMavType(FactMetaData::ValueType_t factType);
    FactMetaData::ValueType_t _mavTypeToFactType(MAV_PARAM_TYPE mavType);
    void _saveToEEPROM(void);
132
    void _checkInitialLoadComplete(bool failIfNoDefaultComponent);
133

134
    /// First mapping is by component id
135
    /// Second mapping is parameter name, to Fact* in QVariant
136
    QMap<int, QVariantMap>            _mapParameterName2Variant;
137

138
    QMap<int, QMap<int, QString> >    _mapParameterId2Name;
139
    
140 141 142 143
    /// First mapping is by component id
    /// Second mapping is group name, to Fact
    QMap<int, QMap<QString, QStringList> > _mapGroup2ParameterName;
    
144 145 146 147
    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
148
    int         _defaultComponentId;
149 150 151 152
    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
153

Don Gagne's avatar
Don Gagne committed
154 155 156 157 158 159 160
    // Wait counts from previous parameter update cycle
    int         _prevWaitingReadParamIndexCount;
    int         _prevWaitingReadParamNameCount;
    int         _prevWaitingWriteParamNameCount;


    static const int _maxInitialRequestListRetry = 4;       ///< Maximum retries for request list
161 162 163
    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
164

165 166 167 168 169
    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
170

171 172
    int _totalParamCount;   ///< Number of parameters across all components
    
173
    QTimer _initialRequestTimeoutTimer;
174 175 176
    QTimer _waitingParamTimeoutTimer;
    
    QMutex _dataMutex;
Don Gagne's avatar
Don Gagne committed
177 178
    
    static Fact _defaultFact;   ///< Used to return default fact, when parameter not found
179 180

    static const char* _cachedMetaDataFilePrefix;
181 182
};

183
#endif