From 1f8db8fb9203b3a700751ba5d017fc5bdc271fe0 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Fri, 2 Aug 2019 13:20:31 -0400 Subject: [PATCH 1/2] Put camera back to Idle if no response. --- src/Camera/QGCCameraControl.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 4ad8b74..22fb253 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -719,10 +719,11 @@ QGCCameraControl::_mavCommandResult(int vehicleId, int component, int command, i switch(command) { case MAV_CMD_IMAGE_START_CAPTURE: case MAV_CMD_IMAGE_STOP_CAPTURE: - if(++_captureInfoRetries < 5) { + if(++_captureInfoRetries < 3) { _captureStatusTimer.start(1000); } else { - qCDebug(CameraControlLog) << "Giving up requesting capture status"; + qCDebug(CameraControlLog) << "Giving up start/stop image capture"; + _setPhotoStatus(PHOTO_CAPTURE_IDLE); } break; case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: From 7e13b6d4939e8d963d6db097fdee6bdf05950b3f Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Fri, 2 Aug 2019 14:37:08 -0400 Subject: [PATCH 2/2] Better debugging message to identify camera issues. --- src/Camera/QGCCameraControl.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index 22fb253..f50a6f5 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -359,7 +359,16 @@ QGCCameraControl::takePhoto() { qCDebug(CameraControlLog) << "takePhoto()"; //-- Check if camera can capture photos or if it can capture it while in Video Mode - if(!capturesPhotos() || (cameraMode() == CAM_MODE_VIDEO && !photosInVideoMode()) || photoStatus() != PHOTO_CAPTURE_IDLE) { + if(!capturesPhotos()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture"; + return false; + } + if(cameraMode() == CAM_MODE_VIDEO && !photosInVideoMode()) { + qCWarning(CameraControlLog) << "Camera does not handle image capture while in video mode"; + return false; + } + if(photoStatus() != PHOTO_CAPTURE_IDLE) { + qCWarning(CameraControlLog) << "Camera not idle"; return false; } if(!_resetting) { @@ -779,6 +788,7 @@ void QGCCameraControl::_setPhotoStatus(PhotoStatus status) { if(_photo_status != status) { + qCDebug(CameraControlLog) << "Set Photo Status:" << status; _photo_status = status; emit photoStatusChanged(); }