diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index dcafbbb09d6f4ea62f769af53028191b8ff9b76e..613dd26f0642c2ca3a34a79edee0a1229c3d401b 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -119,6 +119,8 @@ QGCCameraControl::QGCCameraControl(const mavlink_camera_information_t *info, Veh , _netManager(NULL) , _cameraMode(CAM_MODE_UNDEFINED) , _video_status(VIDEO_CAPTURE_STATUS_UNDEFINED) + , _storageInfoRetries(0) + , _captureInfoRetries(0) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); memcpy(&_info, info, sizeof(mavlink_camera_information_t)); @@ -414,12 +416,40 @@ QGCCameraControl::_mavCommandResult(int vehicleId, int component, int command, i case MAV_CMD_VIDEO_STOP_CAPTURE: _setVideoStatus(VIDEO_CAPTURE_STATUS_STOPPED); break; + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + _captureInfoRetries = 0; + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + _storageInfoRetries = 0; + break; } } else { - if(noReponseFromVehicle) { - qCDebug(CameraControlLog) << "No response for" << command; + if(noReponseFromVehicle || result == MAV_RESULT_TEMPORARILY_REJECTED || result == MAV_RESULT_FAILED) { + if(noReponseFromVehicle) { + qCDebug(CameraControlLog) << "No response for" << command; + } else if (result == MAV_RESULT_TEMPORARILY_REJECTED) { + qCDebug(CameraControlLog) << "Command temporarily rejected for" << command; + } else { + qCDebug(CameraControlLog) << "Command failed for" << command; + } + switch(command) { + case MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS: + if(++_captureInfoRetries < 3) { + _requestCaptureStatus(); + } else { + qCDebug(CameraControlLog) << "Giving up requesting capture status"; + } + break; + case MAV_CMD_REQUEST_STORAGE_INFORMATION: + if(++_storageInfoRetries < 3) { + _requestStorageInfo(); + } else { + qCDebug(CameraControlLog) << "Giving up requesting storage status"; + } + break; + } } else { - qCDebug(CameraControlLog) << "Bad response for" << command; + qCDebug(CameraControlLog) << "Bad response for" << command << result; } } } @@ -951,7 +981,7 @@ QGCCameraControl::_updateRanges(Fact* pFact) } //-- Parameter update requests if(_requestUpdates.contains(pFact->name())) { - _requestAllParameters(); + QTimer::singleShot(250, this, &QGCCameraControl::_requestAllParameters); } //-- Update UI (Asynchronous state where values come back after a while) if(_updates.size()) { @@ -1260,3 +1290,12 @@ QGCCameraControl::_paramDone() //-- All parameters loaded (or timed out) emit parametersReady(); } + +//----------------------------------------------------------------------------- +bool +QGCCameraControl::incomingParameter(Fact* pFact, QVariant& newValue) +{ + Q_UNUSED(pFact); + Q_UNUSED(newValue); + return true; +} diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h index b89260d1167d2e9ed0e8d37e1e7f30c36c4563ae..e91318757729f9b3a785c66a71e7e026d1b74409 100644 --- a/src/Camera/QGCCameraControl.h +++ b/src/Camera/QGCCameraControl.h @@ -142,7 +142,11 @@ public: virtual void handleParamAck (const mavlink_param_ext_ack_t& ack); virtual void handleParamValue (const mavlink_param_ext_value_t& value); virtual void handleStorageInfo (const mavlink_storage_information_t& st); + + //-- Notify controller a parameter has changed virtual void factChanged (Fact* pFact); + //-- Allow controller to modify or invalidate incoming parameter + virtual bool incomingParameter (Fact* pFact, QVariant& newValue); signals: void infoChanged (); @@ -213,6 +217,8 @@ protected: QMap _originalOptValues; QMap _paramIO; QVector _updates; + int _storageInfoRetries; + int _captureInfoRetries; //-- Parameters that require a full update QStringList _requestUpdates; }; diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc index 25c5730e55b5e7a7dd59033c96cc1568785a1feb..4fbc1f2b9da7b0366dd9188393195417570fc5a3 100644 --- a/src/Camera/QGCCameraIO.cc +++ b/src/Camera/QGCCameraIO.cc @@ -211,7 +211,10 @@ void QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value) { _paramRequestTimer.stop(); - _fact->_containerSetRawValue(_valueFromMessage(value.param_value, value.param_type)); + QVariant newValue = _valueFromMessage(value.param_value, value.param_type); + if(_control->incomingParameter(_fact, newValue)) { + _fact->_containerSetRawValue(newValue); + } _paramRequestReceived = true; if(!_done) { _done = true;