QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
MockLink.cc
Go to the documentation of this file.
1#include "MockLink.h"
2#include "MAVLinkLib.h"
3#include "LinkManager.h"
4#include "MAVLinkProtocol.h"
5#include "MAVLinkSigning.h"
6#include "SecureMemory.h"
7#include "MockLinkCamera.h"
8#include "MockLinkFTP.h"
9#include "MockLinkGimbal.h"
10#include "MockLinkWorker.h"
11#include "QGCLoggingCategory.h"
12#include "FirmwarePlugin.h"
13#include "FactMetaData.h"
14#include "ParameterManager.h"
15#include "AppMessages.h"
16#include "QGCMath.h"
17
18#ifdef QGC_GST_STREAMING
19#include "MockVideoStreamServer.h"
20#endif
21
22#include <QtCore/QFile>
23#include <QtCore/QJsonArray>
24#include <QtCore/QJsonDocument>
25#include <QtCore/QJsonObject>
26#include <QtCore/QLatin1StringView>
27#include <QtCore/QMutexLocker>
28#include <QtCore/QSet>
29#include <QtCore/QRandomGenerator>
30#include <QtCore/QTemporaryFile>
31#include <QtCore/QThread>
32#include <QtCore/QTimer>
33
34#include <cmath>
35#include <cstring>
36
37QGC_LOGGING_CATEGORY(MockLinkLog, "Comms.MockLink.MockLink")
38QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "Comms.MockLink.MockLink:verbose")
39
40std::atomic<int> MockLink::_nextVehicleSystemId{128};
41
42QList<MockLink::FlightMode_t> MockLink::_availableFlightModes = {
43 // Mode Name Standard Mode Custom Mode CanBeSet adv
44 { "Manual", 0, PX4CustomMode::MANUAL, true, true },
45 { "Stabilized", 0, PX4CustomMode::STABILIZED, true, true },
46 { "Acro", 0, PX4CustomMode::ACRO, true, true },
47 { "Altitude", 0, PX4CustomMode::ALTCTL, true, false},
48 { "Offboard", 0, PX4CustomMode::OFFBOARD, true, true },
49 { "Position", 0, PX4CustomMode::POSCTL_POSCTL, true, false},
50 { "Orbit", 0, PX4CustomMode::POSCTL_ORBIT, false, true },
51 { "Hold", 0, PX4CustomMode::AUTO_LOITER, true, true },
52 { "Mission", 0, PX4CustomMode::AUTO_MISSION, true, true },
53 { "Return", 0, PX4CustomMode::AUTO_RTL, true, true },
54 { "Land", MAV_STANDARD_MODE_LAND, PX4CustomMode::AUTO_LAND, false, true },
55 { "Precision Landing", 0, PX4CustomMode::AUTO_PRECLAND, true, true },
56 { "Takeoff", MAV_STANDARD_MODE_TAKEOFF, PX4CustomMode::AUTO_TAKEOFF, false, false},
57 { "MockLink Mode", 0, PX4CustomMode::RATTITUDE, true, false},
58 // Deliberately uses the reserved (deleted RTGS) AUTO sub-mode so QGC has no name for it
59 { "(Mode not available)", 0, static_cast<uint32_t>(PX4_CUSTOM_MAIN_MODE_AUTO << 16 | (PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE << 24)), false, false},
60 { "MockLink Mode (delayed)",0, PX4CustomMode::AUTO_FOLLOW_TARGET, true, false},
61};
62
63// The ArduPilot lists below mirror the availableFlightModes lists in the corresponding
64// firmware plugins (names and custom mode values must match) so that StandardModes streaming
65// results in the same mode names the plugins use.
66QList<MockLink::FlightMode_t> MockLink::_apmCopterAvailableFlightModes = {
67 // Mode Name Standard Mode Custom Mode CanBeSet adv
68 { "Stabilize", 0, 0, true, true },
69 { "Acro", 0, 1, true, true },
70 { "Altitude Hold", 0, 2, true, true },
71 { "Auto", 0, 3, true, true },
72 { "Guided", 0, 4, true, true },
73 { "Loiter", 0, 5, true, true },
74 { "RTL", 0, 6, true, true },
75 { "Circle", 0, 7, true, true },
76 { "Land", 0, 9, true, true },
77 { "Drift", 0, 11, true, true },
78 { "Sport", 0, 13, true, true },
79 { "Flip", 0, 14, true, true },
80 { "Autotune", 0, 15, true, true },
81 { "Position Hold", 0, 16, true, true },
82 { "Brake", 0, 17, true, true },
83 { "Throw", 0, 18, true, true },
84 { "Avoid ADSB", 0, 19, true, true },
85 { "Guided No GPS", 0, 20, true, true },
86 { "Smart RTL", 0, 21, true, true },
87 { "Flow Hold", 0, 22, true, true },
88 { "Follow", 0, 23, true, true },
89 { "ZigZag", 0, 24, true, true },
90 { "SystemID", 0, 25, true, true },
91 { "AutoRotate", 0, 26, true, true },
92 { "AutoRTL", 0, 27, true, true },
93 { "Turtle", 0, 28, true, true },
94};
95
96QList<MockLink::FlightMode_t> MockLink::_apmPlaneAvailableFlightModes = {
97 // Mode Name Standard Mode Custom Mode CanBeSet adv
98 { "Manual", 0, 0, true, true },
99 { "Circle", 0, 1, true, true },
100 { "Stabilize", 0, 2, true, true },
101 { "Training", 0, 3, true, true },
102 { "Acro", 0, 4, true, true },
103 { "FBW A", 0, 5, true, true },
104 { "FBW B", 0, 6, true, true },
105 { "Cruise", 0, 7, true, true },
106 { "Autotune", 0, 8, true, true },
107 { "Auto", 0, 10, true, true },
108 { "RTL", 0, 11, true, true },
109 { "Loiter", 0, 12, true, true },
110 { "Takeoff", 0, 13, true, true },
111 { "Avoid ADSB", 0, 14, true, true },
112 { "Guided", 0, 15, true, true },
113 { "Initializing", 0, 16, false, true },
114 { "QuadPlane Stabilize", 0, 17, true, true },
115 { "QuadPlane Hover", 0, 18, true, true },
116 { "QuadPlane Loiter", 0, 19, true, true },
117 { "QuadPlane Land", 0, 20, true, true },
118 { "QuadPlane RTL", 0, 21, true, true },
119 { "QuadPlane AutoTune", 0, 22, true, true },
120 { "QuadPlane Acro", 0, 23, true, true },
121 { "Thermal", 0, 24, true, true },
122 { "Loiter to QLand", 0, 25, true, true },
123 { "Autoland", 0, 26, true, true },
124};
125
126QList<MockLink::FlightMode_t> MockLink::_apmRoverAvailableFlightModes = {
127 // Mode Name Standard Mode Custom Mode CanBeSet adv
128 { "Manual", 0, 0, true, true },
129 { "Acro", 0, 1, true, true },
130 { "Learning", 0, 2, false, true },
131 { "Steering", 0, 3, true, true },
132 { "Hold", 0, 4, true, true },
133 { "Loiter", 0, 5, true, true },
134 { "Follow", 0, 6, true, true },
135 { "Simple", 0, 7, true, true },
136 { "Dock", 0, 8, true, true },
137 { "Circle", 0, 9, true, true },
138 { "Auto", 0, 10, true, true },
139 { "RTL", 0, 11, true, true },
140 { "Smart RTL", 0, 12, true, true },
141 { "Guided", 0, 15, true, true },
142 { "Initializing", 0, 16, false, true },
143};
144
145QList<MockLink::FlightMode_t> MockLink::_apmSubAvailableFlightModes = {
146 // Mode Name Standard Mode Custom Mode CanBeSet adv
147 { "Manual", 0, 19, true, true },
148 { "Stabilize", 0, 0, true, true },
149 { "Acro", 0, 1, true, true },
150 { "Depth Hold", 0, 2, true, true },
151 { "Auto", 0, 3, true, true },
152 { "Guided", 0, 4, true, true },
153 { "Circle", 0, 7, true, true },
154 { "Surface", 0, 9, true, true },
155 { "Position Hold", 0, 16, true, true },
156 { "Motor Detection", 0, 20, false, true },
157 { "Surftrak", 0, 21, true, true },
158};
159
160MockLink::MockLink(SharedLinkConfigurationPtr &config, QObject *parent)
161 : LinkInterface(config, parent)
162 , _mockConfig(qobject_cast<const MockConfiguration*>(_config.get()))
163 , _firmwareType(_mockConfig->firmwareType())
164 , _vehicleType(_mockConfig->vehicleType())
165 , _sendStatusText(_mockConfig->sendStatusText())
166 , _apmStartFreshParams(_mockConfig->apmStartFreshParams())
167 , _enableCamera(_mockConfig->enableCamera())
168 , _enableGimbal(_mockConfig->enableGimbal())
169 , _enableProximity(_mockConfig->enableProximity())
170 , _failureMode(_mockConfig->failureMode())
171 , _stayMavlinkV1(_mockConfig->stayMavlinkV1())
172 , _ftpCapability(_mockConfig->ftpCapability())
173 , _sendRadioStatusEnabled(_mockConfig->sendRadioStatus())
174 , _vehicleSystemId(_mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : static_cast<int>(_nextVehicleSystemId))
175 , _vehicleLatitude(_defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001))
176 , _vehicleLongitude(_defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001))
177 , _boardVendorId(_mockConfig->boardVendorId())
178 , _boardProductId(_mockConfig->boardProductId())
179 , _missionItemHandler(new MockLinkMissionItemHandler(this))
180 , _mockLinkCamera(_enableCamera ? new MockLinkCamera(this,
181 _mockConfig->cameraCaptureVideo(),
182 _mockConfig->cameraCaptureImage(),
183 _mockConfig->cameraHasModes(),
184 _mockConfig->cameraHasVideoStream(),
185 _mockConfig->cameraCanCaptureImageInVideoMode(),
186 _mockConfig->cameraCanCaptureVideoInImageMode(),
187 _mockConfig->cameraHasBasicZoom(),
188 _mockConfig->cameraHasTrackingPoint(),
189 _mockConfig->cameraHasTrackingRectangle())
190 : nullptr)
191 , _mockLinkGimbal(_enableGimbal ? new MockLinkGimbal(this,
192 _mockConfig->gimbalHasRollAxis(),
193 _mockConfig->gimbalHasPitchAxis(),
194 _mockConfig->gimbalHasYawAxis(),
195 _mockConfig->gimbalHasYawFollow(),
196 _mockConfig->gimbalHasYawLock(),
197 _mockConfig->gimbalHasRetract(),
198 _mockConfig->gimbalHasNeutral(),
199 static_cast<uint8_t>(_mockConfig->gimbalDeviceId()))
200 : nullptr)
201 , _mockLinkPX4Calibration(new MockLinkPX4Calibration(this))
202 , _mockLinkFTP(new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this))
203 , _requestedVideoStreamType(_mockConfig->videoStreamTypeEnum())
204{
205 qCDebug(MockLinkLog) << this;
206
207 if (_mockConfig->startArmed()) {
208 setArmed(true);
209 }
210
211 if (_mockConfig->preloadMission()) {
212 _missionItemHandler->loadSimpleMultirotorMission();
213 }
214
215 // Initialize ADS-B vehicles with different starting conditions
216 _adsbVehicles.reserve(_numberOfVehicles);
217 for (int i = 0; i < _numberOfVehicles; ++i) {
218 ADSBVehicle vehicle{};
219 vehicle.angle = i * 72.0; // Different starting directions (angles 0, 72, 144, 216, 288)
220
221 // Set initial coordinates slightly offset from the default coordinates
222 const double latOffset = 0.001 * i;
223 const double lonOffset = 0.001 * (i % 2 == 0 ? i : -i);
224 vehicle.coordinate = QGeoCoordinate(_defaultVehicleLatitude + latOffset, _defaultVehicleLongitude + lonOffset);
225
226 // Set a unique starting altitude for each vehicle near the home altitude
227 vehicle.altitude = _defaultVehicleHomeAltitude + (i * 5);
228
229 _adsbVehicles.append(vehicle);
230 }
231
232 (void) QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection);
233
234 _loadParams();
235 _runningTime.start();
236
237 _workerThread = new QThread(this);
238 _workerThread->setObjectName(QStringLiteral("Mock_%1").arg(_mockConfig->name()));
239 _worker = new MockLinkWorker(this);
240 _worker->moveToThread(_workerThread);
241 (void) connect(_workerThread, &QThread::started, _worker, &MockLinkWorker::startWork);
242 (void) connect(_workerThread, &QThread::finished, _worker, &QObject::deleteLater);
243 _workerThread->start();
244}
245
246MockLink::~MockLink()
247{
248 MockLink::disconnect();
249
250 delete _mockLinkCamera;
251 delete _mockLinkGimbal;
252 delete _mockLinkPX4Calibration;
253
254 if (!_logDownloadFilename.isEmpty()) {
255 QFile::remove(_logDownloadFilename);
256 }
257
258 if (_workerThread) {
259 _workerThread->quit();
260 _workerThread->wait();
261 }
262
263 qCDebug(MockLinkLog) << this;
264}
265
266bool MockLink::_connect()
267{
268 if (!_connected) {
269 _connected = true;
270 _disconnectedEmitted = false;
271 // ArduPilot starts out sending MAVLink v1 and only switches to v2 once it sees a v2
272 // message from the GCS. Emulate that: outgoing traffic starts as v1.
273 // High latency links are exempt: QGC doesn't transmit on them, so the upgrade could never happen.
274 // Note: high latency takes precedence over OptionStayMavlinkV1 (the two are not meant to be combined).
275 _mavlinkV2Upgraded = linkConfiguration()->isHighLatency();
276 mavlink_status_t *const outgoingStatus = mavlink_get_channel_status(_outgoingMavlinkChannel);
277 if (_mavlinkV2Upgraded) {
278 outgoingStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
279 } else {
280 outgoingStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
281 }
282 mavlink_status_t *const incomingStatus = mavlink_get_channel_status(_incomingMavlinkChannel);
283 incomingStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
284
285 _startVideoStreamServer();
286
287 emit connected();
288 }
289
290 return true;
291}
292
293void MockLink::disconnect()
294{
295 _missionItemHandler->shutdown();
296
297 // Stop worker thread first to prevent any more messages from being sent.
298 // This must happen before setting _connected = false to avoid race conditions
299 // where the worker checks _connected (true), then we set it false, then the
300 // worker continues sending messages to a disconnecting/destroyed vehicle.
301 if (_workerThread && _workerThread->isRunning()) {
302 _workerThread->quit();
303 _workerThread->wait();
304 }
305
306 // signing/streams pointers alias MockLink memory — clear before emit in case the disconnected signal frees us.
307 if (_outgoingMavlinkChannelIsSet()) {
308 mavlink_status_t* const outgoingStatus = mavlink_get_channel_status(_outgoingMavlinkChannel);
309 outgoingStatus->signing = nullptr;
310 outgoingStatus->signing_streams = nullptr;
311 mavlink_reset_channel_status(_outgoingMavlinkChannel);
312 }
313
314 // Must run before the disconnected emit below: that signal can release the last
315 // shared_ptr to this MockLink, so touching members afterwards is use-after-free.
316 _stopVideoStreamServer();
317
318 if (_connected) {
319 _connected = false;
320 if (!_disconnectedEmitted.exchange(true)) {
321 emit disconnected();
322 }
323 }
324}
325
326void MockLink::_startVideoStreamServer()
327{
328#ifdef QGC_GST_STREAMING
329 // Only serve a stream when a camera advertising a video stream is present and a type was requested.
330 if (_videoStreamServer || !_mockLinkCamera || !_mockConfig->cameraHasVideoStream()
331 || _requestedVideoStreamType == MockConfiguration::VideoStreamNone) {
332 return;
333 }
334
335 MockVideoStreamServer::StreamType serverType = MockVideoStreamServer::StreamType::RtpUdpH264;
336 quint16 port = 5600;
337 switch (_requestedVideoStreamType) {
338 case MockConfiguration::VideoStreamRtpUdpH264:
339 serverType = MockVideoStreamServer::StreamType::RtpUdpH264;
340 port = 5600;
341 break;
342 case MockConfiguration::VideoStreamRtpUdpH265:
343 serverType = MockVideoStreamServer::StreamType::RtpUdpH265;
344 port = 5601;
345 break;
346 case MockConfiguration::VideoStreamRtspH264:
347 serverType = MockVideoStreamServer::StreamType::RtspH264;
348 port = 8554;
349 break;
350 case MockConfiguration::VideoStreamMpegTsUdp:
351 serverType = MockVideoStreamServer::StreamType::MpegTsUdp;
352 port = 5600;
353 break;
354 case MockConfiguration::VideoStreamMpegTsTcp:
355 serverType = MockVideoStreamServer::StreamType::MpegTsTcp;
356 port = 5600;
357 break;
358 case MockConfiguration::VideoStreamNone:
359 return;
360 }
361
362 auto *server = new MockVideoStreamServer();
363 if (!server->start(serverType, QStringLiteral("127.0.0.1"), port)) {
364 qCWarning(MockLinkLog) << "Failed to start mock video stream server for type" << _requestedVideoStreamType;
365 delete server;
366 return;
367 }
368
369 _videoStreamServer = server;
370 {
371 QMutexLocker locker(&_videoStreamMutex);
372 _servedVideoStreamType = _requestedVideoStreamType;
373 _videoStreamUri = server->servedUri();
374 }
375 qCDebug(MockLinkLog) << "Mock video stream server serving" << server->servedUri();
376#endif
377}
378
379void MockLink::_stopVideoStreamServer()
380{
381#ifdef QGC_GST_STREAMING
382 // Clear the served-stream snapshot first so observers (servedVideoStream) stop
383 // advertising the URI before the server actually goes away.
384 {
385 QMutexLocker locker(&_videoStreamMutex);
386 _servedVideoStreamType = MockConfiguration::VideoStreamNone;
387 _videoStreamUri.clear();
388 }
389 if (_videoStreamServer) {
390 delete _videoStreamServer;
391 _videoStreamServer = nullptr;
392 }
393#endif
394}
395
396void MockLink::servedVideoStream(MockConfiguration::VideoStreamType &type, QString &uri) const
397{
398 QMutexLocker locker(&_videoStreamMutex);
399 type = _servedVideoStreamType;
400 uri = _videoStreamUri;
401}
402
403void MockLink::run1HzTasks()
404{
405 if (!_mavlinkStarted || !_connected || !mavlinkChannelIsSet()) {
406 return;
407 }
408
409 if (linkConfiguration()->isHighLatency() && _highLatencyTransmissionEnabled) {
410 _sendHighLatency2();
411 return;
412 }
413
414 _sendVibration();
415 _sendBatteryStatus();
416 _sendNamedValueFloats();
417 _sendSysStatus();
418 if (_enableADSB) {
419 _sendADSBVehicles();
420 }
421 if (_vehicleType != MAV_TYPE_SUBMARINE) {
422 _sendRemoteIDArmStatus();
423 }
424 _sendAvailableModesMonitor();
425
426 if (_enableGimbal) {
427 _mockLinkGimbal->run1HzTasks();
428 }
429
430 if (_enableProximity) {
431 _sendDistanceSensors();
432 }
433
434 _sendEscInfo();
435 _sendEscStatus();
436 if (_sendRadioStatusEnabled) {
437 _sendRadioStatus();
438 }
439
440 if (_enableCamera) {
441 _mockLinkCamera->sendCameraHeartbeats();
442 }
443
444 if (!QGC::runningUnitTests()) {
445 // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
446 _sendRCChannels();
447 }
448
449 if (_sendHomePositionDelayCount > 0 && !QGC::runningUnitTests()) {
450 // We delay home position to simulate a real vehicle, but not during unit tests to keep them fast
451 _sendHomePositionDelayCount--;
452 } else {
453 _sendHomePosition();
454 }
455}
456
457void MockLink::run10HzTasks()
458{
459 if (linkConfiguration()->isHighLatency()) {
460 return;
461 }
462
463 if (_mavlinkStarted && _connected && mavlinkChannelIsSet()) {
464 _sendHeartBeat();
465 const bool gpsDelayExpired = (_sendGPSPositionDelayCount == 0);
466 if (_sendGPSPositionDelayCount > 0) {
467 // We delay gps position for better testing
468 _sendGPSPositionDelayCount--;
469 }
470 if (gpsDelayExpired || QGC::runningUnitTests()) {
471 if (_vehicleType != MAV_TYPE_SUBMARINE) {
472 _sendGpsRawInt();
473 _sendGlobalPositionInt();
474 }
475 _sendExtendedSysState();
476 }
477
478 _sendAttitudeQuaternion();
479 _sendAttitudeTarget();
480 _sendLocalPositionNed();
481 _sendPositionTargetLocalNed();
482
483 _mockLinkPX4Calibration->run10HzTasks();
484
485 if (_enableCamera) {
486 _mockLinkCamera->run10HzTasks();
487 }
488 }
489}
490
491void MockLink::run500HzTasks()
492{
493 if (_mavlinkStarted && _connected && mavlinkChannelIsSet()) {
494 // Standard modes are served even on high-latency links since the request is still accepted there.
495 _availableModesWorker();
496 }
497
498 if (linkConfiguration()->isHighLatency()) {
499 return;
500 }
501
502 if (_mavlinkStarted && _connected && mavlinkChannelIsSet()) {
503 const int paramSends = QGC::runningUnitTests() ? kTestParamRequestListBatch : 1;
504 for (int i = 0; i < paramSends; ++i) {
505 _paramRequestListWorker();
506 }
507 _logDownloadWorker();
508 _apmCompassCalWorker();
509 _apmAccelCalWorker();
510 }
511}
512
513void MockLink::sendStatusTextMessages()
514{
515 _sendStatusTextMessages();
516}
517
518bool MockLink::_allocateMavlinkChannel()
519{
520 // should only be called by the LinkManager during setup
521 if (_incomingMavlinkChannelIsSet() || _outgoingMavlinkChannelIsSet() || mavlinkChannelIsSet()) {
522 qCWarning(MockLinkLog) << "Mock MAVLink channels are already allocated";
523 return false;
524 }
525
526 if (!LinkInterface::_allocateMavlinkChannel()) {
527 qCWarning(MockLinkLog) << "LinkInterface::_allocateMavlinkChannel failed";
528 return false;
529 }
530
531 _incomingMavlinkChannel = LinkManager::instance()->allocateMavlinkChannel();
532 if (!_incomingMavlinkChannelIsSet()) {
533 qCWarning(MockLinkLog) << "_allocateMavlinkChannel aux failed";
534 LinkInterface::_freeMavlinkChannel();
535 return false;
536 }
537
538 _outgoingMavlinkChannel = LinkManager::instance()->allocateMavlinkChannel();
539 if (!_outgoingMavlinkChannelIsSet()) {
540 qCWarning(MockLinkLog) << "_allocateMavlinkChannel vehicle failed";
541 LinkManager::instance()->freeMavlinkChannel(_incomingMavlinkChannel);
542 _incomingMavlinkChannel = LinkManager::invalidMavlinkChannel();
543 LinkInterface::_freeMavlinkChannel();
544 return false;
545 }
546
547 qCDebug(MockLinkLog) << "_allocateMavlinkChannel aux:" << _incomingMavlinkChannel << "vehicle:" << _outgoingMavlinkChannel;
548 return true;
549}
550
551void MockLink::_freeMavlinkChannel()
552{
553 qCDebug(MockLinkLog) << "_freeMavlinkChannel aux:" << _incomingMavlinkChannel << "vehicle:" << _outgoingMavlinkChannel;
554 if (!_incomingMavlinkChannelIsSet()) {
555 if (_outgoingMavlinkChannelIsSet() || mavlinkChannelIsSet()) {
556 qCWarning(MockLinkLog) << "Releasing a partially allocated mock MAVLink channel set";
557 }
558 }
559
560 if (_outgoingMavlinkChannelIsSet()) {
561 // Detach our _mockSigning before the channel is freed; the bytes back this struct in MockLink memory.
562 mavlink_reset_channel_status(_outgoingMavlinkChannel);
563 LinkManager::instance()->freeMavlinkChannel(_outgoingMavlinkChannel);
564 _outgoingMavlinkChannel = LinkManager::invalidMavlinkChannel();
565 }
566 if (_incomingMavlinkChannelIsSet()) {
567 mavlink_reset_channel_status(_incomingMavlinkChannel);
568 LinkManager::instance()->freeMavlinkChannel(_incomingMavlinkChannel);
569 _incomingMavlinkChannel = LinkManager::invalidMavlinkChannel();
570 }
571 if (mavlinkChannelIsSet()) {
572 LinkInterface::_freeMavlinkChannel();
573 }
574}
575
576bool MockLink::_incomingMavlinkChannelIsSet() const
577{
578 return (LinkManager::invalidMavlinkChannel() != _incomingMavlinkChannel);
579}
580
581bool MockLink::_outgoingMavlinkChannelIsSet() const
582{
583 return (LinkManager::invalidMavlinkChannel() != _outgoingMavlinkChannel);
584}
585
586void MockLink::_loadParams()
587{
588 QFile paramFile;
589 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
590 if (_vehicleType == MAV_TYPE_FIXED_WING) {
591 paramFile.setFileName(":/FirmwarePlugin/APM/Plane.OfflineEditing.params");
592 } else if (_vehicleType == MAV_TYPE_SUBMARINE ) {
593 paramFile.setFileName(":/FirmwarePlugin/APM/Sub.OfflineEditing.params");
594 } else if (_vehicleType == MAV_TYPE_GROUND_ROVER ) {
595 paramFile.setFileName(":/FirmwarePlugin/APM/Rover.OfflineEditing.params");
596 } else {
597 paramFile.setFileName(":/FirmwarePlugin/APM/Copter.OfflineEditing.params");
598 }
599 } else if (_firmwareType == MAV_AUTOPILOT_GENERIC) {
600 paramFile.setFileName(":/MockLink/GenericMockLink.params");
601 } else {
602 paramFile.setFileName(":/MockLink/PX4MockLink.params");
603 }
604
605 if (!paramFile.open(QFile::ReadOnly)) {
606 qCWarning(MockLinkLog) << "Cannot open mock parameters:" << paramFile.fileName() << paramFile.errorString();
607 return;
608 }
609
610 QTextStream paramStream(&paramFile);
611 while (!paramStream.atEnd()) {
612 const QString line = paramStream.readLine();
613
614 if (line.startsWith("#")) {
615 continue;
616 }
617
618 const QStringList paramData = line.split("\t");
619 if (paramData.size() != 5) {
620 qCWarning(MockLinkLog) << "Invalid mock parameter row in" << paramFile.fileName();
621 _mapParamName2Value.clear();
622 _mapParamName2MavParamType.clear();
623 return;
624 }
625
626 const int compId = paramData.at(1).toInt();
627 const QString paramName = paramData.at(2);
628 const QString valStr = paramData.at(3);
629 const uint paramType = paramData.at(4).toUInt();
630
631 QVariant paramValue;
632 switch (paramType) {
633 case MAV_PARAM_TYPE_REAL32:
634 paramValue = QVariant(valStr.toFloat());
635 break;
636 case MAV_PARAM_TYPE_UINT32:
637 paramValue = QVariant(valStr.toUInt());
638 break;
639 case MAV_PARAM_TYPE_INT32:
640 paramValue = QVariant(valStr.toInt());
641 break;
642 case MAV_PARAM_TYPE_UINT16:
643 paramValue = QVariant((quint16)valStr.toUInt());
644 break;
645 case MAV_PARAM_TYPE_INT16:
646 paramValue = QVariant((qint16)valStr.toInt());
647 break;
648 case MAV_PARAM_TYPE_UINT8:
649 paramValue = QVariant((quint8)valStr.toUInt());
650 break;
651 case MAV_PARAM_TYPE_INT8:
652 paramValue = QVariant((qint8)valStr.toUInt());
653 break;
654 default:
655 qCCritical(MockLinkVerboseLog) << "Unknown type" << paramType;
656 paramValue = QVariant(valStr.toInt());
657 break;
658 }
659
660 qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue;
661
662 _mapParamName2Value[compId][paramName] = paramValue;
663 _mapParamName2MavParamType[compId][paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
664 }
665
666 if (_hasNonDefaultParamComponent()) {
667 auto &values = _mapParamName2Value[_nonDefaultParamComponentId];
668 auto &types = _mapParamName2MavParamType[_nonDefaultParamComponentId];
669 values[QStringLiteral("CAN_TERMINATE")] = QVariant(static_cast<qint32>(0));
670 types[QStringLiteral("CAN_TERMINATE")] = MAV_PARAM_TYPE_INT32;
671 values[QLatin1StringView(_nonDefaultFailParam)] = QVariant(static_cast<qint32>(8));
672 types[QLatin1StringView(_nonDefaultFailParam)] = MAV_PARAM_TYPE_INT32;
673 values[QStringLiteral("BATT_AMP_OFFSET")] = QVariant(0.0f);
674 types[QStringLiteral("BATT_AMP_OFFSET")] = MAV_PARAM_TYPE_REAL32;
675 values[QStringLiteral("OPTIONS")] = QVariant(static_cast<qint32>(0));
676 types[QStringLiteral("OPTIONS")] = MAV_PARAM_TYPE_INT32;
677 // Sort after the four above so their indices stay fixed for the shared-index failure mode
678 for (const char *name: {"TEMP1_ADDR", "TEMP1_TYPE", "TEMP2_ADDR", "TEMP2_TYPE", "TEMP3_ADDR", "TEMP3_TYPE", "UAVCAN_NODE_ID", "UAVCAN_RATE", "VOLT_MULT", "VOLT_PIN"}) {
679 values[QLatin1StringView(name)] = QVariant(static_cast<qint32>(0));
680 types[QLatin1StringView(name)] = MAV_PARAM_TYPE_INT32;
681 }
682 }
683
684 if ((_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) && _apmStartFreshParams) {
685 _applyAPMFreshFlashState();
686 }
687}
688
699void MockLink::_resetParamsToDefaults()
700{
701 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
702 // ArduPilot firmware default for calibration-indicator parameters is 0 (uncalibrated).
703 // Resetting these causes QGC's APMSensorsComponent::setupComplete() to return false,
704 // which is the correct post-reset state on a real vehicle.
705 for (auto compIt = _mapParamName2Value.begin(); compIt != _mapParamName2Value.end(); ++compIt) {
706 for (auto paramIt = compIt.value().begin(); paramIt != compIt.value().end(); ++paramIt) {
707 if (kAPMCalOffsetParams.contains(paramIt.key())) {
708 paramIt.value() = QVariant(0.0f);
709 }
710 }
711 }
712 return;
713 }
714
715 if (_firmwareType != MAV_AUTOPILOT_PX4) {
716 qCWarning(MockLinkLog) << "Param reset to defaults not supported for firmware type" << _firmwareType;
717 return;
718 }
719
720 QFile metaDataFile(QStringLiteral(":/MockLink/Parameter.MetaData.json"));
721 if (!metaDataFile.open(QFile::ReadOnly)) {
722 qCWarning(MockLinkLog) << "Unable to open parameter metadata for reset" << metaDataFile.fileName();
723 return;
724 }
725
726 QJsonParseError parseError{};
727 const QJsonDocument doc = QJsonDocument::fromJson(metaDataFile.readAll(), &parseError);
728 if (parseError.error != QJsonParseError::NoError) {
729 qCWarning(MockLinkLog) << "Unable to parse parameter metadata for reset:" << parseError.errorString();
730 return;
731 }
732
733 QHash<QString, QVariant> defaults;
734 const QJsonArray parameters = doc.object().value(QStringLiteral("parameters")).toArray();
735 for (const QJsonValue &parameter : parameters) {
736 const QJsonObject paramObject = parameter.toObject();
737 if (paramObject.contains(QStringLiteral("default"))) {
738 defaults[paramObject.value(QStringLiteral("name")).toString()] = paramObject.value(QStringLiteral("default")).toVariant();
739 }
740 }
741
742 for (auto compIt = _mapParamName2Value.begin(); compIt != _mapParamName2Value.end(); ++compIt) {
743 const int compId = compIt.key();
744 for (auto paramIt = compIt.value().begin(); paramIt != compIt.value().end(); ++paramIt) {
745 const QString &paramName = paramIt.key();
746 if (!_resetSysAutostartOnParamReset && (paramName == QLatin1String("SYS_AUTOSTART"))) {
747 continue;
748 }
749 const auto defaultIt = defaults.constFind(paramName);
750 if (defaultIt == defaults.constEnd()) {
751 continue;
752 }
753 switch (_mapParamName2MavParamType[compId][paramName]) {
754 case MAV_PARAM_TYPE_REAL32:
755 paramIt.value() = QVariant(defaultIt->toFloat());
756 break;
757 case MAV_PARAM_TYPE_UINT32:
758 paramIt.value() = QVariant(defaultIt->toUInt());
759 break;
760 case MAV_PARAM_TYPE_INT32:
761 paramIt.value() = QVariant(defaultIt->toInt());
762 break;
763 case MAV_PARAM_TYPE_UINT16:
764 paramIt.value() = QVariant(static_cast<quint16>(defaultIt->toUInt()));
765 break;
766 case MAV_PARAM_TYPE_INT16:
767 paramIt.value() = QVariant(static_cast<qint16>(defaultIt->toInt()));
768 break;
769 case MAV_PARAM_TYPE_UINT8:
770 paramIt.value() = QVariant(static_cast<quint8>(defaultIt->toUInt()));
771 break;
772 case MAV_PARAM_TYPE_INT8:
773 paramIt.value() = QVariant(static_cast<qint8>(defaultIt->toInt()));
774 break;
775 default:
776 qCWarning(MockLinkLog) << "Param reset skipped unhandled type" << _mapParamName2MavParamType[compId][paramName] << paramName;
777 break;
778 }
779 }
780 }
781}
782
786void MockLink::_applyAPMFreshFlashState()
787{
788 // Compass and accel offsets to 0 (uncalibrated)
789 _resetParamsToDefaults();
790
791 auto &paramMap = _mapParamName2Value[MAV_COMP_ID_AUTOPILOT1];
792 const auto &paramTypeMap = _mapParamName2MavParamType[MAV_COMP_ID_AUTOPILOT1];
793
794 // Set an integer param preserving the storage type used by _loadParams()
795 auto setIntParam = [&paramMap, &paramTypeMap](const QString &paramName, int value) {
796 if (!paramMap.contains(paramName)) {
797 return;
798 }
799 switch (paramTypeMap.value(paramName)) {
800 case MAV_PARAM_TYPE_UINT32: paramMap[paramName] = QVariant(static_cast<quint32>(value)); break;
801 case MAV_PARAM_TYPE_INT32: paramMap[paramName] = QVariant(value); break;
802 case MAV_PARAM_TYPE_UINT16: paramMap[paramName] = QVariant(static_cast<quint16>(value)); break;
803 case MAV_PARAM_TYPE_INT16: paramMap[paramName] = QVariant(static_cast<qint16>(value)); break;
804 case MAV_PARAM_TYPE_UINT8: paramMap[paramName] = QVariant(static_cast<quint8>(value)); break;
805 case MAV_PARAM_TYPE_INT8: paramMap[paramName] = QVariant(static_cast<qint8>(value)); break;
806 default: paramMap[paramName] = QVariant(value); break;
807 }
808 };
809
810 // No airframe selected. Not all vehicle types have FRAME_CLASS (Plane/Sub don't).
811 setIntParam(QStringLiteral("FRAME_CLASS"), 0);
812
813 // Radio uncalibrated: RC min/max/trim at firmware defaults for the mapped attitude channels
814 const QStringList rcMapParams = {
815 QStringLiteral("RCMAP_ROLL"),
816 QStringLiteral("RCMAP_PITCH"),
817 QStringLiteral("RCMAP_YAW"),
818 QStringLiteral("RCMAP_THROTTLE"),
819 };
820 for (const QString &mapParam : rcMapParams) {
821 if (!paramMap.contains(mapParam)) {
822 continue;
823 }
824 const int channel = paramMap[mapParam].toInt();
825 setIntParam(QStringLiteral("RC%1_MIN").arg(channel), 1100);
826 setIntParam(QStringLiteral("RC%1_MAX").arg(channel), 1900);
827 setIntParam(QStringLiteral("RC%1_TRIM").arg(channel), 1500);
828 }
829}
830
831void MockLink::_sendHeartBeat()
832{
833 // Like real firmware: ACTIVE while airborne, STANDBY on the ground (same condition as
834 // EXTENDED_SYS_STATE landed state, and what ArduPilot's flying detection in QGC keys off)
835 const uint8_t mavState = (_vehicleAltitudeAMSL > _defaultVehicleHomeAltitude) ? MAV_STATE_ACTIVE : MAV_STATE_STANDBY;
836
837 mavlink_message_t msg{};
838 (void) mavlink_msg_heartbeat_pack_chan(
839 _vehicleSystemId,
840 _vehicleComponentId,
841 _outgoingMavlinkChannel,
842 &msg,
843 _vehicleType, // MAV_TYPE
844 _firmwareType, // MAV_AUTOPILOT
845 _mavBaseMode, // MAV_MODE
846 _mavCustomMode, // custom mode
847 mavState // MAV_STATE
848 );
849 respondWithMavlinkMessage(msg);
850}
851
852void MockLink::_sendHighLatency2()
853{
854 qCDebug(MockLinkLog) << "Sending" << _mavCustomMode;
855
856 union px4_custom_mode px4_cm{};
857 px4_cm.data = _mavCustomMode;
858
859 mavlink_message_t msg{};
860 (void) mavlink_msg_high_latency2_pack_chan(
861 _vehicleSystemId,
862 _vehicleComponentId,
863 _outgoingMavlinkChannel,
864 &msg,
865 0, // timestamp
866 _vehicleType, // MAV_TYPE
867 _firmwareType, // MAV_AUTOPILOT
868 px4_cm.custom_mode_hl, // custom_mode
869 static_cast<int32_t>(_vehicleLatitude * 1E7),
870 static_cast<int32_t>(_vehicleLongitude * 1E7),
871 static_cast<int16_t>(_vehicleAltitudeAMSL),
872 static_cast<int16_t>(_vehicleAltitudeAMSL), // target_altitude,
873 0, // heading
874 0, // target_heading
875 0, // target_distance
876 0, // throttle
877 0, // airspeed
878 0, // airspeed_sp
879 0, // groundspeed
880 0, // windspeed,
881 0, // wind_heading
882 UINT8_MAX, // eph not known
883 UINT8_MAX, // epv not known
884 0, // temperature_air
885 0, // climb_rate
886 -1, // battery, do not use?
887 0, // wp_num
888 0, // failure_flags
889 0, 0, 0 // custom0, custom1, custom2
890 );
891 respondWithMavlinkMessage(msg);
892}
893
894void MockLink::_sendSysStatus()
895{
896 mavlink_message_t msg{};
897 (void) mavlink_msg_sys_status_pack_chan(
898 _vehicleSystemId,
899 _vehicleComponentId,
900 _outgoingMavlinkChannel,
901 &msg,
902 MAV_SYS_STATUS_SENSOR_GPS, // onboard_control_sensors_present
903 0, // onboard_control_sensors_enabled
904 0, // onboard_control_sensors_health
905 250, // load
906 4200 * 4, // voltage_battery
907 8000, // current_battery
908 _battery1PctRemaining, // battery_remaining
909 0,0,0,0,0,0,0,0,0
910 );
911 respondWithMavlinkMessage(msg);
912}
913
914void MockLink::_sendBatteryStatus()
915{
916 if (_battery1PctRemaining > 1) {
917 _battery1PctRemaining = static_cast<int8_t>(100 - (_runningTime.elapsed() / 1000));
918 _battery1TimeRemaining = static_cast<double>(_batteryMaxTimeRemaining) * (static_cast<double>(_battery1PctRemaining) / 100.0);
919 if (_battery1PctRemaining > 50) {
920 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
921 } else if (_battery1PctRemaining > 30) {
922 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_LOW;
923 } else if (_battery1PctRemaining > 20) {
924 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL;
925 } else {
926 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY;
927 }
928 }
929
930 if (_battery2PctRemaining > 1) {
931 _battery2PctRemaining = static_cast<int8_t>(100 - ((_runningTime.elapsed() / 1000) / 2));
932 _battery2TimeRemaining = static_cast<double>(_batteryMaxTimeRemaining) * (static_cast<double>(_battery2PctRemaining) / 100.0);
933 if (_battery2PctRemaining > 50) {
934 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
935 } else if (_battery2PctRemaining > 30) {
936 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_LOW;
937 } else if (_battery2PctRemaining > 20) {
938 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL;
939 } else {
940 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY;
941 }
942 }
943
944 mavlink_message_t msg{};
945 uint16_t rgVoltages[10]{};
946 uint16_t rgVoltagesNone[10]{};
947 uint16_t rgVoltagesExtNone[4]{};
948
949 for (size_t i = 0; i < std::size(rgVoltages); i++) {
950 rgVoltages[i] = UINT16_MAX;
951 rgVoltagesNone[i] = UINT16_MAX;
952 }
953 rgVoltages[0] = rgVoltages[1] = rgVoltages[2] = 4200;
954
955 (void) mavlink_msg_battery_status_pack_chan(
956 _vehicleSystemId,
957 _vehicleComponentId,
958 _outgoingMavlinkChannel,
959 &msg,
960 1, // battery id
961 MAV_BATTERY_FUNCTION_ALL,
962 MAV_BATTERY_TYPE_LIPO,
963 2100, // temp cdegC
964 rgVoltages,
965 600, // battery cA
966 100, // current consumed mAh
967 -1, // energy consumed not supported
968 _battery1PctRemaining,
969 _battery1TimeRemaining,
970 _battery1ChargeState,
971 rgVoltagesExtNone,
972 0, // MAV_BATTERY_MODE
973 0 // MAV_BATTERY_FAULT
974 );
975 respondWithMavlinkMessage(msg);
976
977 (void) mavlink_msg_battery_status_pack_chan(
978 _vehicleSystemId,
979 _vehicleComponentId,
980 _outgoingMavlinkChannel,
981 &msg,
982 2, // battery id
983 MAV_BATTERY_FUNCTION_ALL,
984 MAV_BATTERY_TYPE_LIPO,
985 INT16_MAX, // temp cdegC
986 rgVoltagesNone,
987 600, // battery cA
988 100, // current consumed mAh
989 -1, // energy consumed not supported
990 _battery2PctRemaining,
991 _battery2TimeRemaining,
992 _battery2ChargeState,
993 rgVoltagesExtNone,
994 0, // MAV_BATTERY_MODE
995 0 // MAV_BATTERY_FAULT
996 );
997 respondWithMavlinkMessage(msg);
998}
999
1000void MockLink::_sendNamedValueFloats()
1001{
1002 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
1003
1004 // Send two named float values with varying data to exercise Inspector instance separation
1005 const float sinVal = static_cast<float>(std::sin(static_cast<double>(timeBootMs) / 1000.0));
1006 const float cosVal = static_cast<float>(std::cos(static_cast<double>(timeBootMs) / 1000.0));
1007
1008 // NAMED_VALUE_FLOAT.name is a fixed 10-byte field; pack_chan memcpys 10 bytes unconditionally.
1009 static constexpr char kSinName[10] = "sin_wave";
1010 static constexpr char kCosName[10] = "cos_wave";
1011
1012 mavlink_message_t msg{};
1013 (void) mavlink_msg_named_value_float_pack_chan(
1014 _vehicleSystemId,
1015 _vehicleComponentId,
1016 _outgoingMavlinkChannel,
1017 &msg,
1018 timeBootMs,
1019 kSinName,
1020 sinVal
1021 );
1022 respondWithMavlinkMessage(msg);
1023
1024 (void) mavlink_msg_named_value_float_pack_chan(
1025 _vehicleSystemId,
1026 _vehicleComponentId,
1027 _outgoingMavlinkChannel,
1028 &msg,
1029 timeBootMs,
1030 kCosName,
1031 cosVal
1032 );
1033 respondWithMavlinkMessage(msg);
1034}
1035
1036void MockLink::_sendVibration()
1037{
1038 mavlink_message_t msg{};
1039 (void) mavlink_msg_vibration_pack_chan(
1040 _vehicleSystemId,
1041 _vehicleComponentId,
1042 _outgoingMavlinkChannel,
1043 &msg,
1044 0, // time_usec
1045 50.5, // vibration_x,
1046 10.5, // vibration_y,
1047 60.0, // vibration_z,
1048 1, // clipping_0
1049 2, // clipping_0
1050 3 // clipping_0
1051 );
1052 respondWithMavlinkMessage(msg);
1053}
1054
1055void MockLink::_sendDistanceSensors()
1056{
1057 // Simulated proximity sensor ring: one DISTANCE_SENSOR message per yaw orientation.
1058 // Two orientations are deliberately never sent so the UI shows sectors with no data.
1059 static constexpr MAV_SENSOR_ORIENTATION rgOrientations[] = {
1060 MAV_SENSOR_ROTATION_NONE,
1061 MAV_SENSOR_ROTATION_YAW_45,
1062 MAV_SENSOR_ROTATION_YAW_90,
1063 MAV_SENSOR_ROTATION_YAW_180,
1064 MAV_SENSOR_ROTATION_YAW_270,
1065 MAV_SENSOR_ROTATION_YAW_315,
1066 };
1067
1068 static constexpr uint16_t minDistanceCm = 20;
1069 static constexpr uint16_t maxDistanceCm = 4000;
1070
1071 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
1072 const float quaternion[4]{};
1073
1074 for (size_t i = 0; i < std::size(rgOrientations); i++) {
1075 // Slow sweep between 5m and 35m, phase shifted per array entry so the arcs move independently
1076 const double sweep = std::sin((timeBootMs / 5000.0) + (i * M_PI / 4));
1077 const uint16_t currentDistanceCm = static_cast<uint16_t>(2000 + (1500 * sweep));
1078
1079 mavlink_message_t msg{};
1080 (void) mavlink_msg_distance_sensor_pack_chan(
1081 _vehicleSystemId,
1082 _vehicleComponentId,
1083 _outgoingMavlinkChannel,
1084 &msg,
1085 timeBootMs,
1086 minDistanceCm,
1087 maxDistanceCm,
1088 currentDistanceCm,
1089 MAV_DISTANCE_SENSOR_LASER,
1090 static_cast<uint8_t>(i), // id
1091 rgOrientations[i],
1092 255, // covariance - unknown
1093 0.0f, // horizontal_fov - unknown
1094 0.0f, // vertical_fov - unknown
1095 quaternion, // valid only for MAV_SENSOR_ROTATION_CUSTOM
1096 0 // signal_quality - unknown
1097 );
1098 respondWithMavlinkMessage(msg);
1099 }
1100}
1101
1102void MockLink::respondWithMavlinkMessage(const mavlink_message_t &msg)
1103{
1104 if (!_commLost) {
1105 uint8_t buffer[MAVLINK_MAX_PACKET_LEN]{};
1106 const int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg);
1107 const QByteArray bytes(reinterpret_cast<char*>(buffer), cBuffer);
1108 emit bytesReceived(this, bytes);
1109 if (_duplicateResponses) {
1110 emit bytesReceived(this, bytes);
1111 }
1112 }
1113}
1114
1115void MockLink::_writeBytes(const QByteArray &bytes)
1116{
1117 // This prevents the responses to mavlink messages from being sent until the _writeBytes returns.
1118 emit writeBytesQueuedSignal(bytes);
1119}
1120
1121void MockLink::_writeBytesQueued(const QByteArray &bytes)
1122{
1123 if (!_connected || !mavlinkChannelIsSet()) {
1124 qCDebug(MockLinkLog) << "Dropping queued bytes on disconnected/uninitialized mock link";
1125 return;
1126 }
1127
1128 if (_inNSH) {
1129 _handleIncomingNSHBytes(bytes.constData(), bytes.length());
1130 return;
1131 }
1132
1133 if (bytes.startsWith(QByteArrayLiteral("\r\r\r"))) {
1134 _inNSH = true;
1135 _handleIncomingNSHBytes(&bytes.constData()[3], bytes.length() - 3);
1136 }
1137
1138 _handleIncomingMavlinkBytes(reinterpret_cast<const uint8_t*>(bytes.constData()), bytes.length());
1139}
1140
1141void MockLink::_handleIncomingNSHBytes(const char *bytes, int cBytes)
1142{
1143 Q_UNUSED(cBytes);
1144
1145 // Drop back out of NSH
1146 if ((cBytes == 4) && (bytes[0] == '\r') && (bytes[1] == '\r') && (bytes[2] == '\r')) {
1147 _inNSH = false;
1148 return;
1149 }
1150
1151 if (cBytes > 0) {
1152 qCDebug(MockLinkLog) << "NSH:" << bytes;
1153#if 0
1154 // MockLink not quite ready to handle this correctly yet
1155 if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) {
1156 // This is the mavlink start command
1157 _mavlinkStarted = true;
1158 }
1159#endif
1160 }
1161}
1162
1163void MockLink::_handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes)
1164{
1165 mavlink_message_t msg{};
1166 mavlink_status_t comm{};
1167
1168 QMutexLocker lock(&_incomingMavlinkMutex);
1169 for (qint64 i = 0; i < cBytes; i++) {
1170 const int parsed = mavlink_parse_char(_incomingMavlinkChannel, bytes[i], &msg, &comm);
1171 if (!parsed) {
1172 continue;
1173 }
1174 if (!_mavlinkV2Upgraded && !_stayMavlinkV1 && (msg.magic == MAVLINK_STX)) {
1175 // First v2 message from GCS: switch outgoing traffic to v2, same as ArduPilot does.
1176 _mavlinkV2Upgraded = true;
1177 mavlink_status_t *const outgoingStatus = mavlink_get_channel_status(_outgoingMavlinkChannel);
1178 outgoingStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
1179 qCDebug(MockLinkLog) << "Received MAVLink v2 message from GCS, upgrading outgoing traffic to v2";
1180 }
1181 lock.unlock();
1182 _handleIncomingMavlinkMsg(msg);
1183 lock.relock();
1184 }
1185}
1186
1187void MockLink::_updateIncomingMessageCounts(const mavlink_message_t &msg)
1188{
1189 _receivedMavlinkMessageCountMap[msg.msgid]++;
1190 _lastReceivedMavlinkMessageMap[msg.msgid] = msg;
1191
1192 // Update command-specific counts if this is a COMMAND_LONG message
1193 if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
1194 mavlink_command_long_t request{};
1195 mavlink_msg_command_long_decode(&msg, &request);
1196
1197 _receivedMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
1198 _receivedMavCommandByCompCountMap[static_cast<MAV_CMD>(request.command)][request.target_component]++;
1199
1200 if (request.command == MAV_CMD_REQUEST_MESSAGE) {
1201 _receivedRequestMessageCountMap[static_cast<uint32_t>(request.param1)]++;
1202 _receivedRequestMessageByCompAndMsgCountMap[request.target_component][static_cast<int>(request.param1)]++;
1203 }
1204 } else if (msg.msgid == MAVLINK_MSG_ID_COMMAND_INT) {
1205 mavlink_command_int_t request{};
1206 mavlink_msg_command_int_decode(&msg, &request);
1207
1208 _receivedMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
1209 _receivedMavCommandByCompCountMap[static_cast<MAV_CMD>(request.command)][request.target_component]++;
1210 }
1211}
1212
1213void MockLink::_handleIncomingMavlinkMsg(const mavlink_message_t &msg)
1214{
1215 _updateIncomingMessageCounts(msg);
1216
1217 if (_missionItemHandler->handleMavlinkMessage(msg)) {
1218 return;
1219 }
1220
1221 if (_enableCamera && _mockLinkCamera->handleMavlinkMessage(msg)) {
1222 return;
1223 }
1224
1225 if (_enableGimbal && _mockLinkGimbal->handleMavlinkMessage(msg)) {
1226 return;
1227 }
1228
1229 switch (msg.msgid) {
1230 case MAVLINK_MSG_ID_HEARTBEAT:
1231 _handleHeartBeat(msg);
1232 break;
1233 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
1234 _handleParamRequestList(msg);
1235 break;
1236 case MAVLINK_MSG_ID_SET_MODE:
1237 _handleSetMode(msg);
1238 break;
1239 case MAVLINK_MSG_ID_PARAM_SET:
1240 _handleParamSet(msg);
1241 break;
1242 case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
1243 _handleParamRequestRead(msg);
1244 break;
1245 case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
1246 _handleFTP(msg);
1247 break;
1248 case MAVLINK_MSG_ID_COMMAND_LONG:
1249 _handleCommandLong(msg);
1250 break;
1251 case MAVLINK_MSG_ID_COMMAND_INT:
1252 _handleCommandInt(msg);
1253 break;
1254 case MAVLINK_MSG_ID_MANUAL_CONTROL:
1255 _handleManualControl(msg);
1256 break;
1257 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
1258 _handleRCChannelsOverride(msg);
1259 break;
1260 case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
1261 _handleLogRequestList(msg);
1262 break;
1263 case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
1264 _handleLogRequestData(msg);
1265 break;
1266 case MAVLINK_MSG_ID_LOG_ERASE:
1267 _handleLogErase(msg);
1268 break;
1269 case MAVLINK_MSG_ID_PARAM_MAP_RC:
1270 _handleParamMapRC(msg);
1271 break;
1272 case MAVLINK_MSG_ID_SETUP_SIGNING:
1273 _handleSetupSigning(msg);
1274 break;
1275 case MAVLINK_MSG_ID_COMMAND_ACK:
1276 // GCS sends COMMAND_ACK(command=0) via nextClicked() to acknowledge each pose
1277 // during APM full accel calibration (the "Next" button press).
1278 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1279 mavlink_command_ack_t ack{};
1280 mavlink_msg_command_ack_decode(&msg, &ack);
1281 if (ack.command == 0) {
1282 QMutexLocker locker(&_apmAccelCalMutex);
1283 if (_apmAccelCalPosIndex >= 0 && _apmAccelCalPosIndex < 6) {
1284 _apmAccelCalGotAck = true;
1285 }
1286 }
1287 }
1288 break;
1289 default:
1290 break;
1291 }
1292}
1293
1294void MockLink::_handleHeartBeat(const mavlink_message_t &msg)
1295{
1296 Q_UNUSED(msg);
1297 qCDebug(MockLinkVerboseLog) << "Heartbeat";
1298}
1299
1300void MockLink::_handleParamMapRC(const mavlink_message_t &msg)
1301{
1302 mavlink_param_map_rc_t paramMapRC{};
1303 mavlink_msg_param_map_rc_decode(&msg, &paramMapRC);
1304
1305 const QString paramName(QString::fromLocal8Bit(paramMapRC.param_id, static_cast<int>(strnlen(paramMapRC.param_id, MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN))));
1306
1307 if (paramMapRC.param_index == -1) {
1308 qCDebug(MockLinkLog) << QStringLiteral("MockLink - PARAM_MAP_RC: param(%1) tuningID(%2) centerValue(%3) scale(%4) min(%5) max(%6)").arg(paramName).arg(paramMapRC.parameter_rc_channel_index).arg(paramMapRC.param_value0).arg(paramMapRC.scale).arg(paramMapRC.param_value_min).arg(paramMapRC.param_value_max);
1309 } else if (paramMapRC.param_index == -2) {
1310 qCDebug(MockLinkLog) << "MockLink - PARAM_MAP_RC: Clear tuningID" << paramMapRC.parameter_rc_channel_index;
1311 } else {
1312 qCWarning(MockLinkLog) << "MockLink - PARAM_MAP_RC: Unsupported param_index" << paramMapRC.param_index;
1313 }
1314}
1315
1316void MockLink::_handleSetupSigning(const mavlink_message_t &msg)
1317{
1318 mavlink_setup_signing_t setupSigning{};
1319 mavlink_msg_setup_signing_decode(&msg, &setupSigning);
1320
1321 if (setupSigning.target_system != _vehicleSystemId) {
1322 return;
1323 }
1324
1325 // All-zero key = disable signing
1326 bool allZeroKey = true;
1327 for (const uint8_t byte : setupSigning.secret_key) {
1328 if (byte != 0) {
1329 allZeroKey = false;
1330 break;
1331 }
1332 }
1333
1334 _signingEnabled = !allZeroKey;
1335
1336 // Write C-state directly; routing through SigningController would clobber its _keyHint mid-confirmation.
1337 mavlink_status_t* const status = mavlink_get_channel_status(_outgoingMavlinkChannel);
1338 if (_signingEnabled) {
1339 memcpy(_mockSigning.secret_key, setupSigning.secret_key, sizeof(_mockSigning.secret_key));
1340 _mockSigning.link_id = _outgoingMavlinkChannel;
1341 _mockSigning.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING;
1342 _mockSigning.timestamp = MAVLinkSigning::currentSigningTimestampTicks();
1343 _mockSigning.accept_unsigned_callback = MAVLinkSigning::insecureConnectionAcceptUnsignedCallback;
1344 status->signing = &_mockSigning;
1345 status->signing_streams = &_mockSigningStreams;
1346 } else {
1347 QGC::secureZero(_mockSigning.secret_key, sizeof(_mockSigning.secret_key));
1348 _mockSigning.accept_unsigned_callback = nullptr;
1349 status->signing = nullptr;
1350 status->signing_streams = nullptr;
1351 }
1352
1353 qCDebug(MockLinkLog) << "Signing" << (_signingEnabled ? "enabled" : "disabled");
1354}
1355
1356void MockLink::_handleSetMode(const mavlink_message_t &msg)
1357{
1358 mavlink_set_mode_t request{};
1359 mavlink_msg_set_mode_decode(&msg, &request);
1360
1361 if (request.target_system != _vehicleSystemId) {
1362 qCDebug(MockLinkLog) << "Ignoring SET_MODE for system" << request.target_system;
1363 return;
1364 }
1365
1366 _mavBaseMode = request.base_mode;
1367 _mavCustomMode = request.custom_mode;
1368}
1369
1370void MockLink::_handleManualControl(const mavlink_message_t &msg)
1371{
1372 mavlink_manual_control_t manualControl{};
1373 mavlink_msg_manual_control_decode(&msg, &manualControl);
1374
1375 // INT16_MAX means "axis invalid/not provided" per MAVLink spec
1376 const auto axisStr = [](int16_t v) -> QString {
1377 return (v == INT16_MAX) ? QStringLiteral("invalid") : QString::number(v);
1378 };
1379 // Extension fields are only valid when the corresponding enabled_extensions bit is set
1380 const auto extStr = [](int16_t v, bool enabled) -> QString {
1381 return enabled ? QString::number(v) : QStringLiteral("disabled");
1382 };
1383
1384 const uint8_t ext = manualControl.enabled_extensions;
1385
1386 qCDebug(MockLinkVerboseLog).noquote()
1387 << "MANUAL_CONTROL"
1388 << "target:" << manualControl.target
1389 << "x:" << axisStr(manualControl.x)
1390 << "y:" << axisStr(manualControl.y)
1391 << "z:" << axisStr(manualControl.z)
1392 << "r:" << axisStr(manualControl.r)
1393 << "buttons:" << QStringLiteral("0x%1").arg(manualControl.buttons, 4, 16, QLatin1Char('0'))
1394 << "buttons2:" << QStringLiteral("0x%1").arg(manualControl.buttons2, 4, 16, QLatin1Char('0'))
1395 << "enabled_extensions:" << QStringLiteral("0x%1").arg(ext, 2, 16, QLatin1Char('0'))
1396 << "s(pitch):" << extStr(manualControl.s, ext & (1 << 0))
1397 << "t(roll):" << extStr(manualControl.t, ext & (1 << 1))
1398 << "aux1:" << extStr(manualControl.aux1, ext & (1 << 2))
1399 << "aux2:" << extStr(manualControl.aux2, ext & (1 << 3))
1400 << "aux3:" << extStr(manualControl.aux3, ext & (1 << 4))
1401 << "aux4:" << extStr(manualControl.aux4, ext & (1 << 5))
1402 << "aux5:" << extStr(manualControl.aux5, ext & (1 << 6))
1403 << "aux6:" << extStr(manualControl.aux6, ext & (1 << 7));
1404}
1405
1406void MockLink::_handleRCChannelsOverride(const mavlink_message_t &msg)
1407{
1408 mavlink_rc_channels_override_t override{};
1409 mavlink_msg_rc_channels_override_decode(&msg, &override);
1410
1411 // Per the MAVLink spec:
1412 // Channels 1-8: UINT16_MAX = ignore (no state change), 0 = release back to RC radio
1413 // Channels 9-18: UINT16_MAX or 0 = ignore, UINT16_MAX-1 = release back to RC radio
1414 const uint16_t rawValues[18] = {
1415 override.chan1_raw, override.chan2_raw, override.chan3_raw, override.chan4_raw,
1416 override.chan5_raw, override.chan6_raw, override.chan7_raw, override.chan8_raw,
1417 override.chan9_raw, override.chan10_raw, override.chan11_raw, override.chan12_raw,
1418 override.chan13_raw, override.chan14_raw, override.chan15_raw, override.chan16_raw,
1419 override.chan17_raw, override.chan18_raw,
1420 };
1421
1422 bool anyChange = false;
1423 for (int i = 0; i < kRcChannelOverrideChannelCount; ++i) {
1424 const uint16_t raw = rawValues[i];
1425 const bool isExtended = (i >= 8);
1426
1427 RCChannelOverride::State newState;
1428 if (isExtended) {
1429 if (raw == 0 || raw == UINT16_MAX) {
1430 continue; // ignore — no change to this channel's state
1431 } else if (raw == static_cast<uint16_t>(UINT16_MAX - 1)) {
1432 newState = RCChannelOverride::State::Released;
1433 } else {
1434 newState = RCChannelOverride::State::Overridden;
1435 }
1436 } else {
1437 if (raw == UINT16_MAX) {
1438 continue; // ignore — no change to this channel's state
1439 } else if (raw == 0) {
1440 newState = RCChannelOverride::State::Released;
1441 } else {
1442 newState = RCChannelOverride::State::Overridden;
1443 }
1444 }
1445
1446 RCChannelOverride &ch = _rcChannelOverrides[i];
1447 if (ch.state == newState) {
1448 continue;
1449 }
1450
1451 anyChange = true;
1452
1453 const auto stateLabel = [](RCChannelOverride::State s) -> const char * {
1454 switch (s) {
1455 case RCChannelOverride::State::Ignore: return "ignore";
1456 case RCChannelOverride::State::Released: return "released";
1457 case RCChannelOverride::State::Overridden: return "overridden";
1458 }
1459 return "unknown";
1460 };
1461 qCDebug(MockLinkLog).noquote() << QStringLiteral("RC_CHANNELS_OVERRIDE ch%1: %2 -> %3").arg(i + 1).arg(stateLabel(ch.state)).arg(stateLabel(newState));
1462
1463 ch.state = newState;
1464 ch.value = (newState == RCChannelOverride::State::Overridden) ? raw : 0;
1465 }
1466
1467 if (anyChange) {
1468 QStringList active;
1469 for (int i = 0; i < kRcChannelOverrideChannelCount; ++i) {
1470 if (_rcChannelOverrides[i].state == RCChannelOverride::State::Overridden) {
1471 active << QStringLiteral("ch%1").arg(i + 1);
1472 }
1473 }
1474 if (active.isEmpty()) {
1475 qCDebug(MockLinkLog) << "RC_CHANNELS_OVERRIDE: no channels currently overridden";
1476 } else {
1477 qCDebug(MockLinkLog).noquote() << "RC_CHANNELS_OVERRIDE active overrides:" << active.join(QStringLiteral(", "));
1478 }
1479 }
1480
1481 for (int i = 0; i < kRcChannelOverrideChannelCount; ++i) {
1482 if (_rcChannelOverrides[i].state == RCChannelOverride::State::Overridden) {
1483 qCDebug(MockLinkVerboseLog).noquote() << QStringLiteral("RC_CHANNELS_OVERRIDE ch%1 value: %2").arg(i + 1).arg(_rcChannelOverrides[i].value);
1484 }
1485 }
1486}
1487
1488void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString &paramName, float paramFloat)
1489{
1490 if (!_mapParamName2Value.contains(componentId) || !_mapParamName2Value[componentId].contains(paramName) ||
1491 !_mapParamName2MavParamType.contains(componentId) ||
1492 !_mapParamName2MavParamType[componentId].contains(paramName)) {
1493 qCWarning(MockLinkLog) << "Cannot update unknown mock parameter" << componentId << paramName;
1494 return;
1495 }
1496
1497 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
1498 QVariant paramVariant;
1499 mavlink_param_union_t valueUnion{};
1500 valueUnion.param_float = paramFloat;
1501 switch (paramType) {
1502 case MAV_PARAM_TYPE_REAL32:
1503 paramVariant = QVariant::fromValue(valueUnion.param_float);
1504 break;
1505 case MAV_PARAM_TYPE_UINT32:
1506 paramVariant = QVariant::fromValue(valueUnion.param_uint32);
1507 break;
1508 case MAV_PARAM_TYPE_INT32:
1509 paramVariant = QVariant::fromValue(valueUnion.param_int32);
1510 break;
1511 case MAV_PARAM_TYPE_UINT16:
1512 paramVariant = QVariant::fromValue(valueUnion.param_uint16);
1513 break;
1514 case MAV_PARAM_TYPE_INT16:
1515 paramVariant = QVariant::fromValue(valueUnion.param_int16);
1516 break;
1517 case MAV_PARAM_TYPE_UINT8:
1518 paramVariant = QVariant::fromValue(valueUnion.param_uint8);
1519 break;
1520 case MAV_PARAM_TYPE_INT8:
1521 paramVariant = QVariant::fromValue(valueUnion.param_int8);
1522 break;
1523 default:
1524 qCCritical(MockLinkLog) << "Invalid parameter type" << paramType;
1525 paramVariant = QVariant::fromValue(valueUnion.param_int32);
1526 break;
1527 }
1528
1529 qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
1530 _mapParamName2Value[componentId][paramName] = paramVariant;
1531}
1532
1533void MockLink::setMockParamValue(int componentId, const QString &paramName, float value)
1534{
1535 mavlink_param_union_t valueUnion{};
1536 valueUnion.param_float = value;
1537 _setParamFloatUnionIntoMap(componentId, paramName, valueUnion.param_float);
1538}
1539
1540float MockLink::_floatUnionForParam(int componentId, const QString &paramName)
1541{
1542 if (!_mapParamName2Value.contains(componentId) || !_mapParamName2Value[componentId].contains(paramName) ||
1543 !_mapParamName2MavParamType.contains(componentId) ||
1544 !_mapParamName2MavParamType[componentId].contains(paramName)) {
1545 qCWarning(MockLinkLog) << "Cannot read unknown mock parameter" << componentId << paramName;
1546 return qQNaN();
1547 }
1548
1549 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
1550 const QVariant paramVar = _mapParamName2Value[componentId][paramName];
1551
1552 mavlink_param_union_t valueUnion{};
1553 switch (paramType) {
1554 case MAV_PARAM_TYPE_REAL32:
1555 valueUnion.param_float = paramVar.toFloat();
1556 break;
1557 case MAV_PARAM_TYPE_UINT32:
1558 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1559 valueUnion.param_float = paramVar.toUInt();
1560 } else {
1561 valueUnion.param_uint32 = paramVar.toUInt();
1562 }
1563 break;
1564 case MAV_PARAM_TYPE_INT32:
1565 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1566 valueUnion.param_float = paramVar.toInt();
1567 } else {
1568 valueUnion.param_int32 = paramVar.toInt();
1569 }
1570 break;
1571 case MAV_PARAM_TYPE_UINT16:
1572 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1573 valueUnion.param_float = paramVar.toUInt();
1574 } else {
1575 valueUnion.param_uint16 = paramVar.toUInt();
1576 }
1577 break;
1578 case MAV_PARAM_TYPE_INT16:
1579 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1580 valueUnion.param_float = paramVar.toInt();
1581 } else {
1582 valueUnion.param_int16 = paramVar.toInt();
1583 }
1584 break;
1585 case MAV_PARAM_TYPE_UINT8:
1586 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1587 valueUnion.param_float = paramVar.toUInt();
1588 } else {
1589 valueUnion.param_uint8 = paramVar.toUInt();
1590 }
1591 break;
1592 case MAV_PARAM_TYPE_INT8:
1593 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1594 valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1();
1595 } else {
1596 valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1();
1597 }
1598 break;
1599 default:
1600 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1601 valueUnion.param_float = paramVar.toInt();
1602 } else {
1603 valueUnion.param_int32 = paramVar.toInt();
1604 }
1605 qCCritical(MockLinkLog) << "Invalid parameter type" << paramType;
1606 }
1607
1608 return valueUnion.param_float;
1609}
1610
1611uint32_t MockLink::_computeParamHash(int componentId) const
1612{
1613 // Volatile parameters are excluded from the hash, matching PX4 firmware and
1614 // ParameterManager::_tryCacheHashLoad. The list comes from the same metadata
1615 // json served to QGC so the two sides always agree.
1616 static const QSet<QString> volatileParams = []() {
1617 QSet<QString> volatiles;
1618 QFile metaDataFile(QStringLiteral(":/MockLink/Parameter.MetaData.json"));
1619 if (metaDataFile.open(QFile::ReadOnly)) {
1620 QJsonParseError parseError{};
1621 const QJsonDocument doc = QJsonDocument::fromJson(metaDataFile.readAll(), &parseError);
1622 if (parseError.error != QJsonParseError::NoError) {
1623 qCWarning(MockLinkLog) << "Unable to parse parameter metadata for volatile param list:" << parseError.errorString();
1624 }
1625 const QJsonArray parameters = doc.object().value(QStringLiteral("parameters")).toArray();
1626 for (const QJsonValue &parameter : parameters) {
1627 const QJsonObject paramObject = parameter.toObject();
1628 if (paramObject.value(QStringLiteral("volatile")).toBool()) {
1629 volatiles.insert(paramObject.value(QStringLiteral("name")).toString());
1630 }
1631 }
1632 } else {
1633 qCWarning(MockLinkLog) << "Unable to open parameter metadata for volatile param list" << metaDataFile.fileName();
1634 }
1635 return volatiles;
1636 }();
1637
1638 uint32_t crc = 0;
1639 const auto &params = _mapParamName2Value[componentId];
1640 for (auto it = params.constBegin(); it != params.constEnd(); ++it) {
1641 const QString &name = it.key();
1642 if (volatileParams.contains(name)) {
1643 continue;
1644 }
1645 const QVariant &value = it.value();
1646 const MAV_PARAM_TYPE mavType = _mapParamName2MavParamType[componentId][name];
1647 const FactMetaData::ValueType_t factType = ParameterManager::mavTypeToFactType(mavType);
1648
1649 crc = QGC::crc32(reinterpret_cast<const uint8_t *>(qPrintable(name)), name.length(), crc);
1650 crc = QGC::crc32(static_cast<const uint8_t *>(value.constData()), FactMetaData::typeToSize(factType), crc);
1651 }
1652 return crc;
1653}
1654
1655bool MockLink::_hasNonDefaultParamComponent() const
1656{
1657 switch (_failureMode) {
1658 case MockConfiguration::FailMissingParamOnAllRequestsNonDefaultComponent:
1659 case MockConfiguration::FailMissingParamSharedIndexAcrossComponents:
1660 case MockConfiguration::FailNonDefaultComponentDead:
1661 case MockConfiguration::FailNonDefaultComponentLossy:
1662 return true;
1663 default:
1664 return false;
1665 }
1666}
1667
1668bool MockLink::_shouldSkipParamSend(int componentId, const QString &paramName, int paramIndex) const
1669{
1670 switch (_failureMode) {
1671 case MockConfiguration::FailMissingParamOnInitialRequest:
1672 case MockConfiguration::FailMissingParamOnAllRequests:
1673 return paramName == _failParam;
1674 case MockConfiguration::FailMissingParamOnAllRequestsNonDefaultComponent:
1675 return (componentId == _nonDefaultParamComponentId) && (paramName == _nonDefaultFailParam);
1676 case MockConfiguration::FailMissingParamSharedIndexAcrossComponents:
1677 return paramIndex == _sharedFailParamIndex;
1678 case MockConfiguration::FailNonDefaultComponentDead:
1679 case MockConfiguration::FailNonDefaultComponentLossy:
1680 return (componentId == _nonDefaultParamComponentId) && (paramIndex >= _nonDefaultStreamedParamCount);
1681 default:
1682 return false;
1683 }
1684}
1685
1686bool MockLink::_shouldSkipParamRead(int componentId, const QString &paramName, int paramIndex)
1687{
1688 switch (_failureMode) {
1689 case MockConfiguration::FailMissingParamOnAllRequests:
1690 return paramName == _failParam;
1691 case MockConfiguration::FailMissingParamOnAllRequestsNonDefaultComponent:
1692 return (componentId == _nonDefaultParamComponentId) && (paramName == _nonDefaultFailParam);
1693 case MockConfiguration::FailMissingParamSharedIndexAcrossComponents:
1694 return (componentId == MAV_COMP_ID_AUTOPILOT1) && (paramIndex == _sharedFailParamIndex);
1695 case MockConfiguration::FailNonDefaultComponentDead:
1696 return componentId == _nonDefaultParamComponentId;
1697 case MockConfiguration::FailNonDefaultComponentLossy:
1698 if (componentId != _nonDefaultParamComponentId) {
1699 return false;
1700 }
1701 if (_nonDefaultReadAttempted.contains(qMakePair(componentId, paramIndex))) {
1702 return false;
1703 }
1704 _nonDefaultReadAttempted.insert(qMakePair(componentId, paramIndex));
1705 return true;
1706 default:
1707 return false;
1708 }
1709}
1710
1711void MockLink::_handleParamRequestList(const mavlink_message_t &msg)
1712{
1713 if (_failureMode == MockConfiguration::FailParamNoResponseToRequestList) {
1714 return;
1715 }
1716
1717 mavlink_param_request_list_t request{};
1718 mavlink_msg_param_request_list_decode(&msg, &request);
1719
1720 if (request.target_system != _vehicleSystemId) {
1721 qCDebug(MockLinkLog) << "Ignoring PARAM_REQUEST_LIST for system" << request.target_system;
1722 return;
1723 }
1724
1725 // Cache component IDs and first component's param names to avoid repeated keys() calls in worker
1726 // Thread safety: Lock mutex before modifying shared state accessed by worker thread
1727 QMutexLocker locker(&_paramRequestListMutex);
1728 if (request.target_component == MAV_COMP_ID_ALL) {
1729 _paramRequestListComponentIds = _mapParamName2Value.keys();
1730 } else {
1731 _paramRequestListComponentIds.clear();
1732 if (_mapParamName2Value.contains(request.target_component)) {
1733 _paramRequestListComponentIds.append(request.target_component);
1734 }
1735 }
1736 // Stream the autopilot last, like PX4 does with bridged DroneCAN nodes. Otherwise QGC finishes the
1737 // initial load as soon as the autopilot completes and never sees the other components.
1738 if (_paramRequestListComponentIds.removeOne(MAV_COMP_ID_AUTOPILOT1)) {
1739 _paramRequestListComponentIds.append(MAV_COMP_ID_AUTOPILOT1);
1740 }
1741 if (!_paramRequestListComponentIds.isEmpty()) {
1742 _paramRequestListParamNames = _mapParamName2Value[_paramRequestListComponentIds.first()].keys();
1743 }
1744
1745 // Start the worker routine
1746 _currentParamRequestListComponentIndex = 0;
1747 _currentParamRequestListParamIndex = 0;
1748 _paramRequestListHashCheckSent = false;
1749}
1750
1751void MockLink::_paramRequestListWorker()
1752{
1753 if (_currentParamRequestListComponentIndex == -1) {
1754 // Initial request complete
1755 return;
1756 }
1757
1758 // Thread safety: Lock mutex before accessing shared state modified by main thread
1759 QMutexLocker locker(&_paramRequestListMutex);
1760
1761 // Use cached lists instead of calling keys() on every iteration (500Hz)
1762 if (_currentParamRequestListComponentIndex >= _paramRequestListComponentIds.count()) {
1763 _currentParamRequestListComponentIndex = -1;
1764 return;
1765 }
1766
1767 const int componentId = _paramRequestListComponentIds.at(_currentParamRequestListComponentIndex);
1768 const int cParameters = _paramRequestListParamNames.count();
1769
1770 if (_currentParamRequestListParamIndex >= cParameters) {
1771 // All regular params sent — for PX4, append _HASH_CHECK as the last entry in the stream.
1772 // Uses param_count=0, param_index=-1 (same as standalone response) so ParameterManager
1773 // handles it via the _HASH_CHECK early-return path without affecting param count tracking.
1774 if (_firmwareType == MAV_AUTOPILOT_PX4 && !_paramRequestListHashCheckSent) {
1775 _paramRequestListHashCheckSent = true;
1776
1777 mavlink_param_union_t valueUnion{};
1778 valueUnion.type = MAV_PARAM_TYPE_UINT32;
1779 valueUnion.param_uint32 = _computeParamHash(componentId);
1780
1781 char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{};
1782 (void) strncpy(paramId, "_HASH_CHECK", MAVLINK_MSG_ID_PARAM_VALUE_LEN);
1783
1784 qCDebug(MockLinkLog) << "Sending _HASH_CHECK in PARAM_REQUEST_LIST stream" << componentId << "hash:" << valueUnion.param_uint32;
1785
1786 mavlink_message_t responseMsg{};
1787 (void) mavlink_msg_param_value_pack_chan(
1788 _vehicleSystemId,
1789 componentId,
1790 _outgoingMavlinkChannel,
1791 &responseMsg,
1792 paramId,
1793 valueUnion.param_float,
1794 MAV_PARAM_TYPE_UINT32,
1795 0, // param_count: 0 to avoid affecting ParameterManager's count tracking
1796 -1 // param_index: -1 signals this is a virtual/out-of-band parameter
1797 );
1798 respondWithMavlinkMessage(responseMsg);
1799 return;
1800 }
1801
1802 // Move to next component
1803 if (++_currentParamRequestListComponentIndex >= _paramRequestListComponentIds.count()) {
1804 _currentParamRequestListComponentIndex = -1;
1805 _paramRequestListComponentIds.clear();
1806 _paramRequestListParamNames.clear();
1807 } else {
1808 // Cache param names for the new component
1809 _paramRequestListParamNames = _mapParamName2Value[_paramRequestListComponentIds.at(_currentParamRequestListComponentIndex)].keys();
1810 _currentParamRequestListParamIndex = 0;
1811 _paramRequestListHashCheckSent = false;
1812 }
1813 return;
1814 }
1815
1816 const QString &paramName = _paramRequestListParamNames.at(_currentParamRequestListParamIndex);
1817
1818 if (_shouldSkipParamSend(componentId, paramName, _currentParamRequestListParamIndex)) {
1819 qCDebug(MockLinkLog) << "Skipping param send:" << paramName;
1820 } else {
1821 char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{};
1822 mavlink_message_t responseMsg{};
1823
1824 if (!_mapParamName2Value[componentId].contains(paramName) ||
1825 !_mapParamName2MavParamType[componentId].contains(paramName) ||
1826 paramName.length() > MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
1827 qCWarning(MockLinkLog) << "Cannot stream invalid mock parameter" << componentId << paramName;
1828 _currentParamRequestListComponentIndex = -1;
1829 _paramRequestListComponentIds.clear();
1830 _paramRequestListParamNames.clear();
1831 return;
1832 }
1833
1834 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
1835
1836 (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);
1837
1838 qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId];
1839
1840 (void) mavlink_msg_param_value_pack_chan(
1841 _vehicleSystemId,
1842 componentId, // component id
1843 _outgoingMavlinkChannel,
1844 &responseMsg, // Outgoing message
1845 paramId, // Parameter name
1846 _floatUnionForParam(componentId, paramName), // Parameter value
1847 paramType, // MAV_PARAM_TYPE
1848 cParameters, // Total number of parameters
1849 _currentParamRequestListParamIndex // Index of this parameter
1850 );
1851 respondWithMavlinkMessage(responseMsg);
1852 }
1853
1854 // Move to next param index
1855 ++_currentParamRequestListParamIndex;
1856}
1857
1858void MockLink::_handleParamSet(const mavlink_message_t &msg)
1859{
1860 mavlink_param_set_t request{};
1861 mavlink_msg_param_set_decode(&msg, &request);
1862
1863 if (request.target_system != _vehicleSystemId) {
1864 qCDebug(MockLinkLog) << "Ignoring PARAM_SET for system" << request.target_system;
1865 return;
1866 }
1867 const int componentId = request.target_component;
1868
1869 // Param may not be null terminated if exactly fits
1870 char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1]{};
1871 paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN] = 0;
1872 (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
1873
1874 qCDebug(MockLinkLog) << "_handleParamSet" << componentId << paramId << request.param_type;
1875
1876 // PX4 special case: _HASH_CHECK is a virtual parameter used by ParameterManager
1877 // to signal cache-hit and stop parameter streaming. It is intentionally not part
1878 // of the normal parameter maps.
1879 if ((_firmwareType == MAV_AUTOPILOT_PX4) && (strncmp(paramId, "_HASH_CHECK", MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN) == 0)) {
1880 QMutexLocker locker(&_paramRequestListMutex);
1881 _currentParamRequestListComponentIndex = -1;
1882 _paramRequestListComponentIds.clear();
1883 _paramRequestListParamNames.clear();
1884 qCDebug(MockLinkLog) << "Received _HASH_CHECK PARAM_SET, stopping parameter stream";
1885 return;
1886 }
1887
1888 // Real firmware rejects a PARAM_SET it doesn't recognize rather than crashing.
1889 // QGC can legitimately send these: loading a QGC-format param file sends params
1890 // not currently known to the vehicle (e.g. params unlocked by another param).
1891 if (!_mapParamName2Value.contains(componentId) || !_mapParamName2MavParamType.contains(componentId)) {
1892 qCDebug(MockLinkLog) << "_handleParamSet unknown component, rejecting with PARAM_ERROR - componentId:" << componentId << "param:" << paramId;
1893 _sendParamError(componentId, paramId, -1, MAV_PARAM_ERROR_COMPONENT_NOT_FOUND);
1894 return;
1895 }
1896 if (!_mapParamName2Value[componentId].contains(paramId)) {
1897 qCDebug(MockLinkLog) << "_handleParamSet unknown param, rejecting with PARAM_ERROR - componentId:" << componentId << "param:" << paramId;
1898 _sendParamError(componentId, paramId, -1, MAV_PARAM_ERROR_DOES_NOT_EXIST);
1899 return;
1900 }
1901 if (request.param_type != _mapParamName2MavParamType[componentId][paramId]) {
1902 qCDebug(MockLinkLog) << "_handleParamSet type mismatch, rejecting with PARAM_ERROR - param:" << paramId
1903 << "requested type:" << request.param_type << "actual type:" << _mapParamName2MavParamType[componentId][paramId];
1904 _sendParamError(componentId, paramId, -1, MAV_PARAM_ERROR_TYPE_MISMATCH);
1905 return;
1906 }
1907
1908 // Apply failure behaviors before committing change.
1909 if (_paramSetFailureMode == FailParamSetFirstAttemptNoAck && _paramSetFailureFirstAttemptPending) {
1910 qCDebug(MockLinkLog) << "Param set failure: first attempt no ack" << paramId;
1911 _paramSetFailureFirstAttemptPending = false;
1912 return;
1913 }
1914
1915 if (_paramSetFailureMode == FailParamSetNoAck) {
1916 qCDebug(MockLinkLog) << "Param set failure: no ack" << paramId;
1917 return;
1918 }
1919
1920 if (_paramSetFailureMode == FailParamSetParamError) {
1921 qCDebug(MockLinkLog) << "Param set failure: PARAM_ERROR" << paramId;
1922 _sendParamError(componentId, paramId,
1923 _mapParamName2Value[componentId].keys().indexOf(paramId),
1924 MAV_PARAM_ERROR_VALUE_OUT_OF_RANGE);
1925 return;
1926 }
1927
1928 // Normal success path
1929 _setParamFloatUnionIntoMap(componentId, paramId, request.param_value);
1930
1931 mavlink_message_t responseMsg;
1932 mavlink_msg_param_value_pack_chan(
1933 _vehicleSystemId,
1934 componentId, // component id
1935 _outgoingMavlinkChannel,
1936 &responseMsg, // Outgoing message
1937 paramId, // Parameter name
1938 request.param_value, // Send same value back
1939 request.param_type, // Send same type back
1940 _mapParamName2Value[componentId].count(), // Total number of parameters
1941 _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter
1942 );
1943 respondWithMavlinkMessage(responseMsg);
1944}
1945
1946void MockLink::_handleParamRequestRead(const mavlink_message_t &msg)
1947{
1948 mavlink_message_t responseMsg{};
1949 mavlink_param_request_read_t request{};
1950 mavlink_msg_param_request_read_decode(&msg, &request);
1951
1952 const QString paramName(QString::fromLocal8Bit(request.param_id, static_cast<int>(strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN))));
1953 const int componentId = request.target_component;
1954
1955 // special case for magic _HASH_CHECK value (PX4 only)
1956 if ((_firmwareType == MAV_AUTOPILOT_PX4) && (paramName == "_HASH_CHECK")) {
1957 _hashCheckRequestCount++;
1958 if (_hashCheckNoResponse) {
1959 return;
1960 }
1961
1962 const int hashComponentId = _mapParamName2Value.contains(MAV_COMP_ID_AUTOPILOT1)
1963 ? MAV_COMP_ID_AUTOPILOT1
1964 : _mapParamName2Value.keys().first();
1965
1966 mavlink_param_union_t valueUnion{};
1967 valueUnion.type = MAV_PARAM_TYPE_UINT32;
1968 valueUnion.param_uint32 = _computeParamHash(hashComponentId);
1969 (void) mavlink_msg_param_value_pack_chan(
1970 _vehicleSystemId,
1971 hashComponentId,
1972 _outgoingMavlinkChannel,
1973 &responseMsg,
1974 request.param_id,
1975 valueUnion.param_float,
1976 MAV_PARAM_TYPE_UINT32,
1977 0,
1978 -1
1979 );
1980 respondWithMavlinkMessage(responseMsg);
1981 return;
1982 }
1983
1984 if (!_mapParamName2Value.contains(componentId)) {
1985 qCDebug(MockLinkLog) << "_handleParamRequestRead unknown component, rejecting with PARAM_ERROR - componentId:" << componentId << "param:" << paramName;
1986 const QByteArray paramIdBytes = paramName.toLocal8Bit();
1987 _sendParamError(componentId, paramIdBytes.constData(), request.param_index, MAV_PARAM_ERROR_COMPONENT_NOT_FOUND);
1988 return;
1989 }
1990
1991 char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]{};
1992 paramId[0] = 0;
1993
1994 if (request.target_system != _vehicleSystemId) {
1995 qCDebug(MockLinkLog) << "Ignoring PARAM_REQUEST_READ for system" << request.target_system;
1996 return;
1997 }
1998
1999 if (request.param_index == -1) {
2000 // Request is by param name. Param may not be null terminated if exactly fits
2001 (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
2002 } else {
2003 // Request is by index
2004 if (request.param_index < 0 || request.param_index >= _mapParamName2Value[componentId].size()) {
2005 qCWarning(MockLinkLog) << "Invalid mock parameter index" << componentId << request.param_index;
2006 _sendParamError(componentId, "", request.param_index, MAV_PARAM_ERROR_DOES_NOT_EXIST);
2007 return;
2008 }
2009
2010 const QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
2011 if (key.length() > MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN) {
2012 qCWarning(MockLinkLog) << "Mock parameter name exceeds the wire limit" << componentId << key;
2013 return;
2014 }
2015 strcpy(paramId, key.toLocal8Bit().constData());
2016 _paramRequestReadIndexLog.append(qMakePair(componentId, static_cast<int>(request.param_index)));
2017 }
2018
2019 if (!_mapParamName2Value[componentId].contains(paramId) || !_mapParamName2MavParamType[componentId].contains(paramId)) {
2020 // Real firmware rejects a read of a parameter it doesn't know about (e.g. QGC
2021 // refreshing a param after a failed PARAM_SET for a param not on the vehicle)
2022 qCDebug(MockLinkLog) << "_handleParamRequestRead unknown param, rejecting with PARAM_ERROR - componentId:" << componentId << "param:" << paramId;
2023 _sendParamError(componentId, paramId, request.param_index, MAV_PARAM_ERROR_DOES_NOT_EXIST);
2024 return;
2025 }
2026
2027 const int paramIndex = (request.param_index == -1) ? static_cast<int>(_mapParamName2Value[componentId].keys().indexOf(paramId)) : request.param_index;
2028 if (_shouldSkipParamRead(componentId, paramId, paramIndex)) {
2029 qCDebug(MockLinkLog) << "Ignoring request read for" << paramId;
2030 return;
2031 }
2032
2033 if (_paramRequestReadFailureMode == FailParamRequestReadFirstAttemptNoResponse && _paramRequestReadFailureFirstAttemptPending) {
2034 qCDebug(MockLinkLog) << "Param request read failure: first attempt no response" << paramId;
2035 _paramRequestReadFailureFirstAttemptPending = false;
2036 return;
2037 }
2038
2039 if (_paramRequestReadFailureMode == FailParamRequestReadNoResponse) {
2040 qCDebug(MockLinkLog) << "Param request read failure: no response" << paramId;
2041 return;
2042 }
2043
2044 if (_paramRequestReadFailureMode == FailParamRequestReadParamError) {
2045 qCDebug(MockLinkLog) << "Param request read failure: PARAM_ERROR" << paramId;
2046 _sendParamError(componentId, paramId, request.param_index, MAV_PARAM_ERROR_DOES_NOT_EXIST);
2047 return;
2048 }
2049
2050 (void) mavlink_msg_param_value_pack_chan(
2051 _vehicleSystemId,
2052 componentId, // component id
2053 _outgoingMavlinkChannel,
2054 &responseMsg, // Outgoing message
2055 paramId, // Parameter name
2056 _floatUnionForParam(componentId, paramId), // Parameter value
2057 _mapParamName2MavParamType[componentId][paramId], // Parameter type
2058 _mapParamName2Value[componentId].count(), // Total number of parameters
2059 _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter
2060 );
2061 respondWithMavlinkMessage(responseMsg);
2062}
2063
2064void MockLink::_sendParamError(int componentId, const char *paramId, int16_t paramIndex, uint8_t errorCode)
2065{
2066 mavlink_message_t responseMsg{};
2067 char paramIdBuf[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN + 1] = {};
2068 (void) strncpy(paramIdBuf, paramId, MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN);
2069
2070 (void) mavlink_msg_param_error_pack_chan(
2071 _vehicleSystemId,
2072 static_cast<uint8_t>(componentId),
2073 _outgoingMavlinkChannel,
2074 &responseMsg,
2075 MAVLinkProtocol::instance()->getSystemId(),
2076 MAVLinkProtocol::getComponentId(),
2077 paramIdBuf,
2078 paramIndex,
2079 errorCode
2080 );
2081 respondWithMavlinkMessage(responseMsg);
2082}
2083
2084void MockLink::_handleFTP(const mavlink_message_t &msg)
2085{
2086 _mockLinkFTP->mavlinkMessageReceived(msg);
2087}
2088
2089void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t &request)
2090{
2091 uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
2092
2093 switch (request.command) {
2094 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED:
2095 // Test command which sends in progress messages and then acceptance ack
2096 commandResult = MAV_RESULT_ACCEPTED;
2097 break;
2098 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED:
2099 // Test command which sends in progress messages and then failure ack
2100 commandResult = MAV_RESULT_FAILED;
2101 break;
2102 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK:
2103 // Test command which sends in progress messages and then never sends final result ack
2104 break;
2105 }
2106
2107 mavlink_message_t commandAck{};
2108 (void) mavlink_msg_command_ack_pack_chan(
2109 _vehicleSystemId,
2110 _vehicleComponentId,
2111 _outgoingMavlinkChannel,
2112 &commandAck,
2113 request.command,
2114 MAV_RESULT_IN_PROGRESS,
2115 1, // progress
2116 0, // result_param2
2117 0, // target_system
2118 0 // target_component
2119 );
2120 respondWithMavlinkMessage(commandAck);
2121
2122 if (request.command != MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK) {
2123 (void) mavlink_msg_command_ack_pack_chan(
2124 _vehicleSystemId,
2125 _vehicleComponentId,
2126 _outgoingMavlinkChannel,
2127 &commandAck,
2128 request.command,
2129 commandResult,
2130 0, // progress
2131 0, // result_param2
2132 0, // target_system
2133 0 // target_component
2134 );
2135 respondWithMavlinkMessage(commandAck);
2136 }
2137}
2138
2139void MockLink::_handleCommandLongSetMessageInterval(const mavlink_command_long_t &request, bool &accepted)
2140{
2141 // Accept only the message IDs that MAVLinkStreamConfig requests for PID tuning.
2142 // Anything else gets MAV_RESULT_UNSUPPORTED so unit tests will catch unexpected usage.
2143 static const QSet<int> kPidTuningMessageIds = {
2144 MAVLINK_MSG_ID_ATTITUDE_QUATERNION,
2145 MAVLINK_MSG_ID_ATTITUDE_TARGET,
2146 MAVLINK_MSG_ID_LOCAL_POSITION_NED,
2147 MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED,
2148 MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT,
2149 MAVLINK_MSG_ID_VFR_HUD,
2150 };
2151 accepted = kPidTuningMessageIds.contains(static_cast<int>(request.param1));
2152}
2153
2154void MockLink::_handleCommandLong(const mavlink_message_t &msg)
2155{
2156 static bool firstCmdUser3 = true;
2157 static bool firstCmdUser4 = true;
2158
2159 mavlink_command_long_t request{};
2160 mavlink_msg_command_long_decode(&msg, &request);
2161
2162 uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
2163
2164 switch (request.command) {
2165 case MAV_CMD_COMPONENT_ARM_DISARM:
2166 if (request.param1 == 0.0f) {
2167 _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
2168 } else {
2169 _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
2170 }
2171 commandResult = MAV_RESULT_ACCEPTED;
2172 break;
2173 case MAV_CMD_DO_SET_MODE:
2174 // Preserve the armed flag like real firmware does
2175 _mavBaseMode = (_mavBaseMode & MAV_MODE_FLAG_SAFETY_ARMED) | static_cast<uint8_t>(request.param1);
2176 _mavCustomMode = static_cast<uint32_t>(request.param2);
2177 commandResult = MAV_RESULT_ACCEPTED;
2178 break;
2179 case MAV_CMD_MISSION_START:
2180 commandResult = MAV_RESULT_ACCEPTED;
2181 break;
2182 case MAV_CMD_PREFLIGHT_CALIBRATION:
2183 _handlePreFlightCalibration(request);
2184 commandResult = MAV_RESULT_ACCEPTED;
2185 break;
2186 case MAV_CMD_DO_MOTOR_TEST:
2187 commandResult = MAV_RESULT_ACCEPTED;
2188 break;
2189 case MAV_CMD_DO_START_MAG_CAL:
2190 // APM onboard compass calibration: start sending MAG_CAL_PROGRESS then MAG_CAL_REPORT.
2191 // Note: does NOT stop stale failed report streaming - like real ArduPilot, only
2192 // MAV_CMD_DO_CANCEL_MAG_CAL stops the failed report stream.
2193 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
2194 if (_apmMagCalStartFailureMode) {
2195 commandResult = MAV_RESULT_FAILED;
2196 } else {
2197 QMutexLocker locker(&_apmCompassCalMutex);
2198 _apmCompassCalProgress = 0;
2199 _apmCompassCalTickCount = 0;
2200 commandResult = MAV_RESULT_ACCEPTED;
2201 }
2202 }
2203 break;
2204 case MAV_CMD_DO_CANCEL_MAG_CAL:
2205 // Stop APM compass calibration worker
2206 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
2207 QMutexLocker locker(&_apmCompassCalMutex);
2208 _apmStaleFailedMagCalReportStreaming = false;
2209 _apmCompassCalProgress = -1;
2210 commandResult = MAV_RESULT_ACCEPTED;
2211 }
2212 break;
2213 case MAV_CMD_CONTROL_HIGH_LATENCY:
2214 if (linkConfiguration()->isHighLatency()) {
2215 _highLatencyTransmissionEnabled = static_cast<int>(request.param1) != 0;
2216 emit highLatencyTransmissionEnabledChanged(_highLatencyTransmissionEnabled);
2217 commandResult = MAV_RESULT_ACCEPTED;
2218 } else {
2219 commandResult = MAV_RESULT_FAILED;
2220 }
2221 break;
2222 case MAV_CMD_PREFLIGHT_STORAGE:
2223 if (static_cast<int>(request.param1) == 2) {
2224 // Reset all parameters to firmware defaults (unit test support)
2225 _resetParamsToDefaults();
2226 }
2227 commandResult = MAV_RESULT_ACCEPTED;
2228 break;
2229 case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
2230 // Unit test support: accept the reboot command so tests can exercise
2231 // flows which restart the vehicle (e.g. airframe Apply and Restart).
2232 // The actual reboot is not simulated.
2233 commandResult = MAV_RESULT_ACCEPTED;
2234 break;
2235 case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
2236 commandResult = MAV_RESULT_ACCEPTED;
2237 _respondWithAutopilotVersion();
2238 break;
2239 case MAV_CMD_REQUEST_MESSAGE:
2240 {
2241 bool accepted = false;
2242 bool noAck = false;
2243 _handleRequestMessage(request, accepted, noAck);
2244 if (noAck) {
2245 // FailRequestMessageCommandNoResponse: don't send any ack, let vehicle timeout
2246 return;
2247 }
2248 if (accepted) {
2249 commandResult = MAV_RESULT_ACCEPTED;
2250 }
2251 break;
2252 }
2253 case MAV_CMD_NAV_TAKEOFF:
2254 _handleTakeoff(request);
2255 commandResult = MAV_RESULT_ACCEPTED;
2256 break;
2257 case MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED:
2258 // Test command which always returns MAV_RESULT_ACCEPTED
2259 commandResult = MAV_RESULT_ACCEPTED;
2260 break;
2261 case MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED:
2262 // Test command which always returns MAV_RESULT_FAILED
2263 commandResult = MAV_RESULT_FAILED;
2264 break;
2265 case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED:
2266 // Test command which does not respond to first request and returns MAV_RESULT_ACCEPTED on second attempt
2267 if (firstCmdUser3) {
2268 firstCmdUser3 = false;
2269 return;
2270 } else {
2271 firstCmdUser3 = true;
2272 commandResult = MAV_RESULT_ACCEPTED;
2273 }
2274 break;
2275 case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED:
2276 // Test command which does not respond to first request and returns MAV_RESULT_FAILED on second attempt
2277 if (firstCmdUser4) {
2278 firstCmdUser4 = false;
2279 return;
2280 } else {
2281 firstCmdUser4 = true;
2282 commandResult = MAV_RESULT_FAILED;
2283 }
2284 break;
2285 case MAV_CMD_MOCKLINK_NO_RESPONSE:
2286 case MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY:
2287 // Test command which never responds
2288 return;
2289 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED:
2290 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED:
2291 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK:
2292 _handleInProgressCommandLong(request);
2293 return;
2294 case MAV_CMD_SET_MESSAGE_INTERVAL:
2295 {
2296 bool accepted = false;
2297
2298 _handleCommandLongSetMessageInterval(request, accepted);
2299 if (accepted) {
2300 commandResult = MAV_RESULT_ACCEPTED;
2301 }
2302 break;
2303 }
2304 }
2305
2306 mavlink_message_t commandAck{};
2307 (void) mavlink_msg_command_ack_pack_chan(
2308 _vehicleSystemId,
2309 _vehicleComponentId,
2310 _outgoingMavlinkChannel,
2311 &commandAck,
2312 request.command,
2313 commandResult,
2314 0, // progress
2315 0, // result_param2
2316 0, // target_system
2317 0 // target_component
2318 );
2319 respondWithMavlinkMessage(commandAck);
2320}
2321
2322void MockLink::_handleCommandInt(const mavlink_message_t &msg)
2323{
2324 mavlink_command_int_t request{};
2325 mavlink_msg_command_int_decode(&msg, &request);
2326
2327 // Unrecognized commands are reported as unsupported (mirroring the COMMAND_LONG default).
2328 // This lets unit tests exercise "try command, fall back to legacy message" code paths
2329 // such as Vehicle::setEstimatorOrigin.
2330 uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
2331
2332 switch (request.command) {
2333 case MAV_CMD_DO_SET_ROI_LOCATION:
2334 // Unit test support: accept ROI commands so tests can verify Vehicle::guidedModeROI
2335 commandResult = MAV_RESULT_ACCEPTED;
2336 break;
2337 case MAV_CMD_DO_SET_MODE:
2338 // Preserve the armed flag like real firmware does
2339 _mavBaseMode = (_mavBaseMode & MAV_MODE_FLAG_SAFETY_ARMED) | static_cast<uint8_t>(request.param1);
2340 _mavCustomMode = static_cast<uint32_t>(request.param2);
2341 commandResult = MAV_RESULT_ACCEPTED;
2342 break;
2343 case MAV_CMD_MISSION_START:
2344 commandResult = MAV_RESULT_ACCEPTED;
2345 break;
2346 default:
2347 break;
2348 }
2349
2350 mavlink_message_t commandAck{};
2351 (void) mavlink_msg_command_ack_pack_chan(
2352 _vehicleSystemId,
2353 _vehicleComponentId,
2354 _outgoingMavlinkChannel,
2355 &commandAck,
2356 request.command,
2357 commandResult,
2358 0, // progress
2359 0, // result_param2
2360 0, // target_system
2361 0 // target_component
2362 );
2363 respondWithMavlinkMessage(commandAck);
2364}
2365
2366void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult)
2367{
2368 mavlink_message_t commandAck{};
2369 (void) mavlink_msg_command_ack_pack_chan(
2370 _vehicleSystemId,
2371 _vehicleComponentId,
2372 _outgoingMavlinkChannel,
2373 &commandAck,
2374 command,
2375 ackResult,
2376 0, // progress
2377 0, // result_param2
2378 0, // target_system
2379 0 // target_component
2380 );
2381 respondWithMavlinkMessage(commandAck);
2382}
2383
2384void MockLink::_respondWithAutopilotVersion()
2385{
2386 union FlightVersion {
2387 uint32_t raw;
2388
2389 struct {
2390 uint8_t type; // bits 0–7
2391 uint8_t patch; // bits 8–15
2392 uint8_t minor; // bits 16–23
2393 uint8_t major; // bits 24–31
2394 } parts;
2395
2396 FlightVersion(uint32_t version = 0) : raw(version) {}
2397 };
2398 FlightVersion flightVersion;
2399
2400 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
2401 flightVersion.parts.major = 4;
2402 flightVersion.parts.minor = 7;
2403 flightVersion.parts.patch = 0;
2404 flightVersion.parts.type = FIRMWARE_VERSION_TYPE_OFFICIAL;
2405 } else if (_firmwareType == MAV_AUTOPILOT_PX4) {
2406 flightVersion.parts.major = 1;
2407 flightVersion.parts.minor = 17;
2408 flightVersion.parts.patch = 0;
2409 flightVersion.parts.type = FIRMWARE_VERSION_TYPE_OFFICIAL;
2410 }
2411
2412 const uint8_t customVersion[8]{};
2413 const uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | MAV_PROTOCOL_CAPABILITY_MISSION_INT
2414 | MAV_PROTOCOL_CAPABILITY_COMMAND_INT // matches modern PX4/ArduPilot so tests exercise the preferred COMMAND_INT path
2415 | ((_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0)
2416 | (_ftpCapability ? MAV_PROTOCOL_CAPABILITY_FTP : 0);
2417
2418 mavlink_message_t msg{};
2419 (void) mavlink_msg_autopilot_version_pack_chan(
2420 _vehicleSystemId,
2421 _vehicleComponentId,
2422 _outgoingMavlinkChannel,
2423 &msg,
2424 capabilities,
2425 flightVersion.raw, // flight_sw_version,
2426 0, // middleware_sw_version,
2427 0, // os_sw_version,
2428 0, // board_version,
2429 reinterpret_cast<const uint8_t*>(&customVersion), // flight_custom_version,
2430 reinterpret_cast<const uint8_t*>(&customVersion), // middleware_custom_version,
2431 reinterpret_cast<const uint8_t*>(&customVersion), // os_custom_version,
2432 _boardVendorId,
2433 _boardProductId,
2434 0, // uid
2435 0 // uid2
2436 );
2437 respondWithMavlinkMessage(msg);
2438}
2439
2440void MockLink::_sendHomePosition()
2441{
2442 const float bogus[4]{};
2443
2444 mavlink_message_t msg{};
2445 (void) mavlink_msg_home_position_pack_chan(
2446 _vehicleSystemId,
2447 _vehicleComponentId,
2448 _outgoingMavlinkChannel,
2449 &msg,
2450 static_cast<int32_t>(_vehicleLatitude * 1E7),
2451 static_cast<int32_t>(_vehicleLongitude * 1E7),
2452 static_cast<int32_t>(_defaultVehicleHomeAltitude * 1000),
2453 0.0f, 0.0f, 0.0f,
2454 &bogus[0],
2455 0.0f, 0.0f, 0.0f,
2456 0
2457 );
2458 respondWithMavlinkMessage(msg);
2459}
2460
2461void MockLink::_sendGpsRawInt()
2462{
2463 static uint64_t timeTick = 0;
2464
2465 mavlink_message_t msg{};
2466 (void) mavlink_msg_gps_raw_int_pack_chan(
2467 _vehicleSystemId,
2468 _vehicleComponentId,
2469 _outgoingMavlinkChannel,
2470 &msg,
2471 timeTick++, // time since boot
2472 GPS_FIX_TYPE_3D_FIX,
2473 static_cast<int32_t>(_vehicleLatitude * 1E7),
2474 static_cast<int32_t>(_vehicleLongitude * 1E7),
2475 static_cast<int32_t>(_vehicleAltitudeAMSL * 1000),
2476 3 * 100, // hdop
2477 3 * 100, // vdop
2478 UINT16_MAX, // velocity not known
2479 UINT16_MAX, // course over ground not known
2480 8, // satellites visible
2481 //-- Extension
2482 0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up).
2483 0, // Position uncertainty in meters * 1000 (positive for up).
2484 0, // Altitude uncertainty in meters * 1000 (positive for up).
2485 0, // Speed uncertainty in meters * 1000 (positive for up).
2486 0, // Heading / track uncertainty in degrees * 1e5.
2487 65535 // Yaw not provided
2488 );
2489 respondWithMavlinkMessage(msg);
2490}
2491
2492void MockLink::_sendGlobalPositionInt()
2493{
2494 static uint64_t timeTick = 0;
2495
2496 mavlink_message_t msg{};
2497 (void) mavlink_msg_global_position_int_pack_chan(
2498 _vehicleSystemId,
2499 _vehicleComponentId,
2500 _outgoingMavlinkChannel,
2501 &msg,
2502 timeTick++, // time since boot
2503 static_cast<int32_t>(_vehicleLatitude * 1E7),
2504 static_cast<int32_t>(_vehicleLongitude * 1E7),
2505 static_cast<int32_t>(_vehicleAltitudeAMSL * 1000),
2506 static_cast<int32_t>((_vehicleAltitudeAMSL - _defaultVehicleHomeAltitude) * 1000),
2507 0, 0, 0, // no speed sent
2508 UINT16_MAX // no heading sent
2509 );
2510 respondWithMavlinkMessage(msg);
2511}
2512
2513void MockLink::_sendAttitudeQuaternion()
2514{
2515 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
2516 const float t = timeBootMs / 1000.0f;
2517
2518 AttitudeOverride override;
2519 {
2520 QMutexLocker locker(&_attitudeOverrideMutex);
2521 override = _attitudeOverride;
2522 }
2523 const bool overridden = override.enabled;
2524
2525 // Synthesize sinusoidal Euler angles (rad) unless a test pinned the attitude
2526 const float roll = overridden ? override.rollRad : 0.20f * std::sin(2.0f * static_cast<float>(M_PI) * 0.50f * t);
2527 const float pitch = overridden ? override.pitchRad : 0.10f * std::sin(2.0f * static_cast<float>(M_PI) * 0.40f * t);
2528 const float yaw = overridden ? override.yawRad : 0.30f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2529
2530 // ZYX Euler → quaternion
2531 const float cr = std::cos(roll / 2.0f), sr = std::sin(roll / 2.0f);
2532 const float cp = std::cos(pitch / 2.0f), sp = std::sin(pitch / 2.0f);
2533 const float cy = std::cos(yaw / 2.0f), sy = std::sin(yaw / 2.0f);
2534 const float q1 = cr * cp * cy + sr * sp * sy; // w
2535 const float q2 = sr * cp * cy - cr * sp * sy; // x
2536 const float q3 = cr * sp * cy + sr * cp * sy; // y
2537 const float q4 = cr * cp * sy - sr * sp * cy; // z
2538
2539 // Body rates = time-derivatives of the Euler angles
2540 const float rollspeed = overridden ? 0.0f : 0.20f * (2.0f * static_cast<float>(M_PI) * 0.50f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.50f * t);
2541 const float pitchspeed = overridden ? 0.0f : 0.10f * (2.0f * static_cast<float>(M_PI) * 0.40f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.40f * t);
2542 const float yawspeed = overridden ? 0.0f : 0.30f * (2.0f * static_cast<float>(M_PI) * 0.10f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2543
2544 const float reprOffsetQ[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // identity
2545
2546 mavlink_message_t msg{};
2547 (void) mavlink_msg_attitude_quaternion_pack_chan(
2548 _vehicleSystemId,
2549 _vehicleComponentId,
2550 _outgoingMavlinkChannel,
2551 &msg,
2552 timeBootMs,
2553 q1, q2, q3, q4,
2554 rollspeed, pitchspeed, yawspeed,
2555 reprOffsetQ
2556 );
2557 respondWithMavlinkMessage(msg);
2558}
2559
2560void MockLink::_sendAttitudeTarget()
2561{
2562 // Setpoint: same shape as actual, phase-shifted by +0.3 rad
2563 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
2564 const float t = timeBootMs / 1000.0f;
2565 static constexpr float kPhase = 0.3f; // rad
2566
2567 const float roll = 0.20f * std::sin(2.0f * static_cast<float>(M_PI) * 0.50f * t + kPhase);
2568 const float pitch = 0.10f * std::sin(2.0f * static_cast<float>(M_PI) * 0.40f * t + kPhase);
2569 const float yaw = 0.30f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t + kPhase);
2570
2571 const float cr = std::cos(roll / 2.0f), sr = std::sin(roll / 2.0f);
2572 const float cp = std::cos(pitch / 2.0f), sp = std::sin(pitch / 2.0f);
2573 const float cy = std::cos(yaw / 2.0f), sy = std::sin(yaw / 2.0f);
2574 const float qSp[4] = {
2575 cr * cp * cy + sr * sp * sy,
2576 sr * cp * cy - cr * sp * sy,
2577 cr * sp * cy + sr * cp * sy,
2578 cr * cp * sy - sr * sp * cy,
2579 };
2580
2581 const float bodyRollRate = 0.20f * (2.0f * static_cast<float>(M_PI) * 0.50f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.50f * t + kPhase);
2582 const float bodyPitchRate = 0.10f * (2.0f * static_cast<float>(M_PI) * 0.40f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.40f * t + kPhase);
2583 const float bodyYawRate = 0.30f * (2.0f * static_cast<float>(M_PI) * 0.10f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.10f * t + kPhase);
2584
2585 mavlink_message_t msg{};
2586 (void) mavlink_msg_attitude_target_pack_chan(
2587 _vehicleSystemId,
2588 _vehicleComponentId,
2589 _outgoingMavlinkChannel,
2590 &msg,
2591 timeBootMs,
2592 0, // type_mask: all fields valid
2593 qSp,
2594 bodyRollRate, bodyPitchRate, bodyYawRate,
2595 0.5f // thrust
2596 );
2597 respondWithMavlinkMessage(msg);
2598}
2599
2600void MockLink::_sendLocalPositionNed()
2601{
2602 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
2603 const float t = timeBootMs / 1000.0f;
2604
2605 const float x = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.08f * t);
2606 const float y = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2607 const float z = -10.0f + 1.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.15f * t); // negative = up in NED
2608 const float vx = 5.0f * (2.0f * static_cast<float>(M_PI) * 0.08f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.08f * t);
2609 const float vy = 5.0f * (2.0f * static_cast<float>(M_PI) * 0.10f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2610 const float vz = 1.0f * (2.0f * static_cast<float>(M_PI) * 0.15f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.15f * t);
2611
2612 mavlink_message_t msg{};
2613 (void) mavlink_msg_local_position_ned_pack_chan(
2614 _vehicleSystemId,
2615 _vehicleComponentId,
2616 _outgoingMavlinkChannel,
2617 &msg,
2618 timeBootMs,
2619 x, y, z,
2620 vx, vy, vz
2621 );
2622 respondWithMavlinkMessage(msg);
2623}
2624
2625void MockLink::_sendPositionTargetLocalNed()
2626{
2627 // Setpoint: same shape as _sendLocalPositionNed, with a 0.5 s time offset (not a phase in rad)
2628 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
2629 const float t = timeBootMs / 1000.0f + 0.5f; // +0.5 s time lead
2630
2631 const float x = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.08f * t);
2632 const float y = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2633 const float z = -10.0f + 1.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.15f * t);
2634 const float vx = 5.0f * (2.0f * static_cast<float>(M_PI) * 0.08f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.08f * t);
2635 const float vy = 5.0f * (2.0f * static_cast<float>(M_PI) * 0.10f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2636 const float vz = 1.0f * (2.0f * static_cast<float>(M_PI) * 0.15f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.15f * t);
2637
2638 mavlink_message_t msg{};
2639 (void) mavlink_msg_position_target_local_ned_pack_chan(
2640 _vehicleSystemId,
2641 _vehicleComponentId,
2642 _outgoingMavlinkChannel,
2643 &msg,
2644 timeBootMs,
2645 MAV_FRAME_LOCAL_NED,
2646 0, // type_mask: all fields valid
2647 x, y, z,
2648 vx, vy, vz,
2649 0.0f, 0.0f, 0.0f, // acceleration not used
2650 0.0f, 0.0f // yaw, yaw_rate not used
2651 );
2652 respondWithMavlinkMessage(msg);
2653}
2654
2655void MockLink::_sendExtendedSysState()
2656{
2657 mavlink_message_t msg{};
2658 (void) mavlink_msg_extended_sys_state_pack_chan(
2659 _vehicleSystemId,
2660 _vehicleComponentId,
2661 _outgoingMavlinkChannel,
2662 &msg,
2663 MAV_VTOL_STATE_UNDEFINED,
2664 (_vehicleAltitudeAMSL > _defaultVehicleHomeAltitude) ? MAV_LANDED_STATE_IN_AIR : MAV_LANDED_STATE_ON_GROUND
2665 );
2666 respondWithMavlinkMessage(msg);
2667}
2668
2669void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks)
2670{
2671 constexpr int cChunks = 4;
2672
2673 int num = 0;
2674 for (int i = 0; i < cChunks; i++) {
2675 if (missingChunks && (i & 1)) {
2676 continue;
2677 }
2678
2679 int cBuf = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
2680 char msgBuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]{};
2681
2682 if (i == cChunks - 1) {
2683 // Last chunk is partial
2684 cBuf /= 2;
2685 }
2686
2687 for (int j = 0; j < cBuf - 1; j++) {
2688 msgBuf[j] = '0' + num++;
2689 if (num > 9) {
2690 num = 0;
2691 }
2692 }
2693 msgBuf[cBuf-1] = 'A' + i;
2694
2695 mavlink_message_t msg{};
2696 (void) mavlink_msg_statustext_pack_chan(
2697 _vehicleSystemId,
2698 _vehicleComponentId,
2699 _outgoingMavlinkChannel,
2700 &msg,
2701 MAV_SEVERITY_INFO,
2702 msgBuf,
2703 chunkId,
2704 i // chunk sequence number
2705 );
2706 respondWithMavlinkMessage(msg);
2707 }
2708}
2709
2710void MockLink::_sendStatusTextMessages()
2711{
2712 struct StatusMessage {
2713 MAV_SEVERITY severity;
2714 const char *msg;
2715 };
2716
2717 static constexpr struct StatusMessage rgMessages[] = {
2718 { MAV_SEVERITY_INFO, "#Testing audio output" },
2719 { MAV_SEVERITY_EMERGENCY, "Status text emergency" },
2720 { MAV_SEVERITY_ALERT, "Status text alert" },
2721 { MAV_SEVERITY_CRITICAL, "Status text critical" },
2722 { MAV_SEVERITY_ERROR, "Status text error" },
2723 { MAV_SEVERITY_WARNING, "Status text warning" },
2724 { MAV_SEVERITY_NOTICE, "Status text notice" },
2725 { MAV_SEVERITY_INFO, "Status text info" },
2726 { MAV_SEVERITY_DEBUG, "Status text debug" },
2727 };
2728
2729 mavlink_message_t msg{};
2730 for (size_t i = 0; i < std::size(rgMessages); i++) {
2731 const struct StatusMessage *status = &rgMessages[i];
2732 char statusText[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {};
2733 (void) std::strncpy(statusText, status->msg, sizeof(statusText) - 1);
2734
2735 (void) mavlink_msg_statustext_pack_chan(
2736 _vehicleSystemId,
2737 _vehicleComponentId,
2738 _outgoingMavlinkChannel,
2739 &msg,
2740 status->severity,
2741 statusText,
2742 0, // Not a chunked sequence
2743 0 // Not a chunked sequence
2744 );
2745 respondWithMavlinkMessage(msg);
2746 }
2747
2748 _sendChunkedStatusText(1, false /* missingChunks */);
2749 _sendChunkedStatusText(2, true /* missingChunks */);
2750 _sendChunkedStatusText(3, false /* missingChunks */); // This should cause the previous incomplete chunk to spit out
2751 _sendChunkedStatusText(4, true /* missingChunks */); // This should cause the timeout to fire
2752}
2753
2754MockLink *MockLink::_startMockLink(MockConfiguration *mockConfig)
2755{
2756 mockConfig->setDynamic(true);
2757 SharedLinkConfigurationPtr config = LinkManager::instance()->addConfiguration(mockConfig);
2758
2759 if (LinkManager::instance()->createConnectedLink(config)) {
2760 return qobject_cast<MockLink*>(config->link());
2761 }
2762
2763 return nullptr;
2764}
2765
2766MockLink *MockLink::_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode, MockConfiguration::VideoStreamType videoStreamType)
2767{
2768 MockConfiguration *const mockConfig = new MockConfiguration(configName);
2769
2770 mockConfig->setFirmwareType(firmwareType);
2771 mockConfig->setVehicleType(vehicleType);
2772 mockConfig->setSendStatusText(options.testFlag(MockConfiguration::OptionSendStatusText));
2773 mockConfig->setEnableCamera(options.testFlag(MockConfiguration::OptionEnableCamera));
2774 mockConfig->setEnableGimbal(options.testFlag(MockConfiguration::OptionEnableGimbal));
2775 mockConfig->setEnableProximity(options.testFlag(MockConfiguration::OptionEnableProximity));
2776 mockConfig->setPreloadMission(options.testFlag(MockConfiguration::OptionPreloadMission));
2777 mockConfig->setStayMavlinkV1(options.testFlag(MockConfiguration::OptionStayMavlinkV1));
2778 mockConfig->setApmStartFreshParams(options.testFlag(MockConfiguration::OptionAPMStartFreshParams));
2779 mockConfig->setFtpCapability(options.testFlag(MockConfiguration::OptionFtpCapability));
2780 mockConfig->setSendRadioStatus(!options.testFlag(MockConfiguration::OptionNoRadioStatus));
2781 mockConfig->setVideoStreamType(videoStreamType);
2782 mockConfig->setFailureMode(failureMode);
2783
2784 return _startMockLink(mockConfig);
2785}
2786
2787MockLink *MockLink::startPX4MockLink(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode, MockConfiguration::VideoStreamType videoStreamType)
2788{
2789 return _startMockLinkWorker(QStringLiteral("PX4 MultiRotor MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, options, failureMode, videoStreamType);
2790}
2791
2792MockLink *MockLink::startPX4MockLinkWithMission(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode)
2793{
2794 return _startMockLinkWorker(QStringLiteral("PX4 MultiRotor MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, options | MockConfiguration::OptionPreloadMission, failureMode);
2795}
2796
2797MockLink *MockLink::startGenericMockLink(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode, MockConfiguration::VideoStreamType videoStreamType)
2798{
2799 return _startMockLinkWorker(QStringLiteral("Generic MockLink"), MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, options, failureMode, videoStreamType);
2800}
2801
2802MockLink *MockLink::startNoInitialConnectMockLink(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode)
2803{
2804 return _startMockLinkWorker(QStringLiteral("No Initial Connect MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, options, failureMode);
2805}
2806
2807MockLink *MockLink::startAPMArduCopterMockLink(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode, MockConfiguration::VideoStreamType videoStreamType)
2808{
2809 return _startMockLinkWorker(QStringLiteral("ArduCopter MockLink"),MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, options, failureMode, videoStreamType);
2810}
2811
2812MockLink *MockLink::startAPMArduPlaneMockLink(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode, MockConfiguration::VideoStreamType videoStreamType)
2813{
2814 return _startMockLinkWorker(QStringLiteral("ArduPlane MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, options, failureMode, videoStreamType);
2815}
2816
2817MockLink *MockLink::startAPMArduSubMockLink(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode, MockConfiguration::VideoStreamType videoStreamType)
2818{
2819 return _startMockLinkWorker(QStringLiteral("ArduSub MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, options, failureMode, videoStreamType);
2820}
2821
2822MockLink *MockLink::startAPMArduRoverMockLink(MockConfiguration::Options options, MockConfiguration::FailureMode_t failureMode, MockConfiguration::VideoStreamType videoStreamType)
2823{
2824 return _startMockLinkWorker(QStringLiteral("ArduRover MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, options, failureMode, videoStreamType);
2825}
2826
2827void MockLink::_sendRCChannels()
2828{
2829 mavlink_message_t msg{};
2830 (void) mavlink_msg_rc_channels_pack_chan(
2831 _vehicleSystemId,
2832 _vehicleComponentId,
2833 _outgoingMavlinkChannel,
2834 &msg,
2835 0, // time_boot_ms
2836 16, // chancount
2837 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 1-8
2838 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 9-16
2839 UINT16_MAX, UINT16_MAX, // channel 17/18 unused
2840 0 // rssi
2841 );
2842 respondWithMavlinkMessage(msg);
2843}
2844
2845void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request)
2846{
2847 if ((request.param1 == 0) && (request.param2 == 0) && (request.param3 == 0) &&
2848 (request.param4 == 0) && (request.param5 == 0) && (request.param6 == 0) &&
2849 (request.param7 == 0)) {
2850 // All zeros is a calibration cancel request.
2851 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
2852 // Send ACCELCAL_VEHICLE_POS_FAILED so controller calls _stopCalibration(Failed)
2853 QMutexLocker locker(&_apmAccelCalMutex);
2854 if (_apmAccelCalPosIndex >= 0) {
2855 mavlink_message_t msg{};
2856 mavlink_command_long_t cmd{};
2857 cmd.target_system = 255;
2858 cmd.target_component = MAV_COMP_ID_MISSIONPLANNER;
2859 cmd.command = MAV_CMD_ACCELCAL_VEHICLE_POS;
2860 cmd.param1 = static_cast<float>(ACCELCAL_VEHICLE_POS_FAILED);
2861 (void) mavlink_msg_command_long_encode_chan(
2862 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &cmd);
2863 respondWithMavlinkMessage(msg);
2864 _apmAccelCalPosIndex = -1;
2865 }
2866 } else {
2867 // PX4: See PX4 calibrate_cancel_check().
2868 (void) _mockLinkPX4Calibration->cancel();
2869 }
2870 return;
2871 }
2872
2873 if (request.param2 == 1) {
2874 // Magnetometer calibration runs the full pose-driven simulation
2875 _mockLinkPX4Calibration->startMagCalibration();
2876 return;
2877 }
2878
2879 if (request.param1 == 1) {
2880 sendStatusTextMessage(MAV_SEVERITY_INFO, QStringLiteral("[cal] calibration started: 2 gyro"));
2881 return;
2882 }
2883
2884 if (request.param5 == 1) {
2885 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
2886 // APM full accelerometer calibration: drive the ACCELCAL_VEHICLE_POS handshake
2887 QMutexLocker locker(&_apmAccelCalMutex);
2888 _apmAccelCalPosIndex = 0;
2889 _apmAccelCalGotAck = false;
2890 _apmAccelCalTickCount = 0;
2891 } else {
2892 // Accelerometer calibration runs the full pose-driven simulation
2893 _mockLinkPX4Calibration->startAccelCalibration();
2894 }
2895 }
2896}
2897
2898void MockLink::sendStatusTextMessage(uint8_t severity, const QString &text)
2899{
2900 char statusText[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {};
2901 (void) std::strncpy(statusText, text.toUtf8().constData(), sizeof(statusText) - 1);
2902
2903 mavlink_message_t msg{};
2904 (void) mavlink_msg_statustext_pack_chan(
2905 _vehicleSystemId,
2906 _vehicleComponentId,
2907 _outgoingMavlinkChannel,
2908 &msg,
2909 severity,
2910 statusText,
2911 0,
2912 0 // Not chunked
2913 );
2914 respondWithMavlinkMessage(msg);
2915}
2916
2917void MockLink::_handleTakeoff(const mavlink_command_long_t &request)
2918{
2919 _vehicleAltitudeAMSL = request.param7 + _defaultVehicleHomeAltitude;
2920 _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
2921}
2922
2923void MockLink::_handleLogRequestList(const mavlink_message_t &msg)
2924{
2925 mavlink_log_request_list_t request{};
2926 mavlink_msg_log_request_list_decode(&msg, &request);
2927
2928 if ((request.start != 0) && (request.end != 0xffff)) {
2929 qCWarning(MockLinkLog) << "_handleLogRequestList cannot handle partial requests";
2930 return;
2931 }
2932
2933 // When simulated FTP log files are set, LOG_ENTRY responses describe the same logs so
2934 // both transports report a consistent log list (matching PX4 behavior).
2935 const QList<MockLinkFTP::LogFile> logFiles = _mockLinkFTP->logFiles();
2936 if (!_logsErased && !logFiles.isEmpty()) {
2937 const uint16_t numLogs = static_cast<uint16_t>(logFiles.count());
2938 for (uint16_t id = 0; id < numLogs; id++) {
2939 mavlink_message_t responseMsg{};
2940 (void) mavlink_msg_log_entry_pack_chan(
2941 _vehicleSystemId,
2942 _vehicleComponentId,
2943 _outgoingMavlinkChannel,
2944 &responseMsg,
2945 id, // log id
2946 numLogs, // num_logs
2947 numLogs - 1, // last_log_num
2948 logFiles[id].mtime, // time_utc
2949 static_cast<uint32_t>(logFiles[id].size) // size
2950 );
2951 respondWithMavlinkMessage(responseMsg);
2952 }
2953 return;
2954 }
2955
2956 const uint16_t numLogs = _logsErased ? 0 : 1;
2957 const uint16_t logId = _logsErased ? 0 : _logDownloadLogId;
2958 const uint32_t logSize = _logsErased ? 0 : _logDownloadFileSize;
2959 mavlink_message_t responseMsg{};
2960 mavlink_msg_log_entry_pack_chan(
2961 _vehicleSystemId,
2962 _vehicleComponentId,
2963 _outgoingMavlinkChannel,
2964 &responseMsg,
2965 logId, // log id
2966 numLogs, // num_logs
2967 numLogs, // last_log_num
2968 0, // time_utc
2969 logSize // size
2970 );
2971 respondWithMavlinkMessage(responseMsg);
2972}
2973
2974void MockLink::_handleLogErase(const mavlink_message_t &msg)
2975{
2976 mavlink_log_erase_t request{};
2977 mavlink_msg_log_erase_decode(&msg, &request);
2978
2979 if ((request.target_system != _vehicleSystemId) || (request.target_component != _vehicleComponentId)) {
2980 return;
2981 }
2982
2983 _logsErased = true;
2984 _mockLinkFTP->setLogFiles({});
2985}
2986
2987QString MockLink::_createRandomFile(uint32_t byteCount)
2988{
2989 QTemporaryFile tempFile;
2990 tempFile.setAutoRemove(false);
2991 if (!tempFile.open()) {
2992 qCWarning(MockLinkLog) << "MockLink::createRandomFile open failed" << tempFile.errorString();
2993 return QString();
2994 }
2995
2996 for (uint32_t bytesWritten = 0; bytesWritten < byteCount; bytesWritten++) {
2997 const unsigned char byte = (QRandomGenerator::global()->generate() * 0xFF) / RAND_MAX;
2998 (void) tempFile.write(reinterpret_cast<const char*>(&byte), 1);
2999 }
3000
3001 tempFile.close();
3002 return tempFile.fileName();
3003}
3004
3005QString MockLink::_createLogContentsFile(const QString &logName)
3006{
3007 QTemporaryFile tempFile;
3008 tempFile.setAutoRemove(false);
3009 if (!tempFile.open()) {
3010 qCWarning(MockLinkLog) << "_createLogContentsFile open failed" << tempFile.errorString();
3011 return QString();
3012 }
3013 (void) tempFile.write(_mockLinkFTP->logFileContents(logName));
3014 tempFile.close();
3015 return tempFile.fileName();
3016}
3017
3018void MockLink::_handleLogRequestData(const mavlink_message_t &msg)
3019{
3020 mavlink_log_request_data_t request{};
3021 mavlink_msg_log_request_data_decode(&msg, &request);
3022
3023 // Serialize with _logDownloadWorker which reads this state every 2ms on the worker thread
3024 QMutexLocker locker(&_logDownloadMutex);
3025
3026 const QList<MockLinkFTP::LogFile> logFiles = _logsErased ? QList<MockLinkFTP::LogFile>() : _mockLinkFTP->logFiles();
3027 if (!logFiles.isEmpty()) {
3028 // Serve the simulated FTP log files so LOG_ENTRY/LOG_REQUEST_DATA stay consistent with the FTP transport
3029 if (request.id >= logFiles.count()) {
3030 qCWarning(MockLinkLog) << "_handleLogRequestData id out of range:" << request.id;
3031 return;
3032 }
3033 if (_logDownloadFilename.isEmpty() || (_logDownloadId != request.id)) {
3034 _logDownloadFilename = _createLogContentsFile(logFiles[request.id].name);
3035 _logDownloadId = request.id;
3036 _logDownloadSize = static_cast<uint32_t>(logFiles[request.id].size);
3037 }
3038 } else {
3039#ifdef QGC_UNITTEST_BUILD
3040 if (_logDownloadFilename.isEmpty()) {
3041 _logDownloadFilename = _createRandomFile(_logDownloadFileSize);
3042 }
3043#endif
3044 if (request.id != _logDownloadLogId) {
3045 qCWarning(MockLinkLog) << "_handleLogRequestData id must be" << _logDownloadLogId;
3046 return;
3047 }
3048 _logDownloadId = _logDownloadLogId;
3049 _logDownloadSize = _logDownloadFileSize;
3050 }
3051
3052 if (request.ofs > (_logDownloadSize - 1)) {
3053 qCWarning(MockLinkLog) << "_handleLogRequestData offset past end of file request.ofs:size" << request.ofs << _logDownloadSize;
3054 return;
3055 }
3056
3057 // This will trigger _logDownloadWorker to send data
3058 _logDownloadCurrentOffset = request.ofs;
3059 if (request.ofs + request.count > _logDownloadSize) {
3060 request.count = _logDownloadSize - request.ofs;
3061 }
3062 _logDownloadBytesRemaining = request.count;
3063}
3064
3065void MockLink::_logDownloadWorker()
3066{
3067 // Runs every 2ms (500Hz on worker thread). Must protect shared state modified by main thread.
3068 // Without lock: main could write new offset/count while we're reading, causing corrupted downloads.
3069 QMutexLocker locker(&_logDownloadMutex);
3070 if (_logDownloadBytesRemaining == 0) {
3071 return;
3072 }
3073
3074 QFile file(_logDownloadFilename);
3075 if (!file.open(QIODevice::ReadOnly)) {
3076 qCWarning(MockLinkLog) << "_logDownloadWorker open failed" << file.errorString();
3077 _logDownloadBytesRemaining = 0; // Abort transfer on I/O failure
3078 return;
3079 }
3080
3081 uint8_t buffer[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN]{};
3082
3083 const qint64 bytesToRead = qMin(_logDownloadBytesRemaining, (uint32_t)MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);
3084 if (!file.seek(_logDownloadCurrentOffset)) {
3085 qCWarning(MockLinkLog) << "_logDownloadWorker seek failed - offset:" << _logDownloadCurrentOffset << file.errorString();
3086 _logDownloadBytesRemaining = 0; // Abort transfer on I/O failure
3087 return;
3088 }
3089 if (file.read(reinterpret_cast<char*>(buffer), bytesToRead) != bytesToRead) {
3090 qCWarning(MockLinkLog) << "_logDownloadWorker read failed - bytesToRead:" << bytesToRead << file.errorString();
3091 _logDownloadBytesRemaining = 0; // Abort transfer on I/O failure
3092 return;
3093 }
3094
3095 qCDebug(MockLinkLog) << "_logDownloadWorker" << _logDownloadCurrentOffset << _logDownloadBytesRemaining;
3096
3097 mavlink_message_t responseMsg{};
3098 (void) mavlink_msg_log_data_pack_chan(
3099 _vehicleSystemId,
3100 _vehicleComponentId,
3101 _outgoingMavlinkChannel,
3102 &responseMsg,
3103 _logDownloadId,
3104 _logDownloadCurrentOffset,
3105 bytesToRead,
3106 &buffer[0]
3107 );
3108 respondWithMavlinkMessage(responseMsg);
3109
3110 _logDownloadCurrentOffset += bytesToRead;
3111 _logDownloadBytesRemaining -= bytesToRead;
3112
3113 file.close();
3114}
3115
3116void MockLink::_sendADSBVehicles()
3117{
3118 for (int i = 0; i < _adsbVehicles.size(); ++i) {
3119 // Slightly change the direction to simulate different paths
3120 _adsbVehicles[i].angle += (i + 1); // Vary the change to make each path unique
3121
3122 // Move each vehicle by a smaller distance to simulate slower speed
3123 _adsbVehicles[i].coordinate = _adsbVehicles[i].coordinate.atDistanceAndAzimuth(5, _adsbVehicles[i].angle); // 50 meters per update for slower speed
3124
3125 // Simulate slight variations in altitude
3126 _adsbVehicles[i].altitude += (i % 2 == 0 ? 0.5 : -0.5); // Increase or decrease altitude
3127
3128 QByteArray callsign = QString("N12345%1").arg(i, 2, 10, QChar('0')).toLatin1();
3129 callsign.resize(MAVLINK_MSG_ADSB_VEHICLE_FIELD_CALLSIGN_LEN);
3130
3131 // Prepare and send MAVLink message for each vehicle
3132 mavlink_message_t responseMsg{};
3133 (void) mavlink_msg_adsb_vehicle_pack_chan(
3134 _vehicleSystemId,
3135 _vehicleComponentId,
3136 _outgoingMavlinkChannel,
3137 &responseMsg,
3138 12345 + i, // Unique ICAO address for each vehicle
3139 _adsbVehicles[i].coordinate.latitude() * 1e7,
3140 _adsbVehicles[i].coordinate.longitude() * 1e7,
3141 ADSB_ALTITUDE_TYPE_GEOMETRIC,
3142 _adsbVehicles[i].altitude * 1000, // Altitude in millimeters
3143 // Use the current angle as heading
3144 static_cast<uint16_t>(_adsbVehicles[i].angle * 100), // Heading in centidegrees
3145 0, 0, // Horizontal/Vertical velocity
3146 callsign.constData(), // Unique callsign
3147 ADSB_EMITTER_TYPE_ROTOCRAFT,
3148 1, // Seconds since last communication
3149 ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED,
3150 0 // Squawk code
3151 );
3152 respondWithMavlinkMessage(responseMsg);
3153 }
3154}
3155
3156void MockLink::_moveADSBVehicle(int vehicleIndex)
3157{
3158 _adsbAngles[vehicleIndex] += 10; // Increment angle for smoother movement
3159 QGeoCoordinate &coord = _adsbVehicleCoordinates[vehicleIndex];
3160
3161 // Update the position based on the new angle
3162 coord = QGeoCoordinate(coord.latitude(), coord.longitude()).atDistanceAndAzimuth(500, _adsbAngles[vehicleIndex]);
3163 coord.setAltitude(100); // Keeping altitude constant for simplicity
3164}
3165
3166void MockLink::_handleRequestMessageAutopilotVersion(const mavlink_command_long_t &/*request*/, bool &accepted)
3167{
3168 accepted = true;
3169
3170 switch (_failureMode) {
3171 case MockConfiguration::FailNone:
3172 break;
3173 case MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionFailure:
3174 accepted = false;
3175 return;
3176 case MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionLost:
3177 accepted = true;
3178 return;
3179 default:
3180 break;
3181 }
3182
3183 _respondWithAutopilotVersion();
3184}
3185
3186void MockLink::_handleRequestMessageDebug(const mavlink_command_long_t &/*request*/, bool &accepted, bool &noAck)
3187{
3188 accepted = true;
3189 noAck = false;
3190
3191 switch (_requestMessageFailureMode) {
3192 case FailRequestMessageNone:
3193 break;
3194 case FailRequestMessageCommandAcceptedMsgNotSent:
3195 return;
3196 case FailRequestMessageCommandUnsupported:
3197 accepted = false;
3198 return;
3199 case FailRequestMessageCommandNoResponse:
3200 accepted = false;
3201 noAck = true;
3202 return;
3203 }
3204
3205 mavlink_message_t responseMsg{};
3206 (void) mavlink_msg_debug_pack_chan(
3207 _vehicleSystemId,
3208 _vehicleComponentId,
3209 _outgoingMavlinkChannel,
3210 &responseMsg,
3211 0, 0, 0
3212 );
3213 respondWithMavlinkMessage(responseMsg);
3214}
3215
3216void MockLink::_handleRequestMessageAvailableModes(const mavlink_command_long_t &request, bool &accepted)
3217{
3218 accepted = true;
3219
3220 if (request.param2 == 0) {
3221 // Request for available modes to be streamed out
3222 QMutexLocker locker(&_availableModesWorkerMutex);
3223 if (_availableModesWorkerNextModeIndex != 0) {
3224 qCWarning(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: _availableModesWorker already running - _availableModesWorkerNextModeIndex:" << _availableModesWorkerNextModeIndex;
3225 accepted = false;
3226 return;
3227 }
3228 qCDebug(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: starting available modes sequence worker";
3229 _availableModesWorkerNextModeIndex = 1; // Start with the first mode in sequence (1-based index)
3230 } else {
3231 // Request for specific mode
3232 if (request.param2 < 1 || request.param2 > _flightModeList().count()) {
3233 qCWarning(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: requested mode index out of range" << request.param2 << _flightModeList().count();
3234 accepted = false;
3235 return;
3236 }
3237 qCDebug(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: received specific mode request for index" << request.param2;
3238 // Match other single-message requests: do not ACK now and leave the payload waiting for a worker timer.
3239 _sendAvailableMode(static_cast<uint8_t>(request.param2));
3240 }
3241}
3242
3243void MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool &accepted, bool &noAck)
3244{
3245 accepted = false;
3246 noAck = false;
3247
3248 const uint32_t requestedMessageId = static_cast<uint32_t>(request.param1);
3249
3250 // Per-message-ID no-response injection: silently drop the request (no ACK, no message)
3251 {
3252 QMutexLocker locker(&_requestMessageNoResponseMutex);
3253 if (_requestMessageNoResponseIds.contains(requestedMessageId)) {
3254 noAck = true;
3255 return;
3256 }
3257 }
3258
3259 switch (static_cast<int>(request.param1)) {
3260 case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
3261 _handleRequestMessageAutopilotVersion(request, accepted);
3262 break;
3263 case MAVLINK_MSG_ID_COMPONENT_METADATA:
3264 if (_firmwareType == MAV_AUTOPILOT_PX4) {
3265 _sendGeneralMetaData();
3266 accepted = true;
3267 }
3268 break;
3269 case MAVLINK_MSG_ID_DEBUG:
3270 _handleRequestMessageDebug(request, accepted, noAck);
3271 break;
3272 case MAVLINK_MSG_ID_AVAILABLE_MODES:
3273 _handleRequestMessageAvailableModes(request, accepted);
3274 break;
3275 }
3276}
3277
3278void MockLink::_sendGeneralMetaData()
3279{
3280 static constexpr const char metaDataURI[MAVLINK_MSG_COMPONENT_METADATA_FIELD_URI_LEN] = "mftp://[;comp=1]general.json";
3281
3282 mavlink_message_t responseMsg{};
3283 (void) mavlink_msg_component_metadata_pack_chan(
3284 _vehicleSystemId,
3285 _vehicleComponentId,
3286 _outgoingMavlinkChannel,
3287 &responseMsg,
3288 0, // time_boot_ms
3289 25436021, // general_metadata_file_crc (crc32 of General.MetaData.json)
3290 metaDataURI
3291 );
3292 respondWithMavlinkMessage(responseMsg);
3293}
3294
3295void MockLink::setRemoteIDArmStatus(uint8_t status, const QString& error)
3296{
3297 QMutexLocker locker(&_remoteIDArmStatusMutex);
3298 _remoteIDArmStatus = status;
3299 _remoteIDArmStatusError = error;
3300}
3301
3302void MockLink::_sendRemoteIDArmStatus()
3303{
3304 uint8_t armStatus;
3305 QByteArray errorUtf8;
3306 {
3307 QMutexLocker locker(&_remoteIDArmStatusMutex);
3308 armStatus = _remoteIDArmStatus;
3309 errorUtf8 = _remoteIDArmStatusError.toUtf8();
3310 }
3311
3312 char armStatusError[MAVLINK_MSG_OPEN_DRONE_ID_ARM_STATUS_FIELD_ERROR_LEN] = {};
3313 std::strncpy(armStatusError, errorUtf8.constData(), sizeof(armStatusError) - 1);
3314
3315 mavlink_message_t msg{};
3316 (void) mavlink_msg_open_drone_id_arm_status_pack_chan(
3317 _vehicleSystemId,
3318 MAV_COMP_ID_ODID_TXRX_1,
3319 static_cast<uint8_t>(_outgoingMavlinkChannel),
3320 &msg,
3321 armStatus,
3322 armStatusError
3323 );
3324 respondWithMavlinkMessage(msg);
3325}
3326
3327void MockLink::_sendEscInfo()
3328{
3329 // Send ESC_INFO for 4 motors starting at index 0.
3330 // count=4 and info bitmask=0x0F (all 4 online) makes the ESC indicator visible.
3331 static const uint16_t failureFlags[4] = {0, 0, 0, 0};
3332 static const uint32_t errorCount[4] = {0, 0, 0, 0};
3333 static const int16_t temperature[4] = {3000, 3000, 3000, 3000}; // centidegrees
3334
3335 mavlink_message_t msg{};
3336 (void) mavlink_msg_esc_info_pack_chan(
3337 _vehicleSystemId,
3338 _vehicleComponentId,
3339 _outgoingMavlinkChannel,
3340 &msg,
3341 0, // index: first group starts at 0
3342 static_cast<uint64_t>(_runningTime.elapsed()) * 1000, // time_usec
3343 0, // counter
3344 4, // count: 4 motors
3345 ESC_CONNECTION_TYPE_DSHOT, // connection_type
3346 0x0F, // info: bitmask — motors 0-3 online
3347 failureFlags,
3348 errorCount,
3349 temperature
3350 );
3351 respondWithMavlinkMessage(msg);
3352}
3353
3354void MockLink::_sendEscStatus()
3355{
3356 static const int32_t rpm[4] = {5000, 5000, 5000, 5000};
3357 static const float voltage[4] = {16.0f, 16.0f, 16.0f, 16.0f};
3358 static const float current[4] = {5.0f, 5.0f, 5.0f, 5.0f};
3359
3360 mavlink_message_t msg{};
3361 (void) mavlink_msg_esc_status_pack_chan(
3362 _vehicleSystemId,
3363 _vehicleComponentId,
3364 _outgoingMavlinkChannel,
3365 &msg,
3366 0, // index: first group
3367 static_cast<uint64_t>(_runningTime.elapsed()) * 1000, // time_usec
3368 rpm,
3369 voltage,
3370 current
3371 );
3372 respondWithMavlinkMessage(msg);
3373}
3374
3375void MockLink::_sendRadioStatus()
3376{
3377 // Send a RADIO_STATUS message to make the TelemetryRSSI indicator visible.
3378 // Any non-zero rssi value triggers showIndicator (TelemetryRSSIIndicator checks lrssi.rawValue != 0).
3379 mavlink_message_t msg{};
3380 (void) mavlink_msg_radio_status_pack_chan(
3381 _vehicleSystemId,
3382 _vehicleComponentId,
3383 _outgoingMavlinkChannel,
3384 &msg,
3385 100, // rssi: local signal strength
3386 100, // remrssi: remote signal strength
3387 50, // txbuf: transmit buffer fill (%)
3388 10, // noise: local background noise
3389 10, // remnoise: remote background noise
3390 0, // rxerrors
3391 0 // fixed
3392 );
3393 respondWithMavlinkMessage(msg);
3394}
3395
3396void MockLink::simulateConnectionRemoved()
3397{
3398 _commLost = true;
3399 _connectionRemoved();
3400}
3401
3402MockLinkFTP *MockLink::mockLinkFTP() const
3403{
3404 return _mockLinkFTP;
3405}
3406
3407void MockLink::setVehicleAttitudeOverrideDeg(float rollDeg, float pitchDeg, float yawDeg)
3408{
3409 QMutexLocker locker(&_attitudeOverrideMutex);
3410 _attitudeOverride.enabled = true;
3411 _attitudeOverride.rollRad = qDegreesToRadians(rollDeg);
3412 _attitudeOverride.pitchRad = qDegreesToRadians(pitchDeg);
3413 _attitudeOverride.yawRad = qDegreesToRadians(yawDeg);
3414}
3415
3416void MockLink::clearVehicleAttitudeOverride()
3417{
3418 QMutexLocker locker(&_attitudeOverrideMutex);
3419 _attitudeOverride.enabled = false;
3420}
3421
3422void MockLink::_sendAvailableMode(uint8_t modeIndexOneBased)
3423{
3424 if (modeIndexOneBased < 1 || modeIndexOneBased > _availableModesCount()) {
3425 qCWarning(MockLinkLog) << "modeIndexOneBased out of range" << modeIndexOneBased << _availableModesCount();
3426 return;
3427 }
3428
3429 qCDebug(MockLinkLog) << "_sendAvailableMode modeIndexOneBased:" << modeIndexOneBased;
3430
3431 const FlightMode_t &availableMode = _flightModeList()[modeIndexOneBased - 1];
3432 char modeName[MAVLINK_MSG_AVAILABLE_MODES_FIELD_MODE_NAME_LEN] = {};
3433 std::strncpy(modeName, availableMode.name, sizeof(modeName) - 1);
3434
3435 mavlink_message_t msg{};
3436
3437 (void) mavlink_msg_available_modes_pack_chan(
3438 _vehicleSystemId,
3439 _vehicleComponentId,
3440 _outgoingMavlinkChannel,
3441 &msg,
3442 _availableModesCount(),
3443 modeIndexOneBased,
3444 availableMode.standard_mode,
3445 availableMode.custom_mode,
3446 availableMode.canBeSet ? 0 : MAV_MODE_PROPERTY_NOT_USER_SELECTABLE,
3447 modeName);
3448 respondWithMavlinkMessage(msg);
3449}
3450
3451void MockLink::_availableModesWorker()
3452{
3453 // Protect the streaming cursor from a concurrent request to start another sequence.
3454 QMutexLocker locker(&_availableModesWorkerMutex);
3455 if (_availableModesWorkerNextModeIndex == 0) {
3456 // Not active
3457 return;
3458 }
3459
3460 _sendAvailableMode(_availableModesWorkerNextModeIndex);
3461 if (++_availableModesWorkerNextModeIndex > _availableModesCount()) {
3462 // All modes sent, stop worker
3463 _availableModesWorkerNextModeIndex = 0;
3464 qCDebug(MockLinkLog) << "_availableModesWorker: all modes sent, stopping worker";
3465 }
3466}
3467
3468void MockLink::_sendAvailableModesMonitor()
3469{
3470 mavlink_message_t msg{};
3471
3472 (void) mavlink_msg_available_modes_monitor_pack_chan(
3473 _vehicleSystemId,
3474 _vehicleComponentId,
3475 _outgoingMavlinkChannel,
3476 &msg,
3477 _availableModesMonitorSeqNumber);
3478 respondWithMavlinkMessage(msg);
3479}
3480
3481int MockLink::_availableModesCount() const
3482{
3483 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
3484 return _flightModeList().count();
3485 }
3486 return _flightModeList().count() - (_availableModesMonitorSeqNumber == 0 ? 1 : 0); // Exclude the delayed mode
3487}
3488
3489const QList<MockLink::FlightMode_t> &MockLink::_flightModeList() const
3490{
3491 if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) {
3492 return _availableFlightModes;
3493 }
3494
3495 switch (_vehicleType) {
3496 case MAV_TYPE_FIXED_WING:
3497 return _apmPlaneAvailableFlightModes;
3498 case MAV_TYPE_GROUND_ROVER:
3499 return _apmRoverAvailableFlightModes;
3500 case MAV_TYPE_SUBMARINE:
3501 return _apmSubAvailableFlightModes;
3502 default:
3503 return _apmCopterAvailableFlightModes;
3504 }
3505}
3506
3507// ---------------------------------------------------------------------------
3508// ArduPilot compass calibration simulation
3509//
3510// Driven by _apmCompassCalProgress: -1 = inactive, 0..100 = current pct.
3511// Main thread sets it to 0 to start and -1 to stop.
3512// Worker sends MAG_CAL_PROGRESS at ~10Hz (every 50 calls of 500Hz worker),
3513// then sends MAG_CAL_REPORT when pct reaches 100.
3514// ---------------------------------------------------------------------------
3515void MockLink::startAPMStaleFailedMagCalReportStreaming()
3516{
3517 QMutexLocker locker(&_apmCompassCalMutex);
3518 _apmStaleFailedMagCalReportStreaming = true;
3519 _apmCompassCalTickCount = 0;
3520}
3521
3522bool MockLink::apmStaleFailedMagCalReportStreamingActive() const
3523{
3524 QMutexLocker locker(&_apmCompassCalMutex);
3525 return _apmStaleFailedMagCalReportStreaming;
3526}
3527
3528void MockLink::_apmCompassCalWorker()
3529{
3530 if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) {
3531 return;
3532 }
3533
3534 QMutexLocker locker(&_apmCompassCalMutex);
3535 if ((_apmCompassCalProgress < 0) && !_apmStaleFailedMagCalReportStreaming) {
3536 return;
3537 }
3538
3539 // Tick ~10 Hz: advance every 50 calls of the 500 Hz worker
3540 if (++_apmCompassCalTickCount < 50) {
3541 return;
3542 }
3543 _apmCompassCalTickCount = 0;
3544
3545 if (_apmStaleFailedMagCalReportStreaming) {
3546 // Simulate ArduPilot streaming the report from a previously failed cal until cancelled
3547 mavlink_message_t msg{};
3548 mavlink_mag_cal_report_t report{};
3549 report.compass_id = 0;
3550 report.cal_mask = 0x01;
3551 report.cal_status = MAG_CAL_FAILED;
3552 report.fitness = 999.0f;
3553 (void) mavlink_msg_mag_cal_report_encode_chan(
3554 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &report);
3555 respondWithMavlinkMessage(msg);
3556 return;
3557 }
3558
3559 const int pct = _apmCompassCalProgress;
3560
3561 // Send MAG_CAL_PROGRESS for all 3 active compasses
3562 mavlink_message_t msg{};
3563 for (uint8_t id = 0; id < 3; ++id) {
3564 mavlink_mag_cal_progress_t progress{};
3565 progress.compass_id = id;
3566 progress.cal_mask = 0x07; // all 3 compasses
3567 progress.cal_status = MAG_CAL_RUNNING_STEP_ONE;
3568 progress.completion_pct = static_cast<uint8_t>(qMin(pct, 100));
3569 (void) mavlink_msg_mag_cal_progress_encode_chan(
3570 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &progress);
3571 respondWithMavlinkMessage(msg);
3572 }
3573
3574 if (pct >= 100) {
3575 // Send MAG_CAL_REPORT for all 3 compasses
3576 for (uint8_t id = 0; id < 3; ++id) {
3577 mavlink_mag_cal_report_t report{};
3578 report.compass_id = id;
3579 report.cal_mask = 0x07;
3580 report.cal_status = MAG_CAL_SUCCESS;
3581 report.fitness = 0.5f;
3582 (void) mavlink_msg_mag_cal_report_encode_chan(
3583 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &report);
3584 respondWithMavlinkMessage(msg);
3585 }
3586
3587 // Write non-zero offsets so QGC sees all compasses as calibrated after refresh.
3588 // _mapParamName2Value is owned by MockLink's main thread; dispatch the write there
3589 // to avoid a data race with concurrent param reads on the main thread.
3590 (void) QMetaObject::invokeMethod(this, [this] {
3591 constexpr int compId = MAV_COMP_ID_AUTOPILOT1;
3592 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS_X")] = QVariant(10.0f);
3593 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS_Y")] = QVariant(10.0f);
3594 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS_Z")] = QVariant(10.0f);
3595 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS2_X")] = QVariant(10.0f);
3596 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS2_Y")] = QVariant(10.0f);
3597 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS2_Z")] = QVariant(10.0f);
3598 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS3_X")] = QVariant(10.0f);
3599 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS3_Y")] = QVariant(10.0f);
3600 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS3_Z")] = QVariant(10.0f);
3601 }, Qt::QueuedConnection);
3602
3603 _apmCompassCalProgress = -1; // deactivate
3604 } else {
3605 _apmCompassCalProgress = qMin(pct + 5, 100);
3606 }
3607}
3608
3609// ---------------------------------------------------------------------------
3610// ArduPilot full accelerometer calibration simulation
3611//
3612// The firmware sends COMMAND_LONG(MAV_CMD_ACCELCAL_VEHICLE_POS, pos) to QGC
3613// and waits for a COMMAND_ACK (the "Next" button press) before advancing.
3614// State: _apmAccelCalPosIndex -1=inactive, 0..5=current pose, 6=done.
3615// ---------------------------------------------------------------------------
3616void MockLink::_apmAccelCalWorker()
3617{
3618 if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) {
3619 return;
3620 }
3621
3622 QMutexLocker locker(&_apmAccelCalMutex);
3623 if (_apmAccelCalPosIndex < 0) {
3624 return;
3625 }
3626
3627 if (_apmAccelCalPosIndex == 6) {
3628 // All poses done: send SUCCESS
3629 mavlink_message_t msg{};
3630 mavlink_command_long_t cmd{};
3631 cmd.target_system = 255; // GCS
3632 cmd.target_component = MAV_COMP_ID_MISSIONPLANNER;
3633 cmd.command = MAV_CMD_ACCELCAL_VEHICLE_POS;
3634 cmd.param1 = static_cast<float>(ACCELCAL_VEHICLE_POS_SUCCESS);
3635 (void) mavlink_msg_command_long_encode_chan(
3636 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &cmd);
3637 respondWithMavlinkMessage(msg);
3638
3639 // Write non-zero accel offsets.
3640 // _mapParamName2Value is owned by MockLink's main thread; dispatch the write there
3641 // to avoid a data race with concurrent param reads on the main thread.
3642 (void) QMetaObject::invokeMethod(this, [this] {
3643 constexpr int compId = MAV_COMP_ID_AUTOPILOT1;
3644 _mapParamName2Value[compId][QStringLiteral("INS_ACCOFFS_X")] = QVariant(0.1f);
3645 _mapParamName2Value[compId][QStringLiteral("INS_ACCOFFS_Y")] = QVariant(0.1f);
3646 _mapParamName2Value[compId][QStringLiteral("INS_ACCOFFS_Z")] = QVariant(0.1f);
3647 }, Qt::QueuedConnection);
3648
3649 _apmAccelCalPosIndex = -1;
3650 return;
3651 }
3652
3653 // Send the current position request once; wait for Next ack before advancing
3654 if (!_apmAccelCalGotAck) {
3655 // Throttle re-sends to ~10 Hz
3656 if (++_apmAccelCalTickCount < 50) {
3657 return;
3658 }
3659 _apmAccelCalTickCount = 0;
3660
3661 mavlink_message_t msg{};
3662 mavlink_command_long_t cmd{};
3663 cmd.target_system = 255;
3664 cmd.target_component = MAV_COMP_ID_MISSIONPLANNER;
3665 cmd.command = MAV_CMD_ACCELCAL_VEHICLE_POS;
3666 cmd.param1 = static_cast<float>(kAPMAccelCalPosSequence[_apmAccelCalPosIndex]);
3667 (void) mavlink_msg_command_long_encode_chan(
3668 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &cmd);
3669 respondWithMavlinkMessage(msg);
3670 } else {
3671 // Ack received — advance to next pose
3672 _apmAccelCalGotAck = false;
3673 _apmAccelCalPosIndex++;
3674 }
3675}
config
Config config
Definition GstSourceFactory.cc:183
SharedLinkConfigurationPtr
std::shared_ptr< LinkConfiguration > SharedLinkConfigurationPtr
Definition LinkConfiguration.h:181
mavlink_get_channel_status
mavlink_status_t * mavlink_get_channel_status(uint8_t chan)
Definition QGCMAVLink.cc:53
mavlink_setup_signing_t
struct __mavlink_setup_signing_t mavlink_setup_signing_t
Definition MAVLinkSigning.h:17
error
Error error
Definition QGCCompression.cc:114
mavlink_message_t
struct __mavlink_message mavlink_message_t
Definition QGCCorePlugin.h:26
QGC_LOGGING_CATEGORY
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Definition QGCLoggingCategory.h:11
mavlink_command_ack_t
struct __mavlink_command_ack_t mavlink_command_ack_t
Definition QGCMAVLinkTypes.h:24
mavlink_param_union_t
struct param_union mavlink_param_union_t
Definition QGCMAVLinkTypes.h:40
mavlink_command_long_t
struct __mavlink_command_long_t mavlink_command_long_t
Definition QGCMAVLinkTypes.h:25
keys
std::array< MsgOutKey, 4 > keys
Definition UBXPlan.cc:13
FactMetaData::typeToSize
static size_t typeToSize(ValueType_t type)
Definition FactMetaData.cc:902
LinkConfiguration::setDynamic
void setDynamic(bool dynamic=true)
Set if this is this a dynamic configuration. (decided at runtime)
Definition LinkConfiguration.cc:150
LinkConfiguration::name
QString name() const
Definition LinkConfiguration.h:35
LinkInterface
The link interface defines the interface for all links used to communicate with the ground station ap...
Definition LinkInterface.h:19
LinkInterface::bytesReceived
void bytesReceived(LinkInterface *link, const QByteArray &data)
LinkInterface::disconnected
void disconnected()
LinkInterface::_freeMavlinkChannel
virtual void _freeMavlinkChannel()
Definition LinkInterface.cc:123
LinkInterface::_connectionRemoved
void _connectionRemoved()
Definition LinkInterface.cc:174
LinkInterface::mavlinkChannelIsSet
bool mavlinkChannelIsSet() const
Definition LinkInterface.cc:89
LinkInterface::_allocateMavlinkChannel
virtual bool _allocateMavlinkChannel()
Definition LinkInterface.cc:94
LinkInterface::connected
void connected()
LinkInterface::linkConfiguration
SharedLinkConfigurationPtr linkConfiguration()
Definition LinkInterface.h:38
LinkManager::addConfiguration
SharedLinkConfigurationPtr addConfiguration(LinkConfiguration *config)
Definition LinkManager.cc:752
LinkManager::allocateMavlinkChannel
uint8_t allocateMavlinkChannel()
Definition LinkManager.cc:777
LinkManager::instance
static LinkManager * instance()
Definition LinkManager.cc:62
LinkManager::freeMavlinkChannel
void freeMavlinkChannel(uint8_t channel)
Definition LinkManager.cc:796
LinkManager::invalidMavlinkChannel
static constexpr uint8_t invalidMavlinkChannel()
Definition LinkManager.h:115
MAVLinkProtocol::getComponentId
static int getComponentId()
Definition MAVLinkProtocol.h:34
MAVLinkProtocol::instance
static MAVLinkProtocol * instance()
Definition MAVLinkProtocol.cc:43
MAVLinkProtocol::getSystemId
int getSystemId() const
Definition MAVLinkProtocol.cc:530
MockConfiguration::preloadMission
bool preloadMission() const
Definition MockConfiguration.h:212
MockConfiguration::VideoStreamRtpUdpH265
@ VideoStreamRtpUdpH265
RTP/UDP H.265 -> udp265://.
Definition MockConfiguration.h:66
MockConfiguration::VideoStreamNone
@ VideoStreamNone
No stream served.
Definition MockConfiguration.h:64
MockConfiguration::VideoStreamRtpUdpH264
@ VideoStreamRtpUdpH264
RTP/UDP H.264 -> udp://.
Definition MockConfiguration.h:65
MockConfiguration::VideoStreamRtspH264
@ VideoStreamRtspH264
RTSP H.264 -> rtsp://.
Definition MockConfiguration.h:67
MockConfiguration::VideoStreamMpegTsTcp
@ VideoStreamMpegTsTcp
MPEG-TS over TCP -> tcp://.
Definition MockConfiguration.h:69
MockConfiguration::VideoStreamMpegTsUdp
@ VideoStreamMpegTsUdp
MPEG-TS over UDP -> mpegts://.
Definition MockConfiguration.h:68
MockConfiguration::setVehicleType
void setVehicleType(MAV_TYPE vehicleType)
Definition MockConfiguration.h:102
MockConfiguration::FailMissingParamOnAllRequestsNonDefaultComponent
@ FailMissingParamOnAllRequestsNonDefaultComponent
Adds a second (non-autopilot) param component which never sends one of its params.
Definition MockConfiguration.h:196
MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionLost
@ FailInitialConnectRequestMessageAutopilotVersionLost
REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent.
Definition MockConfiguration.h:201
MockConfiguration::FailMissingParamOnInitialRequest
@ FailMissingParamOnInitialRequest
Not all params are sent on initial request, should still succeed since QGC will re-query missing para...
Definition MockConfiguration.h:194
MockConfiguration::FailMissingParamOnAllRequests
@ FailMissingParamOnAllRequests
Not all params are sent on initial request, QGC retries will fail as well.
Definition MockConfiguration.h:195
MockConfiguration::FailNonDefaultComponentLossy
@ FailNonDefaultComponentLossy
Second component streams only its first two params and drops the first read of every param.
Definition MockConfiguration.h:199
MockConfiguration::FailMissingParamSharedIndexAcrossComponents
@ FailMissingParamSharedIndexAcrossComponents
Autopilot never sends its param at index 1; second component skips its index 1 on initial request onl...
Definition MockConfiguration.h:197
MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionFailure
@ FailInitialConnectRequestMessageAutopilotVersionFailure
REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure.
Definition MockConfiguration.h:200
MockConfiguration::FailNonDefaultComponentDead
@ FailNonDefaultComponentDead
Second component streams only its first two params and never answers a read.
Definition MockConfiguration.h:198
MockConfiguration::FailParamNoResponseToRequestList
@ FailParamNoResponseToRequestList
Do not respond to PARAM_REQUEST_LIST.
Definition MockConfiguration.h:193
MockConfiguration::setFailureMode
void setFailureMode(FailureMode_t failureMode)
Definition MockConfiguration.h:204
MockConfiguration::setEnableProximity
void setEnableProximity(bool enableProximity)
Definition MockConfiguration.h:112
MockConfiguration::setApmStartFreshParams
void setApmStartFreshParams(bool apmStartFreshParams)
Definition MockConfiguration.h:106
MockConfiguration::setEnableCamera
void setEnableCamera(bool enableCamera)
Definition MockConfiguration.h:108
MockConfiguration::setEnableGimbal
void setEnableGimbal(bool enableGimbal)
Definition MockConfiguration.h:110
MockConfiguration::setSendStatusText
void setSendStatusText(bool sendStatusText)
Definition MockConfiguration.h:104
MockConfiguration::cameraHasVideoStream
bool cameraHasVideoStream() const
Definition MockConfiguration.h:146
MockConfiguration::setFtpCapability
void setFtpCapability(bool ftpCapability)
Definition MockConfiguration.h:223
MockConfiguration::setFirmwareType
void setFirmwareType(MAV_AUTOPILOT firmwareType)
Definition MockConfiguration.h:97
MockConfiguration::setSendRadioStatus
void setSendRadioStatus(bool sendRadioStatus)
Definition MockConfiguration.h:227
MockConfiguration::setVideoStreamType
void setVideoStreamType(int value)
Definition MockConfiguration.h:160
MockConfiguration::setPreloadMission
void setPreloadMission(bool preloadMission)
Definition MockConfiguration.h:213
MockConfiguration::setStayMavlinkV1
void setStayMavlinkV1(bool stayV1)
Definition MockConfiguration.h:218
MockLinkCamera
Simulates MAVLink Camera Protocol v2 components for MockLink.
Definition MockLinkCamera.h:40
MockLinkCamera::sendCameraHeartbeats
void sendCameraHeartbeats()
Send heartbeats for all simulated camera components (call from 1Hz tasks)
Definition MockLinkCamera.cc:74
MockLinkCamera::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkCamera.cc:162
MockLinkCamera::run10HzTasks
void run10HzTasks()
Update camera states (call from 10Hz tasks)
Definition MockLinkCamera.cc:92
MockLinkFTP
Mock implementation of Mavlink FTP server.
Definition MockLinkFTP.h:18
MockLinkFTP::logFiles
QList< LogFile > logFiles() const
Returns the log files served from the @MAV_LOG virtual directory.
Definition MockLinkFTP.h:41
MockLinkFTP::setLogFiles
void setLogFiles(const QList< LogFile > &logFiles)
Definition MockLinkFTP.cc:739
MockLinkFTP::mavlinkMessageReceived
void mavlinkMessageReceived(const mavlink_message_t &message)
Called to handle an FTP message.
Definition MockLinkFTP.cc:528
MockLinkFTP::logFileContents
QByteArray logFileContents(const QString &name) const
Definition MockLinkFTP.cc:719
MockLinkGimbal
Simulates MAVLink Gimbal Manager Protocol for MockLink.
Definition MockLinkGimbal.h:28
MockLinkGimbal::run1HzTasks
void run1HzTasks()
Send periodic gimbal status messages (call from 1Hz tasks)
Definition MockLinkGimbal.cc:36
MockLinkGimbal::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkGimbal.cc:72
MockLinkMissionItemHandler::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkMissionItemHandler.cc:65
MockLinkPX4Calibration
Simulates the PX4 commander magnetometer and accelerometer calibration protocols for MockLink.
Definition MockLinkPX4Calibration.h:41
MockLinkPX4Calibration::run10HzTasks
void run10HzTasks()
Called by MockLink::run10HzTasks on the worker thread.
Definition MockLinkPX4Calibration.cc:91
MockLinkWorker
Worker class that runs periodic tasks for MockLink simulation.
Definition MockLinkWorker.h:13
MockVideoStreamServer
Serves a synthetic (videotestsrc) live video stream for MockLink so the GStreamer receive pipeline ca...
Definition MockVideoStreamServer.h:25
MockVideoStreamServer::StreamType::MpegTsTcp
@ MpegTsTcp
MPEG-TS over TCP -> tcp://.
MockVideoStreamServer::StreamType::RtpUdpH265
@ RtpUdpH265
RTP/H.265 over UDP -> udp265://.
MockVideoStreamServer::StreamType::RtpUdpH264
@ RtpUdpH264
RTP/H.264 over UDP -> udp://.
MockVideoStreamServer::StreamType::RtspH264
@ RtspH264
RTSP (H.264) -> rtsp://.
MockVideoStreamServer::StreamType::MpegTsUdp
@ MpegTsUdp
MPEG-TS over UDP -> mpegts://.
ParameterManager::mavTypeToFactType
static FactMetaData::ValueType_t mavTypeToFactType(MAV_PARAM_TYPE mavType)
Definition ParameterManager.cc:1357
Ashtech::Plan::line
QByteArray line(std::string_view command)
command as one CR/LF-terminated line.
Definition AshtechPlan.cc:34
MAVLinkSigning::currentSigningTimestampTicks
uint64_t currentSigningTimestampTicks()
Current signing timestamp in 10µs ticks since 2015-01-01.
Definition MAVLinkSigning.h:35
MAVLinkSigning::insecureConnectionAcceptUnsignedCallback
bool insecureConnectionAcceptUnsignedCallback(const mavlink_status_t *status, uint32_t message_id)
Definition MAVLinkSigning.cc:46
NMEA::number
std::optional< T > number(std::string_view field, int base=DECIMAL_BASE)
Definition NMEASentence.h:95
NMEA::coordinate
std::optional< double > coordinate(std::string_view field, std::string_view hemisphere, bool latitude)
Definition NMEASentence.cc:147
QGC::crc32
quint32 crc32(const quint8 *src, unsigned len, unsigned state)
Definition QGCMath.cc:66
QGC::runningUnitTests
bool runningUnitTests()
Definition AppMessages.cc:30
QGC::secureZero
void secureZero(void *data, size_t size)
Definition SecureMemory.h:19
SBF::Plan::port
GPSCommandSequence port(QByteArrayView port, unsigned baud)
Clears the SBF output of port, keeps a serial port at the link's baud, and selects SBF and the WGS84 ...
Definition SBFPlan.cc:34
PX4_CUSTOM_MAIN_MODE_AUTO
@ PX4_CUSTOM_MAIN_MODE_AUTO
Definition px4_custom_mode.h:13
PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE
@ PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE
Definition px4_custom_mode.h:30
px4_custom_mode
Definition px4_custom_mode.h:51
px4_custom_mode::data
uint32_t data
Definition px4_custom_mode.h:57