MockLink.h 12.3 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
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9

10

11
#pragma once
12

13
#include <QElapsedTimer>
14
#include <QMap>
15
#include <QLoggingCategory>
16
#include <QGeoCoordinate>
17 18

#include "MockLinkMissionItemHandler.h"
19
#include "MockLinkFileServer.h"
20
#include "LinkManager.h"
21 22 23
#include "QGCMAVLink.h"

Q_DECLARE_LOGGING_CATEGORY(MockLinkLog)
Don Gagne's avatar
Don Gagne committed
24
Q_DECLARE_LOGGING_CATEGORY(MockLinkVerboseLog)
25

26 27
class MockConfiguration : public LinkConfiguration
{
Don Gagne's avatar
Don Gagne committed
28 29
    Q_OBJECT

30
public:
31 32 33 34

    Q_PROPERTY(int      firmware    READ firmware           WRITE setFirmware       NOTIFY firmwareChanged)
    Q_PROPERTY(int      vehicle     READ vehicle            WRITE setVehicle        NOTIFY vehicleChanged)
    Q_PROPERTY(bool     sendStatus  READ sendStatusText     WRITE setSendStatusText NOTIFY sendStatusChanged)
35
    Q_PROPERTY(bool     highLatency READ highLatency        WRITE setHighLatency    NOTIFY highLatencyChanged)
36 37 38 39 40

    // QML Access
    int     firmware        () { return (int)_firmwareType; }
    void    setFirmware     (int type) { _firmwareType = (MAV_AUTOPILOT)type; emit firmwareChanged(); }
    int     vehicle         () { return (int)_vehicleType; }
41
    bool    highLatency     () const { return _highLatency; }
42
    void    setVehicle      (int type) { _vehicleType = (MAV_TYPE)type; emit vehicleChanged(); }
43
    void    setHighLatency  (bool latency) { _highLatency = latency; emit highLatencyChanged(); }
44

45 46 47 48
    MockConfiguration(const QString& name);
    MockConfiguration(MockConfiguration* source);

    MAV_AUTOPILOT firmwareType(void) { return _firmwareType; }
49
    void setFirmwareType(MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; emit firmwareChanged(); }
50

51
    MAV_TYPE vehicleType(void) { return _vehicleType; }
52
    void setVehicleType(MAV_TYPE vehicleType) { _vehicleType = vehicleType; emit vehicleChanged(); }
53

54 55
    /// @param sendStatusText true: mavlink status text messages will be sent for each severity, as well as voice output info message
    bool sendStatusText(void) { return _sendStatusText; }
56
    void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; emit sendStatusChanged(); }
57

Don Gagne's avatar
Don Gagne committed
58 59 60 61 62 63 64 65 66
    typedef enum {
        FailNone,                           // No failures
        FailParamNoReponseToRequestList,    // Do no respond to PARAM_REQUEST_LIST
        FailMissingParamOnInitialReqest,    // Not all params are sent on initial request, should still succeed since QGC will re-query missing params
        FailMissingParamOnAllRequests,      // Not all params are sent on initial request, QGC retries will fail as well
    } FailureMode_t;
    FailureMode_t failureMode(void) { return _failureMode; }
    void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; }

67
    // Overrides from LinkConfiguration
68 69 70 71 72 73
    LinkType    type            (void) { return LinkConfiguration::TypeMock; }
    void        copyFrom        (LinkConfiguration* source);
    void        loadSettings    (QSettings& settings, const QString& root);
    void        saveSettings    (QSettings& settings, const QString& root);
    void        updateSettings  (void);
    QString     settingsURL     () { return "MockLinkSettings.qml"; }
74
    QString     settingsTitle   () { return tr("Mock Link Settings"); }
75 76 77 78 79

signals:
    void firmwareChanged    ();
    void vehicleChanged     ();
    void sendStatusChanged  ();
80
    void highLatencyChanged ();
81 82

private:
83
    MAV_AUTOPILOT   _firmwareType;
84
    MAV_TYPE        _vehicleType;
85
    bool            _sendStatusText;
86
    bool            _highLatency;
Don Gagne's avatar
Don Gagne committed
87
    FailureMode_t   _failureMode;
88

89
    static const char* _firmwareTypeKey;
90
    static const char* _vehicleTypeKey;
91
    static const char* _sendStatusTextKey;
92
    static const char* _highLatencyKey;
Don Gagne's avatar
Don Gagne committed
93
    static const char* _failureModeKey;
94 95
};

96 97 98
class MockLink : public LinkInterface
{
    Q_OBJECT
99

100
public:
101
    MockLink(SharedLinkConfigurationPointer& config);
102
    ~MockLink(void);
103

104
    // MockLink methods
Don Gagne's avatar
Don Gagne committed
105
    int vehicleId(void) { return _vehicleSystemId; }
Don Gagne's avatar
Don Gagne committed
106 107
    MAV_AUTOPILOT getFirmwareType(void) { return _firmwareType; }
    void setFirmwareType(MAV_AUTOPILOT autopilot) { _firmwareType = autopilot; }
108
    void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; }
Don Gagne's avatar
Don Gagne committed
109
    void setFailureMode(MockConfiguration::FailureMode_t failureMode) { _failureMode = failureMode; }
Don Gagne's avatar
Don Gagne committed
110 111 112 113 114 115 116

    /// APM stack has strange handling of the first item of the mission list. If it has no
    /// onboard mission items, sometimes it sends back a home position in position 0 and
    /// sometimes it doesn't. Don't ask. This option allows you to configure that behavior
    /// for unit testing.
    void setAPMMissionResponseMode(bool sendHomePositionOnEmptyList) { _apmSendHomePositionOnEmptyList = sendHomePositionOnEmptyList; }

117
    void emitRemoteControlChannelRawChanged(int channel, uint16_t raw);
118

119 120
    /// Sends the specified mavlink message to QGC
    void respondWithMavlinkMessage(const mavlink_message_t& msg);
121

122
    MockLinkFileServer* getFileServer(void) { return _fileServer; }
123

Don Gagne's avatar
Don Gagne committed
124 125 126 127 128 129 130
    // Virtuals from LinkInterface
    virtual QString getName(void) const { return _name; }
    virtual void requestReset(void){ }
    virtual bool isConnected(void) const { return _connected; }
    virtual qint64 getConnectionSpeed(void) const { return 100000000; }
    virtual qint64 bytesAvailable(void) { return 0; }

131 132 133 134
    // These are left unimplemented in order to cause linker errors which indicate incorrect usage of
    // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
    bool connect(void);
    bool disconnect(void);
135

136 137
    /// Sets a failure mode for unit testing
    ///     @param failureMode Type of failure to simulate
138 139
    ///     @param failureAckResult Error to send if one the ack error modes
    void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult);
140

141 142
    /// Called to send a MISSION_ACK message while the MissionManager is in idle state
    void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); }
143

144 145
    /// Called to send a MISSION_ITEM message while the MissionManager is in idle state
    void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); }
146

147 148
    /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state
    void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); }
149

150 151
    /// Reset the state of the MissionItemHandler to no items, no transactions in progress.
    void resetMissionItemHandler(void) { _missionItemHandler.reset(); }
152

Patrick José Pereira's avatar
Patrick José Pereira committed
153
    /// Returns the filename for the simulated log file. Only available after a download is requested.
154 155
    QString logDownloadFile(void) { return _logDownloadFilename; }

Don Gagne's avatar
Don Gagne committed
156 157 158 159
    static MockLink* startPX4MockLink            (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
    static MockLink* startGenericMockLink        (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
    static MockLink* startAPMArduCopterMockLink  (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
    static MockLink* startAPMArduPlaneMockLink   (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
160
    static MockLink* startAPMArduSubMockLink     (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
161
    static MockLink* startAPMArduRoverMockLink   (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
162

163
private slots:
164
    virtual void _writeBytes(const QByteArray bytes);
165

166 167 168
private slots:
    void _run1HzTasks(void);
    void _run10HzTasks(void);
Don Gagne's avatar
Don Gagne committed
169
    void _run500HzTasks(void);
170

171
private:
172 173
    // From LinkInterface
    virtual bool _connect(void);
Don Gagne's avatar
Don Gagne committed
174
    virtual void _disconnect(void);
175

176 177
    // QThread override
    virtual void run(void);
178

179
    // MockLink methods
180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195
    void _sendHeartBeat                 (void);
    void _sendHighLatency2              (void);
    void _handleIncomingNSHBytes        (const char* bytes, int cBytes);
    void _handleIncomingMavlinkBytes    (const uint8_t* bytes, int cBytes);
    void _loadParams                    (void);
    void _handleHeartBeat               (const mavlink_message_t& msg);
    void _handleSetMode                 (const mavlink_message_t& msg);
    void _handleParamRequestList        (const mavlink_message_t& msg);
    void _handleParamSet                (const mavlink_message_t& msg);
    void _handleParamRequestRead        (const mavlink_message_t& msg);
    void _handleFTP                     (const mavlink_message_t& msg);
    void _handleCommandLong             (const mavlink_message_t& msg);
    void _handleManualControl           (const mavlink_message_t& msg);
    void _handlePreFlightCalibration    (const mavlink_command_long_t& request);
    void _handleLogRequestList          (const mavlink_message_t& msg);
    void _handleLogRequestData          (const mavlink_message_t& msg);
DoinLakeFlyer's avatar
DoinLakeFlyer committed
196
    void _handleParamMapRC              (const mavlink_message_t& msg);
197 198 199 200 201 202 203 204 205 206 207 208 209 210
    float _floatUnionForParam           (int componentId, const QString& paramName);
    void _setParamFloatUnionIntoMap     (int componentId, const QString& paramName, float paramFloat);
    void _sendHomePosition              (void);
    void _sendGpsRawInt                 (void);
    void _sendVibration                 (void);
    void _sendSysStatus                 (void);
    void _sendStatusTextMessages        (void);
    void _sendChunkedStatusText         (uint16_t chunkId, bool missingChunks);
    void _respondWithAutopilotVersion   (void);
    void _sendRCChannels                (void);
    void _paramRequestListWorker        (void);
    void _logDownloadWorker             (void);
    void _sendADSBVehicles              (void);
    void _moveADSBVehicle               (void);
211

212
    static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode);
213 214
    static MockLink* _startMockLink(MockConfiguration* mockConfig);

215
    MockLinkMissionItemHandler  _missionItemHandler;
216

217 218
    QString _name;
    bool    _connected;
219
    int     _mavlinkChannel;
220

221 222
    uint8_t _vehicleSystemId;
    uint8_t _vehicleComponentId;
223

224 225
    bool    _inNSH;
    bool    _mavlinkStarted;
226

227 228
    QMap<int, QMap<QString, QVariant>>          _mapParamName2Value;
    QMap<int, QMap<QString, MAV_PARAM_TYPE>>    _mapParamName2MavParamType;
229

230 231 232
    uint8_t     _mavBaseMode;
    uint32_t    _mavCustomMode;
    uint8_t     _mavState;
233

234
    QElapsedTimer _runningTime;
235 236
    int8_t      _batteryRemaining = 100;

237 238
    MAV_AUTOPILOT       _firmwareType;
    MAV_TYPE            _vehicleType;
239 240 241
    double              _vehicleLatitude;
    double              _vehicleLongitude;
    double              _vehicleAltitude;
242

243
    MockLinkFileServer* _fileServer;
Don Gagne's avatar
Don Gagne committed
244

245
    bool _sendStatusText;
Don Gagne's avatar
Don Gagne committed
246
    bool _apmSendHomePositionOnEmptyList;
Don Gagne's avatar
Don Gagne committed
247
    MockConfiguration::FailureMode_t _failureMode;
248

249
    int _sendHomePositionDelayCount;
250
    int _sendGPSPositionDelayCount;
251

Don Gagne's avatar
Don Gagne committed
252
    int _currentParamRequestListComponentIndex; // Current component index for param request list workflow, -1 for no request in progress
253 254 255 256 257 258 259 260
    int _currentParamRequestListParamIndex;     // Current parameter index for param request list workflow

    static const uint16_t _logDownloadLogId = 0;        ///< Id of siumulated log file
    static const uint32_t _logDownloadFileSize = 1000;  ///< Size of simulated log file

    QString _logDownloadFilename;           ///< Filename for log download which is in progress
    uint32_t    _logDownloadCurrentOffset;  ///< Current offset we are sending from
    uint32_t    _logDownloadBytesRemaining; ///< Number of bytes still to send, 0 = send inactive
Don Gagne's avatar
Don Gagne committed
261

262 263 264
    QGeoCoordinate  _adsbVehicleCoordinate;
    double          _adsbAngle;

265 266 267
    static double       _defaultVehicleLatitude;
    static double       _defaultVehicleLongitude;
    static double       _defaultVehicleAltitude;
Don Gagne's avatar
Don Gagne committed
268 269
    static int          _nextVehicleSystemId;
    static const char*  _failParam;
270 271
};