QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
MockLink.h
Go to the documentation of this file.
1#pragma once
2
3#include "PX4/px4_custom_mode.h"
4#include "LinkInterface.h"
5#include "MAVLinkLib.h"
6#include "MockConfiguration.h"
7#include "MockLinkMissionItemHandler.h"
8
9#include <QtCore/QElapsedTimer>
10#include <QtCore/QLoggingCategory>
11#include <QtCore/QMap>
12#include <QtCore/QMutex>
13#include <QtCore/QSet>
14#include <QtPositioning/QGeoCoordinate>
15
16#include <atomic>
17
18class MockLinkCamera;
19class MockLinkFTP;
20class MockLinkGimbal;
21class MockLinkWorker;
22class QThread;
23
24Q_DECLARE_LOGGING_CATEGORY(MockLinkLog)
25Q_DECLARE_LOGGING_CATEGORY(MockLinkVerboseLog)
26
27class MockLink : public LinkInterface
28{
29 Q_OBJECT
30
31public:
32 explicit MockLink(SharedLinkConfigurationPtr &config, QObject *parent = nullptr);
33 virtual ~MockLink();
34
35 void run1HzTasks();
36 void run10HzTasks();
37 void run500HzTasks();
38 void sendStatusTextMessages();
39
40 bool shouldSendStatusText() const { return _sendStatusText; }
41
42 bool isConnected() const final { return _connected; }
43 void disconnect() final;
44
45 Q_INVOKABLE void setCommLost(bool commLost) { _commLost = commLost; }
46 Q_INVOKABLE void simulateConnectionRemoved();
47
48 int vehicleId() const { return _vehicleSystemId; }
49 MAV_AUTOPILOT getFirmwareType() const { return _firmwareType; }
50
51 double vehicleLatitude() const { return _vehicleLatitude; }
52 double vehicleLongitude() const { return _vehicleLongitude; }
53 double vehicleAltitudeAMSL() const { return _vehicleAltitudeAMSL; }
54
56 void respondWithMavlinkMessage(const mavlink_message_t &msg);
57
58 MockLinkFTP *mockLinkFTP() const;
59
63 void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) const { _missionItemHandler->setFailureMode(failureMode, failureAckResult); }
64
66 void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) const { _missionItemHandler->sendUnexpectedMissionAck(ackType); }
67
69 void sendUnexpectedMissionItem() const { _missionItemHandler->sendUnexpectedMissionItem(); }
70
72 void sendUnexpectedMissionRequest() const { _missionItemHandler->sendUnexpectedMissionRequest(); }
73
74 void sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult);
75
77 void resetMissionItemHandler() const { _missionItemHandler->reset(); }
78
80 QString logDownloadFile() const { return _logDownloadFilename; }
81
82 void clearReceivedMavCommandCounts() { _receivedMavCommandCountMap.clear(); _receivedMavCommandByCompCountMap.clear(); _receivedRequestMessageByCompAndMsgCountMap.clear(); }
83 int receivedMavCommandCount(MAV_CMD command) const { return _receivedMavCommandCountMap.value(command, 0); }
84 int receivedMavCommandCount(MAV_CMD command, int compId) const { return _receivedMavCommandByCompCountMap.value(command).value(compId, 0); }
85 int receivedRequestMessageCount(int compId, int messageId) const { return _receivedRequestMessageByCompAndMsgCountMap.value(compId).value(messageId, 0); }
86 void clearReceivedRequestMessageCounts() { _receivedRequestMessageCountMap.clear(); _receivedRequestMessageByCompAndMsgCountMap.clear(); }
87 int receivedRequestMessageCount(uint32_t messageId) const { return _receivedRequestMessageCountMap.value(messageId, 0); }
88 void clearReceivedMavlinkMessageCounts() { _receivedMavlinkMessageCountMap.clear(); _hashCheckRequestCount = 0; }
89 int receivedMavlinkMessageCount(uint32_t messageId) const { return _receivedMavlinkMessageCountMap.value(messageId, 0); }
90
97 void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; }
98
101 void setRequestMessageNoResponse(uint32_t messageId, bool noResponse = true) {
102 QMutexLocker locker(&_requestMessageNoResponseMutex);
103 if (noResponse) {
104 _requestMessageNoResponseIds.insert(messageId);
105 } else {
106 _requestMessageNoResponseIds.remove(messageId);
107 }
108 }
109
115 void setParamSetFailureMode(ParamSetFailureMode_t mode) {
116 _paramSetFailureMode = mode;
117 _paramSetFailureFirstAttemptPending = (mode == FailParamSetFirstAttemptNoAck);
118 }
119
125 void setParamRequestReadFailureMode(ParamRequestReadFailureMode_t mode) {
126 _paramRequestReadFailureMode = mode;
127 _paramRequestReadFailureFirstAttemptPending = (mode == FailParamRequestReadFirstAttemptNoResponse);
128 }
129
130 void setHashCheckNoResponse(bool noResponse) { _hashCheckNoResponse = noResponse; }
131
133 int hashCheckRequestCount() const { return _hashCheckRequestCount; }
134
136 void setMockParamValue(int componentId, const QString &paramName, float value);
137
138 static MockLink *startPX4MockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
139 static MockLink *startGenericMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
140 static MockLink *startNoInitialConnectMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
141 static MockLink *startAPMArduCopterMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
142 static MockLink *startAPMArduPlaneMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
143 static MockLink *startAPMArduSubMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
144 static MockLink *startAPMArduRoverMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
145
146 // Special commands for testing Vehicle::sendMavCommandWithHandler
147 static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED = MAV_CMD_USER_1;
148 static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED = MAV_CMD_USER_2;
149 static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED = MAV_CMD_USER_3;
150 static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED = MAV_CMD_USER_4;
151 static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE = MAV_CMD_USER_5;
152 static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 1);
153 static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 2);
154 static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 3);
155 static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK = static_cast<MAV_CMD>(MAV_CMD_USER_5 + 4);
156
157signals:
158 void writeBytesQueuedSignal(const QByteArray &bytes);
159 void highLatencyTransmissionEnabledChanged(bool highLatencyTransmissionEnabled);
160
161private slots:
163 void _writeBytes(const QByteArray &bytes) final;
164 void _writeBytesQueued(const QByteArray &bytes);
165
166private:
167 typedef struct {
168 const char *name;
169 uint8_t standard_mode;
170 uint32_t custom_mode;
171 bool canBeSet;
172 bool advanced;
173 } FlightMode_t;
174
175 bool _connect() final;
176 bool _allocateMavlinkChannel() final;
177 void _freeMavlinkChannel() final;
178
179 uint8_t _getMavlinkAuxChannel() const { return _mavlinkAuxChannel; }
180 bool _mavlinkAuxChannelIsSet() const;
181
182 void _loadParams();
183
185 float _floatUnionForParam(int componentId, const QString &paramName);
186 uint32_t _computeParamHash(int componentId) const;
187 void _setParamFloatUnionIntoMap(int componentId, const QString &paramName, float paramFloat);
188
190 void _handleIncomingNSHBytes(const char *bytes, int cBytes);
192 void _handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes);
194 void _updateIncomingMessageCounts(const mavlink_message_t &msg);
195 void _handleIncomingMavlinkMsg(const mavlink_message_t &msg);
196 void _handleHeartBeat(const mavlink_message_t &msg);
197 void _handleSetMode(const mavlink_message_t &msg);
198 void _handleParamRequestList(const mavlink_message_t &msg);
199 void _handleParamSet(const mavlink_message_t &msg);
200 void _handleParamRequestRead(const mavlink_message_t &msg);
201 void _handleFTP(const mavlink_message_t &msg);
202 void _handleCommandLong(const mavlink_message_t &msg);
203 void _handleInProgressCommandLong(const mavlink_command_long_t &request);
204 void _handleCommandLongSetMessageInterval(const mavlink_command_long_t &request, bool &acccepted);
205 void _handleManualControl(const mavlink_message_t &msg);
206 void _handlePreFlightCalibration(const mavlink_command_long_t &request);
207 void _handleTakeoff(const mavlink_command_long_t &request);
208 void _handleLogRequestList(const mavlink_message_t &msg);
209 void _handleLogRequestData(const mavlink_message_t &msg);
210 void _handleParamMapRC(const mavlink_message_t &msg);
211 void _handleRequestMessage(const mavlink_command_long_t &request, bool &accepted, bool &noAck);
212 void _handleRequestMessageAutopilotVersion(const mavlink_command_long_t &request, bool &accepted);
213 void _handleRequestMessageDebug(const mavlink_command_long_t &request, bool &accepted, bool &noAck);
214 void _handleRequestMessageAvailableModes(const mavlink_command_long_t &request, bool &accepted);
215
216 void _sendHeartBeat();
217 void _sendHighLatency2();
218 void _sendHomePosition();
219 void _sendGpsRawInt();
220 void _sendGlobalPositionInt();
221 void _sendExtendedSysState();
222 void _sendVibration();
223 void _sendSysStatus();
224 void _sendBatteryStatus();
225 void _sendChunkedStatusText(uint16_t chunkId, bool missingChunks);
226 void _sendStatusTextMessages();
227 void _respondWithAutopilotVersion();
228 void _sendRCChannels();
229 void _sendADSBVehicles();
230 void _sendGeneralMetaData();
231 void _sendRemoteIDArmStatus();
232 void _sendAvailableModesMonitor();
233
234 void _paramRequestListWorker();
235 void _logDownloadWorker();
236 void _availableModesWorker();
237 void _sendAvailableMode(uint8_t modeIndexOneBased);
238 int _availableModesCount() const;
239 void _moveADSBVehicle(int vehicleIndex);
240
241 static MockLink *_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode);
242 static MockLink *_startMockLink(MockConfiguration *mockConfig);
243
246 static QString _createRandomFile(uint32_t byteCount);
247
248 QThread *_workerThread = nullptr;
249 MockLinkWorker *_worker = nullptr;
250
251 const MockConfiguration *_mockConfig = nullptr;
252 const MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4;
253 const MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR;
254 const bool _sendStatusText = false;
255 const bool _enableCamera = false;
256 const bool _enableGimbal = false;
257 const MockConfiguration::FailureMode_t _failureMode = MockConfiguration::FailNone;
258 const uint8_t _vehicleSystemId = 0;
259 const double _vehicleLatitude = 0.0;
260 const double _vehicleLongitude = 0.0;
261 // These are just set for reporting the fields in _respondWithAutopilotVersion()
262 // and ensuring that the Vehicle reports the fields in Vehicle::firmwareBoardVendorId etc.
263 // They do not control any mock simulation (and it is up to the Custom build to do that).
264 const uint16_t _boardVendorId = 0;
265 const uint16_t _boardProductId = 0;
266 MockLinkMissionItemHandler *const _missionItemHandler = nullptr;
267 MockLinkCamera *const _mockLinkCamera = nullptr;
268 MockLinkGimbal *const _mockLinkGimbal = nullptr;
269 MockLinkFTP *const _mockLinkFTP = nullptr;
270
271 uint8_t _mavlinkAuxChannel = std::numeric_limits<uint8_t>::max();
272 QMutex _mavlinkAuxMutex;
273
274 bool _connected = false;
275 bool _inNSH = false;
276
277 uint8_t _mavBaseMode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
278 uint32_t _mavCustomMode = PX4CustomMode::MANUAL;
279 uint8_t _mavState = MAV_STATE_STANDBY;
280
281 QElapsedTimer _runningTime;
282 static constexpr int32_t _batteryMaxTimeRemaining = 15 * 60;
283 int8_t _battery1PctRemaining = 100;
284 int32_t _battery1TimeRemaining = _batteryMaxTimeRemaining;
285 MAV_BATTERY_CHARGE_STATE _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
286 int8_t _battery2PctRemaining = 100;
287 int32_t _battery2TimeRemaining = _batteryMaxTimeRemaining;
288 MAV_BATTERY_CHARGE_STATE _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
289
290 double _vehicleAltitudeAMSL = _defaultVehicleHomeAltitude;
291 bool _commLost = false;
292 bool _highLatencyTransmissionEnabled = true;
293
294 int _sendHomePositionDelayCount = 10;
295 int _sendGPSPositionDelayCount = 100;
296
297 int _currentParamRequestListComponentIndex = -1;
298 int _currentParamRequestListParamIndex = -1;
299 QList<int> _paramRequestListComponentIds;
300 QStringList _paramRequestListParamNames;
304 QMutex _paramRequestListMutex;
305
306 // Mavlink standard modes worker information
307 int _availableModesWorkerNextModeIndex = 0;
311 QMutex _availableModesWorkerMutex;
312 uint8_t _availableModesMonitorSeqNumber = 0;
313
314 QString _logDownloadFilename;
315 uint32_t _logDownloadCurrentOffset = 0;
316 uint32_t _logDownloadBytesRemaining = 0;
320 QMutex _logDownloadMutex;
321
322 RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone;
323 mutable QMutex _requestMessageNoResponseMutex;
324 QSet<uint32_t> _requestMessageNoResponseIds;
325 ParamSetFailureMode_t _paramSetFailureMode = FailParamSetNone;
326 bool _paramSetFailureFirstAttemptPending = false;
327 ParamRequestReadFailureMode_t _paramRequestReadFailureMode = FailParamRequestReadNone;
328 bool _paramRequestReadFailureFirstAttemptPending = false;
329 bool _hashCheckNoResponse = false;
330 int _hashCheckRequestCount = 0;
331 bool _paramRequestListHashCheckSent = false;
332
333 QMap<MAV_CMD, int> _receivedMavCommandCountMap;
334 QMap<MAV_CMD, QMap<int, int>> _receivedMavCommandByCompCountMap;
335 QMap<uint32_t, int> _receivedRequestMessageCountMap;
336 QMap<int, QMap<int, int>> _receivedRequestMessageByCompAndMsgCountMap;
337 QMap<uint32_t, int> _receivedMavlinkMessageCountMap;
338 QMap<int, QMap<QString, QVariant>> _mapParamName2Value;
339 QMap<int, QMap<QString, MAV_PARAM_TYPE>> _mapParamName2MavParamType;
340
341 struct ADSBVehicle {
342 QGeoCoordinate coordinate;
343 double angle = 0.0;
344 double altitude = 0.0;
345 };
346 QList<ADSBVehicle> _adsbVehicles;
347 QList<QGeoCoordinate> _adsbVehicleCoordinates;
348 static constexpr int _numberOfVehicles = 5;
349 double _adsbAngles[_numberOfVehicles]{};
350
351 static std::atomic<int> _nextVehicleSystemId;
352
353 // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle
354 // testing of a gazebo vehicle and a mocklink vehicle
355 static constexpr double _defaultVehicleLatitude = 47.397;
356 static constexpr double _defaultVehicleLongitude = 8.5455;
357 static constexpr double _defaultVehicleHomeAltitude = 488.056;
358
359 static constexpr const char *_failParam = "COM_FLTMODE6";
360
361 static constexpr uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1;
362
363 static constexpr uint16_t _logDownloadLogId = 0;
364 static constexpr uint32_t _logDownloadFileSize = 1000;
365
366 static constexpr bool _mavlinkStarted = true;
367
368 static QList<FlightMode_t> _availableFlightModes;
369
370 std::atomic<bool> _disconnectedEmitted{false};
371};
Q_DECLARE_LOGGING_CATEGORY
Q_DECLARE_LOGGING_CATEGORY(AndroidSerialLog)
SharedLinkConfigurationPtr
std::shared_ptr< LinkConfiguration > SharedLinkConfigurationPtr
Definition LinkConfiguration.h:138
mavlink_message_t
struct __mavlink_message mavlink_message_t
Definition QGCCorePlugin.h:23
LinkInterface
The link interface defines the interface for all links used to communicate with the ground station ap...
Definition LinkInterface.h:14
MockLinkFTP
Mock implementation of Mavlink FTP server.
Definition MockLinkFTP.h:18
px4_custom_mode.h