MAVLinkInspectorController.h 13.2 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 10 11 12 13 14 15 16 17
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#pragma once

#include "MAVLinkProtocol.h"
#include "Vehicle.h"

#include <QObject>
#include <QString>
#include <QDebug>
18 19
#include <QVariantList>
#include <QtCharts/QAbstractSeries>
20

21 22
Q_DECLARE_LOGGING_CATEGORY(MAVLinkInspectorLog)

23 24 25
QT_CHARTS_USE_NAMESPACE

class QGCMAVLinkMessage;
Gus Grubba's avatar
Gus Grubba committed
26
class QGCMAVLinkVehicle;
27
class MAVLinkChartController;
28 29
class MAVLinkInspectorController;

30 31 32
//-----------------------------------------------------------------------------
class QGCMAVLinkMessageField : public QObject {
    Q_OBJECT
33
public:
34 35 36 37 38
    Q_PROPERTY(QString          name        READ name       CONSTANT)
    Q_PROPERTY(QString          label       READ label      CONSTANT)
    Q_PROPERTY(QString          type        READ type       CONSTANT)
    Q_PROPERTY(QString          value       READ value      NOTIFY valueChanged)
    Q_PROPERTY(bool             selectable  READ selectable NOTIFY selectableChanged)
39
    Q_PROPERTY(int              chartIndex  READ chartIndex CONSTANT)
40
    Q_PROPERTY(QAbstractSeries* series      READ series     NOTIFY seriesChanged)
41

42
    QGCMAVLinkMessageField(QGCMAVLinkMessage* parent, QString name, QString type);
43

44 45 46 47 48 49 50 51 52 53
    QString         name            () { return _name;  }
    QString         label           ();
    QString         type            () { return _type;  }
    QString         value           () { return _value; }
    bool            selectable      () { return _selectable; }
    bool            selected        () { return _pSeries != nullptr; }
    QAbstractSeries*series          () { return _pSeries; }
    QList<QPointF>* values          () { return &_values;}
    qreal           rangeMin        () { return _rangeMin; }
    qreal           rangeMax        () { return _rangeMax; }
54
    int             chartIndex      ();
55 56 57 58

    void            setSelectable   (bool sel);
    void            updateValue     (QString newValue, qreal v);

59
    void            addSeries       (MAVLinkChartController* chart, QAbstractSeries* series);
60
    void            delSeries       ();
Gus Grubba's avatar
Gus Grubba committed
61
    void            updateSeries    ();
62 63

signals:
64 65 66
    void            seriesChanged       ();
    void            selectableChanged   ();
    void            valueChanged        ();
67 68

private:
69 70 71
    QString     _type;
    QString     _name;
    QString     _value;
72 73 74 75
    bool        _selectable = true;
    int         _dataIndex  = 0;
    qreal       _rangeMin   = 0;
    qreal       _rangeMax   = 0;
76 77 78

    QAbstractSeries*    _pSeries = nullptr;
    QGCMAVLinkMessage*  _msg     = nullptr;
79
    MAVLinkChartController*      _chart   = nullptr;
80
    QList<QPointF>      _values;
81 82 83 84 85
};

//-----------------------------------------------------------------------------
class QGCMAVLinkMessage : public QObject {
    Q_OBJECT
86
public:
Gus Grubba's avatar
Gus Grubba committed
87 88 89 90 91 92 93
    Q_PROPERTY(quint32              id              READ id             NOTIFY indexChanged)
    Q_PROPERTY(quint32              cid             READ cid            NOTIFY indexChanged)
    Q_PROPERTY(QString              name            READ name           NOTIFY indexChanged)
    Q_PROPERTY(qreal                messageHz       READ messageHz      NOTIFY freqChanged)
    Q_PROPERTY(quint64              count           READ count          NOTIFY messageChanged)
    Q_PROPERTY(QmlObjectListModel*  fields          READ fields         NOTIFY indexChanged)
    Q_PROPERTY(bool                 fieldSelected   READ fieldSelected  NOTIFY fieldSelectedChanged)
94
    Q_PROPERTY(bool                 selected        READ selected       NOTIFY selectedChanged)
95

96 97
    QGCMAVLinkMessage   (QObject* parent, mavlink_message_t* message);
    ~QGCMAVLinkMessage  ();
98

Gus Grubba's avatar
Gus Grubba committed
99 100 101 102 103 104 105 106
    quint32             id              () { return _message.msgid;  }
    quint8              cid             () { return _message.compid; }
    QString             name            () { return _name;  }
    qreal               messageHz       () { return _messageHz; }
    quint64             count           () { return _count; }
    quint64             lastCount       () { return _lastCount; }
    QmlObjectListModel* fields          () { return &_fields; }
    bool                fieldSelected   () { return _fieldSelected; }
107
    bool                selected        () { return _selected; }
108

Gus Grubba's avatar
Gus Grubba committed
109 110 111
    void                updateFieldSelection();
    void                update          (mavlink_message_t* message);
    void                updateFreq      ();
112
    void                setSelected     (bool sel) { _selected = sel; }
113 114

signals:
Gus Grubba's avatar
Gus Grubba committed
115 116 117 118
    void messageChanged                 ();
    void freqChanged                    ();
    void indexChanged                   ();
    void fieldSelectedChanged           ();
119
    void selectedChanged                ();
120 121 122 123

private:
    QmlObjectListModel  _fields;
    QString             _name;
Gus Grubba's avatar
Gus Grubba committed
124
    qreal               _messageHz  = 0.0;
125
    uint64_t            _count      = 0;
Gus Grubba's avatar
Gus Grubba committed
126
    uint64_t            _lastCount  = 0;
127
    mavlink_message_t   _message;   //-- List of QGCMAVLinkMessageField
Gus Grubba's avatar
Gus Grubba committed
128
    bool                _fieldSelected   = false;
129
    bool                _selected   = false;
130 131 132 133 134
};

//-----------------------------------------------------------------------------
class QGCMAVLinkVehicle : public QObject {
    Q_OBJECT
135
public:
Gus Grubba's avatar
Gus Grubba committed
136 137
    Q_PROPERTY(quint8               id              READ id             CONSTANT)
    Q_PROPERTY(QmlObjectListModel*  messages        READ messages       NOTIFY messagesChanged)
138 139
    Q_PROPERTY(QList<int>           compIDs         READ compIDs        NOTIFY compIDsChanged)
    Q_PROPERTY(QStringList          compIDsStr      READ compIDsStr     NOTIFY compIDsChanged)
140

Gus Grubba's avatar
Gus Grubba committed
141 142
    Q_PROPERTY(int                  selected        READ selected       WRITE setSelected   NOTIFY selectedChanged)

143 144
    QGCMAVLinkVehicle   (QObject* parent, quint8 id);
    ~QGCMAVLinkVehicle  ();
145

Gus Grubba's avatar
Gus Grubba committed
146 147
    quint8              id              () { return _id; }
    QmlObjectListModel* messages        () { return &_messages; }
148 149
    QList<int>          compIDs         () { return _compIDs; }
    QStringList         compIDsStr      () { return _compIDsStr; }
Gus Grubba's avatar
Gus Grubba committed
150
    int                 selected        () { return _selected; }
151

Gus Grubba's avatar
Gus Grubba committed
152
    void                setSelected     (int sel);
Gus Grubba's avatar
Gus Grubba committed
153
    QGCMAVLinkMessage*  findMessage     (uint32_t id, uint8_t cid);
Gus Grubba's avatar
Gus Grubba committed
154
    int                 findMessage     (QGCMAVLinkMessage* message);
Gus Grubba's avatar
Gus Grubba committed
155
    void                append          (QGCMAVLinkMessage* message);
156 157

signals:
Gus Grubba's avatar
Gus Grubba committed
158
    void messagesChanged                ();
159
    void compIDsChanged                 ();
Gus Grubba's avatar
Gus Grubba committed
160
    void selectedChanged                ();
161 162 163

private:
    void _checkCompID                   (QGCMAVLinkMessage *message);
164
    void _resetSelection                ();
165 166 167

private:
    quint8              _id;
168 169 170
    QList<int>          _compIDs;
    QStringList         _compIDsStr;
    QmlObjectListModel  _messages;      //-- List of QGCMAVLinkMessage
Gus Grubba's avatar
Gus Grubba committed
171
    int                 _selected = 0;
172 173 174
};

//-----------------------------------------------------------------------------
175
class MAVLinkChartController : public QObject {
176 177
    Q_OBJECT
public:
178
    MAVLinkChartController(MAVLinkInspectorController* parent, int index);
179

180 181 182 183 184
    Q_PROPERTY(QVariantList chartFields         READ chartFields            NOTIFY chartFieldsChanged)
    Q_PROPERTY(QDateTime    rangeXMin           READ rangeXMin              NOTIFY rangeXMinChanged)
    Q_PROPERTY(QDateTime    rangeXMax           READ rangeXMax              NOTIFY rangeXMaxChanged)
    Q_PROPERTY(qreal        rangeYMin           READ rangeYMin              NOTIFY rangeYMinChanged)
    Q_PROPERTY(qreal        rangeYMax           READ rangeYMax              NOTIFY rangeYMaxChanged)
Gus Grubba's avatar
Gus Grubba committed
185
    Q_PROPERTY(int          chartIndex          READ chartIndex             CONSTANT)
186 187 188 189 190

    Q_PROPERTY(quint32      rangeYIndex         READ rangeYIndex            WRITE setRangeYIndex    NOTIFY rangeYIndexChanged)
    Q_PROPERTY(quint32      rangeXIndex         READ rangeXIndex            WRITE setRangeXIndex    NOTIFY rangeXIndexChanged)

    Q_INVOKABLE void        addSeries           (QGCMAVLinkMessageField* field, QAbstractSeries* series);
191 192
    Q_INVOKABLE void        delSeries           (QGCMAVLinkMessageField* field);

193 194 195 196 197 198 199 200 201
    Q_INVOKABLE MAVLinkInspectorController* controller() { return _controller; }

    QVariantList            chartFields         () { return _chartFields; }
    QDateTime               rangeXMin           () { return _rangeXMin;   }
    QDateTime               rangeXMax           () { return _rangeXMax;   }
    qreal                   rangeYMin           () { return _rangeYMin;   }
    qreal                   rangeYMax           () { return _rangeYMax;   }
    quint32                 rangeXIndex         () { return _rangeXIndex; }
    quint32                 rangeYIndex         () { return _rangeYIndex; }
202
    int                     chartIndex          () { return _index; }
203 204 205

    void                    setRangeXIndex      (quint32 t);
    void                    setRangeYIndex      (quint32 r);
206
    void                    updateXRange        ();
207
    void                    updateYRange        ();
208 209

signals:
210 211 212 213 214 215 216
    void chartFieldsChanged ();
    void rangeXMinChanged   ();
    void rangeXMaxChanged   ();
    void rangeYMinChanged   ();
    void rangeYMaxChanged   ();
    void rangeYIndexChanged ();
    void rangeXIndexChanged ();
217

218
private slots:
219
    void _refreshSeries     ();
220 221

private:
222 223 224
    QTimer              _updateSeriesTimer;
    QDateTime           _rangeXMin;
    QDateTime           _rangeXMax;
225
    int                 _index               = 0;
226 227 228 229 230 231 232
    qreal               _rangeYMin           = 0;
    qreal               _rangeYMax           = 1;
    quint32             _rangeXIndex         = 0;                    ///< 5 Seconds
    quint32             _rangeYIndex         = 0;                    ///< Auto Range
    QVariantList        _chartFields;
    MAVLinkInspectorController* _controller  = nullptr;
};
233

234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257
//-----------------------------------------------------------------------------
class MAVLinkInspectorController : public QObject
{
    Q_OBJECT
public:
    MAVLinkInspectorController();
    ~MAVLinkInspectorController();

    Q_PROPERTY(QStringList          vehicleNames        READ vehicleNames           NOTIFY vehiclesChanged)
    Q_PROPERTY(QmlObjectListModel*  vehicles            READ vehicles               NOTIFY vehiclesChanged)
    Q_PROPERTY(QmlObjectListModel*  charts              READ charts                 NOTIFY chartsChanged)
    Q_PROPERTY(QGCMAVLinkVehicle*   activeVehicle       READ activeVehicle          NOTIFY activeVehiclesChanged)
    Q_PROPERTY(QStringList          timeScales          READ timeScales             NOTIFY timeScalesChanged)
    Q_PROPERTY(QStringList          rangeList           READ rangeList              NOTIFY rangeListChanged)

    Q_INVOKABLE MAVLinkChartController* createChart     ();
    Q_INVOKABLE void                    deleteChart     (MAVLinkChartController* chart);

    QmlObjectListModel*             vehicles            () { return &_vehicles;     }
    QmlObjectListModel*             charts              () { return &_charts;       }
    QGCMAVLinkVehicle*              activeVehicle       () { return _activeVehicle; }
    QStringList                     vehicleNames        () { return _vehicleNames;  }
    QStringList                     timeScales          ();
    QStringList                     rangeList           ();
Gus Grubba's avatar
Gus Grubba committed
258 259 260 261 262 263 264 265

    class TimeScale_st : public QObject {
    public:
        TimeScale_st(QObject* parent, const QString& l, uint32_t t);
        QString     label;
        uint32_t    timeScale;
    };

266 267 268 269 270 271 272
    class Range_st : public QObject {
    public:
        Range_st(QObject* parent, const QString& l, qreal r);
        QString     label;
        qreal       range;
    };

273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294
    const QList<TimeScale_st*>&     timeScaleSt         () { return _timeScaleSt; }
    const QList<Range_st*>&         rangeSt             () { return _rangeSt; }

signals:
    void vehiclesChanged            ();
    void chartsChanged              ();
    void activeVehiclesChanged      ();
    void timeScalesChanged          ();
    void rangeListChanged           ();

private slots:
    void _receiveMessage            (LinkInterface* link, mavlink_message_t message);
    void _vehicleAdded              (Vehicle* vehicle);
    void _vehicleRemoved            (Vehicle* vehicle);
    void _setActiveVehicle          (Vehicle* vehicle);
    void _refreshFrequency          ();

private:
    QGCMAVLinkVehicle* _findVehicle (uint8_t id);

private:

295 296 297
    int                 _selectedSystemID       = 0;                    ///< Currently selected system
    int                 _selectedComponentID    = 0;                    ///< Currently selected component
    QStringList         _timeScales;
298
    QStringList         _rangeList;
299
    QGCMAVLinkVehicle*  _activeVehicle          = nullptr;
Gus Grubba's avatar
Gus Grubba committed
300
    QTimer              _updateFrequencyTimer;
301
    QStringList         _vehicleNames;
302
    QmlObjectListModel  _vehicles;                                      ///< List of QGCMAVLinkVehicle
303
    QmlObjectListModel  _charts;                                        ///< List of MAVLinkCharts
Gus Grubba's avatar
Gus Grubba committed
304
    QList<TimeScale_st*>_timeScaleSt;
305
    QList<Range_st*>    _rangeSt;
306

307
};