From 94504df90684042332d4978e3092d1a6a75c9e98 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Tue, 1 Aug 2017 14:27:51 -0400 Subject: [PATCH] Handle parameter out of bounds when setting options change. Increase param ack timeout. Retry when param set failed. --- src/Camera/QGCCameraControl.cc | 92 ++++++++++++++++++++++++++++------ src/Camera/QGCCameraControl.h | 3 ++ src/Camera/QGCCameraIO.cc | 72 ++++++++++++++++++-------- src/Camera/QGCCameraIO.h | 6 +++ src/FactSystem/Fact.cc | 8 +-- src/FactSystem/FactMetaData.h | 2 +- 6 files changed, 145 insertions(+), 38 deletions(-) diff --git a/src/Camera/QGCCameraControl.cc b/src/Camera/QGCCameraControl.cc index f4bd13d04..8dc2925c4 100644 --- a/src/Camera/QGCCameraControl.cc +++ b/src/Camera/QGCCameraControl.cc @@ -160,6 +160,8 @@ QGCCameraControl::_initWhenReady() } connect(_vehicle, &Vehicle::mavCommandResult, this, &QGCCameraControl::_mavCommandResult); connect(&_captureStatusTimer, &QTimer::timeout, this, &QGCCameraControl::_requestCaptureStatus); + connect(&_updateTimer, &QTimer::timeout, this, &QGCCameraControl::_updateTimeout); + _updateTimer.setSingleShot(true); _captureStatusTimer.setSingleShot(true); QTimer::singleShot(2500, this, &QGCCameraControl::_requestStorageInfo); _captureStatusTimer.start(2750); @@ -871,40 +873,102 @@ QGCCameraControl::_processCondition(const QString condition) void QGCCameraControl::_updateRanges(Fact* pFact) { + QMap valuesSet; + QMap rangesSet; + QMap rangesReset; + QStringList valuesRequested; QStringList changedList; QStringList resetList; - qCDebug(CameraControlLogVerbose) << "_updateRanges(" << pFact->name() << ")"; - //-- Iterate range sets + //-- Iterate range sets looking for limited ranges foreach(QGCCameraOptionRange* pRange, _optionRanges) { //-- If this fact or one of its conditions is part of this range set if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { Fact* pRFact = getFact(pRange->param); //-- This parameter Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) if(pRFact && pTFact) { + qCDebug(CameraControlLogVerbose) << "Check new set of options for" << pTFact->name(); QString option = pRFact->rawValueString(); //-- This parameter value //-- If this value (and condition) triggers a change in the target range + qCDebug(CameraControlLogVerbose) << "Range value:" << pRange->value << "Current value:" << option << "Condition:" << pRange->condition; if(pRange->value == option && _processCondition(pRange->condition)) { - //-- Set limited range set if(pTFact->enumStrings() != pRange->optNames) { - pTFact->setEnumInfo(pRange->optNames, pRange->optVariants); - emit pTFact->enumsChanged(); + //-- Set limited range set + rangesSet[pTFact] = pRange; + //-- Adjust current index if outside new limts + if(!pRange->optVariants.contains(pTFact->rawValue()) && _activeSettings.contains(pTFact->name())) { + qCDebug(CameraControlLogVerbose) << pTFact->name() << "was" << pTFact->enumOrValueString() << "and is now" << pRange->optNames[0]; + //-- We need to preemt the new option + valuesSet[pTFact] = pRange->optVariants[0]; + } qCDebug(CameraControlLogVerbose) << "Limited:" << pRange->targetParam << pRange->optNames; } changedList << pRange->targetParam; - } else { - if(!resetList.contains(pRange->targetParam)) { - if(pTFact->enumStrings() != _originalOptNames[pRange->targetParam]) { - //-- Restore full option set - pTFact->setEnumInfo(_originalOptNames[pRange->targetParam], _originalOptValues[pRange->targetParam]); - emit pTFact->enumsChanged(); - qCDebug(CameraControlLogVerbose) << "Full:" << pRange->targetParam << _originalOptNames[pRange->targetParam]; - } - resetList << pRange->targetParam; + } + } + } + } + //-- Iterate range sets again looking for resets + foreach(QGCCameraOptionRange* pRange, _optionRanges) { + if(!changedList.contains(pRange->targetParam) && (pRange->param == pFact->name() || pRange->condition.contains(pFact->name()))) { + Fact* pTFact = getFact(pRange->targetParam); //-- The target parameter (the one its range is to change) + if(!resetList.contains(pRange->targetParam)) { + if(pTFact->enumStrings() != _originalOptNames[pRange->targetParam]) { + //-- Restore full option set + rangesReset[pTFact] = pRange->targetParam; + qCDebug(CameraControlLogVerbose) << "Restore full set:" << pRange->targetParam << _originalOptNames[pRange->targetParam]; + //-- Retrive the current option for this setting as the index into the options might be pointing to something else. + if(!valuesRequested.contains(pTFact->name())) { + valuesRequested << pTFact->name(); } } + resetList << pRange->targetParam; } } } + //-- Update limited range set + foreach (Fact* pFact, rangesSet.keys()) { + pFact->setEnumInfo(rangesSet[pFact]->optNames, rangesSet[pFact]->optVariants); + if(!_updates.contains(pFact)) { + _paramIO[pFact->name()]->optNames = rangesSet[pFact]->optNames; + _paramIO[pFact->name()]->optVariants = rangesSet[pFact]->optVariants; + _updates << pFact; + } + } + //-- Restore full range set + foreach (Fact* pFact, rangesReset.keys()) { + pFact->setEnumInfo(_originalOptNames[rangesReset[pFact]], _originalOptValues[rangesReset[pFact]]); + if(!_updates.contains(pFact)) { + _paramIO[pFact->name()]->optNames = _originalOptNames[rangesReset[pFact]]; + _paramIO[pFact->name()]->optVariants = _originalOptValues[rangesReset[pFact]]; + _updates << pFact; + } + } + //-- Send new values where new range forced it + foreach(Fact* pFact, valuesSet.keys()) { + pFact->_containerSetRawValue(valuesSet[pFact]); + _paramIO[pFact->name()]->sendParameter(true); + } + //-- Update values for restored ranges + foreach (QString factName, valuesRequested) { + _paramIO[factName]->paramRequest(); + } + //-- Update UI (Asynchronous state where values come back after a while) + if(_updates.size()) { + _updateTimer.start(500); + } +} + +//----------------------------------------------------------------------------- +void +QGCCameraControl::_updateTimeout() +{ + //-- Update UI + foreach (Fact* pFact, _updates) { + pFact->setEnumInfo(_paramIO[pFact->name()]->optNames, _paramIO[pFact->name()]->optVariants); + qCDebug(CameraControlLogVerbose) << "Update enums" << pFact->name() << pFact->enumStrings(); + emit pFact->enumsChanged(); + } + _updates.clear(); } //----------------------------------------------------------------------------- diff --git a/src/Camera/QGCCameraControl.h b/src/Camera/QGCCameraControl.h index 1af2fbc3e..258027168 100644 --- a/src/Camera/QGCCameraControl.h +++ b/src/Camera/QGCCameraControl.h @@ -168,6 +168,7 @@ private slots: void _mavCommandResult (int vehicleId, int component, int command, int result, bool noReponseFromVehicle); void _dataReady (QByteArray data); void _paramDone (); + void _updateTimeout (); private: bool _handleLocalization (QByteArray& bytes); @@ -205,9 +206,11 @@ protected: QStringList _activeSettings; QStringList _settings; QTimer _captureStatusTimer; + QTimer _updateTimer; QList _valueExclusions; QList _optionRanges; QMap _originalOptNames; QMap _originalOptValues; QMap _paramIO; + QVector _updates; }; diff --git a/src/Camera/QGCCameraIO.cc b/src/Camera/QGCCameraIO.cc index 3b3ba8ab9..524cb5edf 100644 --- a/src/Camera/QGCCameraIO.cc +++ b/src/Camera/QGCCameraIO.cc @@ -20,12 +20,13 @@ QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicl , _sentRetries(0) , _requestRetries(0) , _done(false) + , _updateOnSet(false) { QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); _paramWriteTimer.setSingleShot(true); - _paramWriteTimer.setInterval(1000); + _paramWriteTimer.setInterval(3000); _paramRequestTimer.setSingleShot(true); - _paramRequestTimer.setInterval(2000); + _paramRequestTimer.setInterval(3000); connect(&_paramWriteTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramWriteTimeout); connect(&_paramRequestTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramRequestTimeout); connect(_fact, &Fact::rawValueChanged, this, &QGCCameraParamIO::_factChanged); @@ -75,7 +76,7 @@ void QGCCameraParamIO::_factChanged(QVariant value) { Q_UNUSED(value); - qCDebug(CameraControlLog) << "UI Fact" << _fact->name() << "changed"; + qCDebug(CameraIOLog) << "UI Fact" << _fact->name() << "changed"; //-- TODO: Do we really want to update the UI now or only when we receive the ACK? _control->factChanged(_fact); } @@ -85,11 +86,21 @@ void QGCCameraParamIO::_containerRawValueChanged(const QVariant value) { Q_UNUSED(value); - qCDebug(CameraControlLog) << "Send Fact" << _fact->name(); + qCDebug(CameraIOLog) << "Update Fact" << _fact->name(); _sentRetries = 0; _sendParameter(); } +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::sendParameter(bool updateUI) +{ + qCDebug(CameraIOLog) << "Send Fact" << _fact->name(); + _sentRetries = 0; + _updateOnSet = updateUI; + _sendParameter(); +} + //----------------------------------------------------------------------------- void QGCCameraParamIO::_sendParameter() @@ -147,9 +158,10 @@ QGCCameraParamIO::_paramWriteTimeout() { if(++_sentRetries > 3) { qCWarning(CameraIOLog) << "No response for param set:" << _fact->name(); + _updateOnSet = false; } else { //-- Send it again - qCDebug(CameraIOLog) << "Param set retry:" << _fact->name(); + qCDebug(CameraIOLog) << "Param set retry:" << _fact->name() << _sentRetries; _sendParameter(); _paramWriteTimer.start(); } @@ -164,6 +176,10 @@ QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack) QVariant val = _valueFromMessage(ack.param_value, ack.param_type); if(_fact->rawValue() != val) { _fact->_containerSetRawValue(val); + if(_updateOnSet) { + _updateOnSet = false; + _control->factChanged(_fact); + } } } else if(ack.param_result == PARAM_ACK_IN_PROGRESS) { //-- Wait a bit longer for this one @@ -171,7 +187,12 @@ QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack) _paramWriteTimer.start(); } else { if(ack.param_result == PARAM_ACK_FAILED) { - qCWarning(CameraIOLog) << "Param set failed:" << _fact->name(); + if(++_sentRetries > 3) { + //-- Try again + qCWarning(CameraIOLog) << "Param set failed:" << _fact->name() << _sentRetries; + _paramWriteTimer.start(); + } + return; } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) { qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name(); } @@ -248,20 +269,31 @@ QGCCameraParamIO::_paramRequestTimeout() } else { //-- Request it again qCDebug(CameraIOLog) << "Param request retry:" << _fact->name(); - char param_id[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; - memset(param_id, 0, sizeof(param_id)); - strncpy(param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN); - mavlink_message_t msg; - mavlink_msg_param_ext_request_read_pack_chan( - _pMavlink->getSystemId(), - _pMavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - _vehicle->id(), - _control->compID(), - param_id, - -1); - _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + paramRequest(false); _paramRequestTimer.start(); } } + +//----------------------------------------------------------------------------- +void +QGCCameraParamIO::paramRequest(bool reset) +{ + if(reset) { + _requestRetries = 0; + } + char param_id[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; + memset(param_id, 0, sizeof(param_id)); + strncpy(param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN); + mavlink_message_t msg; + mavlink_msg_param_ext_request_read_pack_chan( + _pMavlink->getSystemId(), + _pMavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + _control->compID(), + param_id, + -1); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); + _paramRequestTimer.start(); +} diff --git a/src/Camera/QGCCameraIO.h b/src/Camera/QGCCameraIO.h index ba2850334..796b52377 100644 --- a/src/Camera/QGCCameraIO.h +++ b/src/Camera/QGCCameraIO.h @@ -25,6 +25,11 @@ public: void handleParamValue (const mavlink_param_ext_value_t& value); void setParamRequest (); bool paramDone () { return _done; } + void paramRequest (bool reset = true); + void sendParameter (bool updateUI = false); + + QStringList optNames; + QVariantList optVariants; private slots: void _paramWriteTimeout (); @@ -46,6 +51,7 @@ private: QTimer _paramWriteTimer; QTimer _paramRequestTimer; bool _done; + bool _updateOnSet; MAV_PARAM_TYPE _mavParamType; MAVLinkProtocol* _pMavlink; }; diff --git a/src/FactSystem/Fact.cc b/src/FactSystem/Fact.cc index ff1cf10d7..34f4ec20e 100644 --- a/src/FactSystem/Fact.cc +++ b/src/FactSystem/Fact.cc @@ -84,8 +84,9 @@ void Fact::forceSetRawValue(const QVariant& value) if (_metaData->convertAndValidateRaw(value, true /* convertOnly */, typedValue, errorString)) { _rawValue.setValue(typedValue); _sendValueChangedSignal(cookedValue()); - emit _containerRawValueChanged(rawValue()); + //-- Must be in this order emit rawValueChanged(_rawValue); + emit _containerRawValueChanged(rawValue()); } } else { qWarning() << kMissingMetadata; @@ -102,8 +103,9 @@ void Fact::setRawValue(const QVariant& value) if (typedValue != _rawValue) { _rawValue.setValue(typedValue); _sendValueChangedSignal(cookedValue()); - emit _containerRawValueChanged(rawValue()); + //-- Must be in this order emit rawValueChanged(_rawValue); + emit _containerRawValueChanged(rawValue()); } } } else { @@ -235,7 +237,7 @@ QVariantList Fact::enumValues(void) const void Fact::setEnumInfo(const QStringList& strings, const QVariantList& values) { if (_metaData) { - return _metaData->setEnumInfo(strings, values); + _metaData->setEnumInfo(strings, values); } else { qWarning() << kMissingMetadata; } diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index b1daa7e57..8e34609b9 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -125,7 +125,7 @@ public: void setRawUnits (const QString& rawUnits); void setRebootRequired (bool rebootRequired) { _rebootRequired = rebootRequired; } void setIncrement (double increment) { _increment = increment; } - void setHasControl (bool bValue) { _hasControl = bValue; } + void setHasControl (bool bValue) { _hasControl = bValue; } void setTranslators(Translator rawTranslator, Translator cookedTranslator); -- 2.22.0