QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
MockLinkCamera.cc
Go to the documentation of this file.
1#include "MockLinkCamera.h"
2#include "MAVLinkLib.h"
3#include "MockLink.h"
4#include "MissionManager/MissionCommandTree.h"
5#include "QGCLoggingCategory.h"
6
7#include <QtCore/QDateTime>
8#include <QtCore/QLoggingCategory>
9#include <QtCore/QtMath>
10
11#include <algorithm>
12
13QGC_LOGGING_CATEGORY(MockLinkCameraLog, "Comms.MockLink.MockLinkCamera")
14
15MockLinkCamera::MockLinkCamera(MockLink *mockLink,
16 bool captureVideo,
17 bool captureImage,
18 bool hasModes,
19 bool hasVideoStream,
20 bool canCaptureImageInVideoMode,
21 bool canCaptureVideoInImageMode,
22 bool hasBasicZoom,
23 bool hasTrackingPoint,
24 bool hasTrackingRectangle)
25 : _mockLink(mockLink)
26{
27 // Build capability flags from configuration
28 uint32_t configuredFlags = 0;
29 if (captureVideo) configuredFlags |= CAMERA_CAP_FLAGS_CAPTURE_VIDEO;
30 if (captureImage) configuredFlags |= CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
31 if (hasModes) configuredFlags |= CAMERA_CAP_FLAGS_HAS_MODES;
32 if (hasVideoStream) configuredFlags |= CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM;
33 if (canCaptureImageInVideoMode) configuredFlags |= CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE;
34 if (canCaptureVideoInImageMode) configuredFlags |= CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE;
35 if (hasBasicZoom) configuredFlags |= CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM;
36 if (hasTrackingPoint) configuredFlags |= CAMERA_CAP_FLAGS_HAS_TRACKING_POINT;
37 if (hasTrackingRectangle) configuredFlags |= CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE;
38
39 // Camera 1: full-featured with configurable flags
40 _cameras[0].compId = MAV_COMP_ID_CAMERA;
41 _cameras[0].capFlags = configuredFlags;
42 _cameras[0].cameraMode = CAMERA_MODE_IMAGE;
43 if ((configuredFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM) && (configuredFlags & CAMERA_CAP_FLAGS_HAS_MODES)) {
44 _cameras[0].cameraMode = CAMERA_MODE_VIDEO;
45 }
46
47 // Camera 2: photo-only (always CAPTURE_IMAGE only)
48 _cameras[1].compId = MAV_COMP_ID_CAMERA2;
49 _cameras[1].capFlags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
50 _cameras[1].cameraMode = CAMERA_MODE_IMAGE;
51}
52
53MockLinkCamera::CameraState *MockLinkCamera::_findCamera(uint8_t compId)
54{
55 for (uint8_t i = 0; i < kNumCameras; i++) {
56 if (_cameras[i].compId == compId) {
57 return &_cameras[i];
58 }
59 }
60 return nullptr;
61}
62
63const char *MockLinkCamera::_imageCaptureStatusToString(uint8_t status)
64{
65 switch (status) {
66 case ImageCaptureIdle: return "Idle";
67 case ImageCaptureInProgress: return "InProgress";
68 case ImageCaptureInterval: return "Interval";
69 case ImageCaptureIntervalCapture: return "IntervalCapture";
70 default: return "Unknown";
71 }
72}
73
74void MockLinkCamera::sendCameraHeartbeats()
75{
76 for (uint8_t i = 0; i < kNumCameras; i++) {
77 mavlink_message_t msg{};
78 (void) mavlink_msg_heartbeat_pack_chan(
79 _mockLink->vehicleId(),
80 _cameras[i].compId,
81 _mockLink->outgoingMavlinkChannel(),
82 &msg,
83 MAV_TYPE_CAMERA,
84 MAV_AUTOPILOT_INVALID,
85 0,
86 0,
87 MAV_STATE_ACTIVE);
88 _mockLink->respondWithMavlinkMessage(msg);
89 }
90}
91
92void MockLinkCamera::run10HzTasks()
93{
94 // Runs every 100ms (10Hz on worker thread). Reads and modifies camera state that main thread
95 // also modifies via command handlers. Protect entire state check-and-update to maintain consistency.
96 QMutexLocker locker(&_camerasMutex);
97 const qint64 now = QDateTime::currentMSecsSinceEpoch();
98
99 for (uint8_t i = 0; i < kNumCameras; i++) {
100 CameraState *cam = &_cameras[i];
101
102 // Check for single-shot capture completion (after 500ms)
103 if (cam->singleShotStartMs > 0 && (now - cam->singleShotStartMs) >= 500) {
104 cam->imagesCaptured++;
105 cam->image_status = ImageCaptureIdle;
106 cam->singleShotStartMs = 0;
107
108 qCDebug(MockLinkCameraLog) << "Camera" << cam->compId << "single-shot complete, total:" << cam->imagesCaptured;
109 _sendCameraImageCaptured(cam->compId);
110 }
111
112 // Interval capture: one image per interval
113 if (cam->image_status == ImageCaptureInterval || cam->image_status == ImageCaptureIntervalCapture) {
114 const qint64 intervalMs = static_cast<qint64>(cam->image_interval * 1000.0f);
115 if ((now - cam->intervalLastCaptureMs) >= intervalMs) {
116 cam->intervalLastCaptureMs = now;
117 cam->imagesCaptured++;
118 cam->image_status = ImageCaptureIntervalCapture;
119 qCDebug(MockLinkCameraLog) << "Camera" << cam->compId << "interval capture, total:" << cam->imagesCaptured;
120 _sendCameraImageCaptured(cam->compId);
121
122 if (cam->intervalRemaining > 0 && --cam->intervalRemaining == 0) {
123 cam->image_status = ImageCaptureIdle;
124 cam->image_interval = 0.0f;
125 qCDebug(MockLinkCameraLog) << "Camera" << cam->compId << "interval capture complete";
126 }
127 } else {
128 cam->image_status = ImageCaptureInterval;
129 }
130 }
131
132 // Send periodic tracking image status (with simulated drift)
133 if (cam->trackingMode != CAMERA_TRACKING_MODE_NONE && cam->trackingStatusIntervalUs > 0) {
134 const qint64 intervalMs = (cam->trackingStatusIntervalUs + 999) / 1000;
135 if (cam->trackingStatusLastSentMs == 0 || (now - cam->trackingStatusLastSentMs) >= intervalMs) {
136 // Drift the tracked target in a figure-8 pattern around its anchor
137 const double elapsed = static_cast<double>(now - cam->trackingStartMs) / 1000.0;
138 const float driftX = 0.05f * static_cast<float>(qSin(elapsed * 0.7));
139 const float driftY = 0.05f * static_cast<float>(qSin(elapsed * 1.1));
140
141 if (cam->trackingMode == CAMERA_TRACKING_MODE_POINT) {
142 cam->trackPointX = std::clamp(cam->trackAnchorX + driftX, 0.0f, 1.0f);
143 cam->trackPointY = std::clamp(cam->trackAnchorY + driftY, 0.0f, 1.0f);
144 } else {
145 const float halfW = (cam->trackRecBottomX - cam->trackRecTopX) / 2.0f;
146 const float halfH = (cam->trackRecBottomY - cam->trackRecTopY) / 2.0f;
147 const float cx = std::clamp(cam->trackAnchorX + driftX, halfW, 1.0f - halfW);
148 const float cy = std::clamp(cam->trackAnchorY + driftY, halfH, 1.0f - halfH);
149 cam->trackRecTopX = cx - halfW;
150 cam->trackRecTopY = cy - halfH;
151 cam->trackRecBottomX = cx + halfW;
152 cam->trackRecBottomY = cy + halfH;
153 }
154
155 _sendCameraTrackingImageStatus(cam->compId);
156 cam->trackingStatusLastSentMs = now;
157 }
158 }
159 }
160}
161
162bool MockLinkCamera::handleMavlinkMessage(const mavlink_message_t &msg)
163{
164 if (msg.msgid != MAVLINK_MSG_ID_COMMAND_LONG) {
165 return false;
166 }
167
168 mavlink_command_long_t request{};
169 mavlink_msg_command_long_decode(&msg, &request);
170
171 // Check if this command targets a camera component
172 if (request.target_component < MAV_COMP_ID_CAMERA || request.target_component > MAV_COMP_ID_CAMERA6) {
173 return false;
174 }
175
176 const uint8_t targetCompId = request.target_component;
177
178 if (request.command == MAV_CMD_REQUEST_MESSAGE) {
179 return _handleRequestMessage(request, targetCompId);
180 }
181
182 return _handleCameraCommand(request, targetCompId);
183}
184
185bool MockLinkCamera::_handleCameraCommand(const mavlink_command_long_t &request, uint8_t targetCompId)
186{
187 // Thread-safe access: Main thread modifying camera state that worker thread reads every 100ms.
188 // Serialize all camera state modifications to avoid worker seeing inconsistent state
189 // (e.g., trying to complete capture while main thread is starting a new one).
190 QMutexLocker locker(&_camerasMutex);
191 CameraState *cam = _findCamera(targetCompId);
192 if (!cam) {
193 return false;
194 }
195
196 switch (request.command) {
197 case MAV_CMD_REQUEST_CAMERA_INFORMATION:
198 _sendCameraInformation(targetCompId);
199 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
200 return true;
201
202 case MAV_CMD_REQUEST_CAMERA_SETTINGS:
203 _sendCameraSettings(targetCompId);
204 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
205 return true;
206
207 case MAV_CMD_REQUEST_STORAGE_INFORMATION:
208 _sendStorageInformation(targetCompId);
209 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
210 return true;
211
212 case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS:
213 _sendCameraCaptureStatus(targetCompId);
214 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
215 return true;
216
217 case MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION:
218 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM) {
219 const uint8_t streamId = static_cast<uint8_t>(request.param1);
220 if (streamId == 0) {
221 // Request all streams
222 for (uint8_t s = 1; s <= kNumStreams; s++) {
223 _sendVideoStreamInformation(targetCompId, s);
224 }
225 } else {
226 _sendVideoStreamInformation(targetCompId, streamId);
227 }
228 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
229 } else {
230 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
231 }
232 return true;
233
234 case MAV_CMD_REQUEST_VIDEO_STREAM_STATUS:
235 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM) {
236 const uint8_t streamId = static_cast<uint8_t>(request.param1);
237 if (streamId == 0) {
238 for (uint8_t s = 1; s <= kNumStreams; s++) {
239 _sendVideoStreamStatus(targetCompId, s);
240 }
241 } else {
242 _sendVideoStreamStatus(targetCompId, streamId);
243 }
244 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
245 } else {
246 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
247 }
248 return true;
249
250 case MAV_CMD_SET_CAMERA_MODE:
251 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_MODES) {
252 const uint8_t requestedMode = static_cast<uint8_t>(request.param2);
253
254 if ((requestedMode != CAMERA_MODE_IMAGE) && (requestedMode != CAMERA_MODE_VIDEO)) {
255 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
256 return true;
257 }
258
259 const bool supportsImageMode =
260 (cam->capFlags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE) ||
261 (cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM);
262 const bool supportsVideoMode =
263 (cam->capFlags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO) ||
264 (cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM);
265
266 if ((requestedMode == CAMERA_MODE_IMAGE && !supportsImageMode) ||
267 (requestedMode == CAMERA_MODE_VIDEO && !supportsVideoMode)) {
268 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
269 return true;
270 }
271
272 cam->cameraMode = requestedMode;
273 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "mode set to" << cam->cameraMode;
274 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
275 // Send updated settings after mode change
276 _sendCameraSettings(targetCompId);
277 } else {
278 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
279 }
280 return true;
281
282 case MAV_CMD_IMAGE_START_CAPTURE:
283 if (cam->capFlags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE) {
284 if ((cam->capFlags & CAMERA_CAP_FLAGS_HAS_MODES) &&
285 (cam->cameraMode == CAMERA_MODE_VIDEO) &&
286 !(cam->capFlags & CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE)) {
287 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
288 return true;
289 }
290
291 const float interval = request.param2;
292 const int count = static_cast<int>(request.param3);
293
294 if (interval > 0) {
295 // First interval image is captured on the next 10Hz tick
296 cam->image_status = ImageCaptureInterval;
297 cam->image_interval = interval;
298 cam->intervalRemaining = count;
299 cam->intervalLastCaptureMs = 0;
300 cam->singleShotStartMs = 0;
301 } else {
302 // Single shot - start capture, count will increment after 0.5s
303 cam->image_status = ImageCaptureInProgress;
304 cam->image_interval = 0.0f;
305 cam->singleShotStartMs = QDateTime::currentMSecsSinceEpoch();
306 }
307
308 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "image capture started"
309 << "interval:" << interval << "count:" << count;
310 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
311 } else {
312 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
313 }
314 return true;
315
316 case MAV_CMD_IMAGE_STOP_CAPTURE:
317 if (cam->capFlags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE) {
318 cam->image_status = ImageCaptureIdle;
319 cam->image_interval = 0.0f;
320 cam->intervalRemaining = 0;
321 cam->singleShotStartMs = 0;
322 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "image capture stopped";
323 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
324 } else {
325 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
326 }
327 return true;
328
329 case MAV_CMD_VIDEO_START_CAPTURE:
330 if (cam->capFlags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO) {
331 if ((cam->capFlags & CAMERA_CAP_FLAGS_HAS_MODES) &&
332 (cam->cameraMode == CAMERA_MODE_IMAGE) &&
333 !(cam->capFlags & CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE)) {
334 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
335 return true;
336 }
337
338 cam->recording = true;
339 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "video recording started";
340 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
341 } else {
342 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
343 }
344 return true;
345
346 case MAV_CMD_VIDEO_STOP_CAPTURE:
347 if (cam->capFlags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO) {
348 cam->recording = false;
349 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "video recording stopped";
350 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
351 } else {
352 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
353 }
354 return true;
355
356 case MAV_CMD_STORAGE_FORMAT:
357 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "storage formatted";
358 cam->imagesCaptured = 0;
359 cam->image_status = ImageCaptureIdle;
360 cam->image_interval = 0.0f;
361 cam->intervalRemaining = 0;
362 cam->singleShotStartMs = 0;
363 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
364 _sendStorageInformation(targetCompId);
365 return true;
366
367 case MAV_CMD_SET_CAMERA_ZOOM:
368 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM) {
369 // param2 is an absolute level for ZOOM_TYPE_RANGE and a direction for
370 // ZOOM_TYPE_STEP. Continuous zoom is not modeled.
371 switch (static_cast<int>(request.param1)) {
372 case ZOOM_TYPE_RANGE:
373 cam->zoomLevel = request.param2;
374 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "zoom set to" << cam->zoomLevel;
375 break;
376 case ZOOM_TYPE_STEP:
377 cam->zoomLevel = std::clamp(cam->zoomLevel + (request.param2 * kZoomStepPercent), kZoomMinPercent, kZoomMaxPercent);
378 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "zoom stepped to" << cam->zoomLevel;
379 break;
380 default:
381 break;
382 }
383 // Spec-minimal: CAMERA_SETTINGS is not broadcast after a zoom change.
384 // The GCS must re-request it (MAVLink camera protocol v2).
385 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
386 } else {
387 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
388 }
389 return true;
390
391 case MAV_CMD_SET_CAMERA_FOCUS:
392 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
393 return true;
394
395 case MAV_CMD_RESET_CAMERA_SETTINGS:
396 cam->cameraMode = CAMERA_MODE_IMAGE;
397 cam->zoomLevel = 1.0f;
398 cam->focusLevel = 0.0f;
399 cam->image_status = ImageCaptureIdle;
400 cam->image_interval = 0.0f;
401 cam->intervalRemaining = 0;
402 cam->singleShotStartMs = 0;
403 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "settings reset";
404 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
405 _sendCameraSettings(targetCompId);
406 return true;
407
408 case MAV_CMD_CAMERA_TRACK_POINT:
409 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_TRACKING_POINT) {
410 cam->trackingMode = CAMERA_TRACKING_MODE_POINT;
411 cam->trackPointX = request.param1;
412 cam->trackPointY = request.param2;
413 cam->trackRadius = request.param3;
414 cam->trackAnchorX = request.param1;
415 cam->trackAnchorY = request.param2;
416 cam->trackingStartMs = QDateTime::currentMSecsSinceEpoch();
417 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "tracking point"
418 << cam->trackPointX << cam->trackPointY << "radius" << cam->trackRadius;
419 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
420 } else {
421 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
422 }
423 return true;
424
425 case MAV_CMD_CAMERA_TRACK_RECTANGLE:
426 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE) {
427 cam->trackingMode = CAMERA_TRACKING_MODE_RECTANGLE;
428 cam->trackRecTopX = request.param1;
429 cam->trackRecTopY = request.param2;
430 cam->trackRecBottomX = request.param3;
431 cam->trackRecBottomY = request.param4;
432 cam->trackAnchorX = (request.param1 + request.param3) / 2.0f;
433 cam->trackAnchorY = (request.param2 + request.param4) / 2.0f;
434 cam->trackingStartMs = QDateTime::currentMSecsSinceEpoch();
435 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "tracking rectangle"
436 << cam->trackRecTopX << cam->trackRecTopY
437 << "->" << cam->trackRecBottomX << cam->trackRecBottomY;
438 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
439 } else {
440 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
441 }
442 return true;
443
444 case MAV_CMD_CAMERA_STOP_TRACKING:
445 cam->trackingMode = CAMERA_TRACKING_MODE_NONE;
446 cam->trackingStatusIntervalUs = -1;
447 cam->trackingStatusLastSentMs = 0;
448 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "tracking stopped";
449 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
450 return true;
451
452 case MAV_CMD_SET_MESSAGE_INTERVAL:
453 {
454 const int msgId = static_cast<int>(request.param1);
455 if (msgId == MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS) {
456 cam->trackingStatusIntervalUs = static_cast<qint64>(request.param2);
457 cam->trackingStatusLastSentMs = 0;
458 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId
459 << "tracking status interval" << cam->trackingStatusIntervalUs << "us";
460 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
461 return true;
462 }
463 _sendCommandAck(targetCompId, request.command, MAV_RESULT_UNSUPPORTED);
464 return true;
465 }
466
467 default:
468 break;
469 }
470
471 return false;
472}
473
474bool MockLinkCamera::_handleRequestMessage(const mavlink_command_long_t &request, uint8_t targetCompId)
475{
476 const CameraState *cam = _findCamera(targetCompId);
477 if (!cam) {
478 return false;
479 }
480
481 const int msgId = static_cast<int>(request.param1);
482
483 switch (msgId) {
484 case MAVLINK_MSG_ID_CAMERA_INFORMATION:
485 _sendCameraInformation(targetCompId);
486 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
487 return true;
488
489 case MAVLINK_MSG_ID_CAMERA_SETTINGS:
490 _sendCameraSettings(targetCompId);
491 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
492 return true;
493
494 case MAVLINK_MSG_ID_STORAGE_INFORMATION:
495 _sendStorageInformation(targetCompId);
496 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
497 return true;
498
499 case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
500 _sendCameraCaptureStatus(targetCompId);
501 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
502 return true;
503
504 case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:
505 {
506 if (!(cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM)) {
507 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_DENIED, msgId);
508 return true;
509 }
510 const uint8_t streamId = static_cast<uint8_t>(request.param2);
511 if (streamId == 0) {
512 for (uint8_t s = 1; s <= kNumStreams; s++) {
513 _sendVideoStreamInformation(targetCompId, s);
514 }
515 } else {
516 _sendVideoStreamInformation(targetCompId, streamId);
517 }
518 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
519 return true;
520 }
521
522 case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:
523 {
524 if (!(cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM)) {
525 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_DENIED, msgId);
526 return true;
527 }
528 const uint8_t streamId = static_cast<uint8_t>(request.param2);
529 if (streamId == 0) {
530 for (uint8_t s = 1; s <= kNumStreams; s++) {
531 _sendVideoStreamStatus(targetCompId, s);
532 }
533 } else {
534 _sendVideoStreamStatus(targetCompId, streamId);
535 }
536 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
537 return true;
538 }
539
540 default:
541 // All commands addressed to a component must be acked, even unsupported ones.
542 // Without this the requester's command queue keeps the request pending, blocking
543 // further REQUEST_MESSAGE commands to this component.
544 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_DENIED, msgId);
545 return true;
546 }
547}
548
549void MockLinkCamera::_sendCameraInformation(uint8_t compId)
550{
551 const CameraState *cam = _findCamera(compId);
552 if (!cam) {
553 return;
554 }
555
556 const int cameraIndex = compId - MAV_COMP_ID_CAMERA;
557
558 const uint8_t vendorName[32] = "MockLink";
559 const char cameraDefinitionUri[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN] = {};
560 const QString model = QStringLiteral("MockCam %1").arg(cameraIndex + 1);
561 QByteArray modelBA = model.toLocal8Bit();
562 modelBA.resize(MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN);
563
564 mavlink_message_t msg{};
565 (void) mavlink_msg_camera_information_pack_chan(
566 _mockLink->vehicleId(),
567 compId,
568 _mockLink->outgoingMavlinkChannel(),
569 &msg,
570 0, // time_boot_ms
571 vendorName,
572 reinterpret_cast<const uint8_t *>(modelBA.constData()),
573 0, // firmware_version
574 0, // focal_length
575 0, // sensor_size_h
576 0, // sensor_size_v
577 1920, // resolution_h
578 1080, // resolution_v
579 0, // lens_id
580 cam->capFlags, // flags
581 0, // cam_definition_version
582 cameraDefinitionUri, // cam_definition_uri
583 0, // gimbal_device_id
584 0); // flags (reserved)
585 _mockLink->respondWithMavlinkMessage(msg);
586
587 qCDebug(MockLinkCameraLog) << "Sent CAMERA_INFORMATION for compId:" << compId << "model:" << model;
588}
589
590void MockLinkCamera::_sendCameraSettings(uint8_t compId)
591{
592 const CameraState *cam = _findCamera(compId);
593 if (!cam) {
594 return;
595 }
596
597 mavlink_message_t msg{};
598 (void) mavlink_msg_camera_settings_pack_chan(
599 _mockLink->vehicleId(),
600 compId,
601 _mockLink->outgoingMavlinkChannel(),
602 &msg,
603 0, // time_boot_ms
604 cam->cameraMode, // mode_id
605 cam->zoomLevel, // zoomLevel
606 cam->focusLevel, // focusLevel
607 0); // camera_device_id
608 _mockLink->respondWithMavlinkMessage(msg);
609
610 qCDebug(MockLinkCameraLog) << "Sent CAMERA_SETTINGS for compId:" << compId
611 << "mode:" << cam->cameraMode
612 << "zoom:" << cam->zoomLevel
613 << "focus:" << cam->focusLevel;
614}
615
616void MockLinkCamera::_sendStorageInformation(uint8_t compId)
617{
618 const char storageName[MAVLINK_MSG_STORAGE_INFORMATION_FIELD_NAME_LEN] = {};
619
620 mavlink_message_t msg{};
621 (void) mavlink_msg_storage_information_pack_chan(
622 _mockLink->vehicleId(),
623 compId,
624 _mockLink->outgoingMavlinkChannel(),
625 &msg,
626 0, // time_boot_ms
627 1, // storage_id
628 1, // storage_count
629 STORAGE_STATUS_READY, // status
630 static_cast<float>(kStorageTotalMiB), // total_capacity (MiB)
631 static_cast<float>(kStorageTotalMiB - kStorageFreeMiB), // used_capacity (MiB)
632 static_cast<float>(kStorageFreeMiB), // available_capacity (MiB)
633 NAN, // read_speed
634 NAN, // write_speed
635 STORAGE_TYPE_SD, // type
636 storageName, // name
637 0); // storage_usage
638 _mockLink->respondWithMavlinkMessage(msg);
639
640 qCDebug(MockLinkCameraLog) << "Sent STORAGE_INFORMATION for compId:" << compId;
641}
642
643void MockLinkCamera::_sendCameraCaptureStatus(uint8_t compId)
644{
645 const CameraState *cam = _findCamera(compId);
646 if (!cam) {
647 return;
648 }
649
650 mavlink_message_t msg{};
651 (void) mavlink_msg_camera_capture_status_pack_chan(
652 _mockLink->vehicleId(),
653 compId,
654 _mockLink->outgoingMavlinkChannel(),
655 &msg,
656 0, // time_boot_ms
657 cam->image_status, // image_status (ImageCaptureStatus enum)
658 cam->recording ? 1 : 0, // video_status (0=idle, 1=running)
659 cam->image_interval, // image_interval
660 0, // recording_time_ms
661 static_cast<float>(kStorageFreeMiB), // available_capacity
662 cam->imagesCaptured, // image_count
663 0); // camera_device_id
664 _mockLink->respondWithMavlinkMessage(msg);
665
666 qCDebug(MockLinkCameraLog) << "Sent CAMERA_CAPTURE_STATUS for compId:" << compId
667 << "status:" << _imageCaptureStatusToString(cam->image_status)
668 << "interval:" << cam->image_interval
669 << "recording:" << cam->recording
670 << "images:" << cam->imagesCaptured;
671}
672
673void MockLinkCamera::_sendCameraImageCaptured(uint8_t compId)
674{
675 const CameraState *cam = _findCamera(compId);
676 if (!cam) {
677 return;
678 }
679
680 // Use vehicle's current position for image location
681 // In a real camera this would be the position when the image was taken
682 const int32_t lat = static_cast<int32_t>(_mockLink->vehicleLatitude() * 1e7);
683 const int32_t lon = static_cast<int32_t>(_mockLink->vehicleLongitude() * 1e7);
684 const float alt = static_cast<float>(_mockLink->vehicleAltitudeAMSL());
685 const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // quaternion (not used in this mock, set to identity)
686 const char fileUrl[MAVLINK_MSG_CAMERA_IMAGE_CAPTURED_FIELD_FILE_URL_LEN] = {};
687
688 mavlink_message_t msg{};
689 (void) mavlink_msg_camera_image_captured_pack_chan(
690 _mockLink->vehicleId(),
691 compId,
692 _mockLink->outgoingMavlinkChannel(),
693 &msg,
694 0, // time_boot_ms
695 0, // time_utc
696 (compId - MAV_COMP_ID_CAMERA) + 1, // camera_id (1-based, derived from compId)
697 lat, // lat (degrees * 1E7)
698 lon, // lon (degrees * 1E7)
699 alt, // alt (MSL)
700 alt, // relative_alt (same as MSL for simplicity)
701 q, // q (quaternion, unused)
702 cam->imagesCaptured, // image_index
703 1, // capture_result (1=success)
704 fileUrl); // file_url
705 _mockLink->respondWithMavlinkMessage(msg);
706
707 qCDebug(MockLinkCameraLog) << "Sent CAMERA_IMAGE_CAPTURED for compId:" << compId
708 << "index:" << cam->imagesCaptured;
709}
710
711void MockLinkCamera::_sendVideoStreamInformation(uint8_t compId, uint8_t streamId)
712{
713 const int cameraIndex = compId - MAV_COMP_ID_CAMERA;
714 const QString name = QStringLiteral("Stream %1-%2").arg(cameraIndex + 1).arg(streamId);
715 QByteArray nameBA = name.toLocal8Bit();
716 nameBA.resize(MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_NAME_LEN);
717
718 // Match the transport/codec/URI to whatever MockLink is actually serving so QGC's
719 // receiver auto-configures correctly. Falls back to a static UDP H.264 advertisement
720 // when no live stream is being served (e.g. GStreamer streaming disabled at build).
721 uint8_t streamType = VIDEO_STREAM_TYPE_RTPUDP;
722 uint8_t encoding = VIDEO_STREAM_ENCODING_H264;
723 QString uri = QStringLiteral("udp://127.0.0.1:5600");
724 uint16_t flags = VIDEO_STREAM_STATUS_FLAGS_RUNNING;
725
726 MockConfiguration::VideoStreamType servedType = MockConfiguration::VideoStreamNone;
727 QString servedUri;
728 _mockLink->servedVideoStream(servedType, servedUri);
729 switch (servedType) {
730 case MockConfiguration::VideoStreamRtpUdpH264:
731 streamType = VIDEO_STREAM_TYPE_RTPUDP;
732 encoding = VIDEO_STREAM_ENCODING_H264;
733 uri = servedUri;
734 break;
735 case MockConfiguration::VideoStreamRtpUdpH265:
736 streamType = VIDEO_STREAM_TYPE_RTPUDP;
737 encoding = VIDEO_STREAM_ENCODING_H265;
738 uri = servedUri;
739 break;
740 case MockConfiguration::VideoStreamRtspH264:
741 streamType = VIDEO_STREAM_TYPE_RTSP;
742 encoding = VIDEO_STREAM_ENCODING_H264;
743 uri = servedUri;
744 break;
745 case MockConfiguration::VideoStreamMpegTsUdp:
746 streamType = VIDEO_STREAM_TYPE_MPEG_TS;
747 encoding = VIDEO_STREAM_ENCODING_H264;
748 uri = servedUri;
749 break;
750 case MockConfiguration::VideoStreamMpegTsTcp:
751 streamType = VIDEO_STREAM_TYPE_TCP_MPEG;
752 encoding = VIDEO_STREAM_ENCODING_H264;
753 uri = servedUri;
754 break;
755 case MockConfiguration::VideoStreamNone:
756#ifdef QGC_GST_STREAMING
757 // A specific stream type was requested but no live server is running (start failed).
758 // Advertise it as not-running with an empty URI so QGC doesn't try to open a receiver
759 // on a dead stream (video auto-configuration keys off the URI, not the RUNNING flag).
760 // When nothing was requested (the default), keep the historical static UDP advertisement.
761 if (_mockLink->requestedVideoStreamType() != MockConfiguration::VideoStreamNone) {
762 flags = 0;
763 uri.clear();
764 }
765#endif
766 break;
767 }
768
769 QByteArray uriBA = uri.toLocal8Bit();
770 uriBA.resize(MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_URI_LEN);
771
772 mavlink_message_t msg{};
773 (void) mavlink_msg_video_stream_information_pack_chan(
774 _mockLink->vehicleId(),
775 compId,
776 _mockLink->outgoingMavlinkChannel(),
777 &msg,
778 streamId, // stream_id
779 kNumStreams, // count
780 streamType, // type
781 flags, // flags
782 30, // framerate
783 1280, // resolution_h (matches MockVideoStreamServer test source)
784 720, // resolution_v
785 2000, // bitrate (kbit/s, matches encoder settings)
786 0, // rotation
787 70, // hfov
788 nameBA.constData(),
789 uriBA.constData(),
790 encoding,
791 0); // encoding_sub
792 _mockLink->respondWithMavlinkMessage(msg);
793
794 qCDebug(MockLinkCameraLog) << "Sent VIDEO_STREAM_INFORMATION for compId:" << compId << "stream:" << streamId
795 << "type:" << streamType << "uri:" << uri;
796}
797
798void MockLinkCamera::_sendVideoStreamStatus(uint8_t compId, uint8_t streamId)
799{
800 // Mirror the served/requested logic in _sendVideoStreamInformation: when a specific
801 // stream type was requested but no live server is running, report not-running so
802 // STATUS doesn't overwrite the non-running state advertised in INFORMATION.
803 uint16_t flags = VIDEO_STREAM_STATUS_FLAGS_RUNNING;
804#ifdef QGC_GST_STREAMING
805 MockConfiguration::VideoStreamType servedType = MockConfiguration::VideoStreamNone;
806 QString servedUri;
807 _mockLink->servedVideoStream(servedType, servedUri);
808 if (servedType == MockConfiguration::VideoStreamNone
809 && _mockLink->requestedVideoStreamType() != MockConfiguration::VideoStreamNone) {
810 flags = 0;
811 }
812#endif
813
814 mavlink_message_t msg{};
815 (void) mavlink_msg_video_stream_status_pack_chan(
816 _mockLink->vehicleId(),
817 compId,
818 _mockLink->outgoingMavlinkChannel(),
819 &msg,
820 streamId, // stream_id
821 flags, // flags
822 30, // framerate
823 1280, // resolution_h (matches MockVideoStreamServer test source)
824 720, // resolution_v
825 2000, // bitrate (kbit/s, matches encoder settings)
826 0, // rotation
827 70, // hfov
828 0); // encoding (reserved in status)
829 _mockLink->respondWithMavlinkMessage(msg);
830
831 qCDebug(MockLinkCameraLog) << "Sent VIDEO_STREAM_STATUS for compId:" << compId << "stream:" << streamId;
832}
833
834void MockLinkCamera::_sendCameraTrackingImageStatus(uint8_t compId)
835{
836 const CameraState *cam = _findCamera(compId);
837 if (!cam || cam->trackingMode == CAMERA_TRACKING_MODE_NONE) {
838 return;
839 }
840
841 mavlink_message_t msg{};
842 (void) mavlink_msg_camera_tracking_image_status_pack_chan(
843 _mockLink->vehicleId(),
844 compId,
845 _mockLink->outgoingMavlinkChannel(),
846 &msg,
847 CAMERA_TRACKING_STATUS_FLAGS_ACTIVE, // tracking_status
848 cam->trackingMode, // tracking_mode
849 CAMERA_TRACKING_TARGET_DATA_EMBEDDED, // target_data
850 cam->trackPointX, // point_x
851 cam->trackPointY, // point_y
852 cam->trackRadius, // radius
853 cam->trackRecTopX, // rec_top_x
854 cam->trackRecTopY, // rec_top_y
855 cam->trackRecBottomX, // rec_bottom_x
856 cam->trackRecBottomY, // rec_bottom_y
857 0); // camera_device_id
858 _mockLink->respondWithMavlinkMessage(msg);
859}
860
861void MockLinkCamera::_sendCommandAck(uint8_t compId, uint16_t command, uint8_t result, int requestedMsgId)
862{
863 mavlink_message_t msg{};
864 (void) mavlink_msg_command_ack_pack_chan(
865 _mockLink->vehicleId(),
866 compId,
867 _mockLink->outgoingMavlinkChannel(),
868 &msg,
869 command,
870 result,
871 0, // progress
872 0, // result_param2
873 0, // target_system
874 0); // target_component
875 _mockLink->respondWithMavlinkMessage(msg);
876
877 QString commandName = MissionCommandTree::instance()->rawName(static_cast<MAV_CMD>(command));
878 QString logMsg = QStringLiteral("Sent COMMAND_ACK for compId: %1 command: %2 result: %3")
879 .arg(compId).arg(commandName).arg(result);
880
881 if (command == MAV_CMD_REQUEST_MESSAGE && requestedMsgId >= 0) {
882 const mavlink_message_info_t* info = mavlink_get_message_info_by_id(static_cast<uint32_t>(requestedMsgId));
883 QString msgName = info ? info->name : QString::number(requestedMsgId);
884 logMsg += QStringLiteral(" requestedMsg: %1").arg(msgName);
885 }
886
887 qCDebug(MockLinkCameraLog) << logMsg;
888}
mavlink_message_t
struct __mavlink_message mavlink_message_t
Definition QGCCorePlugin.h:26
QGC_LOGGING_CATEGORY
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Definition QGCLoggingCategory.h:11
mavlink_command_long_t
struct __mavlink_command_long_t mavlink_command_long_t
Definition QGCMAVLinkTypes.h:25
MissionCommandTree::instance
static MissionCommandTree * instance()
Definition MissionCommandTree.cc:57
MissionCommandTree::rawName
QString rawName(MAV_CMD command) const
Returns the raw name for the specified command.
Definition MissionCommandTree.cc:150
MockConfiguration::VideoStreamRtpUdpH265
@ VideoStreamRtpUdpH265
RTP/UDP H.265 -> udp265://.
Definition MockConfiguration.h:66
MockConfiguration::VideoStreamNone
@ VideoStreamNone
No stream served.
Definition MockConfiguration.h:64
MockConfiguration::VideoStreamRtpUdpH264
@ VideoStreamRtpUdpH264
RTP/UDP H.264 -> udp://.
Definition MockConfiguration.h:65
MockConfiguration::VideoStreamRtspH264
@ VideoStreamRtspH264
RTSP H.264 -> rtsp://.
Definition MockConfiguration.h:67
MockConfiguration::VideoStreamMpegTsTcp
@ VideoStreamMpegTsTcp
MPEG-TS over TCP -> tcp://.
Definition MockConfiguration.h:69
MockConfiguration::VideoStreamMpegTsUdp
@ VideoStreamMpegTsUdp
MPEG-TS over UDP -> mpegts://.
Definition MockConfiguration.h:68
MockLinkCamera
Simulates MAVLink Camera Protocol v2 components for MockLink.
Definition MockLinkCamera.h:40
MockLinkCamera::sendCameraHeartbeats
void sendCameraHeartbeats()
Send heartbeats for all simulated camera components (call from 1Hz tasks)
Definition MockLinkCamera.cc:74
MockLinkCamera::handleMavlinkMessage
bool handleMavlinkMessage(const mavlink_message_t &msg)
Definition MockLinkCamera.cc:162
MockLinkCamera::run10HzTasks
void run10HzTasks()
Update camera states (call from 10Hz tasks)
Definition MockLinkCamera.cc:92
MockLinkCamera::ImageCaptureInProgress
@ ImageCaptureInProgress
Single image capture in progress.
Definition MockLinkCamera.h:45
MockLinkCamera::ImageCaptureInterval
@ ImageCaptureInterval
Interval capture enabled.
Definition MockLinkCamera.h:46
MockLinkCamera::ImageCaptureIntervalCapture
@ ImageCaptureIntervalCapture
Interval capture with capture in progress.
Definition MockLinkCamera.h:47
MockLinkCamera::ImageCaptureIdle
@ ImageCaptureIdle
No capture in progress.
Definition MockLinkCamera.h:44
Quectel::Plan::command
GPSCommandSequence::Command command(QByteArrayView body, GPSTextMatcher reply, std::chrono::milliseconds timeout)
body labelled by its text, framed as a sentence, and completed by reply within timeout.
Definition QuectelPlan.cc:50
MockLinkCamera::CameraState
Per-camera simulated state.
Definition MockLinkCamera.h:51
MockLinkCamera::CameraState::intervalRemaining
int intervalRemaining
Images left in interval capture (0 = unlimited)
Definition MockLinkCamera.h:62
MockLinkCamera::CameraState::trackingStatusLastSentMs
qint64 trackingStatusLastSentMs
Timestamp of last tracking status message.
Definition MockLinkCamera.h:75
MockLinkCamera::CameraState::trackAnchorX
float trackAnchorX
Original center X of tracked target.
Definition MockLinkCamera.h:77
MockLinkCamera::CameraState::intervalLastCaptureMs
qint64 intervalLastCaptureMs
Timestamp of last interval capture.
Definition MockLinkCamera.h:63
MockLinkCamera::CameraState::image_interval
float image_interval
Interval between image captures (seconds)
Definition MockLinkCamera.h:60
MockLinkCamera::CameraState::singleShotStartMs
qint64 singleShotStartMs
Timestamp when single-shot capture started (0 = not active)
Definition MockLinkCamera.h:61
MockLinkCamera::CameraState::trackingStatusIntervalUs
qint64 trackingStatusIntervalUs
Interval for CAMERA_TRACKING_IMAGE_STATUS (-1 = disabled)
Definition MockLinkCamera.h:74
MockLinkCamera::CameraState::trackingStartMs
qint64 trackingStartMs
Timestamp when tracking was started (for drift animation)
Definition MockLinkCamera.h:76
MockLinkCamera::CameraState::trackingMode
uint8_t trackingMode
CAMERA_TRACKING_MODE enum.
Definition MockLinkCamera.h:66
MockLinkCamera::CameraState::trackAnchorY
float trackAnchorY
Original center Y of tracked target.
Definition MockLinkCamera.h:78
MockLinkCamera::CameraState::image_status
uint8_t image_status
ImageCaptureStatus enum.
Definition MockLinkCamera.h:59