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#include <QtCore/QFile>
19#include <QtCore/QJsonArray>
20#include <QtCore/QJsonDocument>
21#include <QtCore/QJsonObject>
22#include <QtCore/QMutexLocker>
23#include <QtCore/QSet>
24#include <QtCore/QRandomGenerator>
25#include <QtCore/QTemporaryFile>
26#include <QtCore/QThread>
27#include <QtCore/QTimer>
28
29#include <cmath>
30#include <cstring>
31
32QGC_LOGGING_CATEGORY(MockLinkLog, "Comms.MockLink.MockLink")
33QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "Comms.MockLink.MockLink:verbose")
34
35std::atomic<int> MockLink::_nextVehicleSystemId{128};
36
37QList<MockLink::FlightMode_t> MockLink::_availableFlightModes = {
38 // Mode Name Standard Mode Custom Mode CanBeSet adv
39 { "Manual", 0, PX4CustomMode::MANUAL, true, true },
40 { "Stabilized", 0, PX4CustomMode::STABILIZED, true, true },
41 { "Acro", 0, PX4CustomMode::ACRO, true, true },
42 { "Altitude", 0, PX4CustomMode::ALTCTL, true, false},
43 { "Offboard", 0, PX4CustomMode::OFFBOARD, true, true },
44 { "Position", 0, PX4CustomMode::POSCTL_POSCTL, true, false},
45 { "Orbit", 0, PX4CustomMode::POSCTL_ORBIT, false, true },
46 { "Hold", 0, PX4CustomMode::AUTO_LOITER, true, true },
47 { "Mission", 0, PX4CustomMode::AUTO_MISSION, true, true },
48 { "Return", 0, PX4CustomMode::AUTO_RTL, true, true },
49 { "Land", MAV_STANDARD_MODE_LAND, PX4CustomMode::AUTO_LAND, false, true },
50 { "Precision Landing", 0, PX4CustomMode::AUTO_PRECLAND, true, true },
51 { "Takeoff", MAV_STANDARD_MODE_TAKEOFF, PX4CustomMode::AUTO_TAKEOFF, false, false},
52 { "MockLink Mode", 0, PX4CustomMode::RATTITUDE, true, false},
53 { "(Mode not available)", 0, PX4CustomMode::AUTO_RTGS, false, false},
54 { "MockLink Mode (delayed)",0, PX4CustomMode::AUTO_FOLLOW_TARGET, true, false},
55};
56
57MockLink::MockLink(SharedLinkConfigurationPtr &config, QObject *parent)
58 : LinkInterface(config, parent)
59 , _mockConfig(qobject_cast<const MockConfiguration*>(_config.get()))
60 , _firmwareType(_mockConfig->firmwareType())
61 , _vehicleType(_mockConfig->vehicleType())
62 , _sendStatusText(_mockConfig->sendStatusText())
63 , _enableCamera(_mockConfig->enableCamera())
64 , _enableGimbal(_mockConfig->enableGimbal())
65 , _failureMode(_mockConfig->failureMode())
66 , _vehicleSystemId(_mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : static_cast<int>(_nextVehicleSystemId))
67 , _vehicleLatitude(_defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001))
68 , _vehicleLongitude(_defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001))
69 , _boardVendorId(_mockConfig->boardVendorId())
70 , _boardProductId(_mockConfig->boardProductId())
71 , _missionItemHandler(new MockLinkMissionItemHandler(this))
72 , _mockLinkCamera(_enableCamera ? new MockLinkCamera(this,
73 _mockConfig->cameraCaptureVideo(),
74 _mockConfig->cameraCaptureImage(),
75 _mockConfig->cameraHasModes(),
76 _mockConfig->cameraHasVideoStream(),
77 _mockConfig->cameraCanCaptureImageInVideoMode(),
78 _mockConfig->cameraCanCaptureVideoInImageMode(),
79 _mockConfig->cameraHasBasicZoom(),
80 _mockConfig->cameraHasTrackingPoint(),
81 _mockConfig->cameraHasTrackingRectangle())
82 : nullptr)
83 , _mockLinkGimbal(_enableGimbal ? new MockLinkGimbal(this,
84 _mockConfig->gimbalHasRollAxis(),
85 _mockConfig->gimbalHasPitchAxis(),
86 _mockConfig->gimbalHasYawAxis(),
87 _mockConfig->gimbalHasYawFollow(),
88 _mockConfig->gimbalHasYawLock(),
89 _mockConfig->gimbalHasRetract(),
90 _mockConfig->gimbalHasNeutral())
91 : nullptr)
92 , _mockLinkPX4Calibration(new MockLinkPX4Calibration(this))
93 , _mockLinkFTP(new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this))
94{
95 qCDebug(MockLinkLog) << this;
96
97 if (_mockConfig->startArmed()) {
98 setArmed(true);
99 }
100
101 if (_mockConfig->preloadMission()) {
102 _missionItemHandler->loadSimpleMultirotorMission();
103 }
104
105 // Initialize ADS-B vehicles with different starting conditions
106 _adsbVehicles.reserve(_numberOfVehicles);
107 for (int i = 0; i < _numberOfVehicles; ++i) {
108 ADSBVehicle vehicle{};
109 vehicle.angle = i * 72.0; // Different starting directions (angles 0, 72, 144, 216, 288)
110
111 // Set initial coordinates slightly offset from the default coordinates
112 const double latOffset = 0.001 * i;
113 const double lonOffset = 0.001 * (i % 2 == 0 ? i : -i);
114 vehicle.coordinate = QGeoCoordinate(_defaultVehicleLatitude + latOffset, _defaultVehicleLongitude + lonOffset);
115
116 // Set a unique starting altitude for each vehicle near the home altitude
117 vehicle.altitude = _defaultVehicleHomeAltitude + (i * 5);
118
119 _adsbVehicles.append(vehicle);
120 }
121
122 (void) QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection);
123
124 _loadParams();
125 _runningTime.start();
126
127 _workerThread = new QThread(this);
128 _workerThread->setObjectName(QStringLiteral("Mock_%1").arg(_mockConfig->name()));
129 _worker = new MockLinkWorker(this);
130 _worker->moveToThread(_workerThread);
131 (void) connect(_workerThread, &QThread::started, _worker, &MockLinkWorker::startWork);
132 (void) connect(_workerThread, &QThread::finished, _worker, &QObject::deleteLater);
133 _workerThread->start();
134}
135
136MockLink::~MockLink()
137{
138 MockLink::disconnect();
139
140 delete _mockLinkCamera;
141 delete _mockLinkGimbal;
142 delete _mockLinkPX4Calibration;
143
144 if (!_logDownloadFilename.isEmpty()) {
145 QFile::remove(_logDownloadFilename);
146 }
147
148 if (_workerThread) {
149 _workerThread->quit();
150 _workerThread->wait();
151 }
152
153 qCDebug(MockLinkLog) << this;
154}
155
156bool MockLink::_connect()
157{
158 if (!_connected) {
159 _connected = true;
160 _disconnectedEmitted = false;
161 mavlink_status_t *const outgoingStatus = mavlink_get_channel_status(_outgoingMavlinkChannel);
162 outgoingStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
163 mavlink_status_t *const incomingStatus = mavlink_get_channel_status(_incomingMavlinkChannel);
164 incomingStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
165 emit connected();
166 }
167
168 return true;
169}
170
171void MockLink::disconnect()
172{
173 _missionItemHandler->shutdown();
174
175 // Stop worker thread first to prevent any more messages from being sent.
176 // This must happen before setting _connected = false to avoid race conditions
177 // where the worker checks _connected (true), then we set it false, then the
178 // worker continues sending messages to a disconnecting/destroyed vehicle.
179 if (_workerThread && _workerThread->isRunning()) {
180 _workerThread->quit();
181 _workerThread->wait();
182 }
183
184 // signing/streams pointers alias MockLink memory — clear before emit in case the disconnected signal frees us.
185 if (_outgoingMavlinkChannelIsSet()) {
186 mavlink_status_t* const outgoingStatus = mavlink_get_channel_status(_outgoingMavlinkChannel);
187 outgoingStatus->signing = nullptr;
188 outgoingStatus->signing_streams = nullptr;
189 mavlink_reset_channel_status(_outgoingMavlinkChannel);
190 }
191
192 if (_connected) {
193 _connected = false;
194 if (!_disconnectedEmitted.exchange(true)) {
195 emit disconnected();
196 }
197 }
198}
199
200void MockLink::run1HzTasks()
201{
202 if (!_mavlinkStarted || !_connected || !mavlinkChannelIsSet()) {
203 return;
204 }
205
206 if (linkConfiguration()->isHighLatency() && _highLatencyTransmissionEnabled) {
207 _sendHighLatency2();
208 return;
209 }
210
211 _sendVibration();
212 _sendBatteryStatus();
213 _sendNamedValueFloats();
214 _sendSysStatus();
215 _sendADSBVehicles();
216 if (_vehicleType != MAV_TYPE_SUBMARINE) {
217 _sendRemoteIDArmStatus();
218 }
219 _sendAvailableModesMonitor();
220
221 if (_enableGimbal) {
222 _mockLinkGimbal->run1HzTasks();
223 }
224
225 _sendEscInfo();
226 _sendEscStatus();
227 _sendRadioStatus();
228
229 if (_enableCamera) {
230 _mockLinkCamera->sendCameraHeartbeats();
231 }
232
233 if (!QGC::runningUnitTests()) {
234 // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
235 _sendRCChannels();
236 }
237
238 if (_sendHomePositionDelayCount > 0) {
239 // We delay home position for better testing
240 _sendHomePositionDelayCount--;
241 } else {
242 _sendHomePosition();
243 // We piggy back on this delay to signal we have new standard modes available
244 if (_availableModesMonitorSeqNumber == 0) {
245 qCDebug(MockLinkLog) << "Bumping sequence number for available modes monitor to trigger requery of modes";
246 _availableModesMonitorSeqNumber = 1;
247 }
248 }
249}
250
251void MockLink::run10HzTasks()
252{
253 if (linkConfiguration()->isHighLatency()) {
254 return;
255 }
256
257 if (_mavlinkStarted && _connected && mavlinkChannelIsSet()) {
258 _sendHeartBeat();
259 const bool gpsDelayExpired = (_sendGPSPositionDelayCount == 0);
260 if (_sendGPSPositionDelayCount > 0) {
261 // We delay gps position for better testing
262 _sendGPSPositionDelayCount--;
263 }
264 if (gpsDelayExpired || QGC::runningUnitTests()) {
265 if (_vehicleType != MAV_TYPE_SUBMARINE) {
266 _sendGpsRawInt();
267 _sendGlobalPositionInt();
268 }
269 _sendExtendedSysState();
270 }
271
272 _sendAttitudeQuaternion();
273 _sendAttitudeTarget();
274 _sendLocalPositionNed();
275 _sendPositionTargetLocalNed();
276
277 _mockLinkPX4Calibration->run10HzTasks();
278
279 if (_enableCamera) {
280 _mockLinkCamera->run10HzTasks();
281 }
282 }
283}
284
285void MockLink::run500HzTasks()
286{
287 if (linkConfiguration()->isHighLatency()) {
288 return;
289 }
290
291 if (_mavlinkStarted && _connected && mavlinkChannelIsSet()) {
292 const int paramSends = QGC::runningUnitTests() ? kTestParamRequestListBatch : 1;
293 for (int i = 0; i < paramSends; ++i) {
294 _paramRequestListWorker();
295 }
296 _logDownloadWorker();
297 _availableModesWorker();
298 _apmCompassCalWorker();
299 _apmAccelCalWorker();
300 }
301}
302
303void MockLink::sendStatusTextMessages()
304{
305 _sendStatusTextMessages();
306}
307
308bool MockLink::_allocateMavlinkChannel()
309{
310 // should only be called by the LinkManager during setup
311 Q_ASSERT(!_incomingMavlinkChannelIsSet());
312 Q_ASSERT(!_outgoingMavlinkChannelIsSet());
313 Q_ASSERT(!mavlinkChannelIsSet());
314
315 if (!LinkInterface::_allocateMavlinkChannel()) {
316 qCWarning(MockLinkLog) << "LinkInterface::_allocateMavlinkChannel failed";
317 return false;
318 }
319
320 _incomingMavlinkChannel = LinkManager::instance()->allocateMavlinkChannel();
321 if (!_incomingMavlinkChannelIsSet()) {
322 qCWarning(MockLinkLog) << "_allocateMavlinkChannel aux failed";
323 LinkInterface::_freeMavlinkChannel();
324 return false;
325 }
326
327 _outgoingMavlinkChannel = LinkManager::instance()->allocateMavlinkChannel();
328 if (!_outgoingMavlinkChannelIsSet()) {
329 qCWarning(MockLinkLog) << "_allocateMavlinkChannel vehicle failed";
330 LinkManager::instance()->freeMavlinkChannel(_incomingMavlinkChannel);
331 LinkInterface::_freeMavlinkChannel();
332 return false;
333 }
334
335 qCDebug(MockLinkLog) << "_allocateMavlinkChannel aux:" << _incomingMavlinkChannel << "vehicle:" << _outgoingMavlinkChannel;
336 return true;
337}
338
339void MockLink::_freeMavlinkChannel()
340{
341 qCDebug(MockLinkLog) << "_freeMavlinkChannel aux:" << _incomingMavlinkChannel << "vehicle:" << _outgoingMavlinkChannel;
342 if (!_incomingMavlinkChannelIsSet()) {
343 Q_ASSERT(!_outgoingMavlinkChannelIsSet());
344 Q_ASSERT(!mavlinkChannelIsSet());
345 return;
346 }
347
348 if (_outgoingMavlinkChannelIsSet()) {
349 // Detach our _mockSigning before the channel is freed; the bytes back this struct in MockLink memory.
350 mavlink_reset_channel_status(_outgoingMavlinkChannel);
351 LinkManager::instance()->freeMavlinkChannel(_outgoingMavlinkChannel);
352 _outgoingMavlinkChannel = LinkManager::invalidMavlinkChannel();
353 }
354 mavlink_reset_channel_status(_incomingMavlinkChannel);
355 LinkManager::instance()->freeMavlinkChannel(_incomingMavlinkChannel);
356 _incomingMavlinkChannel = LinkManager::invalidMavlinkChannel();
357 LinkInterface::_freeMavlinkChannel();
358}
359
360bool MockLink::_incomingMavlinkChannelIsSet() const
361{
362 return (LinkManager::invalidMavlinkChannel() != _incomingMavlinkChannel);
363}
364
365bool MockLink::_outgoingMavlinkChannelIsSet() const
366{
367 return (LinkManager::invalidMavlinkChannel() != _outgoingMavlinkChannel);
368}
369
370void MockLink::_loadParams()
371{
372 QFile paramFile;
373 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
374 if (_vehicleType == MAV_TYPE_FIXED_WING) {
375 paramFile.setFileName(":/FirmwarePlugin/APM/Plane.OfflineEditing.params");
376 } else if (_vehicleType == MAV_TYPE_SUBMARINE ) {
377 paramFile.setFileName(":/FirmwarePlugin/APM/Sub.OfflineEditing.params");
378 } else if (_vehicleType == MAV_TYPE_GROUND_ROVER ) {
379 paramFile.setFileName(":/FirmwarePlugin/APM/Rover.OfflineEditing.params");
380 } else {
381 paramFile.setFileName(":/FirmwarePlugin/APM/Copter.OfflineEditing.params");
382 }
383 } else {
384 paramFile.setFileName(":/MockLink/PX4MockLink.params");
385 }
386
387 const bool success = paramFile.open(QFile::ReadOnly);
388 Q_UNUSED(success);
389 Q_ASSERT(success);
390
391 QTextStream paramStream(&paramFile);
392 while (!paramStream.atEnd()) {
393 const QString line = paramStream.readLine();
394
395 if (line.startsWith("#")) {
396 continue;
397 }
398
399 const QStringList paramData = line.split("\t");
400 Q_ASSERT(paramData.count() == 5);
401
402 const int compId = paramData.at(1).toInt();
403 const QString paramName = paramData.at(2);
404 const QString valStr = paramData.at(3);
405 const uint paramType = paramData.at(4).toUInt();
406
407 QVariant paramValue;
408 switch (paramType) {
409 case MAV_PARAM_TYPE_REAL32:
410 paramValue = QVariant(valStr.toFloat());
411 break;
412 case MAV_PARAM_TYPE_UINT32:
413 paramValue = QVariant(valStr.toUInt());
414 break;
415 case MAV_PARAM_TYPE_INT32:
416 paramValue = QVariant(valStr.toInt());
417 break;
418 case MAV_PARAM_TYPE_UINT16:
419 paramValue = QVariant((quint16)valStr.toUInt());
420 break;
421 case MAV_PARAM_TYPE_INT16:
422 paramValue = QVariant((qint16)valStr.toInt());
423 break;
424 case MAV_PARAM_TYPE_UINT8:
425 paramValue = QVariant((quint8)valStr.toUInt());
426 break;
427 case MAV_PARAM_TYPE_INT8:
428 paramValue = QVariant((qint8)valStr.toUInt());
429 break;
430 default:
431 qCCritical(MockLinkVerboseLog) << "Unknown type" << paramType;
432 paramValue = QVariant(valStr.toInt());
433 break;
434 }
435
436 qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue;
437
438 _mapParamName2Value[compId][paramName] = paramValue;
439 _mapParamName2MavParamType[compId][paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
440 }
441}
442
453void MockLink::_resetParamsToDefaults()
454{
455 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
456 // ArduPilot firmware default for calibration-indicator parameters is 0 (uncalibrated).
457 // Resetting these causes QGC's APMSensorsComponent::setupComplete() to return false,
458 // which is the correct post-reset state on a real vehicle.
459 for (auto compIt = _mapParamName2Value.begin(); compIt != _mapParamName2Value.end(); ++compIt) {
460 for (auto paramIt = compIt.value().begin(); paramIt != compIt.value().end(); ++paramIt) {
461 if (kAPMCalOffsetParams.contains(paramIt.key())) {
462 paramIt.value() = QVariant(0.0f);
463 }
464 }
465 }
466 return;
467 }
468
469 if (_firmwareType != MAV_AUTOPILOT_PX4) {
470 qCWarning(MockLinkLog) << "Param reset to defaults not supported for firmware type" << _firmwareType;
471 return;
472 }
473
474 QFile metaDataFile(QStringLiteral(":/MockLink/Parameter.MetaData.json"));
475 if (!metaDataFile.open(QFile::ReadOnly)) {
476 qCWarning(MockLinkLog) << "Unable to open parameter metadata for reset" << metaDataFile.fileName();
477 return;
478 }
479
480 QJsonParseError parseError{};
481 const QJsonDocument doc = QJsonDocument::fromJson(metaDataFile.readAll(), &parseError);
482 if (parseError.error != QJsonParseError::NoError) {
483 qCWarning(MockLinkLog) << "Unable to parse parameter metadata for reset:" << parseError.errorString();
484 return;
485 }
486
487 QHash<QString, QVariant> defaults;
488 const QJsonArray parameters = doc.object().value(QStringLiteral("parameters")).toArray();
489 for (const QJsonValue &parameter : parameters) {
490 const QJsonObject paramObject = parameter.toObject();
491 if (paramObject.contains(QStringLiteral("default"))) {
492 defaults[paramObject.value(QStringLiteral("name")).toString()] = paramObject.value(QStringLiteral("default")).toVariant();
493 }
494 }
495
496 for (auto compIt = _mapParamName2Value.begin(); compIt != _mapParamName2Value.end(); ++compIt) {
497 const int compId = compIt.key();
498 for (auto paramIt = compIt.value().begin(); paramIt != compIt.value().end(); ++paramIt) {
499 const QString &paramName = paramIt.key();
500 if (!_resetSysAutostartOnParamReset && (paramName == QLatin1String("SYS_AUTOSTART"))) {
501 continue;
502 }
503 const auto defaultIt = defaults.constFind(paramName);
504 if (defaultIt == defaults.constEnd()) {
505 continue;
506 }
507 switch (_mapParamName2MavParamType[compId][paramName]) {
508 case MAV_PARAM_TYPE_REAL32:
509 paramIt.value() = QVariant(defaultIt->toFloat());
510 break;
511 case MAV_PARAM_TYPE_UINT32:
512 paramIt.value() = QVariant(defaultIt->toUInt());
513 break;
514 case MAV_PARAM_TYPE_INT32:
515 paramIt.value() = QVariant(defaultIt->toInt());
516 break;
517 case MAV_PARAM_TYPE_UINT16:
518 paramIt.value() = QVariant(static_cast<quint16>(defaultIt->toUInt()));
519 break;
520 case MAV_PARAM_TYPE_INT16:
521 paramIt.value() = QVariant(static_cast<qint16>(defaultIt->toInt()));
522 break;
523 case MAV_PARAM_TYPE_UINT8:
524 paramIt.value() = QVariant(static_cast<quint8>(defaultIt->toUInt()));
525 break;
526 case MAV_PARAM_TYPE_INT8:
527 paramIt.value() = QVariant(static_cast<qint8>(defaultIt->toInt()));
528 break;
529 default:
530 qCWarning(MockLinkLog) << "Param reset skipped unhandled type" << _mapParamName2MavParamType[compId][paramName] << paramName;
531 break;
532 }
533 }
534 }
535}
536
537void MockLink::_sendHeartBeat()
538{
539 mavlink_message_t msg{};
540 (void) mavlink_msg_heartbeat_pack_chan(
541 _vehicleSystemId,
542 _vehicleComponentId,
543 _outgoingMavlinkChannel,
544 &msg,
545 _vehicleType, // MAV_TYPE
546 _firmwareType, // MAV_AUTOPILOT
547 _mavBaseMode, // MAV_MODE
548 _mavCustomMode, // custom mode
549 _mavState // MAV_STATE
550 );
551 respondWithMavlinkMessage(msg);
552}
553
554void MockLink::_sendHighLatency2()
555{
556 qCDebug(MockLinkLog) << "Sending" << _mavCustomMode;
557
558 union px4_custom_mode px4_cm{};
559 px4_cm.data = _mavCustomMode;
560
561 mavlink_message_t msg{};
562 (void) mavlink_msg_high_latency2_pack_chan(
563 _vehicleSystemId,
564 _vehicleComponentId,
565 _outgoingMavlinkChannel,
566 &msg,
567 0, // timestamp
568 _vehicleType, // MAV_TYPE
569 _firmwareType, // MAV_AUTOPILOT
570 px4_cm.custom_mode_hl, // custom_mode
571 static_cast<int32_t>(_vehicleLatitude * 1E7),
572 static_cast<int32_t>(_vehicleLongitude * 1E7),
573 static_cast<int16_t>(_vehicleAltitudeAMSL),
574 static_cast<int16_t>(_vehicleAltitudeAMSL), // target_altitude,
575 0, // heading
576 0, // target_heading
577 0, // target_distance
578 0, // throttle
579 0, // airspeed
580 0, // airspeed_sp
581 0, // groundspeed
582 0, // windspeed,
583 0, // wind_heading
584 UINT8_MAX, // eph not known
585 UINT8_MAX, // epv not known
586 0, // temperature_air
587 0, // climb_rate
588 -1, // battery, do not use?
589 0, // wp_num
590 0, // failure_flags
591 0, 0, 0 // custom0, custom1, custom2
592 );
593 respondWithMavlinkMessage(msg);
594}
595
596void MockLink::_sendSysStatus()
597{
598 mavlink_message_t msg{};
599 (void) mavlink_msg_sys_status_pack_chan(
600 _vehicleSystemId,
601 _vehicleComponentId,
602 _outgoingMavlinkChannel,
603 &msg,
604 MAV_SYS_STATUS_SENSOR_GPS, // onboard_control_sensors_present
605 0, // onboard_control_sensors_enabled
606 0, // onboard_control_sensors_health
607 250, // load
608 4200 * 4, // voltage_battery
609 8000, // current_battery
610 _battery1PctRemaining, // battery_remaining
611 0,0,0,0,0,0,0,0,0
612 );
613 respondWithMavlinkMessage(msg);
614}
615
616void MockLink::_sendBatteryStatus()
617{
618 if (_battery1PctRemaining > 1) {
619 _battery1PctRemaining = static_cast<int8_t>(100 - (_runningTime.elapsed() / 1000));
620 _battery1TimeRemaining = static_cast<double>(_batteryMaxTimeRemaining) * (static_cast<double>(_battery1PctRemaining) / 100.0);
621 if (_battery1PctRemaining > 50) {
622 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
623 } else if (_battery1PctRemaining > 30) {
624 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_LOW;
625 } else if (_battery1PctRemaining > 20) {
626 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL;
627 } else {
628 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY;
629 }
630 }
631
632 if (_battery2PctRemaining > 1) {
633 _battery2PctRemaining = static_cast<int8_t>(100 - ((_runningTime.elapsed() / 1000) / 2));
634 _battery2TimeRemaining = static_cast<double>(_batteryMaxTimeRemaining) * (static_cast<double>(_battery2PctRemaining) / 100.0);
635 if (_battery2PctRemaining > 50) {
636 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
637 } else if (_battery2PctRemaining > 30) {
638 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_LOW;
639 } else if (_battery2PctRemaining > 20) {
640 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL;
641 } else {
642 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY;
643 }
644 }
645
646 mavlink_message_t msg{};
647 uint16_t rgVoltages[10]{};
648 uint16_t rgVoltagesNone[10]{};
649 uint16_t rgVoltagesExtNone[4]{};
650
651 for (size_t i = 0; i < std::size(rgVoltages); i++) {
652 rgVoltages[i] = UINT16_MAX;
653 rgVoltagesNone[i] = UINT16_MAX;
654 }
655 rgVoltages[0] = rgVoltages[1] = rgVoltages[2] = 4200;
656
657 (void) mavlink_msg_battery_status_pack_chan(
658 _vehicleSystemId,
659 _vehicleComponentId,
660 _outgoingMavlinkChannel,
661 &msg,
662 1, // battery id
663 MAV_BATTERY_FUNCTION_ALL,
664 MAV_BATTERY_TYPE_LIPO,
665 2100, // temp cdegC
666 rgVoltages,
667 600, // battery cA
668 100, // current consumed mAh
669 -1, // energy consumed not supported
670 _battery1PctRemaining,
671 _battery1TimeRemaining,
672 _battery1ChargeState,
673 rgVoltagesExtNone,
674 0, // MAV_BATTERY_MODE
675 0 // MAV_BATTERY_FAULT
676 );
677 respondWithMavlinkMessage(msg);
678
679 (void) mavlink_msg_battery_status_pack_chan(
680 _vehicleSystemId,
681 _vehicleComponentId,
682 _outgoingMavlinkChannel,
683 &msg,
684 2, // battery id
685 MAV_BATTERY_FUNCTION_ALL,
686 MAV_BATTERY_TYPE_LIPO,
687 INT16_MAX, // temp cdegC
688 rgVoltagesNone,
689 600, // battery cA
690 100, // current consumed mAh
691 -1, // energy consumed not supported
692 _battery2PctRemaining,
693 _battery2TimeRemaining,
694 _battery2ChargeState,
695 rgVoltagesExtNone,
696 0, // MAV_BATTERY_MODE
697 0 // MAV_BATTERY_FAULT
698 );
699 respondWithMavlinkMessage(msg);
700}
701
702void MockLink::_sendNamedValueFloats()
703{
704 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
705
706 // Send two named float values with varying data to exercise Inspector instance separation
707 const float sinVal = static_cast<float>(std::sin(static_cast<double>(timeBootMs) / 1000.0));
708 const float cosVal = static_cast<float>(std::cos(static_cast<double>(timeBootMs) / 1000.0));
709
710 // NAMED_VALUE_FLOAT.name is a fixed 10-byte field; pack_chan memcpys 10 bytes unconditionally.
711 static constexpr char kSinName[10] = "sin_wave";
712 static constexpr char kCosName[10] = "cos_wave";
713
714 mavlink_message_t msg{};
715 (void) mavlink_msg_named_value_float_pack_chan(
716 _vehicleSystemId,
717 _vehicleComponentId,
718 _outgoingMavlinkChannel,
719 &msg,
720 timeBootMs,
721 kSinName,
722 sinVal
723 );
724 respondWithMavlinkMessage(msg);
725
726 (void) mavlink_msg_named_value_float_pack_chan(
727 _vehicleSystemId,
728 _vehicleComponentId,
729 _outgoingMavlinkChannel,
730 &msg,
731 timeBootMs,
732 kCosName,
733 cosVal
734 );
735 respondWithMavlinkMessage(msg);
736}
737
738void MockLink::_sendVibration()
739{
740 mavlink_message_t msg{};
741 (void) mavlink_msg_vibration_pack_chan(
742 _vehicleSystemId,
743 _vehicleComponentId,
744 _outgoingMavlinkChannel,
745 &msg,
746 0, // time_usec
747 50.5, // vibration_x,
748 10.5, // vibration_y,
749 60.0, // vibration_z,
750 1, // clipping_0
751 2, // clipping_0
752 3 // clipping_0
753 );
754 respondWithMavlinkMessage(msg);
755}
756
757void MockLink::respondWithMavlinkMessage(const mavlink_message_t &msg)
758{
759 if (!_commLost) {
760 uint8_t buffer[MAVLINK_MAX_PACKET_LEN]{};
761 const int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg);
762 const QByteArray bytes(reinterpret_cast<char*>(buffer), cBuffer);
763 emit bytesReceived(this, bytes);
764 }
765}
766
767void MockLink::_writeBytes(const QByteArray &bytes)
768{
769 // This prevents the responses to mavlink messages from being sent until the _writeBytes returns.
770 emit writeBytesQueuedSignal(bytes);
771}
772
773void MockLink::_writeBytesQueued(const QByteArray &bytes)
774{
775 if (!_connected || !mavlinkChannelIsSet()) {
776 qCDebug(MockLinkLog) << "Dropping queued bytes on disconnected/uninitialized mock link";
777 return;
778 }
779
780 if (_inNSH) {
781 _handleIncomingNSHBytes(bytes.constData(), bytes.length());
782 return;
783 }
784
785 if (bytes.startsWith(QByteArrayLiteral("\r\r\r"))) {
786 _inNSH = true;
787 _handleIncomingNSHBytes(&bytes.constData()[3], bytes.length() - 3);
788 }
789
790 _handleIncomingMavlinkBytes(reinterpret_cast<const uint8_t*>(bytes.constData()), bytes.length());
791}
792
793void MockLink::_handleIncomingNSHBytes(const char *bytes, int cBytes)
794{
795 Q_UNUSED(cBytes);
796
797 // Drop back out of NSH
798 if ((cBytes == 4) && (bytes[0] == '\r') && (bytes[1] == '\r') && (bytes[2] == '\r')) {
799 _inNSH = false;
800 return;
801 }
802
803 if (cBytes > 0) {
804 qCDebug(MockLinkLog) << "NSH:" << bytes;
805#if 0
806 // MockLink not quite ready to handle this correctly yet
807 if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) {
808 // This is the mavlink start command
809 _mavlinkStarted = true;
810 }
811#endif
812 }
813}
814
815void MockLink::_handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes)
816{
817 mavlink_message_t msg{};
818 mavlink_status_t comm{};
819
820 QMutexLocker lock(&_incomingMavlinkMutex);
821 for (qint64 i = 0; i < cBytes; i++) {
822 const int parsed = mavlink_parse_char(_incomingMavlinkChannel, bytes[i], &msg, &comm);
823 if (!parsed) {
824 continue;
825 }
826 lock.unlock();
827 _handleIncomingMavlinkMsg(msg);
828 lock.relock();
829 }
830}
831
832void MockLink::_updateIncomingMessageCounts(const mavlink_message_t &msg)
833{
834 _receivedMavlinkMessageCountMap[msg.msgid]++;
835
836 // Update command-specific counts if this is a COMMAND_LONG message
837 if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
838 mavlink_command_long_t request{};
839 mavlink_msg_command_long_decode(&msg, &request);
840
841 _receivedMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
842 _receivedMavCommandByCompCountMap[static_cast<MAV_CMD>(request.command)][request.target_component]++;
843
844 if (request.command == MAV_CMD_REQUEST_MESSAGE) {
845 _receivedRequestMessageCountMap[static_cast<uint32_t>(request.param1)]++;
846 _receivedRequestMessageByCompAndMsgCountMap[request.target_component][static_cast<int>(request.param1)]++;
847 }
848 } else if (msg.msgid == MAVLINK_MSG_ID_COMMAND_INT) {
849 mavlink_command_int_t request{};
850 mavlink_msg_command_int_decode(&msg, &request);
851
852 _receivedMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
853 _receivedMavCommandByCompCountMap[static_cast<MAV_CMD>(request.command)][request.target_component]++;
854 }
855}
856
857void MockLink::_handleIncomingMavlinkMsg(const mavlink_message_t &msg)
858{
859 _updateIncomingMessageCounts(msg);
860
861 if (_missionItemHandler->handleMavlinkMessage(msg)) {
862 return;
863 }
864
865 if (_enableCamera && _mockLinkCamera->handleMavlinkMessage(msg)) {
866 return;
867 }
868
869 if (_enableGimbal && _mockLinkGimbal->handleMavlinkMessage(msg)) {
870 return;
871 }
872
873 switch (msg.msgid) {
874 case MAVLINK_MSG_ID_HEARTBEAT:
875 _handleHeartBeat(msg);
876 break;
877 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
878 _handleParamRequestList(msg);
879 break;
880 case MAVLINK_MSG_ID_SET_MODE:
881 _handleSetMode(msg);
882 break;
883 case MAVLINK_MSG_ID_PARAM_SET:
884 _handleParamSet(msg);
885 break;
886 case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
887 _handleParamRequestRead(msg);
888 break;
889 case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
890 _handleFTP(msg);
891 break;
892 case MAVLINK_MSG_ID_COMMAND_LONG:
893 _handleCommandLong(msg);
894 break;
895 case MAVLINK_MSG_ID_COMMAND_INT:
896 _handleCommandInt(msg);
897 break;
898 case MAVLINK_MSG_ID_MANUAL_CONTROL:
899 _handleManualControl(msg);
900 break;
901 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
902 _handleRCChannelsOverride(msg);
903 break;
904 case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
905 _handleLogRequestList(msg);
906 break;
907 case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
908 _handleLogRequestData(msg);
909 break;
910 case MAVLINK_MSG_ID_PARAM_MAP_RC:
911 _handleParamMapRC(msg);
912 break;
913 case MAVLINK_MSG_ID_SETUP_SIGNING:
914 _handleSetupSigning(msg);
915 break;
916 case MAVLINK_MSG_ID_COMMAND_ACK:
917 // GCS sends COMMAND_ACK(command=0) via nextClicked() to acknowledge each pose
918 // during APM full accel calibration (the "Next" button press).
919 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
920 mavlink_command_ack_t ack{};
921 mavlink_msg_command_ack_decode(&msg, &ack);
922 if (ack.command == 0) {
923 QMutexLocker locker(&_apmAccelCalMutex);
924 if (_apmAccelCalPosIndex >= 0 && _apmAccelCalPosIndex < 6) {
925 _apmAccelCalGotAck = true;
926 }
927 }
928 }
929 break;
930 default:
931 break;
932 }
933}
934
935void MockLink::_handleHeartBeat(const mavlink_message_t &msg)
936{
937 Q_UNUSED(msg);
938 qCDebug(MockLinkVerboseLog) << "Heartbeat";
939}
940
941void MockLink::_handleParamMapRC(const mavlink_message_t &msg)
942{
943 mavlink_param_map_rc_t paramMapRC{};
944 mavlink_msg_param_map_rc_decode(&msg, &paramMapRC);
945
946 const QString paramName(QString::fromLocal8Bit(paramMapRC.param_id, static_cast<int>(strnlen(paramMapRC.param_id, MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN))));
947
948 if (paramMapRC.param_index == -1) {
949 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);
950 } else if (paramMapRC.param_index == -2) {
951 qCDebug(MockLinkLog) << "MockLink - PARAM_MAP_RC: Clear tuningID" << paramMapRC.parameter_rc_channel_index;
952 } else {
953 qCWarning(MockLinkLog) << "MockLink - PARAM_MAP_RC: Unsupported param_index" << paramMapRC.param_index;
954 }
955}
956
957void MockLink::_handleSetupSigning(const mavlink_message_t &msg)
958{
959 mavlink_setup_signing_t setupSigning{};
960 mavlink_msg_setup_signing_decode(&msg, &setupSigning);
961
962 if (setupSigning.target_system != _vehicleSystemId) {
963 return;
964 }
965
966 // All-zero key = disable signing
967 bool allZeroKey = true;
968 for (const uint8_t byte : setupSigning.secret_key) {
969 if (byte != 0) {
970 allZeroKey = false;
971 break;
972 }
973 }
974
975 _signingEnabled = !allZeroKey;
976
977 // Write C-state directly; routing through SigningController would clobber its _keyHint mid-confirmation.
978 mavlink_status_t* const status = mavlink_get_channel_status(_outgoingMavlinkChannel);
979 if (_signingEnabled) {
980 memcpy(_mockSigning.secret_key, setupSigning.secret_key, sizeof(_mockSigning.secret_key));
981 _mockSigning.link_id = _outgoingMavlinkChannel;
982 _mockSigning.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING;
983 _mockSigning.timestamp = MAVLinkSigning::currentSigningTimestampTicks();
984 _mockSigning.accept_unsigned_callback = MAVLinkSigning::insecureConnectionAcceptUnsignedCallback;
985 status->signing = &_mockSigning;
986 status->signing_streams = &_mockSigningStreams;
987 } else {
988 QGC::secureZero(_mockSigning.secret_key, sizeof(_mockSigning.secret_key));
989 _mockSigning.accept_unsigned_callback = nullptr;
990 status->signing = nullptr;
991 status->signing_streams = nullptr;
992 }
993
994 qCDebug(MockLinkLog) << "Signing" << (_signingEnabled ? "enabled" : "disabled");
995}
996
997void MockLink::_handleSetMode(const mavlink_message_t &msg)
998{
999 mavlink_set_mode_t request{};
1000 mavlink_msg_set_mode_decode(&msg, &request);
1001
1002 Q_ASSERT(request.target_system == _vehicleSystemId);
1003
1004 _mavBaseMode = request.base_mode;
1005 _mavCustomMode = request.custom_mode;
1006}
1007
1008void MockLink::_handleManualControl(const mavlink_message_t &msg)
1009{
1010 mavlink_manual_control_t manualControl{};
1011 mavlink_msg_manual_control_decode(&msg, &manualControl);
1012
1013 // INT16_MAX means "axis invalid/not provided" per MAVLink spec
1014 const auto axisStr = [](int16_t v) -> QString {
1015 return (v == INT16_MAX) ? QStringLiteral("invalid") : QString::number(v);
1016 };
1017 // Extension fields are only valid when the corresponding enabled_extensions bit is set
1018 const auto extStr = [](int16_t v, bool enabled) -> QString {
1019 return enabled ? QString::number(v) : QStringLiteral("disabled");
1020 };
1021
1022 const uint8_t ext = manualControl.enabled_extensions;
1023
1024 qCDebug(MockLinkVerboseLog).noquote()
1025 << "MANUAL_CONTROL"
1026 << "target:" << manualControl.target
1027 << "x:" << axisStr(manualControl.x)
1028 << "y:" << axisStr(manualControl.y)
1029 << "z:" << axisStr(manualControl.z)
1030 << "r:" << axisStr(manualControl.r)
1031 << "buttons:" << QStringLiteral("0x%1").arg(manualControl.buttons, 4, 16, QLatin1Char('0'))
1032 << "buttons2:" << QStringLiteral("0x%1").arg(manualControl.buttons2, 4, 16, QLatin1Char('0'))
1033 << "enabled_extensions:" << QStringLiteral("0x%1").arg(ext, 2, 16, QLatin1Char('0'))
1034 << "s(pitch):" << extStr(manualControl.s, ext & (1 << 0))
1035 << "t(roll):" << extStr(manualControl.t, ext & (1 << 1))
1036 << "aux1:" << extStr(manualControl.aux1, ext & (1 << 2))
1037 << "aux2:" << extStr(manualControl.aux2, ext & (1 << 3))
1038 << "aux3:" << extStr(manualControl.aux3, ext & (1 << 4))
1039 << "aux4:" << extStr(manualControl.aux4, ext & (1 << 5))
1040 << "aux5:" << extStr(manualControl.aux5, ext & (1 << 6))
1041 << "aux6:" << extStr(manualControl.aux6, ext & (1 << 7));
1042}
1043
1044void MockLink::_handleRCChannelsOverride(const mavlink_message_t &msg)
1045{
1046 mavlink_rc_channels_override_t override{};
1047 mavlink_msg_rc_channels_override_decode(&msg, &override);
1048
1049 // Per the MAVLink spec:
1050 // Channels 1-8: UINT16_MAX = ignore (no state change), 0 = release back to RC radio
1051 // Channels 9-18: UINT16_MAX or 0 = ignore, UINT16_MAX-1 = release back to RC radio
1052 const uint16_t rawValues[18] = {
1053 override.chan1_raw, override.chan2_raw, override.chan3_raw, override.chan4_raw,
1054 override.chan5_raw, override.chan6_raw, override.chan7_raw, override.chan8_raw,
1055 override.chan9_raw, override.chan10_raw, override.chan11_raw, override.chan12_raw,
1056 override.chan13_raw, override.chan14_raw, override.chan15_raw, override.chan16_raw,
1057 override.chan17_raw, override.chan18_raw,
1058 };
1059
1060 bool anyChange = false;
1061 for (int i = 0; i < kRcChannelOverrideChannelCount; ++i) {
1062 const uint16_t raw = rawValues[i];
1063 const bool isExtended = (i >= 8);
1064
1065 RCChannelOverride::State newState;
1066 if (isExtended) {
1067 if (raw == 0 || raw == UINT16_MAX) {
1068 continue; // ignore — no change to this channel's state
1069 } else if (raw == static_cast<uint16_t>(UINT16_MAX - 1)) {
1070 newState = RCChannelOverride::State::Released;
1071 } else {
1072 newState = RCChannelOverride::State::Overridden;
1073 }
1074 } else {
1075 if (raw == UINT16_MAX) {
1076 continue; // ignore — no change to this channel's state
1077 } else if (raw == 0) {
1078 newState = RCChannelOverride::State::Released;
1079 } else {
1080 newState = RCChannelOverride::State::Overridden;
1081 }
1082 }
1083
1084 RCChannelOverride &ch = _rcChannelOverrides[i];
1085 if (ch.state == newState) {
1086 continue;
1087 }
1088
1089 anyChange = true;
1090
1091 const auto stateLabel = [](RCChannelOverride::State s) -> const char * {
1092 switch (s) {
1093 case RCChannelOverride::State::Ignore: return "ignore";
1094 case RCChannelOverride::State::Released: return "released";
1095 case RCChannelOverride::State::Overridden: return "overridden";
1096 }
1097 return "unknown";
1098 };
1099 qCDebug(MockLinkLog).noquote() << QStringLiteral("RC_CHANNELS_OVERRIDE ch%1: %2 -> %3").arg(i + 1).arg(stateLabel(ch.state)).arg(stateLabel(newState));
1100
1101 ch.state = newState;
1102 ch.value = (newState == RCChannelOverride::State::Overridden) ? raw : 0;
1103 }
1104
1105 if (anyChange) {
1106 QStringList active;
1107 for (int i = 0; i < kRcChannelOverrideChannelCount; ++i) {
1108 if (_rcChannelOverrides[i].state == RCChannelOverride::State::Overridden) {
1109 active << QStringLiteral("ch%1").arg(i + 1);
1110 }
1111 }
1112 if (active.isEmpty()) {
1113 qCDebug(MockLinkLog) << "RC_CHANNELS_OVERRIDE: no channels currently overridden";
1114 } else {
1115 qCDebug(MockLinkLog).noquote() << "RC_CHANNELS_OVERRIDE active overrides:" << active.join(QStringLiteral(", "));
1116 }
1117 }
1118
1119 for (int i = 0; i < kRcChannelOverrideChannelCount; ++i) {
1120 if (_rcChannelOverrides[i].state == RCChannelOverride::State::Overridden) {
1121 qCDebug(MockLinkVerboseLog).noquote() << QStringLiteral("RC_CHANNELS_OVERRIDE ch%1 value: %2").arg(i + 1).arg(_rcChannelOverrides[i].value);
1122 }
1123 }
1124}
1125
1126void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString &paramName, float paramFloat)
1127{
1128 Q_ASSERT(_mapParamName2Value.contains(componentId));
1129 Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
1130 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
1131
1132 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
1133 QVariant paramVariant;
1134 mavlink_param_union_t valueUnion{};
1135 valueUnion.param_float = paramFloat;
1136 switch (paramType) {
1137 case MAV_PARAM_TYPE_REAL32:
1138 paramVariant = QVariant::fromValue(valueUnion.param_float);
1139 break;
1140 case MAV_PARAM_TYPE_UINT32:
1141 paramVariant = QVariant::fromValue(valueUnion.param_uint32);
1142 break;
1143 case MAV_PARAM_TYPE_INT32:
1144 paramVariant = QVariant::fromValue(valueUnion.param_int32);
1145 break;
1146 case MAV_PARAM_TYPE_UINT16:
1147 paramVariant = QVariant::fromValue(valueUnion.param_uint16);
1148 break;
1149 case MAV_PARAM_TYPE_INT16:
1150 paramVariant = QVariant::fromValue(valueUnion.param_int16);
1151 break;
1152 case MAV_PARAM_TYPE_UINT8:
1153 paramVariant = QVariant::fromValue(valueUnion.param_uint8);
1154 break;
1155 case MAV_PARAM_TYPE_INT8:
1156 paramVariant = QVariant::fromValue(valueUnion.param_int8);
1157 break;
1158 default:
1159 qCCritical(MockLinkLog) << "Invalid parameter type" << paramType;
1160 paramVariant = QVariant::fromValue(valueUnion.param_int32);
1161 break;
1162 }
1163
1164 qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
1165 _mapParamName2Value[componentId][paramName] = paramVariant;
1166}
1167
1168void MockLink::setMockParamValue(int componentId, const QString &paramName, float value)
1169{
1170 mavlink_param_union_t valueUnion{};
1171 valueUnion.param_float = value;
1172 _setParamFloatUnionIntoMap(componentId, paramName, valueUnion.param_float);
1173}
1174
1175float MockLink::_floatUnionForParam(int componentId, const QString &paramName)
1176{
1177 Q_ASSERT(_mapParamName2Value.contains(componentId));
1178 Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
1179 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
1180
1181 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
1182 const QVariant paramVar = _mapParamName2Value[componentId][paramName];
1183
1184 mavlink_param_union_t valueUnion{};
1185 switch (paramType) {
1186 case MAV_PARAM_TYPE_REAL32:
1187 valueUnion.param_float = paramVar.toFloat();
1188 break;
1189 case MAV_PARAM_TYPE_UINT32:
1190 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1191 valueUnion.param_float = paramVar.toUInt();
1192 } else {
1193 valueUnion.param_uint32 = paramVar.toUInt();
1194 }
1195 break;
1196 case MAV_PARAM_TYPE_INT32:
1197 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1198 valueUnion.param_float = paramVar.toInt();
1199 } else {
1200 valueUnion.param_int32 = paramVar.toInt();
1201 }
1202 break;
1203 case MAV_PARAM_TYPE_UINT16:
1204 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1205 valueUnion.param_float = paramVar.toUInt();
1206 } else {
1207 valueUnion.param_uint16 = paramVar.toUInt();
1208 }
1209 break;
1210 case MAV_PARAM_TYPE_INT16:
1211 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1212 valueUnion.param_float = paramVar.toInt();
1213 } else {
1214 valueUnion.param_int16 = paramVar.toInt();
1215 }
1216 break;
1217 case MAV_PARAM_TYPE_UINT8:
1218 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1219 valueUnion.param_float = paramVar.toUInt();
1220 } else {
1221 valueUnion.param_uint8 = paramVar.toUInt();
1222 }
1223 break;
1224 case MAV_PARAM_TYPE_INT8:
1225 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1226 valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1();
1227 } else {
1228 valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1();
1229 }
1230 break;
1231 default:
1232 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1233 valueUnion.param_float = paramVar.toInt();
1234 } else {
1235 valueUnion.param_int32 = paramVar.toInt();
1236 }
1237 qCCritical(MockLinkLog) << "Invalid parameter type" << paramType;
1238 }
1239
1240 return valueUnion.param_float;
1241}
1242
1243uint32_t MockLink::_computeParamHash(int componentId) const
1244{
1245 // Volatile parameters are excluded from the hash, matching PX4 firmware and ParameterManager::_tryCacheHashLoad
1246 static const QStringList volatileParams = {
1247 QStringLiteral("COM_FLIGHT_UUID"),
1248 QStringLiteral("EKF2_MAGBIAS_X"),
1249 QStringLiteral("EKF2_MAGBIAS_Y"),
1250 QStringLiteral("EKF2_MAGBIAS_Z"),
1251 QStringLiteral("EKF2_MAG_DECL"),
1252 QStringLiteral("LND_FLIGHT_T_HI"),
1253 QStringLiteral("LND_FLIGHT_T_LO"),
1254 QStringLiteral("SYS_RESTART_TYPE"),
1255 };
1256
1257 uint32_t crc = 0;
1258 const auto &params = _mapParamName2Value[componentId];
1259 for (auto it = params.constBegin(); it != params.constEnd(); ++it) {
1260 const QString &name = it.key();
1261 if (volatileParams.contains(name)) {
1262 continue;
1263 }
1264 const QVariant &value = it.value();
1265 const MAV_PARAM_TYPE mavType = _mapParamName2MavParamType[componentId][name];
1266 const FactMetaData::ValueType_t factType = ParameterManager::mavTypeToFactType(mavType);
1267
1268 crc = QGC::crc32(reinterpret_cast<const uint8_t *>(qPrintable(name)), name.length(), crc);
1269 crc = QGC::crc32(static_cast<const uint8_t *>(value.constData()), FactMetaData::typeToSize(factType), crc);
1270 }
1271 return crc;
1272}
1273
1274void MockLink::_handleParamRequestList(const mavlink_message_t &msg)
1275{
1276 if (_failureMode == MockConfiguration::FailParamNoResponseToRequestList) {
1277 return;
1278 }
1279
1280 mavlink_param_request_list_t request{};
1281 mavlink_msg_param_request_list_decode(&msg, &request);
1282
1283 Q_ASSERT(request.target_system == _vehicleSystemId);
1284 Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);
1285
1286 // Cache component IDs and first component's param names to avoid repeated keys() calls in worker
1287 // Thread safety: Lock mutex before modifying shared state accessed by worker thread
1288 QMutexLocker locker(&_paramRequestListMutex);
1289 _paramRequestListComponentIds = _mapParamName2Value.keys();
1290 if (!_paramRequestListComponentIds.isEmpty()) {
1291 _paramRequestListParamNames = _mapParamName2Value[_paramRequestListComponentIds.first()].keys();
1292 }
1293
1294 // Start the worker routine
1295 _currentParamRequestListComponentIndex = 0;
1296 _currentParamRequestListParamIndex = 0;
1297 _paramRequestListHashCheckSent = false;
1298}
1299
1300void MockLink::_paramRequestListWorker()
1301{
1302 if (_currentParamRequestListComponentIndex == -1) {
1303 // Initial request complete
1304 return;
1305 }
1306
1307 // Thread safety: Lock mutex before accessing shared state modified by main thread
1308 QMutexLocker locker(&_paramRequestListMutex);
1309
1310 // Use cached lists instead of calling keys() on every iteration (500Hz)
1311 if (_currentParamRequestListComponentIndex >= _paramRequestListComponentIds.count()) {
1312 _currentParamRequestListComponentIndex = -1;
1313 return;
1314 }
1315
1316 const int componentId = _paramRequestListComponentIds.at(_currentParamRequestListComponentIndex);
1317 const int cParameters = _paramRequestListParamNames.count();
1318
1319 if (_currentParamRequestListParamIndex >= cParameters) {
1320 // All regular params sent — for PX4, append _HASH_CHECK as the last entry in the stream.
1321 // Uses param_count=0, param_index=-1 (same as standalone response) so ParameterManager
1322 // handles it via the _HASH_CHECK early-return path without affecting param count tracking.
1323 if (_firmwareType == MAV_AUTOPILOT_PX4 && !_paramRequestListHashCheckSent) {
1324 _paramRequestListHashCheckSent = true;
1325
1326 mavlink_param_union_t valueUnion{};
1327 valueUnion.type = MAV_PARAM_TYPE_UINT32;
1328 valueUnion.param_uint32 = _computeParamHash(componentId);
1329
1330 char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{};
1331 (void) strncpy(paramId, "_HASH_CHECK", MAVLINK_MSG_ID_PARAM_VALUE_LEN);
1332
1333 qCDebug(MockLinkLog) << "Sending _HASH_CHECK in PARAM_REQUEST_LIST stream" << componentId << "hash:" << valueUnion.param_uint32;
1334
1335 mavlink_message_t responseMsg{};
1336 (void) mavlink_msg_param_value_pack_chan(
1337 _vehicleSystemId,
1338 componentId,
1339 _outgoingMavlinkChannel,
1340 &responseMsg,
1341 paramId,
1342 valueUnion.param_float,
1343 MAV_PARAM_TYPE_UINT32,
1344 0, // param_count: 0 to avoid affecting ParameterManager's count tracking
1345 -1 // param_index: -1 signals this is a virtual/out-of-band parameter
1346 );
1347 respondWithMavlinkMessage(responseMsg);
1348 return;
1349 }
1350
1351 // Move to next component
1352 if (++_currentParamRequestListComponentIndex >= _paramRequestListComponentIds.count()) {
1353 _currentParamRequestListComponentIndex = -1;
1354 _paramRequestListComponentIds.clear();
1355 _paramRequestListParamNames.clear();
1356 } else {
1357 // Cache param names for the new component
1358 _paramRequestListParamNames = _mapParamName2Value[_paramRequestListComponentIds.at(_currentParamRequestListComponentIndex)].keys();
1359 _currentParamRequestListParamIndex = 0;
1360 _paramRequestListHashCheckSent = false;
1361 }
1362 return;
1363 }
1364
1365 const QString &paramName = _paramRequestListParamNames.at(_currentParamRequestListParamIndex);
1366
1367 if (((_failureMode == MockConfiguration::FailMissingParamOnInitialRequest) || (_failureMode == MockConfiguration::FailMissingParamOnAllRequests)) && (paramName == _failParam)) {
1368 qCDebug(MockLinkLog) << "Skipping param send:" << paramName;
1369 } else {
1370 char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{};
1371 mavlink_message_t responseMsg{};
1372
1373 Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
1374 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
1375
1376 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
1377
1378 Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN);
1379 (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);
1380
1381 qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId];
1382
1383 (void) mavlink_msg_param_value_pack_chan(
1384 _vehicleSystemId,
1385 componentId, // component id
1386 _outgoingMavlinkChannel,
1387 &responseMsg, // Outgoing message
1388 paramId, // Parameter name
1389 _floatUnionForParam(componentId, paramName), // Parameter value
1390 paramType, // MAV_PARAM_TYPE
1391 cParameters, // Total number of parameters
1392 _currentParamRequestListParamIndex // Index of this parameter
1393 );
1394 respondWithMavlinkMessage(responseMsg);
1395 }
1396
1397 // Move to next param index
1398 ++_currentParamRequestListParamIndex;
1399}
1400
1401void MockLink::_handleParamSet(const mavlink_message_t &msg)
1402{
1403 mavlink_param_set_t request{};
1404 mavlink_msg_param_set_decode(&msg, &request);
1405
1406 Q_ASSERT(request.target_system == _vehicleSystemId);
1407 const int componentId = request.target_component;
1408
1409 // Param may not be null terminated if exactly fits
1410 char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1]{};
1411 paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN] = 0;
1412 (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
1413
1414 qCDebug(MockLinkLog) << "_handleParamSet" << componentId << paramId << request.param_type;
1415
1416 // PX4 special case: _HASH_CHECK is a virtual parameter used by ParameterManager
1417 // to signal cache-hit and stop parameter streaming. It is intentionally not part
1418 // of the normal parameter maps.
1419 if ((_firmwareType == MAV_AUTOPILOT_PX4) && (strncmp(paramId, "_HASH_CHECK", MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN) == 0)) {
1420 QMutexLocker locker(&_paramRequestListMutex);
1421 _currentParamRequestListComponentIndex = -1;
1422 _paramRequestListComponentIds.clear();
1423 _paramRequestListParamNames.clear();
1424 qCDebug(MockLinkLog) << "Received _HASH_CHECK PARAM_SET, stopping parameter stream";
1425 return;
1426 }
1427
1428 Q_ASSERT(_mapParamName2Value.contains(componentId));
1429 Q_ASSERT(_mapParamName2MavParamType.contains(componentId));
1430 Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
1431 Q_ASSERT(request.param_type == _mapParamName2MavParamType[componentId][paramId]);
1432
1433 // Apply failure behaviors before committing change.
1434 if (_paramSetFailureMode == FailParamSetFirstAttemptNoAck && _paramSetFailureFirstAttemptPending) {
1435 qCDebug(MockLinkLog) << "Param set failure: first attempt no ack" << paramId;
1436 _paramSetFailureFirstAttemptPending = false;
1437 return;
1438 }
1439
1440 if (_paramSetFailureMode == FailParamSetNoAck) {
1441 qCDebug(MockLinkLog) << "Param set failure: no ack" << paramId;
1442 return;
1443 }
1444
1445 if (_paramSetFailureMode == FailParamSetParamError) {
1446 qCDebug(MockLinkLog) << "Param set failure: PARAM_ERROR" << paramId;
1447 _sendParamError(componentId, paramId,
1448 _mapParamName2Value[componentId].keys().indexOf(paramId),
1449 MAV_PARAM_ERROR_VALUE_OUT_OF_RANGE);
1450 return;
1451 }
1452
1453 // Normal success path
1454 _setParamFloatUnionIntoMap(componentId, paramId, request.param_value);
1455
1456 mavlink_message_t responseMsg;
1457 mavlink_msg_param_value_pack_chan(
1458 _vehicleSystemId,
1459 componentId, // component id
1460 _outgoingMavlinkChannel,
1461 &responseMsg, // Outgoing message
1462 paramId, // Parameter name
1463 request.param_value, // Send same value back
1464 request.param_type, // Send same type back
1465 _mapParamName2Value[componentId].count(), // Total number of parameters
1466 _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter
1467 );
1468 respondWithMavlinkMessage(responseMsg);
1469}
1470
1471void MockLink::_handleParamRequestRead(const mavlink_message_t &msg)
1472{
1473 mavlink_message_t responseMsg{};
1474 mavlink_param_request_read_t request{};
1475 mavlink_msg_param_request_read_decode(&msg, &request);
1476
1477 const QString paramName(QString::fromLocal8Bit(request.param_id, static_cast<int>(strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN))));
1478 const int componentId = request.target_component;
1479
1480 // special case for magic _HASH_CHECK value (PX4 only)
1481 if ((_firmwareType == MAV_AUTOPILOT_PX4) && (paramName == "_HASH_CHECK")) {
1482 _hashCheckRequestCount++;
1483 if (_hashCheckNoResponse) {
1484 return;
1485 }
1486
1487 const int hashComponentId = _mapParamName2Value.contains(MAV_COMP_ID_AUTOPILOT1)
1488 ? MAV_COMP_ID_AUTOPILOT1
1489 : _mapParamName2Value.keys().first();
1490
1491 mavlink_param_union_t valueUnion{};
1492 valueUnion.type = MAV_PARAM_TYPE_UINT32;
1493 valueUnion.param_uint32 = _computeParamHash(hashComponentId);
1494 (void) mavlink_msg_param_value_pack_chan(
1495 _vehicleSystemId,
1496 hashComponentId,
1497 _outgoingMavlinkChannel,
1498 &responseMsg,
1499 request.param_id,
1500 valueUnion.param_float,
1501 MAV_PARAM_TYPE_UINT32,
1502 0,
1503 -1
1504 );
1505 respondWithMavlinkMessage(responseMsg);
1506 return;
1507 }
1508
1509 Q_ASSERT(_mapParamName2Value.contains(componentId));
1510
1511 char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]{};
1512 paramId[0] = 0;
1513
1514 Q_ASSERT(request.target_system == _vehicleSystemId);
1515
1516 if (request.param_index == -1) {
1517 // Request is by param name. Param may not be null terminated if exactly fits
1518 (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
1519 } else {
1520 // Request is by index
1521 Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value[componentId].count());
1522
1523 const QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
1524 Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
1525 strcpy(paramId, key.toLocal8Bit().constData());
1526 }
1527
1528 Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
1529 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramId));
1530
1531 if ((_failureMode == MockConfiguration::FailMissingParamOnAllRequests) && (strcmp(paramId, _failParam) == 0)) {
1532 qCDebug(MockLinkLog) << "Ignoring request read for " << _failParam;
1533 // Fail to send this param no matter what
1534 return;
1535 }
1536
1537 if (_paramRequestReadFailureMode == FailParamRequestReadFirstAttemptNoResponse && _paramRequestReadFailureFirstAttemptPending) {
1538 qCDebug(MockLinkLog) << "Param request read failure: first attempt no response" << paramId;
1539 _paramRequestReadFailureFirstAttemptPending = false;
1540 return;
1541 }
1542
1543 if (_paramRequestReadFailureMode == FailParamRequestReadNoResponse) {
1544 qCDebug(MockLinkLog) << "Param request read failure: no response" << paramId;
1545 return;
1546 }
1547
1548 if (_paramRequestReadFailureMode == FailParamRequestReadParamError) {
1549 qCDebug(MockLinkLog) << "Param request read failure: PARAM_ERROR" << paramId;
1550 _sendParamError(componentId, paramId, request.param_index, MAV_PARAM_ERROR_DOES_NOT_EXIST);
1551 return;
1552 }
1553
1554 (void) mavlink_msg_param_value_pack_chan(
1555 _vehicleSystemId,
1556 componentId, // component id
1557 _outgoingMavlinkChannel,
1558 &responseMsg, // Outgoing message
1559 paramId, // Parameter name
1560 _floatUnionForParam(componentId, paramId), // Parameter value
1561 _mapParamName2MavParamType[componentId][paramId], // Parameter type
1562 _mapParamName2Value[componentId].count(), // Total number of parameters
1563 _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter
1564 );
1565 respondWithMavlinkMessage(responseMsg);
1566}
1567
1568void MockLink::_sendParamError(int componentId, const char *paramId, int16_t paramIndex, uint8_t errorCode)
1569{
1570 mavlink_message_t responseMsg{};
1571 char paramIdBuf[MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN + 1] = {};
1572 (void) strncpy(paramIdBuf, paramId, MAVLINK_MSG_PARAM_ERROR_FIELD_PARAM_ID_LEN);
1573
1574 (void) mavlink_msg_param_error_pack_chan(
1575 _vehicleSystemId,
1576 static_cast<uint8_t>(componentId),
1577 _outgoingMavlinkChannel,
1578 &responseMsg,
1579 MAVLinkProtocol::instance()->getSystemId(),
1580 MAVLinkProtocol::getComponentId(),
1581 paramIdBuf,
1582 paramIndex,
1583 errorCode
1584 );
1585 respondWithMavlinkMessage(responseMsg);
1586}
1587
1588void MockLink::_handleFTP(const mavlink_message_t &msg)
1589{
1590 _mockLinkFTP->mavlinkMessageReceived(msg);
1591}
1592
1593void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t &request)
1594{
1595 uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
1596
1597 switch (request.command) {
1598 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED:
1599 // Test command which sends in progress messages and then acceptance ack
1600 commandResult = MAV_RESULT_ACCEPTED;
1601 break;
1602 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED:
1603 // Test command which sends in progress messages and then failure ack
1604 commandResult = MAV_RESULT_FAILED;
1605 break;
1606 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK:
1607 // Test command which sends in progress messages and then never sends final result ack
1608 break;
1609 }
1610
1611 mavlink_message_t commandAck{};
1612 (void) mavlink_msg_command_ack_pack_chan(
1613 _vehicleSystemId,
1614 _vehicleComponentId,
1615 _outgoingMavlinkChannel,
1616 &commandAck,
1617 request.command,
1618 MAV_RESULT_IN_PROGRESS,
1619 1, // progress
1620 0, // result_param2
1621 0, // target_system
1622 0 // target_component
1623 );
1624 respondWithMavlinkMessage(commandAck);
1625
1626 if (request.command != MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK) {
1627 (void) mavlink_msg_command_ack_pack_chan(
1628 _vehicleSystemId,
1629 _vehicleComponentId,
1630 _outgoingMavlinkChannel,
1631 &commandAck,
1632 request.command,
1633 commandResult,
1634 0, // progress
1635 0, // result_param2
1636 0, // target_system
1637 0 // target_component
1638 );
1639 respondWithMavlinkMessage(commandAck);
1640 }
1641}
1642
1643void MockLink::_handleCommandLongSetMessageInterval(const mavlink_command_long_t &request, bool &accepted)
1644{
1645 // Accept only the message IDs that MAVLinkStreamConfig requests for PID tuning.
1646 // Anything else gets MAV_RESULT_UNSUPPORTED so unit tests will catch unexpected usage.
1647 static const QSet<int> kPidTuningMessageIds = {
1648 MAVLINK_MSG_ID_ATTITUDE_QUATERNION,
1649 MAVLINK_MSG_ID_ATTITUDE_TARGET,
1650 MAVLINK_MSG_ID_LOCAL_POSITION_NED,
1651 MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED,
1652 MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT,
1653 MAVLINK_MSG_ID_VFR_HUD,
1654 };
1655 accepted = kPidTuningMessageIds.contains(static_cast<int>(request.param1));
1656}
1657
1658void MockLink::_handleCommandLong(const mavlink_message_t &msg)
1659{
1660 static bool firstCmdUser3 = true;
1661 static bool firstCmdUser4 = true;
1662
1663 mavlink_command_long_t request{};
1664 mavlink_msg_command_long_decode(&msg, &request);
1665
1666 uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
1667
1668 switch (request.command) {
1669 case MAV_CMD_COMPONENT_ARM_DISARM:
1670 if (request.param1 == 0.0f) {
1671 _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
1672 } else {
1673 _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
1674 }
1675 commandResult = MAV_RESULT_ACCEPTED;
1676 break;
1677 case MAV_CMD_PREFLIGHT_CALIBRATION:
1678 _handlePreFlightCalibration(request);
1679 commandResult = MAV_RESULT_ACCEPTED;
1680 break;
1681 case MAV_CMD_DO_MOTOR_TEST:
1682 commandResult = MAV_RESULT_ACCEPTED;
1683 break;
1684 case MAV_CMD_DO_START_MAG_CAL:
1685 // APM onboard compass calibration: start sending MAG_CAL_PROGRESS then MAG_CAL_REPORT
1686 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1687 QMutexLocker locker(&_apmCompassCalMutex);
1688 _apmCompassCalProgress = 0;
1689 _apmCompassCalTickCount = 0;
1690 commandResult = MAV_RESULT_ACCEPTED;
1691 }
1692 break;
1693 case MAV_CMD_DO_CANCEL_MAG_CAL:
1694 // Stop APM compass calibration worker
1695 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1696 QMutexLocker locker(&_apmCompassCalMutex);
1697 _apmCompassCalProgress = -1;
1698 commandResult = MAV_RESULT_ACCEPTED;
1699 }
1700 break;
1701 case MAV_CMD_CONTROL_HIGH_LATENCY:
1702 if (linkConfiguration()->isHighLatency()) {
1703 _highLatencyTransmissionEnabled = static_cast<int>(request.param1) != 0;
1704 emit highLatencyTransmissionEnabledChanged(_highLatencyTransmissionEnabled);
1705 commandResult = MAV_RESULT_ACCEPTED;
1706 } else {
1707 commandResult = MAV_RESULT_FAILED;
1708 }
1709 break;
1710 case MAV_CMD_PREFLIGHT_STORAGE:
1711 if (static_cast<int>(request.param1) == 2) {
1712 // Reset all parameters to firmware defaults (unit test support)
1713 _resetParamsToDefaults();
1714 }
1715 commandResult = MAV_RESULT_ACCEPTED;
1716 break;
1717 case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
1718 // Unit test support: accept the reboot command so tests can exercise
1719 // flows which restart the vehicle (e.g. airframe Apply and Restart).
1720 // The actual reboot is not simulated.
1721 commandResult = MAV_RESULT_ACCEPTED;
1722 break;
1723 case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
1724 commandResult = MAV_RESULT_ACCEPTED;
1725 _respondWithAutopilotVersion();
1726 break;
1727 case MAV_CMD_REQUEST_MESSAGE:
1728 {
1729 bool accepted = false;
1730 bool noAck = false;
1731 _handleRequestMessage(request, accepted, noAck);
1732 if (noAck) {
1733 // FailRequestMessageCommandNoResponse: don't send any ack, let vehicle timeout
1734 return;
1735 }
1736 if (accepted) {
1737 commandResult = MAV_RESULT_ACCEPTED;
1738 }
1739 break;
1740 }
1741 case MAV_CMD_NAV_TAKEOFF:
1742 _handleTakeoff(request);
1743 commandResult = MAV_RESULT_ACCEPTED;
1744 break;
1745 case MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED:
1746 // Test command which always returns MAV_RESULT_ACCEPTED
1747 commandResult = MAV_RESULT_ACCEPTED;
1748 break;
1749 case MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED:
1750 // Test command which always returns MAV_RESULT_FAILED
1751 commandResult = MAV_RESULT_FAILED;
1752 break;
1753 case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED:
1754 // Test command which does not respond to first request and returns MAV_RESULT_ACCEPTED on second attempt
1755 if (firstCmdUser3) {
1756 firstCmdUser3 = false;
1757 return;
1758 } else {
1759 firstCmdUser3 = true;
1760 commandResult = MAV_RESULT_ACCEPTED;
1761 }
1762 break;
1763 case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED:
1764 // Test command which does not respond to first request and returns MAV_RESULT_FAILED on second attempt
1765 if (firstCmdUser4) {
1766 firstCmdUser4 = false;
1767 return;
1768 } else {
1769 firstCmdUser4 = true;
1770 commandResult = MAV_RESULT_FAILED;
1771 }
1772 break;
1773 case MAV_CMD_MOCKLINK_NO_RESPONSE:
1774 case MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY:
1775 // Test command which never responds
1776 return;
1777 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED:
1778 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED:
1779 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK:
1780 _handleInProgressCommandLong(request);
1781 return;
1782 case MAV_CMD_SET_MESSAGE_INTERVAL:
1783 {
1784 bool accepted = false;
1785
1786 _handleCommandLongSetMessageInterval(request, accepted);
1787 if (accepted) {
1788 commandResult = MAV_RESULT_ACCEPTED;
1789 }
1790 break;
1791 }
1792 }
1793
1794 mavlink_message_t commandAck{};
1795 (void) mavlink_msg_command_ack_pack_chan(
1796 _vehicleSystemId,
1797 _vehicleComponentId,
1798 _outgoingMavlinkChannel,
1799 &commandAck,
1800 request.command,
1801 commandResult,
1802 0, // progress
1803 0, // result_param2
1804 0, // target_system
1805 0 // target_component
1806 );
1807 respondWithMavlinkMessage(commandAck);
1808}
1809
1810void MockLink::_handleCommandInt(const mavlink_message_t &msg)
1811{
1812 mavlink_command_int_t request{};
1813 mavlink_msg_command_int_decode(&msg, &request);
1814
1815 // MockLink does not implement any COMMAND_INT commands yet, so it reports them as
1816 // unsupported (mirroring the COMMAND_LONG default for unrecognized commands). This
1817 // lets unit tests exercise "try command, fall back to legacy message" code paths
1818 // such as Vehicle::setEstimatorOrigin.
1819 const uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
1820
1821 mavlink_message_t commandAck{};
1822 (void) mavlink_msg_command_ack_pack_chan(
1823 _vehicleSystemId,
1824 _vehicleComponentId,
1825 _outgoingMavlinkChannel,
1826 &commandAck,
1827 request.command,
1828 commandResult,
1829 0, // progress
1830 0, // result_param2
1831 0, // target_system
1832 0 // target_component
1833 );
1834 respondWithMavlinkMessage(commandAck);
1835}
1836
1837void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult)
1838{
1839 mavlink_message_t commandAck{};
1840 (void) mavlink_msg_command_ack_pack_chan(
1841 _vehicleSystemId,
1842 _vehicleComponentId,
1843 _outgoingMavlinkChannel,
1844 &commandAck,
1845 command,
1846 ackResult,
1847 0, // progress
1848 0, // result_param2
1849 0, // target_system
1850 0 // target_component
1851 );
1852 respondWithMavlinkMessage(commandAck);
1853}
1854
1855void MockLink::_respondWithAutopilotVersion()
1856{
1857 union FlightVersion {
1858 uint32_t raw;
1859
1860 struct {
1861 uint8_t type; // bits 0–7
1862 uint8_t patch; // bits 8–15
1863 uint8_t minor; // bits 16–23
1864 uint8_t major; // bits 24–31
1865 } parts;
1866
1867 FlightVersion(uint32_t version = 0) : raw(version) {}
1868 };
1869 FlightVersion flightVersion;
1870
1871#ifndef QGC_NO_ARDUPILOT_DIALECT
1872 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1873 flightVersion.parts.major = 4;
1874 flightVersion.parts.minor = 7;
1875 flightVersion.parts.patch = 0;
1876 flightVersion.parts.type = FIRMWARE_VERSION_TYPE_OFFICIAL;
1877 } else if (_firmwareType == MAV_AUTOPILOT_PX4) {
1878#endif
1879 flightVersion.parts.major = 1;
1880 flightVersion.parts.minor = 17;
1881 flightVersion.parts.patch = 0;
1882 flightVersion.parts.type = FIRMWARE_VERSION_TYPE_OFFICIAL;
1883#ifndef QGC_NO_ARDUPILOT_DIALECT
1884 }
1885#endif
1886
1887 const uint8_t customVersion[8]{};
1888 const uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | MAV_PROTOCOL_CAPABILITY_MISSION_INT | ((_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0);
1889
1890 mavlink_message_t msg{};
1891 (void) mavlink_msg_autopilot_version_pack_chan(
1892 _vehicleSystemId,
1893 _vehicleComponentId,
1894 _outgoingMavlinkChannel,
1895 &msg,
1896 capabilities,
1897 flightVersion.raw, // flight_sw_version,
1898 0, // middleware_sw_version,
1899 0, // os_sw_version,
1900 0, // board_version,
1901 reinterpret_cast<const uint8_t*>(&customVersion), // flight_custom_version,
1902 reinterpret_cast<const uint8_t*>(&customVersion), // middleware_custom_version,
1903 reinterpret_cast<const uint8_t*>(&customVersion), // os_custom_version,
1904 _boardVendorId,
1905 _boardProductId,
1906 0, // uid
1907 0 // uid2
1908 );
1909 respondWithMavlinkMessage(msg);
1910}
1911
1912void MockLink::_sendHomePosition()
1913{
1914 const float bogus[4]{};
1915
1916 mavlink_message_t msg{};
1917 (void) mavlink_msg_home_position_pack_chan(
1918 _vehicleSystemId,
1919 _vehicleComponentId,
1920 _outgoingMavlinkChannel,
1921 &msg,
1922 static_cast<int32_t>(_vehicleLatitude * 1E7),
1923 static_cast<int32_t>(_vehicleLongitude * 1E7),
1924 static_cast<int32_t>(_defaultVehicleHomeAltitude * 1000),
1925 0.0f, 0.0f, 0.0f,
1926 &bogus[0],
1927 0.0f, 0.0f, 0.0f,
1928 0
1929 );
1930 respondWithMavlinkMessage(msg);
1931}
1932
1933void MockLink::_sendGpsRawInt()
1934{
1935 static uint64_t timeTick = 0;
1936
1937 mavlink_message_t msg{};
1938 (void) mavlink_msg_gps_raw_int_pack_chan(
1939 _vehicleSystemId,
1940 _vehicleComponentId,
1941 _outgoingMavlinkChannel,
1942 &msg,
1943 timeTick++, // time since boot
1944 GPS_FIX_TYPE_3D_FIX,
1945 static_cast<int32_t>(_vehicleLatitude * 1E7),
1946 static_cast<int32_t>(_vehicleLongitude * 1E7),
1947 static_cast<int32_t>(_vehicleAltitudeAMSL * 1000),
1948 3 * 100, // hdop
1949 3 * 100, // vdop
1950 UINT16_MAX, // velocity not known
1951 UINT16_MAX, // course over ground not known
1952 8, // satellites visible
1953 //-- Extension
1954 0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up).
1955 0, // Position uncertainty in meters * 1000 (positive for up).
1956 0, // Altitude uncertainty in meters * 1000 (positive for up).
1957 0, // Speed uncertainty in meters * 1000 (positive for up).
1958 0, // Heading / track uncertainty in degrees * 1e5.
1959 65535 // Yaw not provided
1960 );
1961 respondWithMavlinkMessage(msg);
1962}
1963
1964void MockLink::_sendGlobalPositionInt()
1965{
1966 static uint64_t timeTick = 0;
1967
1968 mavlink_message_t msg{};
1969 (void) mavlink_msg_global_position_int_pack_chan(
1970 _vehicleSystemId,
1971 _vehicleComponentId,
1972 _outgoingMavlinkChannel,
1973 &msg,
1974 timeTick++, // time since boot
1975 static_cast<int32_t>(_vehicleLatitude * 1E7),
1976 static_cast<int32_t>(_vehicleLongitude * 1E7),
1977 static_cast<int32_t>(_vehicleAltitudeAMSL * 1000),
1978 static_cast<int32_t>((_vehicleAltitudeAMSL - _defaultVehicleHomeAltitude) * 1000),
1979 0, 0, 0, // no speed sent
1980 UINT16_MAX // no heading sent
1981 );
1982 respondWithMavlinkMessage(msg);
1983}
1984
1985void MockLink::_sendAttitudeQuaternion()
1986{
1987 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
1988 const float t = timeBootMs / 1000.0f;
1989
1990 // Synthesize sinusoidal Euler angles (rad)
1991 const float roll = 0.20f * std::sin(2.0f * static_cast<float>(M_PI) * 0.50f * t);
1992 const float pitch = 0.10f * std::sin(2.0f * static_cast<float>(M_PI) * 0.40f * t);
1993 const float yaw = 0.30f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t);
1994
1995 // ZYX Euler → quaternion
1996 const float cr = std::cos(roll / 2.0f), sr = std::sin(roll / 2.0f);
1997 const float cp = std::cos(pitch / 2.0f), sp = std::sin(pitch / 2.0f);
1998 const float cy = std::cos(yaw / 2.0f), sy = std::sin(yaw / 2.0f);
1999 const float q1 = cr * cp * cy + sr * sp * sy; // w
2000 const float q2 = sr * cp * cy - cr * sp * sy; // x
2001 const float q3 = cr * sp * cy + sr * cp * sy; // y
2002 const float q4 = cr * cp * sy - sr * sp * cy; // z
2003
2004 // Body rates = time-derivatives of the Euler angles
2005 const float rollspeed = 0.20f * (2.0f * static_cast<float>(M_PI) * 0.50f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.50f * t);
2006 const float pitchspeed = 0.10f * (2.0f * static_cast<float>(M_PI) * 0.40f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.40f * t);
2007 const float yawspeed = 0.30f * (2.0f * static_cast<float>(M_PI) * 0.10f) * std::cos(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2008
2009 const float reprOffsetQ[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // identity
2010
2011 mavlink_message_t msg{};
2012 (void) mavlink_msg_attitude_quaternion_pack_chan(
2013 _vehicleSystemId,
2014 _vehicleComponentId,
2015 _outgoingMavlinkChannel,
2016 &msg,
2017 timeBootMs,
2018 q1, q2, q3, q4,
2019 rollspeed, pitchspeed, yawspeed,
2020 reprOffsetQ
2021 );
2022 respondWithMavlinkMessage(msg);
2023}
2024
2025void MockLink::_sendAttitudeTarget()
2026{
2027 // Setpoint: same shape as actual, phase-shifted by +0.3 rad
2028 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
2029 const float t = timeBootMs / 1000.0f;
2030 static constexpr float kPhase = 0.3f; // rad
2031
2032 const float roll = 0.20f * std::sin(2.0f * static_cast<float>(M_PI) * 0.50f * t + kPhase);
2033 const float pitch = 0.10f * std::sin(2.0f * static_cast<float>(M_PI) * 0.40f * t + kPhase);
2034 const float yaw = 0.30f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t + kPhase);
2035
2036 const float cr = std::cos(roll / 2.0f), sr = std::sin(roll / 2.0f);
2037 const float cp = std::cos(pitch / 2.0f), sp = std::sin(pitch / 2.0f);
2038 const float cy = std::cos(yaw / 2.0f), sy = std::sin(yaw / 2.0f);
2039 const float qSp[4] = {
2040 cr * cp * cy + sr * sp * sy,
2041 sr * cp * cy - cr * sp * sy,
2042 cr * sp * cy + sr * cp * sy,
2043 cr * cp * sy - sr * sp * cy,
2044 };
2045
2046 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);
2047 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);
2048 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);
2049
2050 mavlink_message_t msg{};
2051 (void) mavlink_msg_attitude_target_pack_chan(
2052 _vehicleSystemId,
2053 _vehicleComponentId,
2054 _outgoingMavlinkChannel,
2055 &msg,
2056 timeBootMs,
2057 0, // type_mask: all fields valid
2058 qSp,
2059 bodyRollRate, bodyPitchRate, bodyYawRate,
2060 0.5f // thrust
2061 );
2062 respondWithMavlinkMessage(msg);
2063}
2064
2065void MockLink::_sendLocalPositionNed()
2066{
2067 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
2068 const float t = timeBootMs / 1000.0f;
2069
2070 const float x = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.08f * t);
2071 const float y = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2072 const float z = -10.0f + 1.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.15f * t); // negative = up in NED
2073 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);
2074 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);
2075 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);
2076
2077 mavlink_message_t msg{};
2078 (void) mavlink_msg_local_position_ned_pack_chan(
2079 _vehicleSystemId,
2080 _vehicleComponentId,
2081 _outgoingMavlinkChannel,
2082 &msg,
2083 timeBootMs,
2084 x, y, z,
2085 vx, vy, vz
2086 );
2087 respondWithMavlinkMessage(msg);
2088}
2089
2090void MockLink::_sendPositionTargetLocalNed()
2091{
2092 // Setpoint: same shape as _sendLocalPositionNed, with a 0.5 s time offset (not a phase in rad)
2093 const uint32_t timeBootMs = static_cast<uint32_t>(_runningTime.elapsed());
2094 const float t = timeBootMs / 1000.0f + 0.5f; // +0.5 s time lead
2095
2096 const float x = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.08f * t);
2097 const float y = 5.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.10f * t);
2098 const float z = -10.0f + 1.0f * std::sin(2.0f * static_cast<float>(M_PI) * 0.15f * t);
2099 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);
2100 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);
2101 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);
2102
2103 mavlink_message_t msg{};
2104 (void) mavlink_msg_position_target_local_ned_pack_chan(
2105 _vehicleSystemId,
2106 _vehicleComponentId,
2107 _outgoingMavlinkChannel,
2108 &msg,
2109 timeBootMs,
2110 MAV_FRAME_LOCAL_NED,
2111 0, // type_mask: all fields valid
2112 x, y, z,
2113 vx, vy, vz,
2114 0.0f, 0.0f, 0.0f, // acceleration not used
2115 0.0f, 0.0f // yaw, yaw_rate not used
2116 );
2117 respondWithMavlinkMessage(msg);
2118}
2119
2120void MockLink::_sendExtendedSysState()
2121{
2122 mavlink_message_t msg{};
2123 (void) mavlink_msg_extended_sys_state_pack_chan(
2124 _vehicleSystemId,
2125 _vehicleComponentId,
2126 _outgoingMavlinkChannel,
2127 &msg,
2128 MAV_VTOL_STATE_UNDEFINED,
2129 (_vehicleAltitudeAMSL > _defaultVehicleHomeAltitude) ? MAV_LANDED_STATE_IN_AIR : MAV_LANDED_STATE_ON_GROUND
2130 );
2131 respondWithMavlinkMessage(msg);
2132}
2133
2134void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks)
2135{
2136 constexpr int cChunks = 4;
2137
2138 int num = 0;
2139 for (int i = 0; i < cChunks; i++) {
2140 if (missingChunks && (i & 1)) {
2141 continue;
2142 }
2143
2144 int cBuf = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
2145 char msgBuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]{};
2146
2147 if (i == cChunks - 1) {
2148 // Last chunk is partial
2149 cBuf /= 2;
2150 }
2151
2152 for (int j = 0; j < cBuf - 1; j++) {
2153 msgBuf[j] = '0' + num++;
2154 if (num > 9) {
2155 num = 0;
2156 }
2157 }
2158 msgBuf[cBuf-1] = 'A' + i;
2159
2160 mavlink_message_t msg{};
2161 (void) mavlink_msg_statustext_pack_chan(
2162 _vehicleSystemId,
2163 _vehicleComponentId,
2164 _outgoingMavlinkChannel,
2165 &msg,
2166 MAV_SEVERITY_INFO,
2167 msgBuf,
2168 chunkId,
2169 i // chunk sequence number
2170 );
2171 respondWithMavlinkMessage(msg);
2172 }
2173}
2174
2175void MockLink::_sendStatusTextMessages()
2176{
2177 struct StatusMessage {
2178 MAV_SEVERITY severity;
2179 const char *msg;
2180 };
2181
2182 static constexpr struct StatusMessage rgMessages[] = {
2183 { MAV_SEVERITY_INFO, "#Testing audio output" },
2184 { MAV_SEVERITY_EMERGENCY, "Status text emergency" },
2185 { MAV_SEVERITY_ALERT, "Status text alert" },
2186 { MAV_SEVERITY_CRITICAL, "Status text critical" },
2187 { MAV_SEVERITY_ERROR, "Status text error" },
2188 { MAV_SEVERITY_WARNING, "Status text warning" },
2189 { MAV_SEVERITY_NOTICE, "Status text notice" },
2190 { MAV_SEVERITY_INFO, "Status text info" },
2191 { MAV_SEVERITY_DEBUG, "Status text debug" },
2192 };
2193
2194 mavlink_message_t msg{};
2195 for (size_t i = 0; i < std::size(rgMessages); i++) {
2196 const struct StatusMessage *status = &rgMessages[i];
2197 char statusText[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {};
2198 (void) std::strncpy(statusText, status->msg, sizeof(statusText) - 1);
2199
2200 (void) mavlink_msg_statustext_pack_chan(
2201 _vehicleSystemId,
2202 _vehicleComponentId,
2203 _outgoingMavlinkChannel,
2204 &msg,
2205 status->severity,
2206 statusText,
2207 0, // Not a chunked sequence
2208 0 // Not a chunked sequence
2209 );
2210 respondWithMavlinkMessage(msg);
2211 }
2212
2213 _sendChunkedStatusText(1, false /* missingChunks */);
2214 _sendChunkedStatusText(2, true /* missingChunks */);
2215 _sendChunkedStatusText(3, false /* missingChunks */); // This should cause the previous incomplete chunk to spit out
2216 _sendChunkedStatusText(4, true /* missingChunks */); // This should cause the timeout to fire
2217}
2218
2219MockLink *MockLink::_startMockLink(MockConfiguration *mockConfig)
2220{
2221 mockConfig->setDynamic(true);
2222 SharedLinkConfigurationPtr config = LinkManager::instance()->addConfiguration(mockConfig);
2223
2224 if (LinkManager::instance()->createConnectedLink(config)) {
2225 return qobject_cast<MockLink*>(config->link());
2226 }
2227
2228 return nullptr;
2229}
2230
2231MockLink *MockLink::_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode, bool preloadMission)
2232{
2233 MockConfiguration *const mockConfig = new MockConfiguration(configName);
2234
2235 mockConfig->setFirmwareType(firmwareType);
2236 mockConfig->setVehicleType(vehicleType);
2237 mockConfig->setSendStatusText(sendStatusText);
2238 mockConfig->setEnableCamera(enableCamera);
2239 mockConfig->setEnableGimbal(enableGimbal);
2240 mockConfig->setFailureMode(failureMode);
2241 mockConfig->setPreloadMission(preloadMission);
2242
2243 return _startMockLink(mockConfig);
2244}
2245
2246MockLink *MockLink::startPX4MockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2247{
2248 return _startMockLinkWorker(QStringLiteral("PX4 MultiRotor MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, enableCamera, enableGimbal, failureMode);
2249}
2250
2251MockLink *MockLink::startPX4MockLinkWithMission(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2252{
2253 return _startMockLinkWorker(QStringLiteral("PX4 MultiRotor MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, enableCamera, enableGimbal, failureMode, true /* preloadMission */);
2254}
2255
2256MockLink *MockLink::startGenericMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2257{
2258 return _startMockLinkWorker(QStringLiteral("Generic MockLink"), MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, enableCamera, enableGimbal, failureMode);
2259}
2260
2261MockLink *MockLink::startNoInitialConnectMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2262{
2263 return _startMockLinkWorker(QStringLiteral("No Initial Connect MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, enableCamera, enableGimbal, failureMode);
2264}
2265
2266MockLink *MockLink::startAPMArduCopterMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2267{
2268 return _startMockLinkWorker(QStringLiteral("ArduCopter MockLink"),MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, enableCamera, enableGimbal, failureMode);
2269}
2270
2271MockLink *MockLink::startAPMArduPlaneMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2272{
2273 return _startMockLinkWorker(QStringLiteral("ArduPlane MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, enableCamera, enableGimbal, failureMode);
2274}
2275
2276MockLink *MockLink::startAPMArduSubMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2277{
2278 return _startMockLinkWorker(QStringLiteral("ArduSub MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, enableCamera, enableGimbal, failureMode);
2279}
2280
2281MockLink *MockLink::startAPMArduRoverMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
2282{
2283 return _startMockLinkWorker(QStringLiteral("ArduRover MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, enableCamera, enableGimbal, failureMode);
2284}
2285
2286void MockLink::_sendRCChannels()
2287{
2288 mavlink_message_t msg{};
2289 (void) mavlink_msg_rc_channels_pack_chan(
2290 _vehicleSystemId,
2291 _vehicleComponentId,
2292 _outgoingMavlinkChannel,
2293 &msg,
2294 0, // time_boot_ms
2295 16, // chancount
2296 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 1-8
2297 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 9-16
2298 UINT16_MAX, UINT16_MAX, // channel 17/18 unused
2299 0 // rssi
2300 );
2301 respondWithMavlinkMessage(msg);
2302}
2303
2304void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request)
2305{
2306 if ((request.param1 == 0) && (request.param2 == 0) && (request.param3 == 0) &&
2307 (request.param4 == 0) && (request.param5 == 0) && (request.param6 == 0) &&
2308 (request.param7 == 0)) {
2309 // All zeros is a calibration cancel request.
2310 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
2311 // Send ACCELCAL_VEHICLE_POS_FAILED so controller calls _stopCalibration(Failed)
2312 QMutexLocker locker(&_apmAccelCalMutex);
2313 if (_apmAccelCalPosIndex >= 0) {
2314 mavlink_message_t msg{};
2315 mavlink_command_long_t cmd{};
2316 cmd.target_system = 255;
2317 cmd.target_component = MAV_COMP_ID_MISSIONPLANNER;
2318 cmd.command = MAV_CMD_ACCELCAL_VEHICLE_POS;
2319 cmd.param1 = static_cast<float>(ACCELCAL_VEHICLE_POS_FAILED);
2320 (void) mavlink_msg_command_long_encode_chan(
2321 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &cmd);
2322 respondWithMavlinkMessage(msg);
2323 _apmAccelCalPosIndex = -1;
2324 }
2325 } else {
2326 // PX4: See PX4 calibrate_cancel_check().
2327 (void) _mockLinkPX4Calibration->cancel();
2328 }
2329 return;
2330 }
2331
2332 if (request.param2 == 1) {
2333 // Magnetometer calibration runs the full pose-driven simulation
2334 _mockLinkPX4Calibration->startMagCalibration();
2335 return;
2336 }
2337
2338 if (request.param1 == 1) {
2339 sendStatusTextMessage(MAV_SEVERITY_INFO, QStringLiteral("[cal] calibration started: 2 gyro"));
2340 return;
2341 }
2342
2343 if (request.param5 == 1) {
2344 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
2345 // APM full accelerometer calibration: drive the ACCELCAL_VEHICLE_POS handshake
2346 QMutexLocker locker(&_apmAccelCalMutex);
2347 _apmAccelCalPosIndex = 0;
2348 _apmAccelCalGotAck = false;
2349 _apmAccelCalTickCount = 0;
2350 } else {
2351 // Accelerometer calibration runs the full pose-driven simulation
2352 _mockLinkPX4Calibration->startAccelCalibration();
2353 }
2354 }
2355}
2356
2357void MockLink::sendStatusTextMessage(uint8_t severity, const QString &text)
2358{
2359 char statusText[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {};
2360 (void) std::strncpy(statusText, text.toUtf8().constData(), sizeof(statusText) - 1);
2361
2362 mavlink_message_t msg{};
2363 (void) mavlink_msg_statustext_pack_chan(
2364 _vehicleSystemId,
2365 _vehicleComponentId,
2366 _outgoingMavlinkChannel,
2367 &msg,
2368 severity,
2369 statusText,
2370 0,
2371 0 // Not chunked
2372 );
2373 respondWithMavlinkMessage(msg);
2374}
2375
2376void MockLink::_handleTakeoff(const mavlink_command_long_t &request)
2377{
2378 _vehicleAltitudeAMSL = request.param7 + _defaultVehicleHomeAltitude;
2379 _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
2380}
2381
2382void MockLink::_handleLogRequestList(const mavlink_message_t &msg)
2383{
2384 mavlink_log_request_list_t request{};
2385 mavlink_msg_log_request_list_decode(&msg, &request);
2386
2387 if ((request.start != 0) && (request.end != 0xffff)) {
2388 qCWarning(MockLinkLog) << "_handleLogRequestList cannot handle partial requests";
2389 return;
2390 }
2391
2392 mavlink_message_t responseMsg{};
2393 mavlink_msg_log_entry_pack_chan(
2394 _vehicleSystemId,
2395 _vehicleComponentId,
2396 _outgoingMavlinkChannel,
2397 &responseMsg,
2398 _logDownloadLogId, // log id
2399 1, // num_logs
2400 1, // last_log_num
2401 0, // time_utc
2402 _logDownloadFileSize // size
2403 );
2404 respondWithMavlinkMessage(responseMsg);
2405}
2406
2407QString MockLink::_createRandomFile(uint32_t byteCount)
2408{
2409 QTemporaryFile tempFile;
2410 tempFile.setAutoRemove(false);
2411 if (!tempFile.open()) {
2412 qCWarning(MockLinkLog) << "MockLink::createRandomFile open failed" << tempFile.errorString();
2413 return QString();
2414 }
2415
2416 for (uint32_t bytesWritten = 0; bytesWritten < byteCount; bytesWritten++) {
2417 const unsigned char byte = (QRandomGenerator::global()->generate() * 0xFF) / RAND_MAX;
2418 (void) tempFile.write(reinterpret_cast<const char*>(&byte), 1);
2419 }
2420
2421 tempFile.close();
2422 return tempFile.fileName();
2423}
2424
2425void MockLink::_handleLogRequestData(const mavlink_message_t &msg)
2426{
2427 mavlink_log_request_data_t request{};
2428 mavlink_msg_log_request_data_decode(&msg, &request);
2429
2430#ifdef QGC_UNITTEST_BUILD
2431 if (_logDownloadFilename.isEmpty()) {
2432 _logDownloadFilename = _createRandomFile(_logDownloadFileSize);
2433 }
2434#endif
2435
2436 if (request.id != 0) {
2437 qCWarning(MockLinkLog) << "_handleLogRequestData id must be 0";
2438 return;
2439 }
2440
2441 if (request.ofs > (_logDownloadFileSize - 1)) {
2442 qCWarning(MockLinkLog) << "_handleLogRequestData offset past end of file request.ofs:size" << request.ofs << _logDownloadFileSize;
2443 return;
2444 }
2445
2446 // This will trigger _logDownloadWorker to send data
2447 // Thread-safe access: Main thread writes, worker thread reads every 2ms. Serialize to avoid
2448 // worker reading inconsistent offset/count or using stale values while downloading.
2449 QMutexLocker locker(&_logDownloadMutex);
2450 _logDownloadCurrentOffset = request.ofs;
2451 if (request.ofs + request.count > _logDownloadFileSize) {
2452 request.count = _logDownloadFileSize - request.ofs;
2453 }
2454 _logDownloadBytesRemaining = request.count;
2455}
2456
2457void MockLink::_logDownloadWorker()
2458{
2459 // Runs every 2ms (500Hz on worker thread). Must protect shared state modified by main thread.
2460 // Without lock: main could write new offset/count while we're reading, causing corrupted downloads.
2461 QMutexLocker locker(&_logDownloadMutex);
2462 if (_logDownloadBytesRemaining == 0) {
2463 return;
2464 }
2465
2466 QFile file(_logDownloadFilename);
2467 if (!file.open(QIODevice::ReadOnly)) {
2468 qCWarning(MockLinkLog) << "_logDownloadWorker open failed" << file.errorString();
2469 return;
2470 }
2471
2472 uint8_t buffer[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN]{};
2473
2474 const qint64 bytesToRead = qMin(_logDownloadBytesRemaining, (uint32_t)MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);
2475 Q_ASSERT(file.seek(_logDownloadCurrentOffset));
2476 Q_ASSERT(file.read(reinterpret_cast<char*>(buffer), bytesToRead) == bytesToRead);
2477
2478 qCDebug(MockLinkLog) << "_logDownloadWorker" << _logDownloadCurrentOffset << _logDownloadBytesRemaining;
2479
2480 mavlink_message_t responseMsg{};
2481 (void) mavlink_msg_log_data_pack_chan(
2482 _vehicleSystemId,
2483 _vehicleComponentId,
2484 _outgoingMavlinkChannel,
2485 &responseMsg,
2486 _logDownloadLogId,
2487 _logDownloadCurrentOffset,
2488 bytesToRead,
2489 &buffer[0]
2490 );
2491 respondWithMavlinkMessage(responseMsg);
2492
2493 _logDownloadCurrentOffset += bytesToRead;
2494 _logDownloadBytesRemaining -= bytesToRead;
2495
2496 file.close();
2497}
2498
2499void MockLink::_sendADSBVehicles()
2500{
2501 for (int i = 0; i < _adsbVehicles.size(); ++i) {
2502 // Slightly change the direction to simulate different paths
2503 _adsbVehicles[i].angle += (i + 1); // Vary the change to make each path unique
2504
2505 // Move each vehicle by a smaller distance to simulate slower speed
2506 _adsbVehicles[i].coordinate = _adsbVehicles[i].coordinate.atDistanceAndAzimuth(5, _adsbVehicles[i].angle); // 50 meters per update for slower speed
2507
2508 // Simulate slight variations in altitude
2509 _adsbVehicles[i].altitude += (i % 2 == 0 ? 0.5 : -0.5); // Increase or decrease altitude
2510
2511 QByteArray callsign = QString("N12345%1").arg(i, 2, 10, QChar('0')).toLatin1();
2512 callsign.resize(MAVLINK_MSG_ADSB_VEHICLE_FIELD_CALLSIGN_LEN);
2513
2514 // Prepare and send MAVLink message for each vehicle
2515 mavlink_message_t responseMsg{};
2516 (void) mavlink_msg_adsb_vehicle_pack_chan(
2517 _vehicleSystemId,
2518 _vehicleComponentId,
2519 _outgoingMavlinkChannel,
2520 &responseMsg,
2521 12345 + i, // Unique ICAO address for each vehicle
2522 _adsbVehicles[i].coordinate.latitude() * 1e7,
2523 _adsbVehicles[i].coordinate.longitude() * 1e7,
2524 ADSB_ALTITUDE_TYPE_GEOMETRIC,
2525 _adsbVehicles[i].altitude * 1000, // Altitude in millimeters
2526 // Use the current angle as heading
2527 static_cast<uint16_t>(_adsbVehicles[i].angle * 100), // Heading in centidegrees
2528 0, 0, // Horizontal/Vertical velocity
2529 callsign.constData(), // Unique callsign
2530 ADSB_EMITTER_TYPE_ROTOCRAFT,
2531 1, // Seconds since last communication
2532 ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED,
2533 0 // Squawk code
2534 );
2535 respondWithMavlinkMessage(responseMsg);
2536 }
2537}
2538
2539void MockLink::_moveADSBVehicle(int vehicleIndex)
2540{
2541 _adsbAngles[vehicleIndex] += 10; // Increment angle for smoother movement
2542 QGeoCoordinate &coord = _adsbVehicleCoordinates[vehicleIndex];
2543
2544 // Update the position based on the new angle
2545 coord = QGeoCoordinate(coord.latitude(), coord.longitude()).atDistanceAndAzimuth(500, _adsbAngles[vehicleIndex]);
2546 coord.setAltitude(100); // Keeping altitude constant for simplicity
2547}
2548
2549void MockLink::_handleRequestMessageAutopilotVersion(const mavlink_command_long_t &/*request*/, bool &accepted)
2550{
2551 accepted = true;
2552
2553 switch (_failureMode) {
2554 case MockConfiguration::FailNone:
2555 break;
2556 case MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionFailure:
2557 accepted = false;
2558 return;
2559 case MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionLost:
2560 accepted = true;
2561 return;
2562 default:
2563 break;
2564 }
2565
2566 _respondWithAutopilotVersion();
2567}
2568
2569void MockLink::_handleRequestMessageDebug(const mavlink_command_long_t &/*request*/, bool &accepted, bool &noAck)
2570{
2571 accepted = true;
2572 noAck = false;
2573
2574 switch (_requestMessageFailureMode) {
2575 case FailRequestMessageNone:
2576 break;
2577 case FailRequestMessageCommandAcceptedMsgNotSent:
2578 return;
2579 case FailRequestMessageCommandUnsupported:
2580 accepted = false;
2581 return;
2582 case FailRequestMessageCommandNoResponse:
2583 accepted = false;
2584 noAck = true;
2585 return;
2586 }
2587
2588 mavlink_message_t responseMsg{};
2589 (void) mavlink_msg_debug_pack_chan(
2590 _vehicleSystemId,
2591 _vehicleComponentId,
2592 _outgoingMavlinkChannel,
2593 &responseMsg,
2594 0, 0, 0
2595 );
2596 respondWithMavlinkMessage(responseMsg);
2597}
2598
2599void MockLink::_handleRequestMessageAvailableModes(const mavlink_command_long_t &request, bool &accepted)
2600{
2601 accepted = true;
2602
2603 // Thread-safe access: Check-then-set pattern must be atomic. Worker increments index every 2ms,
2604 // so check for "already running" and start/stop operations must serialize to prevent race where
2605 // main reads false, worker increments, main overwrites with different value -> lost update.
2606 QMutexLocker locker(&_availableModesWorkerMutex);
2607 if (request.param2 == 0) {
2608 // Request for available modes to be streamed out
2609 if (_availableModesWorkerNextModeIndex != 0) {
2610 qCWarning(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: _availableModesWorker already running - _availableModesWorkerNextModeIndex:" << _availableModesWorkerNextModeIndex;
2611 accepted = false;
2612 return;
2613 }
2614 qCDebug(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: starting available modes sequence worker";
2615 _availableModesWorkerNextModeIndex = 1; // Start with the first mode in sequence (1-based index)
2616 } else {
2617 // Request for specific mode
2618 if (request.param2 > _availableFlightModes.count()) {
2619 qCWarning(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: requested mode index out of range" << request.param2 << _availableFlightModes.count();
2620 accepted = false;
2621 return;
2622 }
2623 qCDebug(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: received specific mode request for index" << request.param2;
2624 _availableModesWorkerNextModeIndex = -request.param2; // Negative index indicates a specific single mode request
2625 }
2626}
2627
2628void MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool &accepted, bool &noAck)
2629{
2630 accepted = false;
2631 noAck = false;
2632
2633 const uint32_t requestedMessageId = static_cast<uint32_t>(request.param1);
2634
2635 // Per-message-ID no-response injection: silently drop the request (no ACK, no message)
2636 {
2637 QMutexLocker locker(&_requestMessageNoResponseMutex);
2638 if (_requestMessageNoResponseIds.contains(requestedMessageId)) {
2639 noAck = true;
2640 return;
2641 }
2642 }
2643
2644 switch (static_cast<int>(request.param1)) {
2645 case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
2646 _handleRequestMessageAutopilotVersion(request, accepted);
2647 break;
2648 case MAVLINK_MSG_ID_COMPONENT_METADATA:
2649 if (_firmwareType == MAV_AUTOPILOT_PX4) {
2650 _sendGeneralMetaData();
2651 accepted = true;
2652 }
2653 break;
2654 case MAVLINK_MSG_ID_DEBUG:
2655 _handleRequestMessageDebug(request, accepted, noAck);
2656 break;
2657 case MAVLINK_MSG_ID_AVAILABLE_MODES:
2658 _handleRequestMessageAvailableModes(request, accepted);
2659 break;
2660 }
2661}
2662
2663void MockLink::_sendGeneralMetaData()
2664{
2665 static constexpr const char metaDataURI[MAVLINK_MSG_COMPONENT_METADATA_FIELD_URI_LEN] = "mftp://[;comp=1]general.json";
2666
2667 mavlink_message_t responseMsg{};
2668 (void) mavlink_msg_component_metadata_pack_chan(
2669 _vehicleSystemId,
2670 _vehicleComponentId,
2671 _outgoingMavlinkChannel,
2672 &responseMsg,
2673 0, // time_boot_ms
2674 100, // general_metadata_file_crc
2675 metaDataURI
2676 );
2677 respondWithMavlinkMessage(responseMsg);
2678}
2679
2680void MockLink::_sendRemoteIDArmStatus()
2681{
2682 char armStatusError[MAVLINK_MSG_OPEN_DRONE_ID_ARM_STATUS_FIELD_ERROR_LEN] = {};
2683 std::strncpy(armStatusError, "No Error", sizeof(armStatusError) - 1);
2684
2685 mavlink_message_t msg{};
2686 (void) mavlink_msg_open_drone_id_arm_status_pack(
2687 _vehicleSystemId,
2688 MAV_COMP_ID_ODID_TXRX_1,
2689 &msg,
2690 MAV_ODID_ARM_STATUS_GOOD_TO_ARM,
2691 armStatusError
2692 );
2693 respondWithMavlinkMessage(msg);
2694}
2695
2696void MockLink::_sendEscInfo()
2697{
2698 // Send ESC_INFO for 4 motors starting at index 0.
2699 // count=4 and info bitmask=0x0F (all 4 online) makes the ESC indicator visible.
2700 static const uint16_t failureFlags[4] = {0, 0, 0, 0};
2701 static const uint32_t errorCount[4] = {0, 0, 0, 0};
2702 static const int16_t temperature[4] = {3000, 3000, 3000, 3000}; // centidegrees
2703
2704 mavlink_message_t msg{};
2705 (void) mavlink_msg_esc_info_pack_chan(
2706 _vehicleSystemId,
2707 _vehicleComponentId,
2708 _outgoingMavlinkChannel,
2709 &msg,
2710 0, // index: first group starts at 0
2711 static_cast<uint64_t>(_runningTime.elapsed()) * 1000, // time_usec
2712 0, // counter
2713 4, // count: 4 motors
2714 ESC_CONNECTION_TYPE_DSHOT, // connection_type
2715 0x0F, // info: bitmask — motors 0-3 online
2716 failureFlags,
2717 errorCount,
2718 temperature
2719 );
2720 respondWithMavlinkMessage(msg);
2721}
2722
2723void MockLink::_sendEscStatus()
2724{
2725 static const int32_t rpm[4] = {5000, 5000, 5000, 5000};
2726 static const float voltage[4] = {16.0f, 16.0f, 16.0f, 16.0f};
2727 static const float current[4] = {5.0f, 5.0f, 5.0f, 5.0f};
2728
2729 mavlink_message_t msg{};
2730 (void) mavlink_msg_esc_status_pack_chan(
2731 _vehicleSystemId,
2732 _vehicleComponentId,
2733 _outgoingMavlinkChannel,
2734 &msg,
2735 0, // index: first group
2736 static_cast<uint64_t>(_runningTime.elapsed()) * 1000, // time_usec
2737 rpm,
2738 voltage,
2739 current
2740 );
2741 respondWithMavlinkMessage(msg);
2742}
2743
2744void MockLink::_sendRadioStatus()
2745{
2746 // Send a RADIO_STATUS message to make the TelemetryRSSI indicator visible.
2747 // Any non-zero rssi value triggers showIndicator (TelemetryRSSIIndicator checks lrssi.rawValue != 0).
2748 mavlink_message_t msg{};
2749 (void) mavlink_msg_radio_status_pack_chan(
2750 _vehicleSystemId,
2751 _vehicleComponentId,
2752 _outgoingMavlinkChannel,
2753 &msg,
2754 100, // rssi: local signal strength
2755 100, // remrssi: remote signal strength
2756 50, // txbuf: transmit buffer fill (%)
2757 10, // noise: local background noise
2758 10, // remnoise: remote background noise
2759 0, // rxerrors
2760 0 // fixed
2761 );
2762 respondWithMavlinkMessage(msg);
2763}
2764
2765void MockLink::simulateConnectionRemoved()
2766{
2767 _commLost = true;
2768 _connectionRemoved();
2769}
2770
2771MockLinkFTP *MockLink::mockLinkFTP() const
2772{
2773 return _mockLinkFTP;
2774}
2775
2776void MockLink::_sendAvailableMode(uint8_t modeIndexOneBased)
2777{
2778 if (modeIndexOneBased > _availableModesCount()) {
2779 qCWarning(MockLinkLog) << "modeIndexOneBased out of range" << modeIndexOneBased << _availableModesCount();
2780 return;
2781 }
2782
2783 qCDebug(MockLinkLog) << "_sendAvailableMode modeIndexOneBased:" << modeIndexOneBased;
2784
2785 const FlightMode_t &availableMode = _availableFlightModes[modeIndexOneBased - 1];
2786 char modeName[MAVLINK_MSG_AVAILABLE_MODES_FIELD_MODE_NAME_LEN] = {};
2787 std::strncpy(modeName, availableMode.name, sizeof(modeName) - 1);
2788
2789 mavlink_message_t msg{};
2790
2791 (void) mavlink_msg_available_modes_pack_chan(
2792 _vehicleSystemId,
2793 _vehicleComponentId,
2794 _outgoingMavlinkChannel,
2795 &msg,
2796 _availableModesCount(),
2797 modeIndexOneBased,
2798 availableMode.standard_mode,
2799 availableMode.custom_mode,
2800 availableMode.canBeSet ? 0 : MAV_MODE_PROPERTY_NOT_USER_SELECTABLE,
2801 modeName);
2802 respondWithMavlinkMessage(msg);
2803}
2804
2805void MockLink::_availableModesWorker()
2806{
2807 // Runs every 2ms (500Hz on worker thread). Reads and increments shared index modified by main.
2808 // Read-modify-write must be atomic to prevent lost updates or incorrect state transitions.
2809 QMutexLocker locker(&_availableModesWorkerMutex);
2810 if (_availableModesWorkerNextModeIndex == 0) {
2811 // Not active
2812 return;
2813 }
2814
2815 _sendAvailableMode(qAbs(_availableModesWorkerNextModeIndex));
2816
2817 if (_availableModesWorkerNextModeIndex < 0) {
2818 // Single mode request, stop worker
2819 _availableModesWorkerNextModeIndex = 0;
2820 } else if (++_availableModesWorkerNextModeIndex > _availableModesCount()) {
2821 // All modes sent, stop worker
2822 _availableModesWorkerNextModeIndex = 0;
2823 qCDebug(MockLinkLog) << "_availableModesWorker: all modes sent, stopping worker";
2824 }
2825}
2826
2827void MockLink::_sendAvailableModesMonitor()
2828{
2829 mavlink_message_t msg{};
2830
2831 (void) mavlink_msg_available_modes_monitor_pack_chan(
2832 _vehicleSystemId,
2833 _vehicleComponentId,
2834 _outgoingMavlinkChannel,
2835 &msg,
2836 _availableModesMonitorSeqNumber);
2837 respondWithMavlinkMessage(msg);
2838}
2839
2840int MockLink::_availableModesCount() const
2841{
2842 return _availableFlightModes.count() - (_availableModesMonitorSeqNumber == 0 ? 1 : 0); // Exclude the delayed mode
2843}
2844
2845// ---------------------------------------------------------------------------
2846// ArduPilot compass calibration simulation
2847//
2848// Driven by _apmCompassCalProgress: -1 = inactive, 0..100 = current pct.
2849// Main thread sets it to 0 to start and -1 to stop.
2850// Worker sends MAG_CAL_PROGRESS at ~10Hz (every 50 calls of 500Hz worker),
2851// then sends MAG_CAL_REPORT when pct reaches 100.
2852// ---------------------------------------------------------------------------
2853void MockLink::_apmCompassCalWorker()
2854{
2855 if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) {
2856 return;
2857 }
2858
2859 QMutexLocker locker(&_apmCompassCalMutex);
2860 if (_apmCompassCalProgress < 0) {
2861 return;
2862 }
2863
2864 // Tick ~10 Hz: advance every 50 calls of the 500 Hz worker
2865 if (++_apmCompassCalTickCount < 50) {
2866 return;
2867 }
2868 _apmCompassCalTickCount = 0;
2869
2870 const int pct = _apmCompassCalProgress;
2871
2872 // Send MAG_CAL_PROGRESS for all 3 active compasses
2873 mavlink_message_t msg{};
2874 for (uint8_t id = 0; id < 3; ++id) {
2875 mavlink_mag_cal_progress_t progress{};
2876 progress.compass_id = id;
2877 progress.cal_mask = 0x07; // all 3 compasses
2878 progress.cal_status = MAG_CAL_RUNNING_STEP_ONE;
2879 progress.completion_pct = static_cast<uint8_t>(qMin(pct, 100));
2880 (void) mavlink_msg_mag_cal_progress_encode_chan(
2881 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &progress);
2882 respondWithMavlinkMessage(msg);
2883 }
2884
2885 if (pct >= 100) {
2886 // Send MAG_CAL_REPORT for all 3 compasses
2887 for (uint8_t id = 0; id < 3; ++id) {
2888 mavlink_mag_cal_report_t report{};
2889 report.compass_id = id;
2890 report.cal_mask = 0x07;
2891 report.cal_status = MAG_CAL_SUCCESS;
2892 report.fitness = 0.5f;
2893 (void) mavlink_msg_mag_cal_report_encode_chan(
2894 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &report);
2895 respondWithMavlinkMessage(msg);
2896 }
2897
2898 // Write non-zero offsets so QGC sees all compasses as calibrated after refresh.
2899 // _mapParamName2Value is owned by MockLink's main thread; dispatch the write there
2900 // to avoid a data race with concurrent param reads on the main thread.
2901 (void) QMetaObject::invokeMethod(this, [this] {
2902 constexpr int compId = MAV_COMP_ID_AUTOPILOT1;
2903 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS_X")] = QVariant(10.0f);
2904 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS_Y")] = QVariant(10.0f);
2905 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS_Z")] = QVariant(10.0f);
2906 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS2_X")] = QVariant(10.0f);
2907 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS2_Y")] = QVariant(10.0f);
2908 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS2_Z")] = QVariant(10.0f);
2909 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS3_X")] = QVariant(10.0f);
2910 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS3_Y")] = QVariant(10.0f);
2911 _mapParamName2Value[compId][QStringLiteral("COMPASS_OFS3_Z")] = QVariant(10.0f);
2912 }, Qt::QueuedConnection);
2913
2914 _apmCompassCalProgress = -1; // deactivate
2915 } else {
2916 _apmCompassCalProgress = qMin(pct + 5, 100);
2917 }
2918}
2919
2920// ---------------------------------------------------------------------------
2921// ArduPilot full accelerometer calibration simulation
2922//
2923// The firmware sends COMMAND_LONG(MAV_CMD_ACCELCAL_VEHICLE_POS, pos) to QGC
2924// and waits for a COMMAND_ACK (the "Next" button press) before advancing.
2925// State: _apmAccelCalPosIndex -1=inactive, 0..5=current pose, 6=done.
2926// ---------------------------------------------------------------------------
2927void MockLink::_apmAccelCalWorker()
2928{
2929 if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) {
2930 return;
2931 }
2932
2933 QMutexLocker locker(&_apmAccelCalMutex);
2934 if (_apmAccelCalPosIndex < 0) {
2935 return;
2936 }
2937
2938 if (_apmAccelCalPosIndex == 6) {
2939 // All poses done: send SUCCESS
2940 mavlink_message_t msg{};
2941 mavlink_command_long_t cmd{};
2942 cmd.target_system = 255; // GCS
2943 cmd.target_component = MAV_COMP_ID_MISSIONPLANNER;
2944 cmd.command = MAV_CMD_ACCELCAL_VEHICLE_POS;
2945 cmd.param1 = static_cast<float>(ACCELCAL_VEHICLE_POS_SUCCESS);
2946 (void) mavlink_msg_command_long_encode_chan(
2947 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &cmd);
2948 respondWithMavlinkMessage(msg);
2949
2950 // Write non-zero accel offsets.
2951 // _mapParamName2Value is owned by MockLink's main thread; dispatch the write there
2952 // to avoid a data race with concurrent param reads on the main thread.
2953 (void) QMetaObject::invokeMethod(this, [this] {
2954 constexpr int compId = MAV_COMP_ID_AUTOPILOT1;
2955 _mapParamName2Value[compId][QStringLiteral("INS_ACCOFFS_X")] = QVariant(0.1f);
2956 _mapParamName2Value[compId][QStringLiteral("INS_ACCOFFS_Y")] = QVariant(0.1f);
2957 _mapParamName2Value[compId][QStringLiteral("INS_ACCOFFS_Z")] = QVariant(0.1f);
2958 }, Qt::QueuedConnection);
2959
2960 _apmAccelCalPosIndex = -1;
2961 return;
2962 }
2963
2964 // Send the current position request once; wait for Next ack before advancing
2965 if (!_apmAccelCalGotAck) {
2966 // Throttle re-sends to ~10 Hz
2967 if (++_apmAccelCalTickCount < 50) {
2968 return;
2969 }
2970 _apmAccelCalTickCount = 0;
2971
2972 mavlink_message_t msg{};
2973 mavlink_command_long_t cmd{};
2974 cmd.target_system = 255;
2975 cmd.target_component = MAV_COMP_ID_MISSIONPLANNER;
2976 cmd.command = MAV_CMD_ACCELCAL_VEHICLE_POS;
2977 cmd.param1 = static_cast<float>(kAPMAccelCalPosSequence[_apmAccelCalPosIndex]);
2978 (void) mavlink_msg_command_long_encode_chan(
2979 _vehicleSystemId, _vehicleComponentId, _outgoingMavlinkChannel, &msg, &cmd);
2980 respondWithMavlinkMessage(msg);
2981 } else {
2982 // Ack received — advance to next pose
2983 _apmAccelCalGotAck = false;
2984 _apmAccelCalPosIndex++;
2985 }
2986}
config
Config config
Definition GstSourceFactory.cc:160
SharedLinkConfigurationPtr
std::shared_ptr< LinkConfiguration > SharedLinkConfigurationPtr
Definition LinkConfiguration.h:179
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
mavlink_message_t
struct __mavlink_message mavlink_message_t
Definition QGCCorePlugin.h:24
QGC_LOGGING_CATEGORY
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Definition QGCLoggingCategory.h:7
mavlink_command_ack_t
struct __mavlink_command_ack_t mavlink_command_ack_t
Definition QGCMAVLinkTypes.h:23
mavlink_param_union_t
struct param_union mavlink_param_union_t
Definition QGCMAVLinkTypes.h:30
mavlink_command_long_t
struct __mavlink_command_long_t mavlink_command_long_t
Definition QGCMAVLinkTypes.h:24
FactMetaData::typeToSize
static size_t typeToSize(ValueType_t type)
Definition FactMetaData.cc:875
LinkConfiguration::setDynamic
void setDynamic(bool dynamic=true)
Set if this is this a dynamic configuration. (decided at runtime)
Definition LinkConfiguration.cc:143
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:16
LinkInterface::bytesReceived
void bytesReceived(LinkInterface *link, const QByteArray &data)
LinkInterface::disconnected
void disconnected()
LinkInterface::_freeMavlinkChannel
virtual void _freeMavlinkChannel()
Definition LinkInterface.cc:72
LinkInterface::_connectionRemoved
void _connectionRemoved()
Definition LinkInterface.cc:123
LinkInterface::mavlinkChannelIsSet
bool mavlinkChannelIsSet() const
Definition LinkInterface.cc:38
LinkInterface::_allocateMavlinkChannel
virtual bool _allocateMavlinkChannel()
Definition LinkInterface.cc:43
LinkInterface::connected
void connected()
LinkInterface::linkConfiguration
SharedLinkConfigurationPtr linkConfiguration()
Definition LinkInterface.h:31
LinkManager::addConfiguration
SharedLinkConfigurationPtr addConfiguration(LinkConfiguration *config)
Definition LinkManager.cc:681
LinkManager::allocateMavlinkChannel
uint8_t allocateMavlinkChannel()
Definition LinkManager.cc:699
LinkManager::instance
static LinkManager * instance()
Definition LinkManager.cc:58
LinkManager::freeMavlinkChannel
void freeMavlinkChannel(uint8_t channel)
Definition LinkManager.cc:718
LinkManager::invalidMavlinkChannel
static constexpr uint8_t invalidMavlinkChannel()
Definition LinkManager.h:114
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:524
MockConfiguration::preloadMission
bool preloadMission() const
Definition MockConfiguration.h:116
MockConfiguration::setVehicleType
void setVehicleType(MAV_TYPE vehicleType)
Definition MockConfiguration.h:57
MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionLost
@ FailInitialConnectRequestMessageAutopilotVersionLost
REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent.
Definition MockConfiguration.h:105
MockConfiguration::FailMissingParamOnInitialRequest
@ FailMissingParamOnInitialRequest
Not all params are sent on initial request, should still succeed since QGC will re-query missing para...
Definition MockConfiguration.h:102
MockConfiguration::FailMissingParamOnAllRequests
@ FailMissingParamOnAllRequests
Not all params are sent on initial request, QGC retries will fail as well.
Definition MockConfiguration.h:103
MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionFailure
@ FailInitialConnectRequestMessageAutopilotVersionFailure
REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure.
Definition MockConfiguration.h:104
MockConfiguration::FailParamNoResponseToRequestList
@ FailParamNoResponseToRequestList
Do not respond to PARAM_REQUEST_LIST.
Definition MockConfiguration.h:101
MockConfiguration::setFailureMode
void setFailureMode(FailureMode_t failureMode)
Definition MockConfiguration.h:108
MockConfiguration::setEnableCamera
void setEnableCamera(bool enableCamera)
Definition MockConfiguration.h:61
MockConfiguration::setEnableGimbal
void setEnableGimbal(bool enableGimbal)
Definition MockConfiguration.h:63
MockConfiguration::setSendStatusText
void setSendStatusText(bool sendStatusText)
Definition MockConfiguration.h:59
MockConfiguration::setFirmwareType
void setFirmwareType(MAV_AUTOPILOT firmwareType)
Definition MockConfiguration.h:52
MockConfiguration::setPreloadMission
void setPreloadMission(bool preloadMission)
Definition MockConfiguration.h:117
MockLinkCamera
Simulates MAVLink Camera Protocol v2 components for MockLink.
Definition MockLinkCamera.h:36
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:143
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:16
MockLinkFTP::mavlinkMessageReceived
void mavlinkMessageReceived(const mavlink_message_t &message)
Called to handle an FTP message.
Definition MockLinkFTP.cc:406
MockLinkGimbal
Simulates MAVLink Gimbal Manager Protocol for MockLink.
Definition MockLinkGimbal.h:24
MockLinkGimbal::run1HzTasks
void run1HzTasks()
Send periodic gimbal status messages (call from 1Hz tasks)
Definition MockLinkGimbal.cc:32
MockLinkGimbal::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkGimbal.cc:68
MockLinkMissionItemHandler::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkMissionItemHandler.cc:62
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
ParameterManager::mavTypeToFactType
static FactMetaData::ValueType_t mavTypeToFactType(MAV_PARAM_TYPE mavType)
Definition ParameterManager.cc:1263
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
QGC::crc32
quint32 crc32(const quint8 *src, unsigned len, unsigned state)
Definition QGCMath.cc:100
QGC::runningUnitTests
bool runningUnitTests()
Definition AppMessages.cc:30
QGC::secureZero
void secureZero(void *data, size_t size)
Definition SecureMemory.h:19
px4_custom_mode
Definition px4_custom_mode.h:34
px4_custom_mode::data
uint32_t data
Definition px4_custom_mode.h:40