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 "LinkManager.h"
3#include "MockLinkCamera.h"
4#include "MockLinkFTP.h"
5#include "MockLinkGimbal.h"
6#include "MockLinkWorker.h"
7#include "QGCApplication.h"
8#include "QGCLoggingCategory.h"
9#include "FirmwarePlugin.h"
10#include "FactMetaData.h"
11#include "ParameterManager.h"
12#include "QGC.h"
13
14#include <QtCore/QFile>
15#include <QtCore/QMutexLocker>
16#include <QtCore/QRandomGenerator>
17#include <QtCore/QTemporaryFile>
18#include <QtCore/QThread>
19#include <QtCore/QTimer>
20
21#include <cstring>
22
23QGC_LOGGING_CATEGORY(MockLinkLog, "Comms.MockLink.MockLink")
24QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "Comms.MockLink.MockLink:verbose")
25
26std::atomic<int> MockLink::_nextVehicleSystemId{128};
27
28QList<MockLink::FlightMode_t> MockLink::_availableFlightModes = {
29 // Mode Name Standard Mode Custom Mode CanBeSet adv
30 { "Manual", 0, PX4CustomMode::MANUAL, true, true },
31 { "Stabilized", 0, PX4CustomMode::STABILIZED, true, true },
32 { "Acro", 0, PX4CustomMode::ACRO, true, true },
33 { "Altitude", 0, PX4CustomMode::ALTCTL, true, false},
34 { "Offboard", 0, PX4CustomMode::OFFBOARD, true, true },
35 { "Position", 0, PX4CustomMode::POSCTL_POSCTL, true, false},
36 { "Orbit", 0, PX4CustomMode::POSCTL_ORBIT, false, true },
37 { "Hold", 0, PX4CustomMode::AUTO_LOITER, true, true },
38 { "Mission", 0, PX4CustomMode::AUTO_MISSION, true, true },
39 { "Return", 0, PX4CustomMode::AUTO_RTL, true, true },
40 { "Land", MAV_STANDARD_MODE_LAND, PX4CustomMode::AUTO_LAND, false, true },
41 { "Precision Landing", 0, PX4CustomMode::AUTO_PRECLAND, true, true },
42 { "Takeoff", MAV_STANDARD_MODE_TAKEOFF, PX4CustomMode::AUTO_TAKEOFF, false, false},
43 { "MockLink Mode", 0, PX4CustomMode::RATTITUDE, true, false},
44 { "(Mode not available)", 0, PX4CustomMode::AUTO_RTGS, false, false},
45 { "MockLink Mode (delayed)",0, PX4CustomMode::AUTO_FOLLOW_TARGET, true, false},
46};
47
48MockLink::MockLink(SharedLinkConfigurationPtr &config, QObject *parent)
49 : LinkInterface(config, parent)
50 , _mockConfig(qobject_cast<const MockConfiguration*>(_config.get()))
51 , _firmwareType(_mockConfig->firmwareType())
52 , _vehicleType(_mockConfig->vehicleType())
53 , _sendStatusText(_mockConfig->sendStatusText())
54 , _enableCamera(_mockConfig->enableCamera())
55 , _enableGimbal(_mockConfig->enableGimbal())
56 , _failureMode(_mockConfig->failureMode())
57 , _vehicleSystemId(_mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : static_cast<int>(_nextVehicleSystemId))
58 , _vehicleLatitude(_defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001))
59 , _vehicleLongitude(_defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001))
60 , _boardVendorId(_mockConfig->boardVendorId())
61 , _boardProductId(_mockConfig->boardProductId())
62 , _missionItemHandler(new MockLinkMissionItemHandler(this))
63 , _mockLinkCamera(_enableCamera ? new MockLinkCamera(this,
64 _mockConfig->cameraCaptureVideo(),
65 _mockConfig->cameraCaptureImage(),
66 _mockConfig->cameraHasModes(),
67 _mockConfig->cameraHasVideoStream(),
68 _mockConfig->cameraCanCaptureImageInVideoMode(),
69 _mockConfig->cameraCanCaptureVideoInImageMode(),
70 _mockConfig->cameraHasBasicZoom(),
71 _mockConfig->cameraHasTrackingPoint(),
72 _mockConfig->cameraHasTrackingRectangle())
73 : nullptr)
74 , _mockLinkGimbal(_enableGimbal ? new MockLinkGimbal(this,
75 _mockConfig->gimbalHasRollAxis(),
76 _mockConfig->gimbalHasPitchAxis(),
77 _mockConfig->gimbalHasYawAxis(),
78 _mockConfig->gimbalHasYawFollow(),
79 _mockConfig->gimbalHasYawLock(),
80 _mockConfig->gimbalHasRetract(),
81 _mockConfig->gimbalHasNeutral())
82 : nullptr)
83 , _mockLinkFTP(new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this))
84{
85 qCDebug(MockLinkLog) << this;
86
87 // Initialize ADS-B vehicles with different starting conditions
88 _adsbVehicles.reserve(_numberOfVehicles);
89 for (int i = 0; i < _numberOfVehicles; ++i) {
90 ADSBVehicle vehicle{};
91 vehicle.angle = i * 72.0; // Different starting directions (angles 0, 72, 144, 216, 288)
92
93 // Set initial coordinates slightly offset from the default coordinates
94 const double latOffset = 0.001 * i;
95 const double lonOffset = 0.001 * (i % 2 == 0 ? i : -i);
96 vehicle.coordinate = QGeoCoordinate(_defaultVehicleLatitude + latOffset, _defaultVehicleLongitude + lonOffset);
97
98 // Set a unique starting altitude for each vehicle near the home altitude
99 vehicle.altitude = _defaultVehicleHomeAltitude + (i * 5);
100
101 _adsbVehicles.append(vehicle);
102 }
103
104 (void) QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection);
105
106 _loadParams();
107 _runningTime.start();
108
109 _workerThread = new QThread(this);
110 _workerThread->setObjectName(QStringLiteral("Mock_%1").arg(_mockConfig->name()));
111 _worker = new MockLinkWorker(this);
112 _worker->moveToThread(_workerThread);
113 (void) connect(_workerThread, &QThread::started, _worker, &MockLinkWorker::startWork);
114 (void) connect(_workerThread, &QThread::finished, _worker, &QObject::deleteLater);
115 _workerThread->start();
116}
117
118MockLink::~MockLink()
119{
120 MockLink::disconnect();
121
122 delete _mockLinkCamera;
123 delete _mockLinkGimbal;
124
125 if (!_logDownloadFilename.isEmpty()) {
126 QFile::remove(_logDownloadFilename);
127 }
128
129 if (_workerThread) {
130 _workerThread->quit();
131 _workerThread->wait();
132 }
133
134 qCDebug(MockLinkLog) << this;
135}
136
137bool MockLink::_connect()
138{
139 if (!_connected) {
140 _connected = true;
141 _disconnectedEmitted = false;
142 mavlink_status_t *const mavlinkStatus = mavlink_get_channel_status(mavlinkChannel());
143 mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
144 mavlink_status_t *const auxStatus = mavlink_get_channel_status(_getMavlinkAuxChannel());
145 auxStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
146 emit connected();
147 }
148
149 return true;
150}
151
152void MockLink::disconnect()
153{
154 _missionItemHandler->shutdown();
155
156 // Stop worker thread first to prevent any more messages from being sent.
157 // This must happen before setting _connected = false to avoid race conditions
158 // where the worker checks _connected (true), then we set it false, then the
159 // worker continues sending messages to a disconnecting/destroyed vehicle.
160 if (_workerThread && _workerThread->isRunning()) {
161 _workerThread->quit();
162 _workerThread->wait();
163 }
164
165 if (_connected) {
166 _connected = false;
167 if (!_disconnectedEmitted.exchange(true)) {
168 emit disconnected();
169 }
170 }
171}
172
173void MockLink::run1HzTasks()
174{
175 if (!_mavlinkStarted || !_connected || !mavlinkChannelIsSet()) {
176 return;
177 }
178
179 if (linkConfiguration()->isHighLatency() && _highLatencyTransmissionEnabled) {
180 _sendHighLatency2();
181 return;
182 }
183
184 _sendVibration();
185 _sendBatteryStatus();
186 _sendSysStatus();
187 _sendADSBVehicles();
188 if (_vehicleType != MAV_TYPE_SUBMARINE) {
189 _sendRemoteIDArmStatus();
190 }
191 _sendAvailableModesMonitor();
192
193 if (_enableGimbal) {
194 _mockLinkGimbal->run1HzTasks();
195 }
196
197 if (_enableCamera) {
198 _mockLinkCamera->sendCameraHeartbeats();
199 }
200
201 if (!qgcApp()->runningUnitTests()) {
202 // Sending RC Channels during unit test breaks RC tests which does it's own RC simulation
203 _sendRCChannels();
204 }
205
206 if (_sendHomePositionDelayCount > 0) {
207 // We delay home position for better testing
208 _sendHomePositionDelayCount--;
209 } else {
210 _sendHomePosition();
211 // We piggy back on this delay to signal we have new standard modes available
212 if (_availableModesMonitorSeqNumber == 0) {
213 qCDebug(MockLinkLog) << "Bumping sequence number for available modes monitor to trigger requery of modes";
214 _availableModesMonitorSeqNumber = 1;
215 }
216 }
217}
218
219void MockLink::run10HzTasks()
220{
221 if (linkConfiguration()->isHighLatency()) {
222 return;
223 }
224
225 if (_mavlinkStarted && _connected && mavlinkChannelIsSet()) {
226 _sendHeartBeat();
227 if (_sendGPSPositionDelayCount > 0) {
228 // We delay gps position for better testing
229 _sendGPSPositionDelayCount--;
230 } else {
231 if (_vehicleType != MAV_TYPE_SUBMARINE) {
232 _sendGpsRawInt();
233 _sendGlobalPositionInt();
234 }
235 _sendExtendedSysState();
236 }
237
238 if (_enableCamera) {
239 _mockLinkCamera->run10HzTasks();
240 }
241 }
242}
243
244void MockLink::run500HzTasks()
245{
246 if (linkConfiguration()->isHighLatency()) {
247 return;
248 }
249
250 if (_mavlinkStarted && _connected && mavlinkChannelIsSet()) {
251 _paramRequestListWorker();
252 _logDownloadWorker();
253 _availableModesWorker();
254 }
255}
256
257void MockLink::sendStatusTextMessages()
258{
259 _sendStatusTextMessages();
260}
261
262bool MockLink::_allocateMavlinkChannel()
263{
264 // should only be called by the LinkManager during setup
265 Q_ASSERT(!_mavlinkAuxChannelIsSet());
266 Q_ASSERT(!mavlinkChannelIsSet());
267
268 if (!LinkInterface::_allocateMavlinkChannel()) {
269 qCWarning(MockLinkLog) << "LinkInterface::_allocateMavlinkChannel failed";
270 return false;
271 }
272
273 _mavlinkAuxChannel = LinkManager::instance()->allocateMavlinkChannel();
274 if (!_mavlinkAuxChannelIsSet()) {
275 qCWarning(MockLinkLog) << "_allocateMavlinkChannel failed";
276 LinkInterface::_freeMavlinkChannel();
277 return false;
278 }
279
280 qCDebug(MockLinkLog) << "_allocateMavlinkChannel" << _mavlinkAuxChannel;
281 return true;
282}
283
284void MockLink::_freeMavlinkChannel()
285{
286 qCDebug(MockLinkLog) << "_freeMavlinkChannel" << _mavlinkAuxChannel;
287 if (!_mavlinkAuxChannelIsSet()) {
288 Q_ASSERT(!mavlinkChannelIsSet());
289 return;
290 }
291
292 LinkManager::instance()->freeMavlinkChannel(_mavlinkAuxChannel);
293 LinkInterface::_freeMavlinkChannel();
294}
295
296bool MockLink::_mavlinkAuxChannelIsSet() const
297{
298 return (LinkManager::invalidMavlinkChannel() != _mavlinkAuxChannel);
299}
300
301void MockLink::_loadParams()
302{
303 QFile paramFile;
304 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
305 if (_vehicleType == MAV_TYPE_FIXED_WING) {
306 paramFile.setFileName(":/FirmwarePlugin/APM/Plane.OfflineEditing.params");
307 } else if (_vehicleType == MAV_TYPE_SUBMARINE ) {
308 paramFile.setFileName(":/FirmwarePlugin/APM/Sub.OfflineEditing.params");
309 } else if (_vehicleType == MAV_TYPE_GROUND_ROVER ) {
310 paramFile.setFileName(":/FirmwarePlugin/APM/Rover.OfflineEditing.params");
311 } else {
312 paramFile.setFileName(":/FirmwarePlugin/APM/Copter.OfflineEditing.params");
313 }
314 } else {
315 paramFile.setFileName(":/MockLink/PX4MockLink.params");
316 }
317
318 const bool success = paramFile.open(QFile::ReadOnly);
319 Q_UNUSED(success);
320 Q_ASSERT(success);
321
322 QTextStream paramStream(&paramFile);
323 while (!paramStream.atEnd()) {
324 const QString line = paramStream.readLine();
325
326 if (line.startsWith("#")) {
327 continue;
328 }
329
330 const QStringList paramData = line.split("\t");
331 Q_ASSERT(paramData.count() == 5);
332
333 const int compId = paramData.at(1).toInt();
334 const QString paramName = paramData.at(2);
335 const QString valStr = paramData.at(3);
336 const uint paramType = paramData.at(4).toUInt();
337
338 QVariant paramValue;
339 switch (paramType) {
340 case MAV_PARAM_TYPE_REAL32:
341 paramValue = QVariant(valStr.toFloat());
342 break;
343 case MAV_PARAM_TYPE_UINT32:
344 paramValue = QVariant(valStr.toUInt());
345 break;
346 case MAV_PARAM_TYPE_INT32:
347 paramValue = QVariant(valStr.toInt());
348 break;
349 case MAV_PARAM_TYPE_UINT16:
350 paramValue = QVariant((quint16)valStr.toUInt());
351 break;
352 case MAV_PARAM_TYPE_INT16:
353 paramValue = QVariant((qint16)valStr.toInt());
354 break;
355 case MAV_PARAM_TYPE_UINT8:
356 paramValue = QVariant((quint8)valStr.toUInt());
357 break;
358 case MAV_PARAM_TYPE_INT8:
359 paramValue = QVariant((qint8)valStr.toUInt());
360 break;
361 default:
362 qCCritical(MockLinkVerboseLog) << "Unknown type" << paramType;
363 paramValue = QVariant(valStr.toInt());
364 break;
365 }
366
367 qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue;
368
369 _mapParamName2Value[compId][paramName] = paramValue;
370 _mapParamName2MavParamType[compId][paramName] = static_cast<MAV_PARAM_TYPE>(paramType);
371 }
372}
373
374void MockLink::_sendHeartBeat()
375{
376 mavlink_message_t msg{};
377 (void) mavlink_msg_heartbeat_pack_chan(
378 _vehicleSystemId,
379 _vehicleComponentId,
380 mavlinkChannel(),
381 &msg,
382 _vehicleType, // MAV_TYPE
383 _firmwareType, // MAV_AUTOPILOT
384 _mavBaseMode, // MAV_MODE
385 _mavCustomMode, // custom mode
386 _mavState // MAV_STATE
387 );
388 respondWithMavlinkMessage(msg);
389}
390
391void MockLink::_sendHighLatency2()
392{
393 qCDebug(MockLinkLog) << "Sending" << _mavCustomMode;
394
395 union px4_custom_mode px4_cm{};
396 px4_cm.data = _mavCustomMode;
397
398 mavlink_message_t msg{};
399 (void) mavlink_msg_high_latency2_pack_chan(
400 _vehicleSystemId,
401 _vehicleComponentId,
402 mavlinkChannel(),
403 &msg,
404 0, // timestamp
405 _vehicleType, // MAV_TYPE
406 _firmwareType, // MAV_AUTOPILOT
407 px4_cm.custom_mode_hl, // custom_mode
408 static_cast<int32_t>(_vehicleLatitude * 1E7),
409 static_cast<int32_t>(_vehicleLongitude * 1E7),
410 static_cast<int16_t>(_vehicleAltitudeAMSL),
411 static_cast<int16_t>(_vehicleAltitudeAMSL), // target_altitude,
412 0, // heading
413 0, // target_heading
414 0, // target_distance
415 0, // throttle
416 0, // airspeed
417 0, // airspeed_sp
418 0, // groundspeed
419 0, // windspeed,
420 0, // wind_heading
421 UINT8_MAX, // eph not known
422 UINT8_MAX, // epv not known
423 0, // temperature_air
424 0, // climb_rate
425 -1, // battery, do not use?
426 0, // wp_num
427 0, // failure_flags
428 0, 0, 0 // custom0, custom1, custom2
429 );
430 respondWithMavlinkMessage(msg);
431}
432
433void MockLink::_sendSysStatus()
434{
435 mavlink_message_t msg{};
436 (void) mavlink_msg_sys_status_pack_chan(
437 _vehicleSystemId,
438 _vehicleComponentId,
439 static_cast<uint8_t>(mavlinkChannel()),
440 &msg,
441 MAV_SYS_STATUS_SENSOR_GPS, // onboard_control_sensors_present
442 0, // onboard_control_sensors_enabled
443 0, // onboard_control_sensors_health
444 250, // load
445 4200 * 4, // voltage_battery
446 8000, // current_battery
447 _battery1PctRemaining, // battery_remaining
448 0,0,0,0,0,0,0,0,0
449 );
450 respondWithMavlinkMessage(msg);
451}
452
453void MockLink::_sendBatteryStatus()
454{
455 if (_battery1PctRemaining > 1) {
456 _battery1PctRemaining = static_cast<int8_t>(100 - (_runningTime.elapsed() / 1000));
457 _battery1TimeRemaining = static_cast<double>(_batteryMaxTimeRemaining) * (static_cast<double>(_battery1PctRemaining) / 100.0);
458 if (_battery1PctRemaining > 50) {
459 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
460 } else if (_battery1PctRemaining > 30) {
461 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_LOW;
462 } else if (_battery1PctRemaining > 20) {
463 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL;
464 } else {
465 _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY;
466 }
467 }
468
469 if (_battery2PctRemaining > 1) {
470 _battery2PctRemaining = static_cast<int8_t>(100 - ((_runningTime.elapsed() / 1000) / 2));
471 _battery2TimeRemaining = static_cast<double>(_batteryMaxTimeRemaining) * (static_cast<double>(_battery2PctRemaining) / 100.0);
472 if (_battery2PctRemaining > 50) {
473 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK;
474 } else if (_battery2PctRemaining > 30) {
475 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_LOW;
476 } else if (_battery2PctRemaining > 20) {
477 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_CRITICAL;
478 } else {
479 _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_EMERGENCY;
480 }
481 }
482
483 mavlink_message_t msg{};
484 uint16_t rgVoltages[10]{};
485 uint16_t rgVoltagesNone[10]{};
486 uint16_t rgVoltagesExtNone[4]{};
487
488 for (size_t i = 0; i < std::size(rgVoltages); i++) {
489 rgVoltages[i] = UINT16_MAX;
490 rgVoltagesNone[i] = UINT16_MAX;
491 }
492 rgVoltages[0] = rgVoltages[1] = rgVoltages[2] = 4200;
493
494 (void) mavlink_msg_battery_status_pack_chan(
495 _vehicleSystemId,
496 _vehicleComponentId,
497 static_cast<uint8_t>(mavlinkChannel()),
498 &msg,
499 1, // battery id
500 MAV_BATTERY_FUNCTION_ALL,
501 MAV_BATTERY_TYPE_LIPO,
502 2100, // temp cdegC
503 rgVoltages,
504 600, // battery cA
505 100, // current consumed mAh
506 -1, // energy consumed not supported
507 _battery1PctRemaining,
508 _battery1TimeRemaining,
509 _battery1ChargeState,
510 rgVoltagesExtNone,
511 0, // MAV_BATTERY_MODE
512 0 // MAV_BATTERY_FAULT
513 );
514 respondWithMavlinkMessage(msg);
515
516 (void) mavlink_msg_battery_status_pack_chan(
517 _vehicleSystemId,
518 _vehicleComponentId,
519 static_cast<uint8_t>(mavlinkChannel()),
520 &msg,
521 2, // battery id
522 MAV_BATTERY_FUNCTION_ALL,
523 MAV_BATTERY_TYPE_LIPO,
524 INT16_MAX, // temp cdegC
525 rgVoltagesNone,
526 600, // battery cA
527 100, // current consumed mAh
528 -1, // energy consumed not supported
529 _battery2PctRemaining,
530 _battery2TimeRemaining,
531 _battery2ChargeState,
532 rgVoltagesExtNone,
533 0, // MAV_BATTERY_MODE
534 0 // MAV_BATTERY_FAULT
535 );
536 respondWithMavlinkMessage(msg);
537}
538
539void MockLink::_sendVibration()
540{
541 mavlink_message_t msg{};
542 (void) mavlink_msg_vibration_pack_chan(
543 _vehicleSystemId,
544 _vehicleComponentId,
545 mavlinkChannel(),
546 &msg,
547 0, // time_usec
548 50.5, // vibration_x,
549 10.5, // vibration_y,
550 60.0, // vibration_z,
551 1, // clipping_0
552 2, // clipping_0
553 3 // clipping_0
554 );
555 respondWithMavlinkMessage(msg);
556}
557
558void MockLink::respondWithMavlinkMessage(const mavlink_message_t &msg)
559{
560 if (!_commLost) {
561 uint8_t buffer[MAVLINK_MAX_PACKET_LEN]{};
562 const int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg);
563 const QByteArray bytes(reinterpret_cast<char*>(buffer), cBuffer);
564 emit bytesReceived(this, bytes);
565 }
566}
567
568void MockLink::_writeBytes(const QByteArray &bytes)
569{
570 // This prevents the responses to mavlink messages from being sent until the _writeBytes returns.
571 emit writeBytesQueuedSignal(bytes);
572}
573
574void MockLink::_writeBytesQueued(const QByteArray &bytes)
575{
576 if (!_connected || !mavlinkChannelIsSet()) {
577 qCDebug(MockLinkLog) << "Dropping queued bytes on disconnected/uninitialized mock link";
578 return;
579 }
580
581 if (_inNSH) {
582 _handleIncomingNSHBytes(bytes.constData(), bytes.length());
583 return;
584 }
585
586 if (bytes.startsWith(QByteArrayLiteral("\r\r\r"))) {
587 _inNSH = true;
588 _handleIncomingNSHBytes(&bytes.constData()[3], bytes.length() - 3);
589 }
590
591 _handleIncomingMavlinkBytes(reinterpret_cast<const uint8_t*>(bytes.constData()), bytes.length());
592}
593
594void MockLink::_handleIncomingNSHBytes(const char *bytes, int cBytes)
595{
596 Q_UNUSED(cBytes);
597
598 // Drop back out of NSH
599 if ((cBytes == 4) && (bytes[0] == '\r') && (bytes[1] == '\r') && (bytes[2] == '\r')) {
600 _inNSH = false;
601 return;
602 }
603
604 if (cBytes > 0) {
605 qCDebug(MockLinkLog) << "NSH:" << bytes;
606#if 0
607 // MockLink not quite ready to handle this correctly yet
608 if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) {
609 // This is the mavlink start command
610 _mavlinkStarted = true;
611 }
612#endif
613 }
614}
615
616void MockLink::_handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes)
617{
618 mavlink_message_t msg{};
619 mavlink_status_t comm{};
620
621 QMutexLocker lock(&_mavlinkAuxMutex);
622 for (qint64 i = 0; i < cBytes; i++) {
623 const int parsed = mavlink_parse_char(_getMavlinkAuxChannel(), bytes[i], &msg, &comm);
624 if (!parsed) {
625 continue;
626 }
627 lock.unlock();
628 _handleIncomingMavlinkMsg(msg);
629 lock.relock();
630 }
631}
632
633void MockLink::_updateIncomingMessageCounts(const mavlink_message_t &msg)
634{
635 _receivedMavlinkMessageCountMap[msg.msgid]++;
636
637 // Update command-specific counts if this is a COMMAND_LONG message
638 if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
639 mavlink_command_long_t request{};
640 mavlink_msg_command_long_decode(&msg, &request);
641
642 _receivedMavCommandCountMap[static_cast<MAV_CMD>(request.command)]++;
643 _receivedMavCommandByCompCountMap[static_cast<MAV_CMD>(request.command)][request.target_component]++;
644
645 if (request.command == MAV_CMD_REQUEST_MESSAGE) {
646 _receivedRequestMessageCountMap[static_cast<uint32_t>(request.param1)]++;
647 _receivedRequestMessageByCompAndMsgCountMap[request.target_component][static_cast<int>(request.param1)]++;
648 }
649 }
650}
651
652void MockLink::_handleIncomingMavlinkMsg(const mavlink_message_t &msg)
653{
654 _updateIncomingMessageCounts(msg);
655
656 if (_missionItemHandler->handleMavlinkMessage(msg)) {
657 return;
658 }
659
660 if (_enableCamera && _mockLinkCamera->handleMavlinkMessage(msg)) {
661 return;
662 }
663
664 if (_enableGimbal && _mockLinkGimbal->handleMavlinkMessage(msg)) {
665 return;
666 }
667
668 switch (msg.msgid) {
669 case MAVLINK_MSG_ID_HEARTBEAT:
670 _handleHeartBeat(msg);
671 break;
672 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
673 _handleParamRequestList(msg);
674 break;
675 case MAVLINK_MSG_ID_SET_MODE:
676 _handleSetMode(msg);
677 break;
678 case MAVLINK_MSG_ID_PARAM_SET:
679 _handleParamSet(msg);
680 break;
681 case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
682 _handleParamRequestRead(msg);
683 break;
684 case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
685 _handleFTP(msg);
686 break;
687 case MAVLINK_MSG_ID_COMMAND_LONG:
688 _handleCommandLong(msg);
689 break;
690 case MAVLINK_MSG_ID_MANUAL_CONTROL:
691 _handleManualControl(msg);
692 break;
693 case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
694 _handleLogRequestList(msg);
695 break;
696 case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
697 _handleLogRequestData(msg);
698 break;
699 case MAVLINK_MSG_ID_PARAM_MAP_RC:
700 _handleParamMapRC(msg);
701 break;
702 default:
703 break;
704 }
705}
706
707void MockLink::_handleHeartBeat(const mavlink_message_t &msg)
708{
709 Q_UNUSED(msg);
710 qCDebug(MockLinkLog) << "Heartbeat";
711}
712
713void MockLink::_handleParamMapRC(const mavlink_message_t &msg)
714{
715 mavlink_param_map_rc_t paramMapRC{};
716 mavlink_msg_param_map_rc_decode(&msg, &paramMapRC);
717
718 const QString paramName(QString::fromLocal8Bit(paramMapRC.param_id, static_cast<int>(strnlen(paramMapRC.param_id, MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN))));
719
720 if (paramMapRC.param_index == -1) {
721 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);
722 } else if (paramMapRC.param_index == -2) {
723 qCDebug(MockLinkLog) << "MockLink - PARAM_MAP_RC: Clear tuningID" << paramMapRC.parameter_rc_channel_index;
724 } else {
725 qCWarning(MockLinkLog) << "MockLink - PARAM_MAP_RC: Unsupported param_index" << paramMapRC.param_index;
726 }
727}
728
729void MockLink::_handleSetMode(const mavlink_message_t &msg)
730{
731 mavlink_set_mode_t request{};
732 mavlink_msg_set_mode_decode(&msg, &request);
733
734 Q_ASSERT(request.target_system == _vehicleSystemId);
735
736 _mavBaseMode = request.base_mode;
737 _mavCustomMode = request.custom_mode;
738}
739
740void MockLink::_handleManualControl(const mavlink_message_t &msg)
741{
742 mavlink_manual_control_t manualControl{};
743 mavlink_msg_manual_control_decode(&msg, &manualControl);
744
745 qCDebug(MockLinkLog) << "MANUAL_CONTROL" << manualControl.x << manualControl.y << manualControl.z << manualControl.r;
746}
747
748void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString &paramName, float paramFloat)
749{
750 Q_ASSERT(_mapParamName2Value.contains(componentId));
751 Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
752 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
753
754 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
755 QVariant paramVariant;
756 mavlink_param_union_t valueUnion{};
757 valueUnion.param_float = paramFloat;
758 switch (paramType) {
759 case MAV_PARAM_TYPE_REAL32:
760 paramVariant = QVariant::fromValue(valueUnion.param_float);
761 break;
762 case MAV_PARAM_TYPE_UINT32:
763 paramVariant = QVariant::fromValue(valueUnion.param_uint32);
764 break;
765 case MAV_PARAM_TYPE_INT32:
766 paramVariant = QVariant::fromValue(valueUnion.param_int32);
767 break;
768 case MAV_PARAM_TYPE_UINT16:
769 paramVariant = QVariant::fromValue(valueUnion.param_uint16);
770 break;
771 case MAV_PARAM_TYPE_INT16:
772 paramVariant = QVariant::fromValue(valueUnion.param_int16);
773 break;
774 case MAV_PARAM_TYPE_UINT8:
775 paramVariant = QVariant::fromValue(valueUnion.param_uint8);
776 break;
777 case MAV_PARAM_TYPE_INT8:
778 paramVariant = QVariant::fromValue(valueUnion.param_int8);
779 break;
780 default:
781 qCCritical(MockLinkLog) << "Invalid parameter type" << paramType;
782 paramVariant = QVariant::fromValue(valueUnion.param_int32);
783 break;
784 }
785
786 qCDebug(MockLinkLog) << "_setParamFloatUnionIntoMap" << paramName << paramVariant;
787 _mapParamName2Value[componentId][paramName] = paramVariant;
788}
789
790void MockLink::setMockParamValue(int componentId, const QString &paramName, float value)
791{
792 mavlink_param_union_t valueUnion{};
793 valueUnion.param_float = value;
794 _setParamFloatUnionIntoMap(componentId, paramName, valueUnion.param_float);
795}
796
797float MockLink::_floatUnionForParam(int componentId, const QString &paramName)
798{
799 Q_ASSERT(_mapParamName2Value.contains(componentId));
800 Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
801 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
802
803 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
804 const QVariant paramVar = _mapParamName2Value[componentId][paramName];
805
806 mavlink_param_union_t valueUnion{};
807 switch (paramType) {
808 case MAV_PARAM_TYPE_REAL32:
809 valueUnion.param_float = paramVar.toFloat();
810 break;
811 case MAV_PARAM_TYPE_UINT32:
812 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
813 valueUnion.param_float = paramVar.toUInt();
814 } else {
815 valueUnion.param_uint32 = paramVar.toUInt();
816 }
817 break;
818 case MAV_PARAM_TYPE_INT32:
819 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
820 valueUnion.param_float = paramVar.toInt();
821 } else {
822 valueUnion.param_int32 = paramVar.toInt();
823 }
824 break;
825 case MAV_PARAM_TYPE_UINT16:
826 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
827 valueUnion.param_float = paramVar.toUInt();
828 } else {
829 valueUnion.param_uint16 = paramVar.toUInt();
830 }
831 break;
832 case MAV_PARAM_TYPE_INT16:
833 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
834 valueUnion.param_float = paramVar.toInt();
835 } else {
836 valueUnion.param_int16 = paramVar.toInt();
837 }
838 break;
839 case MAV_PARAM_TYPE_UINT8:
840 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
841 valueUnion.param_float = paramVar.toUInt();
842 } else {
843 valueUnion.param_uint8 = paramVar.toUInt();
844 }
845 break;
846 case MAV_PARAM_TYPE_INT8:
847 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
848 valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1();
849 } else {
850 valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1();
851 }
852 break;
853 default:
854 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
855 valueUnion.param_float = paramVar.toInt();
856 } else {
857 valueUnion.param_int32 = paramVar.toInt();
858 }
859 qCCritical(MockLinkLog) << "Invalid parameter type" << paramType;
860 }
861
862 return valueUnion.param_float;
863}
864
865uint32_t MockLink::_computeParamHash(int componentId) const
866{
867 // Volatile parameters are excluded from the hash, matching PX4 firmware and ParameterManager::_tryCacheHashLoad
868 static const QStringList volatileParams = {
869 QStringLiteral("COM_FLIGHT_UUID"),
870 QStringLiteral("EKF2_MAGBIAS_X"),
871 QStringLiteral("EKF2_MAGBIAS_Y"),
872 QStringLiteral("EKF2_MAGBIAS_Z"),
873 QStringLiteral("EKF2_MAG_DECL"),
874 QStringLiteral("LND_FLIGHT_T_HI"),
875 QStringLiteral("LND_FLIGHT_T_LO"),
876 QStringLiteral("SYS_RESTART_TYPE"),
877 };
878
879 uint32_t crc = 0;
880 const auto &params = _mapParamName2Value[componentId];
881 for (auto it = params.constBegin(); it != params.constEnd(); ++it) {
882 const QString &name = it.key();
883 if (volatileParams.contains(name)) {
884 continue;
885 }
886 const QVariant &value = it.value();
887 const MAV_PARAM_TYPE mavType = _mapParamName2MavParamType[componentId][name];
888 const FactMetaData::ValueType_t factType = ParameterManager::mavTypeToFactType(mavType);
889
890 crc = QGC::crc32(reinterpret_cast<const uint8_t *>(qPrintable(name)), name.length(), crc);
891 crc = QGC::crc32(static_cast<const uint8_t *>(value.constData()), FactMetaData::typeToSize(factType), crc);
892 }
893 return crc;
894}
895
896void MockLink::_handleParamRequestList(const mavlink_message_t &msg)
897{
898 if (_failureMode == MockConfiguration::FailParamNoResponseToRequestList) {
899 return;
900 }
901
902 mavlink_param_request_list_t request{};
903 mavlink_msg_param_request_list_decode(&msg, &request);
904
905 Q_ASSERT(request.target_system == _vehicleSystemId);
906 Q_ASSERT(request.target_component == MAV_COMP_ID_ALL);
907
908 // Cache component IDs and first component's param names to avoid repeated keys() calls in worker
909 // Thread safety: Lock mutex before modifying shared state accessed by worker thread
910 QMutexLocker locker(&_paramRequestListMutex);
911 _paramRequestListComponentIds = _mapParamName2Value.keys();
912 if (!_paramRequestListComponentIds.isEmpty()) {
913 _paramRequestListParamNames = _mapParamName2Value[_paramRequestListComponentIds.first()].keys();
914 }
915
916 // Start the worker routine
917 _currentParamRequestListComponentIndex = 0;
918 _currentParamRequestListParamIndex = 0;
919 _paramRequestListHashCheckSent = false;
920}
921
922void MockLink::_paramRequestListWorker()
923{
924 if (_currentParamRequestListComponentIndex == -1) {
925 // Initial request complete
926 return;
927 }
928
929 // Thread safety: Lock mutex before accessing shared state modified by main thread
930 QMutexLocker locker(&_paramRequestListMutex);
931
932 // Use cached lists instead of calling keys() on every iteration (500Hz)
933 if (_currentParamRequestListComponentIndex >= _paramRequestListComponentIds.count()) {
934 _currentParamRequestListComponentIndex = -1;
935 return;
936 }
937
938 const int componentId = _paramRequestListComponentIds.at(_currentParamRequestListComponentIndex);
939 const int cParameters = _paramRequestListParamNames.count();
940
941 if (_currentParamRequestListParamIndex >= cParameters) {
942 // All regular params sent — for PX4, append _HASH_CHECK as the last entry in the stream.
943 // Uses param_count=0, param_index=-1 (same as standalone response) so ParameterManager
944 // handles it via the _HASH_CHECK early-return path without affecting param count tracking.
945 if (_firmwareType == MAV_AUTOPILOT_PX4 && !_paramRequestListHashCheckSent) {
946 _paramRequestListHashCheckSent = true;
947
948 mavlink_param_union_t valueUnion{};
949 valueUnion.type = MAV_PARAM_TYPE_UINT32;
950 valueUnion.param_uint32 = _computeParamHash(componentId);
951
952 char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{};
953 (void) strncpy(paramId, "_HASH_CHECK", MAVLINK_MSG_ID_PARAM_VALUE_LEN);
954
955 qCDebug(MockLinkLog) << "Sending _HASH_CHECK in PARAM_REQUEST_LIST stream" << componentId << "hash:" << valueUnion.param_uint32;
956
957 mavlink_message_t responseMsg{};
958 (void) mavlink_msg_param_value_pack_chan(
959 _vehicleSystemId,
960 componentId,
961 mavlinkChannel(),
962 &responseMsg,
963 paramId,
964 valueUnion.param_float,
965 MAV_PARAM_TYPE_UINT32,
966 0, // param_count: 0 to avoid affecting ParameterManager's count tracking
967 -1 // param_index: -1 signals this is a virtual/out-of-band parameter
968 );
969 respondWithMavlinkMessage(responseMsg);
970 return;
971 }
972
973 // Move to next component
974 if (++_currentParamRequestListComponentIndex >= _paramRequestListComponentIds.count()) {
975 _currentParamRequestListComponentIndex = -1;
976 _paramRequestListComponentIds.clear();
977 _paramRequestListParamNames.clear();
978 } else {
979 // Cache param names for the new component
980 _paramRequestListParamNames = _mapParamName2Value[_paramRequestListComponentIds.at(_currentParamRequestListComponentIndex)].keys();
981 _currentParamRequestListParamIndex = 0;
982 _paramRequestListHashCheckSent = false;
983 }
984 return;
985 }
986
987 const QString &paramName = _paramRequestListParamNames.at(_currentParamRequestListParamIndex);
988
989 if (((_failureMode == MockConfiguration::FailMissingParamOnInitialRequest) || (_failureMode == MockConfiguration::FailMissingParamOnAllRequests)) && (paramName == _failParam)) {
990 qCDebug(MockLinkLog) << "Skipping param send:" << paramName;
991 } else {
992 char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{};
993 mavlink_message_t responseMsg{};
994
995 Q_ASSERT(_mapParamName2Value[componentId].contains(paramName));
996 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramName));
997
998 const MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[componentId][paramName];
999
1000 Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN);
1001 (void) strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN);
1002
1003 qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId];
1004
1005 (void) mavlink_msg_param_value_pack_chan(
1006 _vehicleSystemId,
1007 componentId, // component id
1008 mavlinkChannel(),
1009 &responseMsg, // Outgoing message
1010 paramId, // Parameter name
1011 _floatUnionForParam(componentId, paramName), // Parameter value
1012 paramType, // MAV_PARAM_TYPE
1013 cParameters, // Total number of parameters
1014 _currentParamRequestListParamIndex // Index of this parameter
1015 );
1016 respondWithMavlinkMessage(responseMsg);
1017 }
1018
1019 // Move to next param index
1020 ++_currentParamRequestListParamIndex;
1021}
1022
1023void MockLink::_handleParamSet(const mavlink_message_t &msg)
1024{
1025 mavlink_param_set_t request{};
1026 mavlink_msg_param_set_decode(&msg, &request);
1027
1028 Q_ASSERT(request.target_system == _vehicleSystemId);
1029 const int componentId = request.target_component;
1030
1031 // Param may not be null terminated if exactly fits
1032 char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1]{};
1033 paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN] = 0;
1034 (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
1035
1036 qCDebug(MockLinkLog) << "_handleParamSet" << componentId << paramId << request.param_type;
1037
1038 // PX4 special case: _HASH_CHECK is a virtual parameter used by ParameterManager
1039 // to signal cache-hit and stop parameter streaming. It is intentionally not part
1040 // of the normal parameter maps.
1041 if ((_firmwareType == MAV_AUTOPILOT_PX4) && (strncmp(paramId, "_HASH_CHECK", MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN) == 0)) {
1042 QMutexLocker locker(&_paramRequestListMutex);
1043 _currentParamRequestListComponentIndex = -1;
1044 _paramRequestListComponentIds.clear();
1045 _paramRequestListParamNames.clear();
1046 qCDebug(MockLinkLog) << "Received _HASH_CHECK PARAM_SET, stopping parameter stream";
1047 return;
1048 }
1049
1050 Q_ASSERT(_mapParamName2Value.contains(componentId));
1051 Q_ASSERT(_mapParamName2MavParamType.contains(componentId));
1052 Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
1053 Q_ASSERT(request.param_type == _mapParamName2MavParamType[componentId][paramId]);
1054
1055 // Apply failure behaviors before committing change.
1056 if (_paramSetFailureMode == FailParamSetFirstAttemptNoAck && _paramSetFailureFirstAttemptPending) {
1057 qCDebug(MockLinkLog) << "Param set failure: first attempt no ack" << paramId;
1058 _paramSetFailureFirstAttemptPending = false;
1059 return;
1060 }
1061
1062 if (_paramSetFailureMode == FailParamSetNoAck) {
1063 qCDebug(MockLinkLog) << "Param set failure: no ack" << paramId;
1064 return;
1065 }
1066
1067 // Normal success path
1068 _setParamFloatUnionIntoMap(componentId, paramId, request.param_value);
1069
1070 mavlink_message_t responseMsg;
1071 mavlink_msg_param_value_pack_chan(
1072 _vehicleSystemId,
1073 componentId, // component id
1074 mavlinkChannel(),
1075 &responseMsg, // Outgoing message
1076 paramId, // Parameter name
1077 request.param_value, // Send same value back
1078 request.param_type, // Send same type back
1079 _mapParamName2Value[componentId].count(), // Total number of parameters
1080 _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter
1081 );
1082 respondWithMavlinkMessage(responseMsg);
1083}
1084
1085void MockLink::_handleParamRequestRead(const mavlink_message_t &msg)
1086{
1087 mavlink_message_t responseMsg{};
1088 mavlink_param_request_read_t request{};
1089 mavlink_msg_param_request_read_decode(&msg, &request);
1090
1091 const QString paramName(QString::fromLocal8Bit(request.param_id, static_cast<int>(strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN))));
1092 const int componentId = request.target_component;
1093
1094 // special case for magic _HASH_CHECK value (PX4 only)
1095 if ((_firmwareType == MAV_AUTOPILOT_PX4) && (paramName == "_HASH_CHECK")) {
1096 _hashCheckRequestCount++;
1097 if (_hashCheckNoResponse) {
1098 return;
1099 }
1100
1101 const int hashComponentId = _mapParamName2Value.contains(MAV_COMP_ID_AUTOPILOT1)
1102 ? MAV_COMP_ID_AUTOPILOT1
1103 : _mapParamName2Value.keys().first();
1104
1105 mavlink_param_union_t valueUnion{};
1106 valueUnion.type = MAV_PARAM_TYPE_UINT32;
1107 valueUnion.param_uint32 = _computeParamHash(hashComponentId);
1108 (void) mavlink_msg_param_value_pack_chan(
1109 _vehicleSystemId,
1110 hashComponentId,
1111 mavlinkChannel(),
1112 &responseMsg,
1113 request.param_id,
1114 valueUnion.param_float,
1115 MAV_PARAM_TYPE_UINT32,
1116 0,
1117 -1
1118 );
1119 respondWithMavlinkMessage(responseMsg);
1120 return;
1121 }
1122
1123 Q_ASSERT(_mapParamName2Value.contains(componentId));
1124
1125 char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]{};
1126 paramId[0] = 0;
1127
1128 Q_ASSERT(request.target_system == _vehicleSystemId);
1129
1130 if (request.param_index == -1) {
1131 // Request is by param name. Param may not be null terminated if exactly fits
1132 (void) strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
1133 } else {
1134 // Request is by index
1135 Q_ASSERT(request.param_index >= 0 && request.param_index < _mapParamName2Value[componentId].count());
1136
1137 const QString key = _mapParamName2Value[componentId].keys().at(request.param_index);
1138 Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
1139 strcpy(paramId, key.toLocal8Bit().constData());
1140 }
1141
1142 Q_ASSERT(_mapParamName2Value[componentId].contains(paramId));
1143 Q_ASSERT(_mapParamName2MavParamType[componentId].contains(paramId));
1144
1145 if ((_failureMode == MockConfiguration::FailMissingParamOnAllRequests) && (strcmp(paramId, _failParam) == 0)) {
1146 qCDebug(MockLinkLog) << "Ignoring request read for " << _failParam;
1147 // Fail to send this param no matter what
1148 return;
1149 }
1150
1151 if (_paramRequestReadFailureMode == FailParamRequestReadFirstAttemptNoResponse && _paramRequestReadFailureFirstAttemptPending) {
1152 qCDebug(MockLinkLog) << "Param request read failure: first attempt no response" << paramId;
1153 _paramRequestReadFailureFirstAttemptPending = false;
1154 return;
1155 }
1156
1157 if (_paramRequestReadFailureMode == FailParamRequestReadNoResponse) {
1158 qCDebug(MockLinkLog) << "Param request read failure: no response" << paramId;
1159 return;
1160 }
1161
1162 (void) mavlink_msg_param_value_pack_chan(
1163 _vehicleSystemId,
1164 componentId, // component id
1165 mavlinkChannel(),
1166 &responseMsg, // Outgoing message
1167 paramId, // Parameter name
1168 _floatUnionForParam(componentId, paramId), // Parameter value
1169 _mapParamName2MavParamType[componentId][paramId], // Parameter type
1170 _mapParamName2Value[componentId].count(), // Total number of parameters
1171 _mapParamName2Value[componentId].keys().indexOf(paramId) // Index of this parameter
1172 );
1173 respondWithMavlinkMessage(responseMsg);
1174}
1175
1176void MockLink::_handleFTP(const mavlink_message_t &msg)
1177{
1178 _mockLinkFTP->mavlinkMessageReceived(msg);
1179}
1180
1181void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t &request)
1182{
1183 uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
1184
1185 switch (request.command) {
1186 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED:
1187 // Test command which sends in progress messages and then acceptance ack
1188 commandResult = MAV_RESULT_ACCEPTED;
1189 break;
1190 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED:
1191 // Test command which sends in progress messages and then failure ack
1192 commandResult = MAV_RESULT_FAILED;
1193 break;
1194 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK:
1195 // Test command which sends in progress messages and then never sends final result ack
1196 break;
1197 }
1198
1199 mavlink_message_t commandAck{};
1200 (void) mavlink_msg_command_ack_pack_chan(
1201 _vehicleSystemId,
1202 _vehicleComponentId,
1203 mavlinkChannel(),
1204 &commandAck,
1205 request.command,
1206 MAV_RESULT_IN_PROGRESS,
1207 1, // progress
1208 0, // result_param2
1209 0, // target_system
1210 0 // target_component
1211 );
1212 respondWithMavlinkMessage(commandAck);
1213
1214 if (request.command != MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK) {
1215 (void) mavlink_msg_command_ack_pack_chan(
1216 _vehicleSystemId,
1217 _vehicleComponentId,
1218 mavlinkChannel(),
1219 &commandAck,
1220 request.command,
1221 commandResult,
1222 0, // progress
1223 0, // result_param2
1224 0, // target_system
1225 0 // target_component
1226 );
1227 respondWithMavlinkMessage(commandAck);
1228 }
1229}
1230
1231void MockLink::_handleCommandLongSetMessageInterval(const mavlink_command_long_t &request, bool &accepted)
1232{
1233 Q_UNUSED(request);
1234 accepted = false;
1235}
1236
1237void MockLink::_handleCommandLong(const mavlink_message_t &msg)
1238{
1239 static bool firstCmdUser3 = true;
1240 static bool firstCmdUser4 = true;
1241
1242 mavlink_command_long_t request{};
1243 mavlink_msg_command_long_decode(&msg, &request);
1244
1245 uint8_t commandResult = MAV_RESULT_UNSUPPORTED;
1246
1247 switch (request.command) {
1248 case MAV_CMD_COMPONENT_ARM_DISARM:
1249 if (request.param1 == 0.0f) {
1250 _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
1251 } else {
1252 _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
1253 }
1254 commandResult = MAV_RESULT_ACCEPTED;
1255 break;
1256 case MAV_CMD_PREFLIGHT_CALIBRATION:
1257 _handlePreFlightCalibration(request);
1258 commandResult = MAV_RESULT_ACCEPTED;
1259 break;
1260 case MAV_CMD_DO_MOTOR_TEST:
1261 commandResult = MAV_RESULT_ACCEPTED;
1262 break;
1263 case MAV_CMD_CONTROL_HIGH_LATENCY:
1264 if (linkConfiguration()->isHighLatency()) {
1265 _highLatencyTransmissionEnabled = static_cast<int>(request.param1) != 0;
1266 emit highLatencyTransmissionEnabledChanged(_highLatencyTransmissionEnabled);
1267 commandResult = MAV_RESULT_ACCEPTED;
1268 } else {
1269 commandResult = MAV_RESULT_FAILED;
1270 }
1271 break;
1272 case MAV_CMD_PREFLIGHT_STORAGE:
1273 commandResult = MAV_RESULT_ACCEPTED;
1274 break;
1275 case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
1276 commandResult = MAV_RESULT_ACCEPTED;
1277 _respondWithAutopilotVersion();
1278 break;
1279 case MAV_CMD_REQUEST_MESSAGE:
1280 {
1281 bool accepted = false;
1282 bool noAck = false;
1283 _handleRequestMessage(request, accepted, noAck);
1284 if (noAck) {
1285 // FailRequestMessageCommandNoResponse: don't send any ack, let vehicle timeout
1286 return;
1287 }
1288 if (accepted) {
1289 commandResult = MAV_RESULT_ACCEPTED;
1290 }
1291 break;
1292 }
1293 case MAV_CMD_NAV_TAKEOFF:
1294 _handleTakeoff(request);
1295 commandResult = MAV_RESULT_ACCEPTED;
1296 break;
1297 case MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED:
1298 // Test command which always returns MAV_RESULT_ACCEPTED
1299 commandResult = MAV_RESULT_ACCEPTED;
1300 break;
1301 case MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED:
1302 // Test command which always returns MAV_RESULT_FAILED
1303 commandResult = MAV_RESULT_FAILED;
1304 break;
1305 case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED:
1306 // Test command which does not respond to first request and returns MAV_RESULT_ACCEPTED on second attempt
1307 if (firstCmdUser3) {
1308 firstCmdUser3 = false;
1309 return;
1310 } else {
1311 firstCmdUser3 = true;
1312 commandResult = MAV_RESULT_ACCEPTED;
1313 }
1314 break;
1315 case MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED:
1316 // Test command which does not respond to first request and returns MAV_RESULT_FAILED on second attempt
1317 if (firstCmdUser4) {
1318 firstCmdUser4 = false;
1319 return;
1320 } else {
1321 firstCmdUser4 = true;
1322 commandResult = MAV_RESULT_FAILED;
1323 }
1324 break;
1325 case MAV_CMD_MOCKLINK_NO_RESPONSE:
1326 case MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY:
1327 // Test command which never responds
1328 return;
1329 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED:
1330 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED:
1331 case MockLink::MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK:
1332 _handleInProgressCommandLong(request);
1333 return;
1334 case MAV_CMD_SET_MESSAGE_INTERVAL:
1335 {
1336 bool accepted = false;
1337
1338 _handleCommandLongSetMessageInterval(request, accepted);
1339 if (accepted) {
1340 commandResult = MAV_RESULT_ACCEPTED;
1341 }
1342 break;
1343 }
1344 }
1345
1346 mavlink_message_t commandAck{};
1347 (void) mavlink_msg_command_ack_pack_chan(
1348 _vehicleSystemId,
1349 _vehicleComponentId,
1350 mavlinkChannel(),
1351 &commandAck,
1352 request.command,
1353 commandResult,
1354 0, // progress
1355 0, // result_param2
1356 0, // target_system
1357 0 // target_component
1358 );
1359 respondWithMavlinkMessage(commandAck);
1360}
1361
1362void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult)
1363{
1364 mavlink_message_t commandAck{};
1365 (void) mavlink_msg_command_ack_pack_chan(
1366 _vehicleSystemId,
1367 _vehicleComponentId,
1368 mavlinkChannel(),
1369 &commandAck,
1370 command,
1371 ackResult,
1372 0, // progress
1373 0, // result_param2
1374 0, // target_system
1375 0 // target_component
1376 );
1377 respondWithMavlinkMessage(commandAck);
1378}
1379
1380void MockLink::_respondWithAutopilotVersion()
1381{
1382 union FlightVersion {
1383 uint32_t raw;
1384
1385 struct {
1386 uint8_t type; // bits 0–7
1387 uint8_t patch; // bits 8–15
1388 uint8_t minor; // bits 16–23
1389 uint8_t major; // bits 24–31
1390 } parts;
1391
1392 FlightVersion(uint32_t version = 0) : raw(version) {}
1393 };
1394 FlightVersion flightVersion;
1395
1396#ifndef QGC_NO_ARDUPILOT_DIALECT
1397 if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
1398 if (_vehicleType == MAV_TYPE_SUBMARINE ) {
1399 flightVersion.parts.major = 4;
1400 flightVersion.parts.minor = 5;
1401 flightVersion.parts.patch = 7;
1402 } else {
1403 flightVersion.parts.major = 4;
1404 flightVersion.parts.minor = 6;
1405 flightVersion.parts.patch = 3;
1406 }
1407 flightVersion.parts.type = FIRMWARE_VERSION_TYPE_OFFICIAL;
1408 } else if (_firmwareType == MAV_AUTOPILOT_PX4) {
1409#endif
1410 flightVersion.parts.major = 1;
1411 flightVersion.parts.minor = 4;
1412 flightVersion.parts.patch = 1;
1413 flightVersion.parts.type = FIRMWARE_VERSION_TYPE_DEV;
1414#ifndef QGC_NO_ARDUPILOT_DIALECT
1415 }
1416#endif
1417
1418 const uint8_t customVersion[8]{};
1419 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);
1420
1421 mavlink_message_t msg{};
1422 (void) mavlink_msg_autopilot_version_pack_chan(
1423 _vehicleSystemId,
1424 _vehicleComponentId,
1425 mavlinkChannel(),
1426 &msg,
1427 capabilities,
1428 flightVersion.raw, // flight_sw_version,
1429 0, // middleware_sw_version,
1430 0, // os_sw_version,
1431 0, // board_version,
1432 reinterpret_cast<const uint8_t*>(&customVersion), // flight_custom_version,
1433 reinterpret_cast<const uint8_t*>(&customVersion), // middleware_custom_version,
1434 reinterpret_cast<const uint8_t*>(&customVersion), // os_custom_version,
1435 _boardVendorId,
1436 _boardProductId,
1437 0, // uid
1438 0 // uid2
1439 );
1440 respondWithMavlinkMessage(msg);
1441}
1442
1443void MockLink::_sendHomePosition()
1444{
1445 const float bogus[4]{};
1446
1447 mavlink_message_t msg{};
1448 (void) mavlink_msg_home_position_pack_chan(
1449 _vehicleSystemId,
1450 _vehicleComponentId,
1451 mavlinkChannel(),
1452 &msg,
1453 static_cast<int32_t>(_vehicleLatitude * 1E7),
1454 static_cast<int32_t>(_vehicleLongitude * 1E7),
1455 static_cast<int32_t>(_defaultVehicleHomeAltitude * 1000),
1456 0.0f, 0.0f, 0.0f,
1457 &bogus[0],
1458 0.0f, 0.0f, 0.0f,
1459 0
1460 );
1461 respondWithMavlinkMessage(msg);
1462}
1463
1464void MockLink::_sendGpsRawInt()
1465{
1466 static uint64_t timeTick = 0;
1467
1468 mavlink_message_t msg{};
1469 (void) mavlink_msg_gps_raw_int_pack_chan(
1470 _vehicleSystemId,
1471 _vehicleComponentId,
1472 mavlinkChannel(),
1473 &msg,
1474 timeTick++, // time since boot
1475 GPS_FIX_TYPE_3D_FIX,
1476 static_cast<int32_t>(_vehicleLatitude * 1E7),
1477 static_cast<int32_t>(_vehicleLongitude * 1E7),
1478 static_cast<int32_t>(_vehicleAltitudeAMSL * 1000),
1479 3 * 100, // hdop
1480 3 * 100, // vdop
1481 UINT16_MAX, // velocity not known
1482 UINT16_MAX, // course over ground not known
1483 8, // satellites visible
1484 //-- Extension
1485 0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up).
1486 0, // Position uncertainty in meters * 1000 (positive for up).
1487 0, // Altitude uncertainty in meters * 1000 (positive for up).
1488 0, // Speed uncertainty in meters * 1000 (positive for up).
1489 0, // Heading / track uncertainty in degrees * 1e5.
1490 65535 // Yaw not provided
1491 );
1492 respondWithMavlinkMessage(msg);
1493}
1494
1495void MockLink::_sendGlobalPositionInt()
1496{
1497 static uint64_t timeTick = 0;
1498
1499 mavlink_message_t msg{};
1500 (void) mavlink_msg_global_position_int_pack_chan(
1501 _vehicleSystemId,
1502 _vehicleComponentId,
1503 mavlinkChannel(),
1504 &msg,
1505 timeTick++, // time since boot
1506 static_cast<int32_t>(_vehicleLatitude * 1E7),
1507 static_cast<int32_t>(_vehicleLongitude * 1E7),
1508 static_cast<int32_t>(_vehicleAltitudeAMSL * 1000),
1509 static_cast<int32_t>((_vehicleAltitudeAMSL - _defaultVehicleHomeAltitude) * 1000),
1510 0, 0, 0, // no speed sent
1511 UINT16_MAX // no heading sent
1512 );
1513 respondWithMavlinkMessage(msg);
1514}
1515
1516void MockLink::_sendExtendedSysState()
1517{
1518 mavlink_message_t msg{};
1519 (void) mavlink_msg_extended_sys_state_pack_chan(
1520 _vehicleSystemId,
1521 _vehicleComponentId,
1522 mavlinkChannel(),
1523 &msg,
1524 MAV_VTOL_STATE_UNDEFINED,
1525 (_vehicleAltitudeAMSL > _defaultVehicleHomeAltitude) ? MAV_LANDED_STATE_IN_AIR : MAV_LANDED_STATE_ON_GROUND
1526 );
1527 respondWithMavlinkMessage(msg);
1528}
1529
1530void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks)
1531{
1532 constexpr int cChunks = 4;
1533
1534 int num = 0;
1535 for (int i = 0; i < cChunks; i++) {
1536 if (missingChunks && (i & 1)) {
1537 continue;
1538 }
1539
1540 int cBuf = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
1541 char msgBuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]{};
1542
1543 if (i == cChunks - 1) {
1544 // Last chunk is partial
1545 cBuf /= 2;
1546 }
1547
1548 for (int j = 0; j < cBuf - 1; j++) {
1549 msgBuf[j] = '0' + num++;
1550 if (num > 9) {
1551 num = 0;
1552 }
1553 }
1554 msgBuf[cBuf-1] = 'A' + i;
1555
1556 mavlink_message_t msg{};
1557 (void) mavlink_msg_statustext_pack_chan(
1558 _vehicleSystemId,
1559 _vehicleComponentId,
1560 mavlinkChannel(),
1561 &msg,
1562 MAV_SEVERITY_INFO,
1563 msgBuf,
1564 chunkId,
1565 i // chunk sequence number
1566 );
1567 respondWithMavlinkMessage(msg);
1568 }
1569}
1570
1571void MockLink::_sendStatusTextMessages()
1572{
1573 struct StatusMessage {
1574 MAV_SEVERITY severity;
1575 const char *msg;
1576 };
1577
1578 static constexpr struct StatusMessage rgMessages[] = {
1579 { MAV_SEVERITY_INFO, "#Testing audio output" },
1580 { MAV_SEVERITY_EMERGENCY, "Status text emergency" },
1581 { MAV_SEVERITY_ALERT, "Status text alert" },
1582 { MAV_SEVERITY_CRITICAL, "Status text critical" },
1583 { MAV_SEVERITY_ERROR, "Status text error" },
1584 { MAV_SEVERITY_WARNING, "Status text warning" },
1585 { MAV_SEVERITY_NOTICE, "Status text notice" },
1586 { MAV_SEVERITY_INFO, "Status text info" },
1587 { MAV_SEVERITY_DEBUG, "Status text debug" },
1588 };
1589
1590 mavlink_message_t msg{};
1591 for (size_t i = 0; i < std::size(rgMessages); i++) {
1592 const struct StatusMessage *status = &rgMessages[i];
1593 char statusText[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {};
1594 (void) std::strncpy(statusText, status->msg, sizeof(statusText) - 1);
1595
1596 (void) mavlink_msg_statustext_pack_chan(
1597 _vehicleSystemId,
1598 _vehicleComponentId,
1599 mavlinkChannel(),
1600 &msg,
1601 status->severity,
1602 statusText,
1603 0, // Not a chunked sequence
1604 0 // Not a chunked sequence
1605 );
1606 respondWithMavlinkMessage(msg);
1607 }
1608
1609 _sendChunkedStatusText(1, false /* missingChunks */);
1610 _sendChunkedStatusText(2, true /* missingChunks */);
1611 _sendChunkedStatusText(3, false /* missingChunks */); // This should cause the previous incomplete chunk to spit out
1612 _sendChunkedStatusText(4, true /* missingChunks */); // This should cause the timeout to fire
1613}
1614
1615MockLink *MockLink::_startMockLink(MockConfiguration *mockConfig)
1616{
1617 mockConfig->setDynamic(true);
1618 SharedLinkConfigurationPtr config = LinkManager::instance()->addConfiguration(mockConfig);
1619
1620 if (LinkManager::instance()->createConnectedLink(config)) {
1621 return qobject_cast<MockLink*>(config->link());
1622 }
1623
1624 return nullptr;
1625}
1626
1627MockLink *MockLink::_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1628{
1629 MockConfiguration *const mockConfig = new MockConfiguration(configName);
1630
1631 mockConfig->setFirmwareType(firmwareType);
1632 mockConfig->setVehicleType(vehicleType);
1633 mockConfig->setSendStatusText(sendStatusText);
1634 mockConfig->setEnableCamera(enableCamera);
1635 mockConfig->setEnableGimbal(enableGimbal);
1636 mockConfig->setFailureMode(failureMode);
1637
1638 return _startMockLink(mockConfig);
1639}
1640
1641MockLink *MockLink::startPX4MockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1642{
1643 return _startMockLinkWorker(QStringLiteral("PX4 MultiRotor MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, enableCamera, enableGimbal, failureMode);
1644}
1645
1646MockLink *MockLink::startGenericMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1647{
1648 return _startMockLinkWorker(QStringLiteral("Generic MockLink"), MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, enableCamera, enableGimbal, failureMode);
1649}
1650
1651MockLink *MockLink::startNoInitialConnectMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1652{
1653 return _startMockLinkWorker(QStringLiteral("No Initial Connect MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, enableCamera, enableGimbal, failureMode);
1654}
1655
1656MockLink *MockLink::startAPMArduCopterMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1657{
1658 return _startMockLinkWorker(QStringLiteral("ArduCopter MockLink"),MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, enableCamera, enableGimbal, failureMode);
1659}
1660
1661MockLink *MockLink::startAPMArduPlaneMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1662{
1663 return _startMockLinkWorker(QStringLiteral("ArduPlane MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, enableCamera, enableGimbal, failureMode);
1664}
1665
1666MockLink *MockLink::startAPMArduSubMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1667{
1668 return _startMockLinkWorker(QStringLiteral("ArduSub MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, enableCamera, enableGimbal, failureMode);
1669}
1670
1671MockLink *MockLink::startAPMArduRoverMockLink(bool sendStatusText, bool enableCamera, bool enableGimbal, MockConfiguration::FailureMode_t failureMode)
1672{
1673 return _startMockLinkWorker(QStringLiteral("ArduRover MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, enableCamera, enableGimbal, failureMode);
1674}
1675
1676void MockLink::_sendRCChannels()
1677{
1678 mavlink_message_t msg{};
1679 (void) mavlink_msg_rc_channels_pack_chan(
1680 _vehicleSystemId,
1681 _vehicleComponentId,
1682 mavlinkChannel(),
1683 &msg,
1684 0, // time_boot_ms
1685 16, // chancount
1686 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 1-8
1687 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, // channel 9-16
1688 UINT16_MAX, UINT16_MAX, // channel 17/18 unused
1689 0 // rssi
1690 );
1691 respondWithMavlinkMessage(msg);
1692}
1693
1694void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request)
1695{
1696 static constexpr const char *gyroCalResponse = "[cal] calibration started: 2 gyro";
1697 static constexpr const char *magCalResponse = "[cal] calibration started: 2 mag";
1698 static constexpr const char *accelCalResponse = "[cal] calibration started: 2 accel";
1699 const char *pCalMessage;
1700
1701 if (request.param1 == 1) {
1702 pCalMessage = gyroCalResponse;
1703 } else if (request.param2 == 1) {
1704 pCalMessage = magCalResponse;
1705 } else if (request.param5 == 1) {
1706 pCalMessage = accelCalResponse;
1707 } else {
1708 return;
1709 }
1710
1711 char statusText[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {};
1712 (void) std::strncpy(statusText, pCalMessage, sizeof(statusText) - 1);
1713
1714 mavlink_message_t msg{};
1715 (void) mavlink_msg_statustext_pack_chan(
1716 _vehicleSystemId,
1717 _vehicleComponentId,
1718 mavlinkChannel(),
1719 &msg,
1720 MAV_SEVERITY_INFO,
1721 statusText,
1722 0,
1723 0 // Not chunked
1724 );
1725 respondWithMavlinkMessage(msg);
1726}
1727
1728void MockLink::_handleTakeoff(const mavlink_command_long_t &request)
1729{
1730 _vehicleAltitudeAMSL = request.param7 + _defaultVehicleHomeAltitude;
1731 _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
1732}
1733
1734void MockLink::_handleLogRequestList(const mavlink_message_t &msg)
1735{
1736 mavlink_log_request_list_t request{};
1737 mavlink_msg_log_request_list_decode(&msg, &request);
1738
1739 if ((request.start != 0) && (request.end != 0xffff)) {
1740 qCWarning(MockLinkLog) << "_handleLogRequestList cannot handle partial requests";
1741 return;
1742 }
1743
1744 mavlink_message_t responseMsg{};
1745 mavlink_msg_log_entry_pack_chan(
1746 _vehicleSystemId,
1747 _vehicleComponentId,
1748 mavlinkChannel(),
1749 &responseMsg,
1750 _logDownloadLogId, // log id
1751 1, // num_logs
1752 1, // last_log_num
1753 0, // time_utc
1754 _logDownloadFileSize // size
1755 );
1756 respondWithMavlinkMessage(responseMsg);
1757}
1758
1759QString MockLink::_createRandomFile(uint32_t byteCount)
1760{
1761 QTemporaryFile tempFile;
1762 tempFile.setAutoRemove(false);
1763 if (!tempFile.open()) {
1764 qCWarning(MockLinkLog) << "MockLink::createRandomFile open failed" << tempFile.errorString();
1765 return QString();
1766 }
1767
1768 for (uint32_t bytesWritten = 0; bytesWritten < byteCount; bytesWritten++) {
1769 const unsigned char byte = (QRandomGenerator::global()->generate() * 0xFF) / RAND_MAX;
1770 (void) tempFile.write(reinterpret_cast<const char*>(&byte), 1);
1771 }
1772
1773 tempFile.close();
1774 return tempFile.fileName();
1775}
1776
1777void MockLink::_handleLogRequestData(const mavlink_message_t &msg)
1778{
1779 mavlink_log_request_data_t request{};
1780 mavlink_msg_log_request_data_decode(&msg, &request);
1781
1782#ifdef QGC_UNITTEST_BUILD
1783 if (_logDownloadFilename.isEmpty()) {
1784 _logDownloadFilename = _createRandomFile(_logDownloadFileSize);
1785 }
1786#endif
1787
1788 if (request.id != 0) {
1789 qCWarning(MockLinkLog) << "_handleLogRequestData id must be 0";
1790 return;
1791 }
1792
1793 if (request.ofs > (_logDownloadFileSize - 1)) {
1794 qCWarning(MockLinkLog) << "_handleLogRequestData offset past end of file request.ofs:size" << request.ofs << _logDownloadFileSize;
1795 return;
1796 }
1797
1798 // This will trigger _logDownloadWorker to send data
1799 // Thread-safe access: Main thread writes, worker thread reads every 2ms. Serialize to avoid
1800 // worker reading inconsistent offset/count or using stale values while downloading.
1801 QMutexLocker locker(&_logDownloadMutex);
1802 _logDownloadCurrentOffset = request.ofs;
1803 if (request.ofs + request.count > _logDownloadFileSize) {
1804 request.count = _logDownloadFileSize - request.ofs;
1805 }
1806 _logDownloadBytesRemaining = request.count;
1807}
1808
1809void MockLink::_logDownloadWorker()
1810{
1811 // Runs every 2ms (500Hz on worker thread). Must protect shared state modified by main thread.
1812 // Without lock: main could write new offset/count while we're reading, causing corrupted downloads.
1813 QMutexLocker locker(&_logDownloadMutex);
1814 if (_logDownloadBytesRemaining == 0) {
1815 return;
1816 }
1817
1818 QFile file(_logDownloadFilename);
1819 if (!file.open(QIODevice::ReadOnly)) {
1820 qCWarning(MockLinkLog) << "_logDownloadWorker open failed" << file.errorString();
1821 return;
1822 }
1823
1824 uint8_t buffer[MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN]{};
1825
1826 const qint64 bytesToRead = qMin(_logDownloadBytesRemaining, (uint32_t)MAVLINK_MSG_LOG_DATA_FIELD_DATA_LEN);
1827 Q_ASSERT(file.seek(_logDownloadCurrentOffset));
1828 Q_ASSERT(file.read(reinterpret_cast<char*>(buffer), bytesToRead) == bytesToRead);
1829
1830 qCDebug(MockLinkLog) << "_logDownloadWorker" << _logDownloadCurrentOffset << _logDownloadBytesRemaining;
1831
1832 mavlink_message_t responseMsg{};
1833 (void) mavlink_msg_log_data_pack_chan(
1834 _vehicleSystemId,
1835 _vehicleComponentId,
1836 mavlinkChannel(),
1837 &responseMsg,
1838 _logDownloadLogId,
1839 _logDownloadCurrentOffset,
1840 bytesToRead,
1841 &buffer[0]
1842 );
1843 respondWithMavlinkMessage(responseMsg);
1844
1845 _logDownloadCurrentOffset += bytesToRead;
1846 _logDownloadBytesRemaining -= bytesToRead;
1847
1848 file.close();
1849}
1850
1851void MockLink::_sendADSBVehicles()
1852{
1853 for (int i = 0; i < _adsbVehicles.size(); ++i) {
1854 // Slightly change the direction to simulate different paths
1855 _adsbVehicles[i].angle += (i + 1); // Vary the change to make each path unique
1856
1857 // Move each vehicle by a smaller distance to simulate slower speed
1858 _adsbVehicles[i].coordinate = _adsbVehicles[i].coordinate.atDistanceAndAzimuth(5, _adsbVehicles[i].angle); // 50 meters per update for slower speed
1859
1860 // Simulate slight variations in altitude
1861 _adsbVehicles[i].altitude += (i % 2 == 0 ? 0.5 : -0.5); // Increase or decrease altitude
1862
1863 QByteArray callsign = QString("N12345%1").arg(i, 2, 10, QChar('0')).toLatin1();
1864 callsign.resize(MAVLINK_MSG_ADSB_VEHICLE_FIELD_CALLSIGN_LEN);
1865
1866 // Prepare and send MAVLink message for each vehicle
1867 mavlink_message_t responseMsg{};
1868 (void) mavlink_msg_adsb_vehicle_pack_chan(
1869 _vehicleSystemId,
1870 _vehicleComponentId,
1871 mavlinkChannel(),
1872 &responseMsg,
1873 12345 + i, // Unique ICAO address for each vehicle
1874 _adsbVehicles[i].coordinate.latitude() * 1e7,
1875 _adsbVehicles[i].coordinate.longitude() * 1e7,
1876 ADSB_ALTITUDE_TYPE_GEOMETRIC,
1877 _adsbVehicles[i].altitude * 1000, // Altitude in millimeters
1878 // Use the current angle as heading
1879 static_cast<uint16_t>(_adsbVehicles[i].angle * 100), // Heading in centidegrees
1880 0, 0, // Horizontal/Vertical velocity
1881 callsign.constData(), // Unique callsign
1882 ADSB_EMITTER_TYPE_ROTOCRAFT,
1883 1, // Seconds since last communication
1884 ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED,
1885 0 // Squawk code
1886 );
1887 respondWithMavlinkMessage(responseMsg);
1888 }
1889}
1890
1891void MockLink::_moveADSBVehicle(int vehicleIndex)
1892{
1893 _adsbAngles[vehicleIndex] += 10; // Increment angle for smoother movement
1894 QGeoCoordinate &coord = _adsbVehicleCoordinates[vehicleIndex];
1895
1896 // Update the position based on the new angle
1897 coord = QGeoCoordinate(coord.latitude(), coord.longitude()).atDistanceAndAzimuth(500, _adsbAngles[vehicleIndex]);
1898 coord.setAltitude(100); // Keeping altitude constant for simplicity
1899}
1900
1901void MockLink::_handleRequestMessageAutopilotVersion(const mavlink_command_long_t &/*request*/, bool &accepted)
1902{
1903 accepted = true;
1904
1905 switch (_failureMode) {
1906 case MockConfiguration::FailNone:
1907 break;
1908 case MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionFailure:
1909 accepted = false;
1910 return;
1911 case MockConfiguration::FailInitialConnectRequestMessageAutopilotVersionLost:
1912 accepted = true;
1913 return;
1914 default:
1915 break;
1916 }
1917
1918 _respondWithAutopilotVersion();
1919}
1920
1921void MockLink::_handleRequestMessageDebug(const mavlink_command_long_t &/*request*/, bool &accepted, bool &noAck)
1922{
1923 accepted = true;
1924 noAck = false;
1925
1926 switch (_requestMessageFailureMode) {
1927 case FailRequestMessageNone:
1928 break;
1929 case FailRequestMessageCommandAcceptedMsgNotSent:
1930 return;
1931 case FailRequestMessageCommandUnsupported:
1932 accepted = false;
1933 return;
1934 case FailRequestMessageCommandNoResponse:
1935 accepted = false;
1936 noAck = true;
1937 return;
1938 }
1939
1940 mavlink_message_t responseMsg{};
1941 (void) mavlink_msg_debug_pack_chan(
1942 _vehicleSystemId,
1943 _vehicleComponentId,
1944 mavlinkChannel(),
1945 &responseMsg,
1946 0, 0, 0
1947 );
1948 respondWithMavlinkMessage(responseMsg);
1949}
1950
1951void MockLink::_handleRequestMessageAvailableModes(const mavlink_command_long_t &request, bool &accepted)
1952{
1953 accepted = true;
1954
1955 // Thread-safe access: Check-then-set pattern must be atomic. Worker increments index every 2ms,
1956 // so check for "already running" and start/stop operations must serialize to prevent race where
1957 // main reads false, worker increments, main overwrites with different value -> lost update.
1958 QMutexLocker locker(&_availableModesWorkerMutex);
1959 if (request.param2 == 0) {
1960 // Request for available modes to be streamed out
1961 if (_availableModesWorkerNextModeIndex != 0) {
1962 qCWarning(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: _availableModesWorker already running - _availableModesWorkerNextModeIndex:" << _availableModesWorkerNextModeIndex;
1963 accepted = false;
1964 return;
1965 }
1966 qCDebug(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: starting available modes sequence worker";
1967 _availableModesWorkerNextModeIndex = 1; // Start with the first mode in sequence (1-based index)
1968 } else {
1969 // Request for specific mode
1970 if (request.param2 > _availableFlightModes.count()) {
1971 qCWarning(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: requested mode index out of range" << request.param2 << _availableFlightModes.count();
1972 accepted = false;
1973 return;
1974 }
1975 qCDebug(MockLinkLog) << "MAVLINK_MSG_ID_AVAILABLE_MODES: received specific mode request for index" << request.param2;
1976 _availableModesWorkerNextModeIndex = -request.param2; // Negative index indicates a specific single mode request
1977 }
1978}
1979
1980void MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool &accepted, bool &noAck)
1981{
1982 accepted = false;
1983 noAck = false;
1984
1985 const uint32_t requestedMessageId = static_cast<uint32_t>(request.param1);
1986
1987 // Per-message-ID no-response injection: silently drop the request (no ACK, no message)
1988 {
1989 QMutexLocker locker(&_requestMessageNoResponseMutex);
1990 if (_requestMessageNoResponseIds.contains(requestedMessageId)) {
1991 noAck = true;
1992 return;
1993 }
1994 }
1995
1996 switch (static_cast<int>(request.param1)) {
1997 case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
1998 _handleRequestMessageAutopilotVersion(request, accepted);
1999 break;
2000 case MAVLINK_MSG_ID_COMPONENT_METADATA:
2001 if (_firmwareType == MAV_AUTOPILOT_PX4) {
2002 _sendGeneralMetaData();
2003 accepted = true;
2004 }
2005 break;
2006 case MAVLINK_MSG_ID_DEBUG:
2007 _handleRequestMessageDebug(request, accepted, noAck);
2008 break;
2009 case MAVLINK_MSG_ID_AVAILABLE_MODES:
2010 _handleRequestMessageAvailableModes(request, accepted);
2011 break;
2012 }
2013}
2014
2015void MockLink::_sendGeneralMetaData()
2016{
2017 static constexpr const char metaDataURI[MAVLINK_MSG_COMPONENT_METADATA_FIELD_URI_LEN] = "mftp://[;comp=1]general.json";
2018
2019 mavlink_message_t responseMsg{};
2020 (void) mavlink_msg_component_metadata_pack_chan(
2021 _vehicleSystemId,
2022 _vehicleComponentId,
2023 mavlinkChannel(),
2024 &responseMsg,
2025 0, // time_boot_ms
2026 100, // general_metadata_file_crc
2027 metaDataURI
2028 );
2029 respondWithMavlinkMessage(responseMsg);
2030}
2031
2032void MockLink::_sendRemoteIDArmStatus()
2033{
2034 char armStatusError[MAVLINK_MSG_OPEN_DRONE_ID_ARM_STATUS_FIELD_ERROR_LEN] = {};
2035 std::strncpy(armStatusError, "No Error", sizeof(armStatusError) - 1);
2036
2037 mavlink_message_t msg{};
2038 (void) mavlink_msg_open_drone_id_arm_status_pack(
2039 _vehicleSystemId,
2040 MAV_COMP_ID_ODID_TXRX_1,
2041 &msg,
2042 MAV_ODID_ARM_STATUS_GOOD_TO_ARM,
2043 armStatusError
2044 );
2045 respondWithMavlinkMessage(msg);
2046}
2047
2048void MockLink::simulateConnectionRemoved()
2049{
2050 _commLost = true;
2051 _connectionRemoved();
2052}
2053
2054MockLinkFTP *MockLink::mockLinkFTP() const
2055{
2056 return _mockLinkFTP;
2057}
2058
2059void MockLink::_sendAvailableMode(uint8_t modeIndexOneBased)
2060{
2061 if (modeIndexOneBased > _availableModesCount()) {
2062 qCWarning(MockLinkLog) << "modeIndexOneBased out of range" << modeIndexOneBased << _availableModesCount();
2063 return;
2064 }
2065
2066 qCDebug(MockLinkLog) << "_sendAvailableMode modeIndexOneBased:" << modeIndexOneBased;
2067
2068 const FlightMode_t &availableMode = _availableFlightModes[modeIndexOneBased - 1];
2069 char modeName[MAVLINK_MSG_AVAILABLE_MODES_FIELD_MODE_NAME_LEN] = {};
2070 std::strncpy(modeName, availableMode.name, sizeof(modeName) - 1);
2071
2072 mavlink_message_t msg{};
2073
2074 (void) mavlink_msg_available_modes_pack_chan(
2075 _vehicleSystemId,
2076 _vehicleComponentId,
2077 mavlinkChannel(),
2078 &msg,
2079 _availableModesCount(),
2080 modeIndexOneBased,
2081 availableMode.standard_mode,
2082 availableMode.custom_mode,
2083 availableMode.canBeSet ? 0 : MAV_MODE_PROPERTY_NOT_USER_SELECTABLE,
2084 modeName);
2085 respondWithMavlinkMessage(msg);
2086}
2087
2088void MockLink::_availableModesWorker()
2089{
2090 // Runs every 2ms (500Hz on worker thread). Reads and increments shared index modified by main.
2091 // Read-modify-write must be atomic to prevent lost updates or incorrect state transitions.
2092 QMutexLocker locker(&_availableModesWorkerMutex);
2093 if (_availableModesWorkerNextModeIndex == 0) {
2094 // Not active
2095 return;
2096 }
2097
2098 _sendAvailableMode(qAbs(_availableModesWorkerNextModeIndex));
2099
2100 if (_availableModesWorkerNextModeIndex < 0) {
2101 // Single mode request, stop worker
2102 _availableModesWorkerNextModeIndex = 0;
2103 } else if (++_availableModesWorkerNextModeIndex > _availableModesCount()) {
2104 // All modes sent, stop worker
2105 _availableModesWorkerNextModeIndex = 0;
2106 qCDebug(MockLinkLog) << "_availableModesWorker: all modes sent, stopping worker";
2107 }
2108}
2109
2110void MockLink::_sendAvailableModesMonitor()
2111{
2112 mavlink_message_t msg{};
2113
2114 (void) mavlink_msg_available_modes_monitor_pack_chan(
2115 _vehicleSystemId,
2116 _vehicleComponentId,
2117 mavlinkChannel(),
2118 &msg,
2119 _availableModesMonitorSeqNumber);
2120 respondWithMavlinkMessage(msg);
2121}
2122
2123int MockLink::_availableModesCount() const
2124{
2125 return _availableFlightModes.count() - (_availableModesMonitorSeqNumber == 0 ? 1 : 0); // Exclude the delayed mode
2126}
SharedLinkConfigurationPtr
std::shared_ptr< LinkConfiguration > SharedLinkConfigurationPtr
Definition LinkConfiguration.h:138
mavlink_get_channel_status
mavlink_status_t * mavlink_get_channel_status(uint8_t chan)
Definition QGCMAVLink.cc:50
qgcApp
#define qgcApp()
Definition QGCApplication.h:34
mavlink_message_t
struct __mavlink_message mavlink_message_t
Definition QGCCorePlugin.h:23
QGC_LOGGING_CATEGORY
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Definition QGCLoggingCategory.h:14
FactMetaData::typeToSize
static size_t typeToSize(ValueType_t type)
Definition FactMetaData.cc:863
LinkInterface
The link interface defines the interface for all links used to communicate with the ground station ap...
Definition LinkInterface.h:14
LinkInterface::bytesReceived
void bytesReceived(LinkInterface *link, const QByteArray &data)
LinkInterface::disconnected
void disconnected()
LinkInterface::mavlinkChannel
uint8_t mavlinkChannel() const
Definition LinkInterface.cc:29
LinkInterface::_freeMavlinkChannel
virtual void _freeMavlinkChannel()
Definition LinkInterface.cc:88
LinkInterface::_connectionRemoved
void _connectionRemoved()
Definition LinkInterface.cc:116
LinkInterface::mavlinkChannelIsSet
bool mavlinkChannelIsSet() const
Definition LinkInterface.cc:38
LinkInterface::_allocateMavlinkChannel
virtual bool _allocateMavlinkChannel()
Definition LinkInterface.cc:64
LinkInterface::connected
void connected()
LinkInterface::linkConfiguration
SharedLinkConfigurationPtr linkConfiguration()
Definition LinkInterface.h:29
MockLinkCamera::sendCameraHeartbeats
void sendCameraHeartbeats()
Send heartbeats for all simulated camera components (call from 1Hz tasks)
Definition MockLinkCamera.cc:70
MockLinkCamera::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkCamera.cc:111
MockLinkCamera::run10HzTasks
void run10HzTasks()
Update camera states (call from 10Hz tasks)
Definition MockLinkCamera.cc:88
MockLinkFTP
Mock implementation of Mavlink FTP server.
Definition MockLinkFTP.h:18
MockLinkFTP::mavlinkMessageReceived
void mavlinkMessageReceived(const mavlink_message_t &message)
Called to handle an FTP message.
Definition MockLinkFTP.cc:386
MockLinkGimbal::run1HzTasks
void run1HzTasks()
Send periodic gimbal status messages (call from 1Hz tasks)
Definition MockLinkGimbal.cc:31
MockLinkGimbal::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkGimbal.cc:67
MockLinkMissionItemHandler::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkMissionItemHandler.cc:30
ParameterManager::mavTypeToFactType
static FactMetaData::ValueType_t mavTypeToFactType(MAV_PARAM_TYPE mavType)
Definition ParameterManager.cc:1153
QGC::crc32
quint32 crc32(const quint8 *src, unsigned len, unsigned state)
Definition QGC.cc:97
px4_custom_mode
Definition px4_custom_mode.h:34
px4_custom_mode::data
uint32_t data
Definition px4_custom_mode.h:40