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->mavlinkChannel(),
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 cam->zoomLevel = request.param2;
354 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "zoom set to" << cam->zoomLevel;
355 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
356 _sendCameraSettings(targetCompId);
357 } else {
358 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
359 }
360 return true;
361
362 case MAV_CMD_SET_CAMERA_FOCUS:
363 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
364 return true;
365
366 case MAV_CMD_RESET_CAMERA_SETTINGS:
367 cam->cameraMode = CAMERA_MODE_IMAGE;
368 cam->zoomLevel = 1.0f;
369 cam->focusLevel = 0.0f;
370 cam->image_status = ImageCaptureIdle;
371 cam->image_interval = 0.0f;
372 cam->singleShotStartMs = 0;
373 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "settings reset";
374 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
375 _sendCameraSettings(targetCompId);
376 return true;
377
378 case MAV_CMD_CAMERA_TRACK_POINT:
379 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_TRACKING_POINT) {
380 cam->trackingMode = CAMERA_TRACKING_MODE_POINT;
381 cam->trackPointX = request.param1;
382 cam->trackPointY = request.param2;
383 cam->trackRadius = request.param3;
384 cam->trackAnchorX = request.param1;
385 cam->trackAnchorY = request.param2;
386 cam->trackingStartMs = QDateTime::currentMSecsSinceEpoch();
387 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "tracking point"
388 << cam->trackPointX << cam->trackPointY << "radius" << cam->trackRadius;
389 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
390 } else {
391 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
392 }
393 return true;
394
395 case MAV_CMD_CAMERA_TRACK_RECTANGLE:
396 if (cam->capFlags & CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE) {
397 cam->trackingMode = CAMERA_TRACKING_MODE_RECTANGLE;
398 cam->trackRecTopX = request.param1;
399 cam->trackRecTopY = request.param2;
400 cam->trackRecBottomX = request.param3;
401 cam->trackRecBottomY = request.param4;
402 cam->trackAnchorX = (request.param1 + request.param3) / 2.0f;
403 cam->trackAnchorY = (request.param2 + request.param4) / 2.0f;
404 cam->trackingStartMs = QDateTime::currentMSecsSinceEpoch();
405 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "tracking rectangle"
406 << cam->trackRecTopX << cam->trackRecTopY
407 << "->" << cam->trackRecBottomX << cam->trackRecBottomY;
408 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
409 } else {
410 _sendCommandAck(targetCompId, request.command, MAV_RESULT_DENIED);
411 }
412 return true;
413
414 case MAV_CMD_CAMERA_STOP_TRACKING:
415 cam->trackingMode = CAMERA_TRACKING_MODE_NONE;
416 cam->trackingStatusIntervalUs = -1;
417 cam->trackingStatusLastSentMs = 0;
418 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId << "tracking stopped";
419 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
420 return true;
421
422 case MAV_CMD_SET_MESSAGE_INTERVAL:
423 {
424 const int msgId = static_cast<int>(request.param1);
425 if (msgId == MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS) {
426 cam->trackingStatusIntervalUs = static_cast<qint64>(request.param2);
427 cam->trackingStatusLastSentMs = 0;
428 qCDebug(MockLinkCameraLog) << "Camera" << targetCompId
429 << "tracking status interval" << cam->trackingStatusIntervalUs << "us";
430 _sendCommandAck(targetCompId, request.command, MAV_RESULT_ACCEPTED);
431 return true;
432 }
433 _sendCommandAck(targetCompId, request.command, MAV_RESULT_UNSUPPORTED);
434 return true;
435 }
436
437 default:
438 break;
439 }
440
441 return false;
442}
443
444bool MockLinkCamera::_handleRequestMessage(const mavlink_command_long_t &request, uint8_t targetCompId)
445{
446 const CameraState *cam = _findCamera(targetCompId);
447 if (!cam) {
448 return false;
449 }
450
451 const int msgId = static_cast<int>(request.param1);
452
453 switch (msgId) {
454 case MAVLINK_MSG_ID_CAMERA_INFORMATION:
455 _sendCameraInformation(targetCompId);
456 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
457 return true;
458
459 case MAVLINK_MSG_ID_CAMERA_SETTINGS:
460 _sendCameraSettings(targetCompId);
461 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
462 return true;
463
464 case MAVLINK_MSG_ID_STORAGE_INFORMATION:
465 _sendStorageInformation(targetCompId);
466 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
467 return true;
468
469 case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:
470 _sendCameraCaptureStatus(targetCompId);
471 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
472 return true;
473
474 case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:
475 {
476 if (!(cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM)) {
477 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_DENIED, msgId);
478 return true;
479 }
480 const uint8_t streamId = static_cast<uint8_t>(request.param2);
481 if (streamId == 0) {
482 for (uint8_t s = 1; s <= kNumStreams; s++) {
483 _sendVideoStreamInformation(targetCompId, s);
484 }
485 } else {
486 _sendVideoStreamInformation(targetCompId, streamId);
487 }
488 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
489 return true;
490 }
491
492 case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:
493 {
494 if (!(cam->capFlags & CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM)) {
495 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_DENIED, msgId);
496 return true;
497 }
498 const uint8_t streamId = static_cast<uint8_t>(request.param2);
499 if (streamId == 0) {
500 for (uint8_t s = 1; s <= kNumStreams; s++) {
501 _sendVideoStreamStatus(targetCompId, s);
502 }
503 } else {
504 _sendVideoStreamStatus(targetCompId, streamId);
505 }
506 _sendCommandAck(targetCompId, MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED, msgId);
507 return true;
508 }
509
510 default:
511 break;
512 }
513
514 return false;
515}
516
517void MockLinkCamera::_sendCameraInformation(uint8_t compId)
518{
519 const CameraState *cam = _findCamera(compId);
520 if (!cam) {
521 return;
522 }
523
524 const int cameraIndex = compId - MAV_COMP_ID_CAMERA;
525
526 const uint8_t vendorName[32] = "MockLink";
527 const char cameraDefinitionUri[MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN] = {};
528 const QString model = QStringLiteral("MockCam %1").arg(cameraIndex + 1);
529 QByteArray modelBA = model.toLocal8Bit();
530 modelBA.resize(MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN);
531
532 mavlink_message_t msg{};
533 (void) mavlink_msg_camera_information_pack_chan(
534 _mockLink->vehicleId(),
535 compId,
536 _mockLink->mavlinkChannel(),
537 &msg,
538 0, // time_boot_ms
539 vendorName,
540 reinterpret_cast<const uint8_t *>(modelBA.constData()),
541 0, // firmware_version
542 0, // focal_length
543 0, // sensor_size_h
544 0, // sensor_size_v
545 1920, // resolution_h
546 1080, // resolution_v
547 0, // lens_id
548 cam->capFlags, // flags
549 0, // cam_definition_version
550 cameraDefinitionUri, // cam_definition_uri
551 0, // gimbal_device_id
552 0); // flags (reserved)
553 _mockLink->respondWithMavlinkMessage(msg);
554
555 qCDebug(MockLinkCameraLog) << "Sent CAMERA_INFORMATION for compId:" << compId << "model:" << model;
556}
557
558void MockLinkCamera::_sendCameraSettings(uint8_t compId)
559{
560 const CameraState *cam = _findCamera(compId);
561 if (!cam) {
562 return;
563 }
564
565 mavlink_message_t msg{};
566 (void) mavlink_msg_camera_settings_pack_chan(
567 _mockLink->vehicleId(),
568 compId,
569 _mockLink->mavlinkChannel(),
570 &msg,
571 0, // time_boot_ms
572 cam->cameraMode, // mode_id
573 cam->zoomLevel, // zoomLevel
574 cam->focusLevel, // focusLevel
575 0); // camera_device_id
576 _mockLink->respondWithMavlinkMessage(msg);
577
578 qCDebug(MockLinkCameraLog) << "Sent CAMERA_SETTINGS for compId:" << compId
579 << "mode:" << cam->cameraMode
580 << "zoom:" << cam->zoomLevel
581 << "focus:" << cam->focusLevel;
582}
583
584void MockLinkCamera::_sendStorageInformation(uint8_t compId)
585{
586 const char storageName[MAVLINK_MSG_STORAGE_INFORMATION_FIELD_NAME_LEN] = {};
587
588 mavlink_message_t msg{};
589 (void) mavlink_msg_storage_information_pack_chan(
590 _mockLink->vehicleId(),
591 compId,
592 _mockLink->mavlinkChannel(),
593 &msg,
594 0, // time_boot_ms
595 1, // storage_id
596 1, // storage_count
597 STORAGE_STATUS_READY, // status
598 static_cast<float>(kStorageTotalMiB), // total_capacity (MiB)
599 static_cast<float>(kStorageTotalMiB - kStorageFreeMiB), // used_capacity (MiB)
600 static_cast<float>(kStorageFreeMiB), // available_capacity (MiB)
601 NAN, // read_speed
602 NAN, // write_speed
603 STORAGE_TYPE_SD, // type
604 storageName, // name
605 0); // storage_usage
606 _mockLink->respondWithMavlinkMessage(msg);
607
608 qCDebug(MockLinkCameraLog) << "Sent STORAGE_INFORMATION for compId:" << compId;
609}
610
611void MockLinkCamera::_sendCameraCaptureStatus(uint8_t compId)
612{
613 const CameraState *cam = _findCamera(compId);
614 if (!cam) {
615 return;
616 }
617
618 mavlink_message_t msg{};
619 (void) mavlink_msg_camera_capture_status_pack_chan(
620 _mockLink->vehicleId(),
621 compId,
622 _mockLink->mavlinkChannel(),
623 &msg,
624 0, // time_boot_ms
625 cam->image_status, // image_status (ImageCaptureStatus enum)
626 cam->recording ? 1 : 0, // video_status (0=idle, 1=running)
627 cam->image_interval, // image_interval
628 0, // recording_time_ms
629 static_cast<float>(kStorageFreeMiB), // available_capacity
630 cam->imagesCaptured, // image_count
631 0); // camera_device_id
632 _mockLink->respondWithMavlinkMessage(msg);
633
634 qCDebug(MockLinkCameraLog) << "Sent CAMERA_CAPTURE_STATUS for compId:" << compId
635 << "status:" << _imageCaptureStatusToString(cam->image_status)
636 << "interval:" << cam->image_interval
637 << "recording:" << cam->recording
638 << "images:" << cam->imagesCaptured;
639}
640
641void MockLinkCamera::_sendCameraImageCaptured(uint8_t compId)
642{
643 const CameraState *cam = _findCamera(compId);
644 if (!cam) {
645 return;
646 }
647
648 // Use vehicle's current position for image location
649 // In a real camera this would be the position when the image was taken
650 const int32_t lat = static_cast<int32_t>(_mockLink->vehicleLatitude() * 1e7);
651 const int32_t lon = static_cast<int32_t>(_mockLink->vehicleLongitude() * 1e7);
652 const float alt = static_cast<float>(_mockLink->vehicleAltitudeAMSL());
653 const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // quaternion (not used in this mock, set to identity)
654 const char fileUrl[MAVLINK_MSG_CAMERA_IMAGE_CAPTURED_FIELD_FILE_URL_LEN] = {};
655
656 mavlink_message_t msg{};
657 (void) mavlink_msg_camera_image_captured_pack_chan(
658 _mockLink->vehicleId(),
659 compId,
660 _mockLink->mavlinkChannel(),
661 &msg,
662 0, // time_boot_ms
663 0, // time_utc
664 (compId - MAV_COMP_ID_CAMERA) + 1, // camera_id (1-based, derived from compId)
665 lat, // lat (degrees * 1E7)
666 lon, // lon (degrees * 1E7)
667 alt, // alt (MSL)
668 alt, // relative_alt (same as MSL for simplicity)
669 q, // q (quaternion, unused)
670 cam->imagesCaptured, // image_index
671 1, // capture_result (1=success)
672 fileUrl); // file_url
673 _mockLink->respondWithMavlinkMessage(msg);
674
675 qCDebug(MockLinkCameraLog) << "Sent CAMERA_IMAGE_CAPTURED for compId:" << compId
676 << "index:" << cam->imagesCaptured;
677}
678
679void MockLinkCamera::_sendVideoStreamInformation(uint8_t compId, uint8_t streamId)
680{
681 const int cameraIndex = compId - MAV_COMP_ID_CAMERA;
682 const QString name = QStringLiteral("Stream %1-%2").arg(cameraIndex + 1).arg(streamId);
683 QByteArray nameBA = name.toLocal8Bit();
684 nameBA.resize(MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_NAME_LEN);
685
686 const QString uri = QStringLiteral("udp://127.0.0.1:5600");
687 QByteArray uriBA = uri.toLocal8Bit();
688 uriBA.resize(MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_URI_LEN);
689
690 mavlink_message_t msg{};
691 (void) mavlink_msg_video_stream_information_pack_chan(
692 _mockLink->vehicleId(),
693 compId,
694 _mockLink->mavlinkChannel(),
695 &msg,
696 streamId, // stream_id
697 kNumStreams, // count
698 VIDEO_STREAM_TYPE_RTPUDP, // type
699 VIDEO_STREAM_STATUS_FLAGS_RUNNING, // flags
700 30, // framerate
701 1920, // resolution_h
702 1080, // resolution_v
703 4000, // bitrate (kbit/s)
704 0, // rotation
705 70, // hfov
706 nameBA.constData(),
707 uriBA.constData(),
708 VIDEO_STREAM_ENCODING_H264,
709 0); // encoding_sub
710 _mockLink->respondWithMavlinkMessage(msg);
711
712 qCDebug(MockLinkCameraLog) << "Sent VIDEO_STREAM_INFORMATION for compId:" << compId << "stream:" << streamId;
713}
714
715void MockLinkCamera::_sendVideoStreamStatus(uint8_t compId, uint8_t streamId)
716{
717 mavlink_message_t msg{};
718 (void) mavlink_msg_video_stream_status_pack_chan(
719 _mockLink->vehicleId(),
720 compId,
721 _mockLink->mavlinkChannel(),
722 &msg,
723 streamId, // stream_id
724 VIDEO_STREAM_STATUS_FLAGS_RUNNING, // flags
725 30, // framerate
726 1920, // resolution_h
727 1080, // resolution_v
728 4000, // bitrate (kbit/s)
729 0, // rotation
730 70, // hfov
731 0); // encoding (reserved in status)
732 _mockLink->respondWithMavlinkMessage(msg);
733
734 qCDebug(MockLinkCameraLog) << "Sent VIDEO_STREAM_STATUS for compId:" << compId << "stream:" << streamId;
735}
736
737void MockLinkCamera::_sendCameraTrackingImageStatus(uint8_t compId)
738{
739 const CameraState *cam = _findCamera(compId);
740 if (!cam || cam->trackingMode == CAMERA_TRACKING_MODE_NONE) {
741 return;
742 }
743
744 mavlink_message_t msg{};
745 (void) mavlink_msg_camera_tracking_image_status_pack_chan(
746 _mockLink->vehicleId(),
747 compId,
748 _mockLink->mavlinkChannel(),
749 &msg,
750 CAMERA_TRACKING_STATUS_FLAGS_ACTIVE, // tracking_status
751 cam->trackingMode, // tracking_mode
752 CAMERA_TRACKING_TARGET_DATA_EMBEDDED, // target_data
753 cam->trackPointX, // point_x
754 cam->trackPointY, // point_y
755 cam->trackRadius, // radius
756 cam->trackRecTopX, // rec_top_x
757 cam->trackRecTopY, // rec_top_y
758 cam->trackRecBottomX, // rec_bottom_x
759 cam->trackRecBottomY, // rec_bottom_y
760 0); // camera_device_id
761 _mockLink->respondWithMavlinkMessage(msg);
762}
763
764void MockLinkCamera::_sendCommandAck(uint8_t compId, uint16_t command, uint8_t result, int requestedMsgId)
765{
766 mavlink_message_t msg{};
767 (void) mavlink_msg_command_ack_pack_chan(
768 _mockLink->vehicleId(),
769 compId,
770 _mockLink->mavlinkChannel(),
771 &msg,
772 command,
773 result,
774 0, // progress
775 0, // result_param2
776 0, // target_system
777 0); // target_component
778 _mockLink->respondWithMavlinkMessage(msg);
779
780 QString commandName = MissionCommandTree::instance()->rawName(static_cast<MAV_CMD>(command));
781 QString logMsg = QStringLiteral("Sent COMMAND_ACK for compId: %1 command: %2 result: %3")
782 .arg(compId).arg(commandName).arg(result);
783
784 if (command == MAV_CMD_REQUEST_MESSAGE && requestedMsgId >= 0) {
785 const mavlink_message_info_t* info = mavlink_get_message_info_by_id(static_cast<uint32_t>(requestedMsgId));
786 QString msgName = info ? info->name : QString::number(requestedMsgId);
787 logMsg += QStringLiteral(" requestedMsg: %1").arg(msgName);
788 }
789
790 qCDebug(MockLinkCameraLog) << logMsg;
791}
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
LinkInterface::mavlinkChannel
uint8_t mavlinkChannel() const
Definition LinkInterface.cc:29
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
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