ParameterManager.h 9.76 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

10
#pragma once
11 12 13 14 15

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

#include "FactSystem.h"
21 22
#include "MAVLinkProtocol.h"
#include "AutoPilotPlugin.h"
23
#include "QGCMAVLink.h"
24
#include "Vehicle.h"
25

26 27
Q_DECLARE_LOGGING_CATEGORY(ParameterManagerVerbose1Log)
Q_DECLARE_LOGGING_CATEGORY(ParameterManagerVerbose2Log)
28
Q_DECLARE_LOGGING_CATEGORY(ParameterManagerDebugCacheFailureLog)
29

30 31
class ParameterEditorController;

32
class ParameterManager : public QObject
33 34
{
    Q_OBJECT
35

36 37
    friend class ParameterEditorController;

38 39
public:
    /// @param uas Uas which this set of facts is associated with
40
    ParameterManager(Vehicle* vehicle);
41

42 43 44
    Q_PROPERTY(bool     parametersReady     READ parametersReady    NOTIFY parametersReadyChanged)      ///< true: Parameters are ready for use
    Q_PROPERTY(bool     missingParameters   READ missingParameters  NOTIFY missingParametersChanged)    ///< true: Parameters are missing from firmware response, false: all parameters received from firmware
    Q_PROPERTY(double   loadProgress        READ loadProgress       NOTIFY loadProgressChanged)
45
    Q_PROPERTY(bool     pendingWrites       READ pendingWrites      NOTIFY pendingWritesChanged)        ///< true: There are still pending write updates against the vehicle
46

47 48 49
    bool parametersReady    (void) const { return _parametersReady; }
    bool missingParameters  (void) const { return _missingParameters; }
    double loadProgress     (void) const { return _loadProgress; }
50

51 52 53
    /// @return Directory of parameter caches
    static QDir parameterCacheDir();

54
    /// @return Location of parameter cache file
55
    static QString parameterCacheFile(int vehicleId, int componentId);
Don Gagne's avatar
Don Gagne committed
56

57 58
    void mavlinkMessageReceived(mavlink_message_t message);

Don Gagne's avatar
Don Gagne committed
59
    QList<int> componentIds(void);
60

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

64
    /// Request a refresh on the specific parameter
65
    void refreshParameter(int componentId, const QString& paramName);
66

67 68
    /// Request a refresh on all parameters that begin with the specified prefix
    void refreshParametersPrefix(int componentId, const QString& namePrefix);
69 70 71

    void resetAllParametersToDefaults();
    void resetAllToVehicleConfiguration();
72

73
    /// Returns true if the specifed parameter exists
Gus Grubba's avatar
Gus Grubba committed
74 75
    ///     @param componentId: Component id or FactSystem::defaultComponentId
    ///     @param name: Parameter name
76
    bool parameterExists(int componentId, const QString& paramName);
77

78 79 80
    /// Returns all parameter names
    QStringList parameterNames(int componentId);

81 82
    /// Returns the specified Parameter. Returns a default empty fact is parameter does not exists. Also will pop
    /// a missing parameter error to user if parameter does not exist.
Gus Grubba's avatar
Gus Grubba committed
83 84
    ///     @param componentId: Component id or FactSystem::defaultComponentId
    ///     @param name: Parameter name
85
    Fact* getParameter(int componentId, const QString& paramName);
86

87 88
    /// Returns error messages from loading
    QString readParametersFromStream(QTextStream& stream);
89

90
    void writeParametersToStream(QTextStream& stream);
91

92 93
    bool pendingWrites(void);

94 95
    Vehicle* vehicle(void) { return _vehicle; }

96 97 98
    static MAV_PARAM_TYPE               factTypeToMavType(FactMetaData::ValueType_t factType);
    static FactMetaData::ValueType_t    mavTypeToFactType(MAV_PARAM_TYPE mavType);

99
signals:
100 101 102 103
    void parametersReadyChanged     (bool parametersReady);
    void missingParametersChanged   (bool missingParameters);
    void loadProgressChanged        (float value);
    void pendingWritesChanged       (bool pendingWrites);
104
    void factAdded                  (int componentId, Fact* fact);
105

106 107
private slots:
    void    _factRawValueUpdated                (const QVariant& rawValue);
dogmaphobic's avatar
dogmaphobic committed
108

109
private:
110 111 112 113 114
    void    _handleParamValue                   (int componentId, QString parameterName, int parameterCount, int parameterIndex, MAV_PARAM_TYPE mavParamType, QVariant parameterValue);
    void    _factRawValueUpdateWorker           (int componentId, const QString& name, FactMetaData::ValueType_t valueType, const QVariant& rawValue);
    void    _waitingParamTimeout                (void);
    void    _tryCacheLookup                     (void);
    void    _initialRequestTimeout              (void);
115 116
    int     _actualComponentId                  (int componentId);
    void    _readParameterRaw                   (int componentId, const QString& paramName, int paramIndex);
117
    void    _sendParamSetToVehicle              (int componentId, const QString& paramName, FactMetaData::ValueType_t valueType, const QVariant& value);
118 119 120 121 122 123 124 125 126 127
    void    _writeLocalParamCache               (int vehicleId, int componentId);
    void    _tryCacheHashLoad                   (int vehicleId, int componentId, QVariant hash_value);
    void    _loadMetaData                       (void);
    void    _clearMetaData                      (void);
    QString _remapParamNameToVersion            (const QString& paramName);
    void    _loadOfflineEditingParams           (void);
    QString _logVehiclePrefix                   (int componentId);
    void    _setLoadProgress                    (double loadProgress);
    bool    _fillIndexBatchQueue                (bool waitingParamTimeout);
    void    _updateProgressBar                  (void);
128
    void    _checkInitialLoadComplete           (void);
129

130
    static QVariant _stringToTypedVariant(const QString& string, FactMetaData::ValueType_t type, bool failOk = false);
131

132 133
    Vehicle*            _vehicle;
    MAVLinkProtocol*    _mavlink;
134

135
    QMap<int /* comp id */, QMap<QString /* parameter name */, Fact*>> _mapCompId2FactMap;
136

137
    double      _loadProgress;                  ///< Parameter load progess, [0.0,1.0]
138 139
    bool        _parametersReady;               ///< true: parameter load complete
    bool        _missingParameters;             ///< true: parameter missing from initial load
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
140
    bool        _initialLoadComplete;           ///< true: Initial load of all parameters complete, whether successful or not
141 142
    bool        _waitingForDefaultComponent;    ///< true: last chance wait for default component params
    bool        _saveRequired;                  ///< true: _saveToEEPROM should be called
143
    bool        _metaDataAddedToFacts;          ///< true: FactMetaData has been adde to the default component facts
Don Gagne's avatar
Don Gagne committed
144
    bool        _logReplay;                     ///< true: running with log replay link
145

146 147 148 149 150 151 152
    typedef QPair<int /* FactMetaData::ValueType_t */, QVariant /* Fact::rawValue */> ParamTypeVal;
    typedef QMap<QString /* parameter name */, ParamTypeVal> CacheMapName2ParamTypeVal;

    QMap<int /* component id */, bool>                                              _debugCacheCRC; ///< true: debug cache crc failure
    QMap<int /* component id */, CacheMapName2ParamTypeVal>                         _debugCacheMap;
    QMap<int /* component id */, QMap<QString /* param name */, bool /* seen */>>   _debugCacheParamSeen;

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

    bool _readParamIndexProgressActive =    false;
    bool _readParamNameProgressActive =     false;
    bool _writeParamProgressActive =        false;
Don Gagne's avatar
Don Gagne committed
161

162 163 164 165 166
    static const int    _maxInitialRequestListRetry = 4;        ///< Maximum retries for request list
    int                 _initialRequestRetryCount;              ///< Current retry count for request list
    static const int    _maxInitialLoadRetrySingleParam = 5;    ///< Maximum retries for initial index based load of a single param
    static const int    _maxReadWriteRetry = 5;                 ///< Maximum retries read/write
    bool                _disableAllRetries;                     ///< true: Don't retry any requests (used for testing)
167

168 169 170
    bool        _indexBatchQueueActive; ///< true: we are actively batching re-requests for missing index base params, false: index based re-request has not yet started
    QList<int>  _indexBatchQueue;       ///< The current queue of index re-requests

171 172 173 174 175
    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
176

177 178 179
    int _totalParamCount;                       ///< Number of parameters across all components
    int _waitingWriteParamBatchCount = 0;       ///< Number of parameters which are batched up waiting on write responses
    int _waitingReadParamNameBatchCount = 0;    ///< Number of parameters which are batched up waiting on read responses
180

181
    QTimer _initialRequestTimeoutTimer;
182
    QTimer _waitingParamTimeoutTimer;
183

184
    Fact _defaultFact;   ///< Used to return default fact, when parameter not found
185

186 187 188 189
    static const char* _jsonParametersKey;
    static const char* _jsonCompIdKey;
    static const char* _jsonParamNameKey;
    static const char* _jsonParamValueKey;
190
};