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