From 4ec33ff642dc3cb94e094f4551461e49ff021066 Mon Sep 17 00:00:00 2001 From: Holden Date: Fri, 8 Aug 2025 08:07:53 -0400 Subject: [PATCH] Camera: Cleanup CameraManager --- .../src/FirmwarePlugin/CustomFirmwarePlugin.h | 1 - src/Camera/CameraMetaData.cc | 76 ++ src/Camera/CameraMetaData.h | 5 +- src/Camera/QGCCameraManager.cc | 860 +++++++++--------- src/Camera/QGCCameraManager.h | 183 ++-- src/QmlControls/QmlObjectListModel.h | 1 + src/Vehicle/Vehicle.cc | 59 +- src/Vehicle/Vehicle.h | 38 +- src/VideoManager/VideoManager.cc | 1 + test/Camera/QGCCameraManagerTest.cc | 3 +- test/Vehicle/RequestMessageTest.cc | 2 +- 11 files changed, 649 insertions(+), 580 deletions(-) diff --git a/custom-example/src/FirmwarePlugin/CustomFirmwarePlugin.h b/custom-example/src/FirmwarePlugin/CustomFirmwarePlugin.h index fbd9c0605449..83a412d8c299 100644 --- a/custom-example/src/FirmwarePlugin/CustomFirmwarePlugin.h +++ b/custom-example/src/FirmwarePlugin/CustomFirmwarePlugin.h @@ -12,7 +12,6 @@ #include "PX4FirmwarePlugin.h" class AutoPilotPlugin; -class CustomCameraManager; class Vehicle; class CustomFirmwarePlugin : public PX4FirmwarePlugin diff --git a/src/Camera/CameraMetaData.cc b/src/Camera/CameraMetaData.cc index 745187d3f925..40053000721e 100644 --- a/src/Camera/CameraMetaData.cc +++ b/src/Camera/CameraMetaData.cc @@ -8,6 +8,12 @@ ****************************************************************************/ #include "CameraMetaData.h" + +#include +#include +#include + +#include "JsonHelper.h" #include "QGCLoggingCategory.h" QGC_LOGGING_CATEGORY(CameraMetaDataLog, "qgc.camera.camerametadata") @@ -44,3 +50,73 @@ CameraMetaData::~CameraMetaData() { qCDebug(CameraMetaDataLog) << this; } + +QList CameraMetaData::parseCameraMetaData() +{ + QList cameraList; + + QString errorString; + int version = 0; + const QJsonObject jsonObject = JsonHelper::openInternalQGCJsonFile(QStringLiteral(":/json/CameraMetaData.json"), "CameraMetaData", 1, 1, version, errorString); + if (!errorString.isEmpty()) { + qCWarning(CameraMetaDataLog) << "Internal Error:" << errorString; + return cameraList; + } + + static const QList rootKeyInfoList = { + { "cameraMetaData", QJsonValue::Array, true } + }; + if (!JsonHelper::validateKeys(jsonObject, rootKeyInfoList, errorString)) { + qCWarning(CameraMetaDataLog) << errorString; + return cameraList; + } + + static const QList cameraKeyInfoList = { + { "canonicalName", QJsonValue::String, true }, + { "brand", QJsonValue::String, true }, + { "model", QJsonValue::String, true }, + { "sensorWidth", QJsonValue::Double, true }, + { "sensorHeight", QJsonValue::Double, true }, + { "imageWidth", QJsonValue::Double, true }, + { "imageHeight", QJsonValue::Double, true }, + { "focalLength", QJsonValue::Double, true }, + { "landscape", QJsonValue::Bool, true }, + { "fixedOrientation", QJsonValue::Bool, true }, + { "minTriggerInterval", QJsonValue::Double, true }, + { "deprecatedTranslatedName", QJsonValue::String, true }, + }; + const QJsonArray cameraInfo = jsonObject["cameraMetaData"].toArray(); + for (const QJsonValue &jsonValue : cameraInfo) { + if (!jsonValue.isObject()) { + qCWarning(CameraMetaDataLog) << "Entry in CameraMetaData array is not object"; + return cameraList; + } + + const QJsonObject obj = jsonValue.toObject(); + if (!JsonHelper::validateKeys(obj, cameraKeyInfoList, errorString)) { + qCWarning(CameraMetaDataLog) << errorString; + return cameraList; + } + + const QString canonicalName = obj["canonicalName"].toString(); + const QString brand = obj["brand"].toString(); + const QString model = obj["model"].toString(); + const double sensorWidth = obj["sensorWidth"].toDouble(); + const double sensorHeight = obj["sensorHeight"].toDouble(); + const double imageWidth = obj["imageWidth"].toDouble(); + const double imageHeight = obj["imageHeight"].toDouble(); + const double focalLength = obj["focalLength"].toDouble(); + const bool landscape = obj["landscape"].toBool(); + const bool fixedOrientation = obj["fixedOrientation"].toBool(); + const double minTriggerInterval = obj["minTriggerInterval"].toDouble(); + const QString deprecatedTranslatedName = obj["deprecatedTranslatedName"].toString(); + + CameraMetaData *metaData = new CameraMetaData( + canonicalName, brand, model, sensorWidth, sensorHeight, + imageWidth, imageHeight, focalLength, landscape, + fixedOrientation, minTriggerInterval, deprecatedTranslatedName); + cameraList.append(metaData); + } + + return cameraList; +} diff --git a/src/Camera/CameraMetaData.h b/src/Camera/CameraMetaData.h index f9729856c9a2..5fb99fcc9c3a 100644 --- a/src/Camera/CameraMetaData.h +++ b/src/Camera/CameraMetaData.h @@ -10,6 +10,7 @@ #pragma once #include +#include Q_DECLARE_LOGGING_CATEGORY(CameraMetaDataLog) @@ -45,6 +46,8 @@ class CameraMetaData const QString &deprecatedTranslatedName); ~CameraMetaData(); + static QList parseCameraMetaData(); + const QString canonicalName; ///< Canonical name saved in plan files. Not translated. const QString brand; ///< Camera brand. Used for grouping. const QString model; ///< Camerar model @@ -63,4 +66,4 @@ class CameraMetaData /// Newly added CameraMetaData entries should leave this value empty. const QString deprecatedTranslatedName; }; -Q_DECLARE_METATYPE(CameraMetaData) +Q_DECLARE_METATYPE(CameraMetaData*) diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 16a625681386..c4f16eaf7db1 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -8,78 +8,106 @@ ****************************************************************************/ #include "QGCCameraManager.h" +#include "CameraMetaData.h" +#include "FirmwarePlugin.h" +#include "Joystick.h" #include "JoystickManager.h" -#include "SimulatedCameraControl.h" +#include "MavlinkCameraControl.h" #include "MultiVehicleManager.h" -#include "Vehicle.h" -#include "FirmwarePlugin.h" #include "QGCLoggingCategory.h" -#include "Joystick.h" -#include "CameraMetaData.h" -#include "JsonHelper.h" #include "QGCVideoStreamInfo.h" - -#include -#include -#include -#include +#include "SimulatedCameraControl.h" QGC_LOGGING_CATEGORY(CameraManagerLog, "qgc.camera.qgccameramanager") +namespace { + constexpr int kHeartbeatTickMs = 500; + constexpr int kSilentTimeoutMs = 5000; + constexpr int kMaxRetryCount = 10; +} + QVariantList QGCCameraManager::_cameraList; -//----------------------------------------------------------------------------- -QGCCameraManager::CameraStruct::CameraStruct(QObject* parent, uint8_t compID_, Vehicle* vehicle_) - : QObject (parent) - , compID (compID_) - , vehicle (vehicle_) +/*===========================================================================*/ + +QGCCameraManager::CameraStruct::CameraStruct(QGCCameraManager *manager_, uint8_t compID_, Vehicle *vehicle_) + : manager(manager_) + , compID(compID_) + , vehicle(vehicle_) { - backoffTimer = new QTimer(this); - backoffTimer->setSingleShot(true); + qCDebug(CameraManagerLog) << this; + backoffTimer.setSingleShot(true); } -//----------------------------------------------------------------------------- -QGCCameraManager::QGCCameraManager(Vehicle *vehicle) - : _vehicle (vehicle) - , _simulatedCameraControl (new SimulatedCameraControl(vehicle, this)) +QGCCameraManager::CameraStruct::~CameraStruct() { - qCDebug(CameraManagerLog) << "QGCCameraManager Created"; + qCDebug(CameraManagerLog) << this; +} - (void) qRegisterMetaType("CameraMetaData"); +/*===========================================================================*/ - QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); +QGCCameraManager::QGCCameraManager(Vehicle *vehicle) + : QObject(vehicle) + , _vehicle(vehicle) + , _simulatedCameraControl(new SimulatedCameraControl(vehicle, this)) +{ + qCDebug(CameraManagerLog) << this; + + (void) qRegisterMetaType("CameraMetaData*"); _addCameraControlToLists(_simulatedCameraControl); - connect(MultiVehicleManager::instance(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady); - connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived); - connect(&_camerasLostHeartbeatTimer, &QTimer::timeout, this, &QGCCameraManager::_checkForLostCameras); + (void) connect(MultiVehicleManager::instance(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady); + (void) connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived); + (void) connect(&_camerasLostHeartbeatTimer, &QTimer::timeout, this, &QGCCameraManager::_checkForLostCameras); _camerasLostHeartbeatTimer.setSingleShot(false); _lastZoomChange.start(); _lastCameraChange.start(); - _camerasLostHeartbeatTimer.start(500); + _camerasLostHeartbeatTimer.start(kHeartbeatTickMs); } QGCCameraManager::~QGCCameraManager() { - // Stop all camera info request timers and clean up - for (auto* cameraInfo : _cameraInfoRequest) { - if (cameraInfo->backoffTimer) { - cameraInfo->backoffTimer->stop(); - QObject::disconnect(cameraInfo->backoffTimer, nullptr, nullptr, nullptr); - } - delete cameraInfo; - } + _camerasLostHeartbeatTimer.stop(); + + qDeleteAll(_cameraInfoRequest); _cameraInfoRequest.clear(); - // Stop the main heartbeat timer + qCDebug(CameraManagerLog) << this; +} + +void QGCCameraManager::start() +{ + if (_requestingEnabled) { + return; + } + _requestingEnabled = true; + + if (!_camerasLostHeartbeatTimer.isActive()) { + _camerasLostHeartbeatTimer.start(kHeartbeatTickMs); + } +} + +void QGCCameraManager::stop() +{ + if (!_requestingEnabled) { + return; + } + _requestingEnabled = false; + _camerasLostHeartbeatTimer.stop(); + + for (auto *info : std::as_const(_cameraInfoRequest)) { + if (info) { + info->backoffTimer.stop(); + } + } } void QGCCameraManager::setCurrentCamera(int sel) { - if(sel != _currentCameraIndex && sel >= 0 && sel < _cameras.count()) { + if ((sel != _currentCameraIndex) && (sel >= 0) && (sel < _cameras.count())) { _currentCameraIndex = sel; emit currentCameraChanged(); emit streamChanged(); @@ -88,414 +116,377 @@ void QGCCameraManager::setCurrentCamera(int sel) void QGCCameraManager::_vehicleReady(bool ready) { - qCDebug(CameraManagerLog) << "_vehicleReady(" << ready << ")"; - if(ready) { - if(MultiVehicleManager::instance()->activeVehicle() == _vehicle) { - _vehicleReadyState = true; - _activeJoystickChanged(JoystickManager::instance()->activeJoystick()); - connect(JoystickManager::instance(), &JoystickManager::activeJoystickChanged, this, &QGCCameraManager::_activeJoystickChanged); - } + qCDebug(CameraManagerLog) << ready; + if (!ready) { + return; } + if (MultiVehicleManager::instance()->activeVehicle() != _vehicle) { + return; + } + + _vehicleReadyState = true; + _activeJoystickChanged(JoystickManager::instance()->activeJoystick()); + (void) connect(JoystickManager::instance(), &JoystickManager::activeJoystickChanged, this, &QGCCameraManager::_activeJoystickChanged, Qt::UniqueConnection); } -void QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message) +void QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t &message) { - //-- Only pay attention to the camera components, as identified by their compId, - // as well as the autopilot, as it might have a non-MAVLink camera connected. - if(message.sysid == _vehicle->id() && (message.compid == MAV_COMP_ID_AUTOPILOT1 || - (message.compid >= MAV_COMP_ID_CAMERA && message.compid <= MAV_COMP_ID_CAMERA6))) { + // Only pay attention to camera components (MAV_COMP_ID_CAMERA..CAMERA6) + // and the autopilot (it might proxy a non-MAVLink camera). + const bool fromAutopilot = message.compid == MAV_COMP_ID_AUTOPILOT1; + const bool fromCamera = (message.compid >= MAV_COMP_ID_CAMERA) && (message.compid <= MAV_COMP_ID_CAMERA6); + if ((message.sysid == _vehicle->id()) && (fromAutopilot || fromCamera)) { switch (message.msgid) { - case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS: - _handleCaptureStatus(message); - break; - case MAVLINK_MSG_ID_STORAGE_INFORMATION: - _handleStorageInfo(message); - break; - case MAVLINK_MSG_ID_HEARTBEAT: - _handleHeartbeat(message); - break; - case MAVLINK_MSG_ID_CAMERA_INFORMATION: - _handleCameraInfo(message); - break; - case MAVLINK_MSG_ID_CAMERA_SETTINGS: - _handleCameraSettings(message); - break; - case MAVLINK_MSG_ID_PARAM_EXT_ACK: - _handleParamAck(message); - break; - case MAVLINK_MSG_ID_PARAM_EXT_VALUE: - _handleParamValue(message); - break; - case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION: - _handleVideoStreamInfo(message); - break; - case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS: - _handleVideoStreamStatus(message); - break; - case MAVLINK_MSG_ID_BATTERY_STATUS: - _handleBatteryStatus(message); - break; - case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS: - _handleTrackingImageStatus(message); - break; + case MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS: + _handleCaptureStatus(message); + break; + case MAVLINK_MSG_ID_STORAGE_INFORMATION: + _handleStorageInfo(message); + break; + case MAVLINK_MSG_ID_HEARTBEAT: + _handleHeartbeat(message); + break; + case MAVLINK_MSG_ID_CAMERA_INFORMATION: + _handleCameraInfo(message); + break; + case MAVLINK_MSG_ID_CAMERA_SETTINGS: + _handleCameraSettings(message); + break; + case MAVLINK_MSG_ID_PARAM_EXT_ACK: + _handleParamAck(message); + break; + case MAVLINK_MSG_ID_PARAM_EXT_VALUE: + _handleParamValue(message); + break; + case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION: + _handleVideoStreamInfo(message); + break; + case MAVLINK_MSG_ID_VIDEO_STREAM_STATUS: + _handleVideoStreamStatus(message); + break; + case MAVLINK_MSG_ID_BATTERY_STATUS: + _handleBatteryStatus(message); + break; + case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS: + _handleTrackingImageStatus(message); + break; + default: + break; } } } void QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message) { - QString sCompID = QString::number(message.compid); + const QString sCompID = QString::number(message.compid); if (!_cameraInfoRequest.contains(sCompID)) { - // This is the first time we are heading from this camera - qCDebug(CameraManagerLog) << "Hearbeat from " << message.compid; - CameraStruct* pInfo = new CameraStruct(this, message.compid, _vehicle); + qCDebug(CameraManagerLog) << "Heartbeat from" << message.compid; + CameraStruct *pInfo = new CameraStruct(this, message.compid, _vehicle); pInfo->lastHeartbeat.start(); _cameraInfoRequest[sCompID] = pInfo; - _requestCameraInfo(pInfo); - } else { - if (_cameraInfoRequest[sCompID]) { - CameraStruct* pInfo = _cameraInfoRequest[sCompID]; - //-- Check if we have indeed received the camera info - if (pInfo->infoReceived) { - //-- We have it. Just update the heartbeat timeout - pInfo->lastHeartbeat.start(); - } else { - //-- Camera info not received yet. Check if camera was silent and is now back - if (pInfo->lastHeartbeat.elapsed() > 5000) { - qCDebug(CameraManagerLog) << "Camera" << message.compid << "reappeared after being silent. Resetting retry count and requesting info."; - pInfo->retryCount = 0; // Reset retry count for fresh attempts - pInfo->backoffTimer->stop(); // Stop any pending backoff timer - pInfo->lastHeartbeat.start(); - _requestCameraInfo(pInfo); - } else { - //-- Just update heartbeat - pInfo->lastHeartbeat.start(); - } - } - } else { - qWarning() << Q_FUNC_INFO << "_cameraInfoRequest[" << sCompID << "] is null"; + if (_requestingEnabled) { + _requestCameraInfo(pInfo); } + return; } + + CameraStruct *pInfo = _cameraInfoRequest[sCompID]; + if (!pInfo) { + qCWarning(CameraManagerLog) << sCompID << "is null"; + return; + } + + if (pInfo->infoReceived) { + pInfo->lastHeartbeat.start(); + return; + } + + if (pInfo->lastHeartbeat.elapsed() > kSilentTimeoutMs) { + qCDebug(CameraManagerLog) << "Camera" << message.compid << "reappeared after being silent. Resetting retry count and requesting info."; + pInfo->retryCount = 0; + pInfo->backoffTimer.stop(); + pInfo->lastHeartbeat.start(); + if (_requestingEnabled) { + _requestCameraInfo(pInfo); + } + return; + } + + pInfo->lastHeartbeat.start(); } -MavlinkCameraControl* QGCCameraManager::currentCameraInstance() +MavlinkCameraControl *QGCCameraManager::currentCameraInstance() { - if(_currentCameraIndex < _cameras.count() && _cameras.count()) { - auto pCamera = qobject_cast(_cameras[_currentCameraIndex]); + if ((_currentCameraIndex < _cameras.count()) && !_cameras.isEmpty()) { + MavlinkCameraControl *pCamera = qobject_cast(_cameras[_currentCameraIndex]); return pCamera; } return nullptr; } -QGCVideoStreamInfo* QGCCameraManager::currentStreamInstance() +QGCVideoStreamInfo *QGCCameraManager::currentStreamInstance() { - auto pCamera = currentCameraInstance(); - if(pCamera) { - QGCVideoStreamInfo* pInfo = pCamera->currentStreamInstance(); + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { + QGCVideoStreamInfo *pInfo = pCamera->currentStreamInstance(); return pInfo; } return nullptr; } -QGCVideoStreamInfo* QGCCameraManager::thermalStreamInstance() +QGCVideoStreamInfo *QGCCameraManager::thermalStreamInstance() { - auto pCamera = currentCameraInstance(); - if(pCamera) { - QGCVideoStreamInfo* pInfo = pCamera->thermalStreamInstance(); + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { + QGCVideoStreamInfo *pInfo = pCamera->thermalStreamInstance(); return pInfo; } return nullptr; } -MavlinkCameraControl* QGCCameraManager::_findCamera(int id) +MavlinkCameraControl *QGCCameraManager::_findCamera(int id) { - for(int i = 0; i < _cameras.count(); i++) { - if(_cameras[i]) { - auto pCamera = qobject_cast(_cameras[i]); - if(pCamera) { - if(pCamera->compID() == id) { - return pCamera; - } - } else { - qCritical() << "Null MavlinkCameraControl instance"; - } + for (int i = 0; i < _cameras.count(); i++) { + if (!_cameras[i]) { + continue; + } + MavlinkCameraControl *pCamera = qobject_cast(_cameras[i]); + if (!pCamera) { + qCCritical(CameraManagerLog) << "Invalid MavlinkCameraControl instance"; + continue; + } + if (pCamera->compID() == id) { + return pCamera; } } - //qWarning() << "Camera component id not found:" << id; + + // qCWarning(CameraManagerLog) << "Camera component id not found:" << id; return nullptr; } -void QGCCameraManager::_addCameraControlToLists(MavlinkCameraControl* cameraControl) +void QGCCameraManager::_addCameraControlToLists(MavlinkCameraControl *cameraControl) { - QQmlEngine::setObjectOwnership(cameraControl, QQmlEngine::CppOwnership); _cameras.append(cameraControl); _cameraLabels.append(cameraControl->modelName()); emit camerasChanged(); emit cameraLabelsChanged(); - // If the simulated camera is already in the list, remove it since we have a real camera now - if (_cameras.count() == 2 && _cameras[0] == _simulatedCameraControl) { - _cameras.removeAt(0); - _cameraLabels.removeAt(0); + // If simulated camera is in list, remove it when a real camera appears + if ((_cameras.count() == 2) && (_cameras[0] == _simulatedCameraControl)) { + (void) _cameras.removeAt(0); + (void) _cameraLabels.removeAt(0); emit camerasChanged(); emit cameraLabelsChanged(); emit currentCameraChanged(); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) +void QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) { - qCDebug(CameraManagerLog) << "_handleCameraInfo"; - //-- Have we requested it? - QString sCompID = QString::number(message.compid); - if(_cameraInfoRequest.contains(sCompID) && !_cameraInfoRequest[sCompID]->infoReceived) { - //-- Flag it as done + const QString sCompID = QString::number(message.compid); + if (!_cameraInfoRequest.contains(sCompID) || _cameraInfoRequest[sCompID]->infoReceived) { + return; + } + + mavlink_camera_information_t info{}; + mavlink_msg_camera_information_decode(&message, &info); + qCDebug(CameraManagerLog) << reinterpret_cast(info.model_name) + << reinterpret_cast(info.vendor_name) + << "Comp ID:" << message.compid; + + MavlinkCameraControl *pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); + if (pCamera) { + _addCameraControlToLists(pCamera); + _cameraInfoRequest[sCompID]->infoReceived = true; - _cameraInfoRequest[sCompID]->retryCount = 0; // Reset retry counter on success - _cameraInfoRequest[sCompID]->backoffTimer->stop(); // Stop any pending backoff timer - qCDebug(CameraManagerLog) << "_handleCameraInfo: Success for compId" << message.compid << "- reset retry counter"; - mavlink_camera_information_t info; - mavlink_msg_camera_information_decode(&message, &info); - qCDebug(CameraManagerLog) << "_handleCameraInfo:" << reinterpret_cast(info.model_name) << reinterpret_cast(info.vendor_name) << "Comp ID:" << message.compid; - auto pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); - if(pCamera) { - _addCameraControlToLists(pCamera); - } + _cameraInfoRequest[sCompID]->retryCount = 0; + _cameraInfoRequest[sCompID]->backoffTimer.stop(); + qCDebug(CameraManagerLog) << "Success for compId" << message.compid << "- reset retry counter"; } } -/// Called to check for cameras which are no longer sending a heartbeat void QGCCameraManager::_checkForLostCameras() { - //-- Iterate cameras - for (auto it = _cameraInfoRequest.constBegin(); it != _cameraInfoRequest.constEnd(); ++it) { - const QString &sCompID = it.key(); - if (_cameraInfoRequest[sCompID]) { - CameraStruct* pInfo = _cameraInfoRequest[sCompID]; - //-- Have we received a camera info message? - if (pInfo->infoReceived) { - //-- Has the camera stopped talking to us? - if (pInfo->lastHeartbeat.elapsed() > 5000) { - auto pCamera = _findCamera(pInfo->compID); - - if (pCamera) { - // Before removing the current camera from the list add the simulated camera back into the list if thera are no other cameras. - // This way we smaoothly transition from a real camera to the simulated camera. - if (_cameras.count() == 1) { - qCDebug(CameraManagerLog) << "Adding simulated camera back to list."; - _addCameraControlToLists(_simulatedCameraControl); - } - - qWarning() << "Camera" << pCamera->modelName() << "stopped transmitting. Removing from list."; - _cameraLabels.removeOne(pCamera->modelName()); - _cameras.removeOne(pCamera); - emit cameraLabelsChanged(); - emit camerasChanged(); - - pCamera->deleteLater(); - delete pInfo; - - // There will always be at least one camera in the list, so we don't need to check if the list is empty. - // We specifically don't use setCurrentCamera since that checks for a index change. But in this case we may be using the same index. - _currentCameraIndex = 0; - emit currentCameraChanged(); - emit streamChanged(); - } - - _cameraInfoRequest.remove(sCompID); - - //-- Exit loop. - return; - } + QList stale; + for (auto it = _cameraInfoRequest.cbegin(), end = _cameraInfoRequest.cend(); it != end; ++it) { + const auto *info = it.value(); + if (info && info->infoReceived && (info->lastHeartbeat.elapsed() > kSilentTimeoutMs)) { + stale.push_back(it.key()); + } + } + if (stale.isEmpty()) { + return; + } + + bool removedAny = false; + for (const QString& key : std::as_const(stale)) { + CameraStruct* pInfo = _cameraInfoRequest.take(key); + if (!pInfo) { + continue; + } + + MavlinkCameraControl* pCamera = _findCamera(pInfo->compID); + if (pCamera) { + const int idx = _cameras.indexOf(pCamera); + if (idx >= 0) { + removedAny = true; + (void) _cameraLabels.removeAt(idx); + (void) _cameras.removeAt(idx); + pCamera->deleteLater(); } } + + delete pInfo; + } + + if (!removedAny) { + return; } + + if (_cameras.isEmpty()) { + _addCameraControlToLists(_simulatedCameraControl); + } + + emit cameraLabelsChanged(); + emit camerasChanged(); + + if (_currentCameraIndex != 0) { + _currentCameraIndex = 0; + emit currentCameraChanged(); + } + emit streamChanged(); } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleCaptureStatus(const mavlink_message_t &message) +void QGCCameraManager::_handleCaptureStatus(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_camera_capture_status_t cap; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_camera_capture_status_t cap{}; mavlink_msg_camera_capture_status_decode(&message, &cap); pCamera->handleCaptureStatus(cap); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleStorageInfo(const mavlink_message_t& message) +void QGCCameraManager::_handleStorageInfo(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_storage_information_t st; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_storage_information_t st{}; mavlink_msg_storage_information_decode(&message, &st); pCamera->handleStorageInfo(st); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleCameraSettings(const mavlink_message_t& message) +void QGCCameraManager::_handleCameraSettings(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_camera_settings_t settings; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_camera_settings_t settings{}; mavlink_msg_camera_settings_decode(&message, &settings); pCamera->handleSettings(settings); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleParamAck(const mavlink_message_t& message) +void QGCCameraManager::_handleParamAck(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_param_ext_ack_t ack; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_param_ext_ack_t ack{}; mavlink_msg_param_ext_ack_decode(&message, &ack); pCamera->handleParamAck(ack); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleParamValue(const mavlink_message_t& message) +void QGCCameraManager::_handleParamValue(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_param_ext_value_t value; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_param_ext_value_t value{}; mavlink_msg_param_ext_value_decode(&message, &value); pCamera->handleParamValue(value); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleVideoStreamInfo(const mavlink_message_t& message) +void QGCCameraManager::_handleVideoStreamInfo(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_video_stream_information_t streamInfo; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_video_stream_information_t streamInfo{}; mavlink_msg_video_stream_information_decode(&message, &streamInfo); pCamera->handleVideoInfo(&streamInfo); emit streamChanged(); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleVideoStreamStatus(const mavlink_message_t& message) +void QGCCameraManager::_handleVideoStreamStatus(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_video_stream_status_t streamStatus; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_video_stream_status_t streamStatus{}; mavlink_msg_video_stream_status_decode(&message, &streamStatus); pCamera->handleVideoStatus(&streamStatus); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleBatteryStatus(const mavlink_message_t& message) +void QGCCameraManager::_handleBatteryStatus(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_battery_status_t batteryStatus; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_battery_status_t batteryStatus{}; mavlink_msg_battery_status_decode(&message, &batteryStatus); pCamera->handleBatteryStatus(batteryStatus); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_handleTrackingImageStatus(const mavlink_message_t& message) +void QGCCameraManager::_handleTrackingImageStatus(const mavlink_message_t &message) { - auto pCamera = _findCamera(message.compid); - if(pCamera) { - mavlink_camera_tracking_image_status_t tis; + MavlinkCameraControl *pCamera = _findCamera(message.compid); + if (pCamera) { + mavlink_camera_tracking_image_status_t tis{}; mavlink_msg_camera_tracking_image_status_decode(&message, &tis); pCamera->handleTrackingImageStatus(&tis); } } -// Forward declarations for mutually recursive handler functions -static void _requestCameraInfoCommandResultHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode); -static void _requestCameraInfoMessageResultHandler(void* resultHandlerData, MAV_RESULT result, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message); -static void _requestCameraInfoHelper(QGCCameraManager* manager, QGCCameraManager::CameraStruct* pInfo); - +static void _handleCameraInfoRetry(QGCCameraManager::CameraStruct *cameraInfo); -static void _handleCameraInfoRetry(QGCCameraManager::CameraStruct* cameraInfo) +static void _requestCameraInfoCommandResultHandler(void *resultHandlerData, int /*compId*/, const mavlink_command_ack_t &ack, Vehicle::MavCmdResultFailureCode_t failureCode) { - cameraInfo->retryCount++; - auto manager = static_cast(cameraInfo->parent()); - - // For even attempts >= 2, use exponential backoff - if (cameraInfo->retryCount >= 2 && cameraInfo->retryCount % 2 == 0) { - // Calculate delay: 2^(retryCount/2) seconds - int delaySeconds = 1 << (cameraInfo->retryCount / 2); - int delayMs = delaySeconds * 1000; - - qCDebug(CameraManagerLog) << "Waiting" << delaySeconds << "seconds before retry for compId" << cameraInfo->compID; - - // Stop any existing timer and set up new one - cameraInfo->backoffTimer->stop(); - QObject::disconnect(cameraInfo->backoffTimer, nullptr, nullptr, nullptr); - QObject::connect(cameraInfo->backoffTimer, &QTimer::timeout, manager, [=]() { - _requestCameraInfoHelper(manager, cameraInfo); - }); - - cameraInfo->backoffTimer->start(delayMs); - } else { - // Make immediate retry - _requestCameraInfoHelper(manager, cameraInfo); - } -} - -static void _requestCameraInfoCommandResultHandler(void* resultHandlerData, int compId, const mavlink_command_ack_t& ack, Vehicle::MavCmdResultFailureCode_t failureCode) -{ - auto cameraInfo = static_cast(resultHandlerData); + auto *cameraInfo = static_cast(resultHandlerData); if (ack.result != MAV_RESULT_ACCEPTED) { qCDebug(CameraManagerLog) << "MAV_CMD_REQUEST_CAMERA_INFORMATION failed. compId" << cameraInfo->compID << "Result:" << ack.result << "FailureCode:" << failureCode << "retryCount:" << cameraInfo->retryCount; - _handleCameraInfoRetry(cameraInfo); } } -static void _requestCameraInfoMessageResultHandler(void* resultHandlerData, MAV_RESULT result, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, [[maybe_unused]] const mavlink_message_t& message) +static void _requestCameraInfoMessageResultHandler(void *resultHandlerData, MAV_RESULT result, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, [[maybe_unused]] const mavlink_message_t &message) { - auto cameraInfo = static_cast(resultHandlerData); + auto *cameraInfo = static_cast(resultHandlerData); if (result != MAV_RESULT_ACCEPTED) { qCDebug(CameraManagerLog) << "MAV_CMD_REQUEST_MESSAGE:MAVLINK_MSG_ID_CAMERA_INFORMATION failed. compId" << cameraInfo->compID << "Result:" << result << "FailureCode:" << failureCode << "retryCount:" << cameraInfo->retryCount; - _handleCameraInfoRetry(cameraInfo); } } -//----------------------------------------------------------------------------- -static void _requestCameraInfoHelper(QGCCameraManager* manager, QGCCameraManager::CameraStruct* pInfo) +static void _requestCameraInfoHelper(QGCCameraManager *manager, QGCCameraManager::CameraStruct *pInfo) { - // Give up after 10 attempts - if (pInfo->retryCount >= 10) { + // Give up after max attempts + if (pInfo->retryCount >= kMaxRetryCount) { qCWarning(CameraManagerLog) << "Giving up requesting camera info after" << pInfo->retryCount << "attempts for compId" << pInfo->compID; return; } - // Make immediate request - alternate between REQUEST_MESSAGE and REQUEST_CAMERA_INFORMATION - if (pInfo->retryCount % 2 == 0) { + // Alternate between REQUEST_MESSAGE and REQUEST_CAMERA_INFORMATION + if ((pInfo->retryCount % 2) == 0) { qCDebug(CameraManagerLog) << "Using MAV_CMD_REQUEST_MESSAGE for compId" << pInfo->compID; manager->vehicle()->requestMessage(_requestCameraInfoMessageResultHandler, pInfo, pInfo->compID, MAVLINK_MSG_ID_CAMERA_INFORMATION); } else { qCDebug(CameraManagerLog) << "Using MAV_CMD_REQUEST_CAMERA_INFORMATION for compId" << pInfo->compID; - Vehicle::MavCmdAckHandlerInfo_t ackHandlerInfo; + Vehicle::MavCmdAckHandlerInfo_t ackHandlerInfo{}; ackHandlerInfo.resultHandler = _requestCameraInfoCommandResultHandler; ackHandlerInfo.resultHandlerData = pInfo; ackHandlerInfo.progressHandler = nullptr; @@ -505,230 +496,195 @@ static void _requestCameraInfoHelper(QGCCameraManager* manager, QGCCameraManager } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_requestCameraInfo(CameraStruct* pInfo) +static void _handleCameraInfoRetry(QGCCameraManager::CameraStruct *cameraInfo) +{ + if (!cameraInfo) { + return; + } + + QGCCameraManager *manager = cameraInfo->manager; + if (!manager || !manager->requestingEnabled()) { + qCDebug(CameraManagerLog) << "manager is unavailable for compId" << cameraInfo->compID; + return; + } + + cameraInfo->retryCount++; + + // For even attempts >= 2, use exponential backoff + if ((cameraInfo->retryCount >= 2) && ((cameraInfo->retryCount % 2) == 0)) { + const int delaySeconds = 1 << (cameraInfo->retryCount / 2); + const int delayMs = delaySeconds * 1000; + + qCDebug(CameraManagerLog) << "Waiting" << delaySeconds << "seconds before retry for compId" << cameraInfo->compID; + + cameraInfo->backoffTimer.stop(); + (void) QObject::disconnect(&cameraInfo->backoffTimer, nullptr, nullptr, nullptr); + + // Capture compID by value and look up the struct + const uint8_t compId = cameraInfo->compID; + QPointer mgrGuard(manager); + + (void) QObject::connect(&cameraInfo->backoffTimer, &QTimer::timeout, + &cameraInfo->backoffTimer, // context ensures timer is still alive + [mgrGuard, compId]() { + if (!mgrGuard) { + return; + } + + auto* info = mgrGuard->findCameraStruct(compId); + if (info) { + _requestCameraInfoHelper(mgrGuard.data(), info); + } + }, Qt::UniqueConnection); + + cameraInfo->backoffTimer.start(delayMs); + } else { + _requestCameraInfoHelper(manager, cameraInfo); + } +} + +void QGCCameraManager::_requestCameraInfo(CameraStruct *pInfo) { + if (!_requestingEnabled || !pInfo) { + return; + } _requestCameraInfoHelper(this, pInfo); } -//---------------------------------------------------------------------------------------- -void -QGCCameraManager::_activeJoystickChanged(Joystick* joystick) +void QGCCameraManager::_activeJoystickChanged(Joystick *joystick) { qCDebug(CameraManagerLog) << "Joystick changed"; - if(_activeJoystick) { - disconnect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); - disconnect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom); - disconnect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom); - disconnect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); - disconnect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); - disconnect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera); - disconnect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording); - disconnect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording); - disconnect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording); + if (_activeJoystick) { + (void) disconnect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); + (void) disconnect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom); + (void) disconnect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom); + (void) disconnect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); + (void) disconnect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); + (void) disconnect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera); + (void) disconnect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording); + (void) disconnect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording); + (void) disconnect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording); } + _activeJoystick = joystick; - if(_activeJoystick) { - connect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); - connect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom); - connect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom); - connect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); - connect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); - connect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera); - connect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording); - connect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording); - connect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording); + + if (_activeJoystick) { + (void) connect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::startContinuousZoom, this, &QGCCameraManager::_startZoom, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::stopContinuousZoom, this, &QGCCameraManager::_stopZoom, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording, Qt::UniqueConnection); + (void) connect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording, Qt::UniqueConnection); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_triggerCamera() +void QGCCameraManager::_triggerCamera() { - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { pCamera->takePhoto(); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_startVideoRecording() +void QGCCameraManager::_startVideoRecording() { - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { pCamera->startVideoRecording(); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_stopVideoRecording() +void QGCCameraManager::_stopVideoRecording() { - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { pCamera->stopVideoRecording(); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_toggleVideoRecording() +void QGCCameraManager::_toggleVideoRecording() { - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { pCamera->toggleVideoRecording(); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_stepZoom(int direction) +void QGCCameraManager::_stepZoom(int direction) { - if(_lastZoomChange.elapsed() > 40) { + if (_lastZoomChange.elapsed() > 40) { _lastZoomChange.start(); qCDebug(CameraManagerLog) << "Step Camera Zoom" << direction; - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { pCamera->stepZoom(direction); } } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_startZoom(int direction) +void QGCCameraManager::_startZoom(int direction) { qCDebug(CameraManagerLog) << "Start Camera Zoom" << direction; - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { pCamera->startZoom(direction); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_stopZoom() +void QGCCameraManager::_stopZoom() { qCDebug(CameraManagerLog) << "Stop Camera Zoom"; - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { pCamera->stopZoom(); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_stepCamera(int direction) +void QGCCameraManager::_stepCamera(int direction) { - if(_lastCameraChange.elapsed() > 1000) { + if (_lastCameraChange.elapsed() > 1000) { _lastCameraChange.start(); qCDebug(CameraManagerLog) << "Step Camera" << direction; - int c = _currentCameraIndex + direction; - if(c < 0) c = _cameras.count() - 1; - if(c >= _cameras.count()) c = 0; - setCurrentCamera(c); + int camera = _currentCameraIndex + direction; + if (camera < 0) { + camera = _cameras.count() - 1; + } else if (camera >= _cameras.count()) { + camera = 0; + } + setCurrentCamera(camera); } } -//----------------------------------------------------------------------------- -void -QGCCameraManager::_stepStream(int direction) +void QGCCameraManager::_stepStream(int direction) { - if(_lastCameraChange.elapsed() > 1000) { + if (_lastCameraChange.elapsed() > 1000) { _lastCameraChange.start(); - auto pCamera = currentCameraInstance(); - if(pCamera) { + MavlinkCameraControl *pCamera = currentCameraInstance(); + if (pCamera) { qCDebug(CameraManagerLog) << "Step Camera Stream" << direction; - int c = pCamera->currentStream() + direction; - if(c < 0) c = pCamera->streams()->count() - 1; - if(c >= pCamera->streams()->count()) c = 0; - pCamera->setCurrentStream(c); + int stream = pCamera->currentStream() + direction; + if (stream < 0) { + stream = pCamera->streams()->count() - 1; + } else if (stream >= pCamera->streams()->count()) { + stream = 0; + } + pCamera->setCurrentStream(stream); } } } -const QVariantList &QGCCameraManager::cameraList() +const QVariantList &QGCCameraManager::cameraList() const { if (_cameraList.isEmpty()) { - const QList cams = _parseCameraMetaData(QStringLiteral(":/json/CameraMetaData.json")); + const QList cams = CameraMetaData::parseCameraMetaData(); _cameraList.reserve(cams.size()); - for (CameraMetaData* cam : cams) { + for (CameraMetaData *cam : cams) { _cameraList << QVariant::fromValue(cam); } } - return _cameraList; } - -QList QGCCameraManager::_parseCameraMetaData(const QString &jsonFilePath) -{ - QList cameraList; - - QString errorString; - int version; - const QJsonObject jsonObject = JsonHelper::openInternalQGCJsonFile(jsonFilePath, "CameraMetaData", 1, 1, version, errorString); - if (!errorString.isEmpty()) { - qCWarning(CameraManagerLog) << "Internal Error:" << errorString; - return cameraList; - } - - static const QList rootKeyInfoList = { - { "cameraMetaData", QJsonValue::Array, true } - }; - if (!JsonHelper::validateKeys(jsonObject, rootKeyInfoList, errorString)) { - qCWarning(CameraManagerLog) << errorString; - return cameraList; - } - - static const QList cameraKeyInfoList = { - { "canonicalName", QJsonValue::String, true }, - { "brand", QJsonValue::String, true }, - { "model", QJsonValue::String, true }, - { "sensorWidth", QJsonValue::Double, true }, - { "sensorHeight", QJsonValue::Double, true }, - { "imageWidth", QJsonValue::Double, true }, - { "imageHeight", QJsonValue::Double, true }, - { "focalLength", QJsonValue::Double, true }, - { "landscape", QJsonValue::Bool, true }, - { "fixedOrientation", QJsonValue::Bool, true }, - { "minTriggerInterval", QJsonValue::Double, true }, - { "deprecatedTranslatedName", QJsonValue::String, true }, - }; - const QJsonArray cameraInfo = jsonObject["cameraMetaData"].toArray(); - for (const QJsonValue &jsonValue : cameraInfo) { - if (!jsonValue.isObject()) { - qCWarning(CameraManagerLog) << "Entry in CameraMetaData array is not object"; - return cameraList; - } - - const QJsonObject obj = jsonValue.toObject(); - if (!JsonHelper::validateKeys(obj, cameraKeyInfoList, errorString)) { - qCWarning(CameraManagerLog) << errorString; - return cameraList; - } - - const QString canonicalName = obj["canonicalName"].toString(); - const QString brand = obj["brand"].toString(); - const QString model = obj["model"].toString(); - const double sensorWidth = obj["sensorWidth"].toDouble(); - const double sensorHeight = obj["sensorHeight"].toDouble(); - const double imageWidth = obj["imageWidth"].toDouble(); - const double imageHeight = obj["imageHeight"].toDouble(); - const double focalLength = obj["focalLength"].toDouble(); - const bool landscape = obj["landscape"].toBool(); - const bool fixedOrientation = obj["fixedOrientation"].toBool(); - const double minTriggerInterval = obj["minTriggerInterval"].toDouble(); - const QString deprecatedTranslatedName = obj["deprecatedTranslatedName"].toString(); - - CameraMetaData *const metaData = new CameraMetaData( - canonicalName, brand, model, sensorWidth, sensorHeight, - imageWidth, imageHeight, focalLength, landscape, - fixedOrientation, minTriggerInterval, deprecatedTranslatedName); - cameraList.append(metaData); - } - - return cameraList; -} diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index 74c658cbc567..15b5e8e48d8a 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -9,27 +9,28 @@ #pragma once -#include "QmlObjectListModel.h" -#include "MavlinkCameraControl.h" - #include #include #include #include +#include #include #include #include +#include "MAVLinkLib.h" +#include "QmlObjectListModel.h" +#include "Vehicle.h" + Q_DECLARE_LOGGING_CATEGORY(CameraManagerLog) -class Joystick; -class SimulatedCameraControl; -class QGCVideoStreamInfo; -class Vehicle; class CameraMetaData; +class Joystick; +class MavlinkCameraControl; class QGCCameraManagerTest; +class QGCVideoStreamInfo; +class SimulatedCameraControl; -//----------------------------------------------------------------------------- /// Camera Manager class QGCCameraManager : public QObject { @@ -37,92 +38,102 @@ class QGCCameraManager : public QObject QML_ELEMENT QML_UNCREATABLE("") Q_MOC_INCLUDE("Joystick.h") + Q_MOC_INCLUDE("MavlinkCameraControl.h") + + Q_PROPERTY(QmlObjectListModel* cameras READ cameras NOTIFY camerasChanged) + Q_PROPERTY(QStringList cameraLabels READ cameraLabels NOTIFY cameraLabelsChanged) + Q_PROPERTY(MavlinkCameraControl* currentCameraInstance READ currentCameraInstance NOTIFY currentCameraChanged) + Q_PROPERTY(int currentCamera READ currentCamera WRITE setCurrentCamera NOTIFY currentCameraChanged) + friend class QGCCameraManagerTest; + public: - QGCCameraManager(Vehicle* vehicle); - virtual ~QGCCameraManager(); - - Q_PROPERTY(QmlObjectListModel* cameras READ cameras NOTIFY camerasChanged) - Q_PROPERTY(QStringList cameraLabels READ cameraLabels NOTIFY cameraLabelsChanged) - Q_PROPERTY(MavlinkCameraControl* currentCameraInstance READ currentCameraInstance NOTIFY currentCameraChanged) - Q_PROPERTY(int currentCamera READ currentCamera WRITE setCurrentCamera NOTIFY currentCameraChanged) - - virtual QmlObjectListModel* cameras () { return &_cameras; } ///< List of cameras provided by current vehicle - virtual QStringList cameraLabels () { return _cameraLabels; } ///< Camera names to show the user (for selection) - virtual int currentCamera () { return _currentCameraIndex; } ///< Current selected camera - virtual MavlinkCameraControl* currentCameraInstance(); - virtual void setCurrentCamera (int sel); - virtual QGCVideoStreamInfo* currentStreamInstance(); - virtual QGCVideoStreamInfo* thermalStreamInstance(); - - /// Returns a list of CameraMetaData objects for available cameras on the vehicle. - virtual const QVariantList &cameraList(); - - // Helper method for static functions to access vehicle - Vehicle* vehicle() const { return _vehicle; } + explicit QGCCameraManager(Vehicle* vehicle); + ~QGCCameraManager(); - // This is public to avoid some circular include problems caused by statics - class CameraStruct : public QObject { - public: - CameraStruct(QObject* parent, uint8_t compID_, Vehicle* vehicle); + struct CameraStruct { + CameraStruct(QGCCameraManager* manager_, uint8_t compID_, Vehicle* vehicle_); + ~CameraStruct(); + + bool infoReceived = false; + uint8_t compID = 0; + int retryCount = 0; QElapsedTimer lastHeartbeat; - bool infoReceived = false; - uint8_t compID = 0; - Vehicle* vehicle = nullptr; - int retryCount = 0; - QTimer* backoffTimer = nullptr; + QTimer backoffTimer; + QPointer vehicle; + QPointer manager; + + private: + Q_DISABLE_COPY_MOVE(CameraStruct) }; + void start(); + void stop(); + bool requestingEnabled() const { return _requestingEnabled; } + QmlObjectListModel* cameras() { return &_cameras; } + const QmlObjectListModel* cameras() const { return &_cameras; } + QStringList cameraLabels() const { return _cameraLabels; } + int currentCamera() const { return _currentCameraIndex; } + MavlinkCameraControl* currentCameraInstance(); + void setCurrentCamera(int sel); + QGCVideoStreamInfo* currentStreamInstance(); + QGCVideoStreamInfo* thermalStreamInstance(); + + const QVariantList& cameraList() const; + + Vehicle* vehicle() const { return _vehicle; } + + CameraStruct* findCameraStruct(uint8_t compId) const { return _cameraInfoRequest.value(QString::number(compId), nullptr); } + signals: - void camerasChanged (); - void cameraLabelsChanged (); - void currentCameraChanged (); - void streamChanged (); + void camerasChanged(); + void cameraLabelsChanged(); + void currentCameraChanged(); + void streamChanged(); protected slots: - virtual void _vehicleReady (bool ready); - virtual void _mavlinkMessageReceived (const mavlink_message_t& message); - virtual void _activeJoystickChanged (Joystick* joystick); - virtual void _stepZoom (int direction); - virtual void _startZoom (int direction); - virtual void _stopZoom (); - virtual void _stepCamera (int direction); - virtual void _stepStream (int direction); - virtual void _checkForLostCameras (); - virtual void _triggerCamera (); - virtual void _startVideoRecording (); - virtual void _stopVideoRecording (); - virtual void _toggleVideoRecording (); - -protected: - virtual MavlinkCameraControl* _findCamera(int id); - virtual void _requestCameraInfo (CameraStruct* cameraInfo); - virtual void _handleHeartbeat (const mavlink_message_t& message); - virtual void _handleCameraInfo (const mavlink_message_t& message); - virtual void _handleStorageInfo (const mavlink_message_t& message); - virtual void _handleCameraSettings (const mavlink_message_t& message); - virtual void _handleParamAck (const mavlink_message_t& message); - virtual void _handleParamValue (const mavlink_message_t& message); - virtual void _handleCaptureStatus (const mavlink_message_t& message); - virtual void _handleVideoStreamInfo (const mavlink_message_t& message); - virtual void _handleVideoStreamStatus(const mavlink_message_t& message); - virtual void _handleBatteryStatus (const mavlink_message_t& message); - virtual void _handleTrackingImageStatus(const mavlink_message_t& message); - virtual void _addCameraControlToLists(MavlinkCameraControl* cameraControl); - - static QList _parseCameraMetaData(const QString &jsonFilePath); - - Vehicle* _vehicle = nullptr; - Joystick* _activeJoystick = nullptr; - bool _vehicleReadyState = false; - int _currentTask = 0; - QmlObjectListModel _cameras; - QStringList _cameraLabels; - int _currentCameraIndex = 0; - QElapsedTimer _lastZoomChange; - QElapsedTimer _lastCameraChange; - QTimer _camerasLostHeartbeatTimer; + void _vehicleReady(bool ready); + void _mavlinkMessageReceived(const mavlink_message_t& message); + void _activeJoystickChanged(Joystick* joystick); + void _stepZoom(int direction); + void _startZoom(int direction); + void _stopZoom(); + void _stepCamera(int direction); + void _stepStream(int direction); + void _checkForLostCameras(); + void _triggerCamera(); + void _startVideoRecording(); + void _stopVideoRecording(); + void _toggleVideoRecording(); + +private: + MavlinkCameraControl* _findCamera(int id); + void _requestCameraInfo(CameraStruct* cameraInfo); + void _handleHeartbeat(const mavlink_message_t& message); + void _handleCameraInfo(const mavlink_message_t& message); + void _handleStorageInfo(const mavlink_message_t& message); + void _handleCameraSettings(const mavlink_message_t& message); + void _handleParamAck(const mavlink_message_t& message); + void _handleParamValue(const mavlink_message_t& message); + void _handleCaptureStatus(const mavlink_message_t& message); + void _handleVideoStreamInfo(const mavlink_message_t& message); + void _handleVideoStreamStatus(const mavlink_message_t& message); + void _handleBatteryStatus(const mavlink_message_t& message); + void _handleTrackingImageStatus(const mavlink_message_t& message); + void _addCameraControlToLists(MavlinkCameraControl* cameraControl); + + QPointer _vehicle; + QPointer _simulatedCameraControl; + QPointer _activeJoystick; + bool _vehicleReadyState = false; + bool _requestingEnabled = false; + int _currentTask = 0; + QmlObjectListModel _cameras; + QStringList _cameraLabels; + int _currentCameraIndex = 0; + QElapsedTimer _lastZoomChange; + QElapsedTimer _lastCameraChange; + QTimer _camerasLostHeartbeatTimer; QMap _cameraInfoRequest; - SimulatedCameraControl* _simulatedCameraControl = nullptr; - static QVariantList _cameraList; ///< Standard QGC camera list + static QVariantList _cameraList; }; diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index 21b0018daf25..b0663340f915 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -37,6 +37,7 @@ class QmlObjectListModel : public QAbstractListModel // Property accessors int count () const; + bool isEmpty () const { return (count() == 0); } bool dirty () const { return _dirty; } void setDirty (bool dirty); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7a0b3e1448c1..80432fbd5da3 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -363,8 +363,7 @@ void Vehicle::_commonInit() _gimbalController = new GimbalController(this); - // Create camera manager instance - _cameraManager = _firmwarePlugin->createCameraManager(this); + _createCameraManager(); } Vehicle::~Vehicle() @@ -380,24 +379,7 @@ Vehicle::~Vehicle() void Vehicle::prepareDelete() { - // Clean up camera manager to stop all timers and prevent crashes during destruction - if(_cameraManager) { - // because of _cameraManager QML bindings check for nullptr won't work in the binding pipeline - // the dangling pointer access will cause a runtime fault - auto tmpCameras = _cameraManager; - _cameraManager = nullptr; - delete tmpCameras; - emit cameraManagerChanged(); - // Note: Removed qApp->processEvents() to prevent MAVLink crashes during destruction - } -} -void Vehicle::deleteCameraManager() -{ - if(_cameraManager) { - delete _cameraManager; - _cameraManager = nullptr; - } } void Vehicle::deleteGimbalController() @@ -3401,15 +3383,6 @@ const QVariantList& Vehicle::modeIndicators() return emptyList; } -const QVariantList& Vehicle::staticCameraList() const -{ - if (_cameraManager) { - return _cameraManager->cameraList(); - } - static QVariantList emptyList; - return emptyList; -} - void Vehicle::_setupAutoDisarmSignalling() { QString param = _firmwarePlugin->autoDisarmParameter(this); @@ -4404,3 +4377,33 @@ MAVLinkLogManager *Vehicle::mavlinkLogManager() const } /*---------------------------------------------------------------------------*/ +/*===========================================================================*/ +/* Camera Manager */ +/*===========================================================================*/ + +void Vehicle::_createCameraManager() +{ + if (!_cameraManager && _firmwarePlugin) { + _cameraManager = _firmwarePlugin->createCameraManager(this); + emit cameraManagerChanged(); + } +} + +void Vehicle::stopCameraManager() +{ + if (_cameraManager) { + _cameraManager->stop(); + } +} + +const QVariantList &Vehicle::staticCameraList() const +{ + if (_cameraManager) { + return _cameraManager->cameraList(); + } + + static QVariantList emptyCameraList; + return emptyCameraList; +} + +/*---------------------------------------------------------------------------*/ diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index d9abd6a81a9f..6b2037fa982f 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -98,7 +98,6 @@ class Vehicle : public VehicleFactGroup Q_MOC_INCLUDE("VehicleObjectAvoidance.h") Q_MOC_INCLUDE("Autotune.h") Q_MOC_INCLUDE("RemoteIDManager.h") - Q_MOC_INCLUDE("QGCCameraManager.h") Q_MOC_INCLUDE("Actuators.h") Q_MOC_INCLUDE("MAVLinkLogManager.h") Q_MOC_INCLUDE("LinkInterface.h") @@ -207,8 +206,6 @@ class Vehicle : public VehicleFactGroup Q_PROPERTY(QVariantList toolIndicators READ toolIndicators NOTIFY toolIndicatorsChanged) Q_PROPERTY(QVariantList modeIndicators READ modeIndicators NOTIFY modeIndicatorsChanged) Q_PROPERTY(bool initialPlanRequestComplete READ initialPlanRequestComplete NOTIFY initialPlanRequestCompleteChanged) - Q_PROPERTY(QVariantList staticCameraList READ staticCameraList CONSTANT) - Q_PROPERTY(QGCCameraManager* cameraManager READ cameraManager NOTIFY cameraManagerChanged) Q_PROPERTY(QString hobbsMeter READ hobbsMeter NOTIFY hobbsMeterChanged) Q_PROPERTY(bool inFwdFlight READ inFwdFlight NOTIFY inFwdFlightChanged) Q_PROPERTY(bool vtolInFwdFlight READ vtolInFwdFlight WRITE setVtolInFwdFlight NOTIFY vtolInFwdFlightChanged) @@ -775,12 +772,10 @@ class Vehicle : public VehicleFactGroup QVariant mainStatusIndicatorContentItem (); const QVariantList& toolIndicators (); const QVariantList& modeIndicators (); - const QVariantList& staticCameraList () const; bool capabilitiesKnown () const { return _capabilityBitsKnown; } uint64_t capabilityBits () const { return _capabilityBits; } // Change signalled by capabilityBitsChanged - QGCCameraManager* cameraManager () { return _cameraManager; } QString hobbsMeter (); /// The vehicle is responsible for making the initial request for the Plan. @@ -801,9 +796,6 @@ class Vehicle : public VehicleFactGroup /// Delete gimbal controller, handy for RequestMessageTest.cc, otherwise gimbal controller message requests will mess with this test void deleteGimbalController(); - /// Delete camera manager, just for testing - void deleteCameraManager(); - quint64 mavlinkSentCount () const{ return _mavlinkSentCount; } /// Calculated total number of messages sent to us quint64 mavlinkReceivedCount () const{ return _mavlinkReceivedCount; } /// Total number of sucessful messages received quint64 mavlinkLossCount () const{ return _mavlinkLossCount; } /// Total number of lost messages @@ -847,7 +839,6 @@ public slots: void defaultHoverSpeedChanged (double hoverSpeed); void firmwareTypeChanged (); void vehicleTypeChanged (); - void cameraManagerChanged (); void hobbsMeterChanged (); void capabilitiesKnownChanged (bool capabilitiesKnown); void initialPlanRequestCompleteChanged(bool initialPlanRequestComplete); @@ -1063,7 +1054,6 @@ private slots: SysStatusSensorInfo _sysStatusSensorInfo; - QGCCameraManager* _cameraManager = nullptr; QString _prearmError; QTimer _prearmErrorTimer; @@ -1321,6 +1311,7 @@ private slots: QMultiHash _unsupportedMessageIds; uint16_t _lastSetMsgIntervalMsgId = 0; +/*---------------------------------------------------------------------------*/ /*===========================================================================*/ /* ardupilotmega Dialect */ /*===========================================================================*/ @@ -1329,6 +1320,7 @@ private slots: /// Command vehicle to Enable/Disable Motor Interlock Q_INVOKABLE void motorInterlock(bool enable); + /*---------------------------------------------------------------------------*/ /*===========================================================================*/ /* CONTROL STATUS HANDLER */ @@ -1421,6 +1413,7 @@ private slots: void _createStatusTextHandler(); StatusTextHandler *m_statusTextHandler = nullptr; + /*---------------------------------------------------------------------------*/ /*===========================================================================*/ /* Image Protocol Manager */ @@ -1438,6 +1431,7 @@ private slots: void _createImageProtocolManager(); ImageProtocolManager *_imageProtocolManager = nullptr; + /*---------------------------------------------------------------------------*/ /*===========================================================================*/ /* MAVLink Log Manager */ @@ -1456,6 +1450,30 @@ private slots: MAVLinkLogManager *_mavlinkLogManager = nullptr; +/*---------------------------------------------------------------------------*/ +/*===========================================================================*/ +/* Camera Manager */ +/*===========================================================================*/ +private: + Q_MOC_INCLUDE("QGCCameraManager.h") + Q_PROPERTY(QGCCameraManager *cameraManager READ cameraManager NOTIFY cameraManagerChanged) + Q_PROPERTY(QVariantList staticCameraList READ staticCameraList CONSTANT) + +public: + QGCCameraManager *cameraManager() { return _cameraManager; } + const QVariantList &staticCameraList() const; + + /// Stop CameraManager requests, just for testing + void stopCameraManager(); + +signals: + void cameraManagerChanged(); + +private: + void _createCameraManager(); + + QGCCameraManager *_cameraManager = nullptr; + /*---------------------------------------------------------------------------*/ }; Q_DECLARE_METATYPE(Vehicle::MavCmdResultFailureCode_t) diff --git a/src/VideoManager/VideoManager.cc b/src/VideoManager/VideoManager.cc index d1a3aee3e6ea..97d51a99cabd 100644 --- a/src/VideoManager/VideoManager.cc +++ b/src/VideoManager/VideoManager.cc @@ -9,6 +9,7 @@ #include "VideoManager.h" #include "AppSettings.h" +#include "MavlinkCameraControl.h" #include "MultiVehicleManager.h" #include "QGCApplication.h" #include "QGCCameraManager.h" diff --git a/test/Camera/QGCCameraManagerTest.cc b/test/Camera/QGCCameraManagerTest.cc index c856e82f7b54..2c21e65c2b3c 100644 --- a/test/Camera/QGCCameraManagerTest.cc +++ b/test/Camera/QGCCameraManagerTest.cc @@ -9,12 +9,13 @@ #include "QGCCameraManagerTest.h" #include "QGCCameraManager.h" +#include "CameraMetaData.h" #include void QGCCameraManagerTest::_testCameraList() { - const QList cameraList = QGCCameraManager::_parseCameraMetaData(QStringLiteral(":/json/CameraMetaData.json")); + const QList cameraList = CameraMetaData::parseCameraMetaData(); QVERIFY(!cameraList.isEmpty()); diff --git a/test/Vehicle/RequestMessageTest.cc b/test/Vehicle/RequestMessageTest.cc index 483f9a197406..7f3b9e832bca 100644 --- a/test/Vehicle/RequestMessageTest.cc +++ b/test/Vehicle/RequestMessageTest.cc @@ -43,7 +43,7 @@ void RequestMessageTest::_testCaseWorker(TestCase_t& testCase) vehicle->deleteGimbalController(); // Camera manager also messes with it. - vehicle->deleteCameraManager(); + vehicle->stopCameraManager(); _mockLink->clearReceivedMavCommandCounts(); _mockLink->setRequestMessageFailureMode(testCase.failureMode);