diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 9473bd340c8e5ae84bbae3310f5fd6006e6d723c..ee8953724a8d54b361eb1d9c38eaee611a4c1a5b 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -78,7 +78,7 @@ QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message) mavlink_msg_heartbeat_decode(&message, &heartbeat); //-- If this heartbeat is from a different node within the vehicle if(_vehicleReadyState && _vehicle->id() == message.sysid && _vehicle->defaultComponentId() != message.compid) { - if(!_cameraInfoRequested[message.compid]) { + if(!_cameraInfoRequested.contains(message.compid)) { _cameraInfoRequested[message.compid] = true; //-- Request camera info _requestCameraInfo(message.compid); @@ -110,14 +110,19 @@ QGCCameraManager::_findCamera(int id) void QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) { - mavlink_camera_information_t info; - mavlink_msg_camera_information_decode(&message, &info); - qCDebug(CameraManagerLog) << "_handleCameraInfo:" << (const char*)(void*)&info.model_name[0] << (const char*)(void*)&info.vendor_name[0] << "Comp ID:" << message.compid; - QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); - if(pCamera) { - QQmlEngine::setObjectOwnership(pCamera, QQmlEngine::CppOwnership); - _cameras.append(pCamera); - emit camerasChanged(); + //-- Have we requested it? + if(_cameraInfoRequested.contains(message.compid) && _cameraInfoRequested[message.compid]) { + //-- Flag it as done + _cameraInfoRequested[message.compid] = false; + mavlink_camera_information_t info; + mavlink_msg_camera_information_decode(&message, &info); + qCDebug(CameraManagerLog) << "_handleCameraInfo:" << (const char*)(void*)&info.model_name[0] << (const char*)(void*)&info.vendor_name[0] << "Comp ID:" << message.compid; + QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this); + if(pCamera) { + QQmlEngine::setObjectOwnership(pCamera, QQmlEngine::CppOwnership); + _cameras.append(pCamera); + emit camerasChanged(); + } } }