From de5fc6a5bd58688335a28a6ccbe238776f3672dc Mon Sep 17 00:00:00 2001 From: tstellanova Date: Mon, 26 Aug 2013 23:11:41 -0700 Subject: [PATCH] Reimplement write and read timeout retransmission Also move default component ID into data model --- src/uas/QGCUASParamManager.cc | 25 +- src/uas/QGCUASParamManager.h | 1 - src/uas/UASParameterCommsMgr.cc | 447 ++++++++++++------------------- src/uas/UASParameterCommsMgr.h | 45 ++-- src/uas/UASParameterDataModel.cc | 23 +- src/uas/UASParameterDataModel.h | 5 + 6 files changed, 230 insertions(+), 316 deletions(-) diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc index 38450eb81..90991f899 100644 --- a/src/uas/QGCUASParamManager.cc +++ b/src/uas/QGCUASParamManager.cc @@ -11,8 +11,7 @@ QGCUASParamManager::QGCUASParamManager(QObject *parent) : QObject(parent), mav(NULL), paramDataModel(this), - paramCommsMgr(NULL), - defaultComponentId(-1) + paramCommsMgr(NULL) { @@ -63,22 +62,7 @@ void QGCUASParamManager::clearAllPendingParams() int QGCUASParamManager::getDefaultComponentId() { - int result = 0; - - if (-1 != defaultComponentId) - return defaultComponentId; - - QList components = getComponentForParam("SYS_AUTOSTART");//TODO is this the best way to find the right component? - - // Guard against multiple components responding - this will never show in practice - if (1 == components.count()) { - result = components.first(); - defaultComponentId = result; - } - - qDebug() << "Default compId: " << result; - - return result; + return paramDataModel.getDefaultComponentId(); } QList QGCUASParamManager::getComponentForParam(const QString& parameter) const @@ -119,7 +103,6 @@ void QGCUASParamManager::requestParameterListIfEmpty() if (mav) { int totalOnboard = paramDataModel.countOnboardParams(); if (totalOnboard < 2) { //TODO arbitrary constant, maybe 0 is OK? - defaultComponentId = -1; //reset this ...we have no idea what the default component ID is requestParameterList(); } } @@ -135,7 +118,7 @@ void QGCUASParamManager::setParameter(int compId, QString paramName, QVariant va { if ((0 == compId) || (-1 == compId)) { //attempt to get an actual component ID - compId = getDefaultComponentId(); + compId = paramDataModel.getDefaultComponentId(); } paramDataModel.updatePendingParamWithValue(compId,paramName,value); } @@ -152,7 +135,7 @@ void QGCUASParamManager::setPendingParam(int compId, const QString& paramName, { if ((0 == compId) || (-1 == compId)) { //attempt to get an actual component ID - compId = getDefaultComponentId(); + compId = paramDataModel.getDefaultComponentId(); } paramDataModel.updatePendingParamWithValue(compId,paramName,value); } diff --git a/src/uas/QGCUASParamManager.h b/src/uas/QGCUASParamManager.h index 2c31136e2..84efadfee 100644 --- a/src/uas/QGCUASParamManager.h +++ b/src/uas/QGCUASParamManager.h @@ -125,7 +125,6 @@ protected: UASInterface* mav; ///< The MAV this manager is controlling UASParameterDataModel paramDataModel;///< Shared data model of parameters UASParameterCommsMgr* paramCommsMgr; ///< Shared comms mgr for parameters - int defaultComponentId; ///< Cached default component ID }; diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc index a62ab19c5..a176114e9 100644 --- a/src/uas/UASParameterCommsMgr.cc +++ b/src/uas/UASParameterCommsMgr.cc @@ -5,18 +5,18 @@ #include "QGCUASParamManager.h" #include "UASInterface.h" + + #define RC_CAL_CHAN_MAX 8 UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent) : QObject(parent), mav(NULL), + maxSilenceTimeout(30000), paramDataModel(NULL), - transmissionListMode(false), - transmissionActive(false), - transmissionTimeout(0), - retransmissionTimeout(1000), - rewriteTimeout(1000), - retransmissionBurstRequestSize(5) + retransmitBurstLimit(5), + silenceTimeout(1000), + transmissionListMode(false) { @@ -40,29 +40,27 @@ UASParameterCommsMgr* UASParameterCommsMgr::initWithUAS(UASInterface* uas) connect(mav, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant))); - //connect to retransmissionTimer - connect(&retransmissionTimer, SIGNAL(timeout()), - this, SLOT(retransmissionGuardTick())); + connect(&silenceTimer, SIGNAL(timeout()), + this,SLOT(silenceTimerExpired())); return this; } + void UASParameterCommsMgr::loadParamCommsSettings() { QSettings settings; + //TODO these are duplicates of MAVLinkProtocol settings...seems wrong to use them in two places settings.beginGroup("QGC_MAVLINK_PROTOCOL"); bool ok; - int val = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", retransmissionTimeout).toInt(&ok); - if (ok) { - retransmissionTimeout = val; - qDebug() << "retransmissionTimeout" << retransmissionTimeout; - } - val = settings.value("PARAMETER_REWRITE_TIMEOUT", rewriteTimeout).toInt(&ok); + int val = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", 1000).toInt(&ok); if (ok) { - rewriteTimeout = val; + silenceTimeout = val; + qDebug() << "silenceTimeout" << silenceTimeout; } + settings.endGroup(); } @@ -79,20 +77,11 @@ void UASParameterCommsMgr::requestParameterList() } if (!transmissionListMode) { - // Clear transmission state - receivedParamsList.clear(); - transmissionListSizeKnown.clear(); - - transmissionListMode = true; - foreach (int key, missingReadPackets.keys()) { - missingReadPackets.value(key)->clear(); - } - transmissionActive = true; - - setParameterStatusMsg(tr("Requested param list.. waiting")); - listRecvTimeout = QGC::groundTimeMilliseconds() + 10000; + transmissionListMode = true;//TODO eliminate? + //we use (compId 0, paramId 0) as indicating all params for the system + markReadParamWaiting(0,0); mav->requestParameters(); - setRetransmissionGuardEnabled(true); + updateSilenceTimer(); } else { qDebug() << __FILE__ << __LINE__ << "Ignoring requestParameterList because we're receiving params list"; @@ -101,6 +90,26 @@ void UASParameterCommsMgr::requestParameterList() } +void UASParameterCommsMgr::markReadParamWaiting(int compId, int paramId) +{ + if (!readsWaiting.contains(compId)) { + readsWaiting.insert(compId, new QSet()); + } + + readsWaiting.value(compId)->insert(paramId); +} + +void UASParameterCommsMgr::markWriteParamWaiting(int compId, QString paramName, QVariant value) +{ + //ensure we have a map for this compId + if (!writesWaiting.contains(compId)) { + writesWaiting.insert(compId, new QMap()); + } + + // Insert it in missing write ACK list + writesWaiting.value(compId)->insert(paramName, value); +} + /* Empty read retransmission list Empty write retransmission list @@ -110,17 +119,17 @@ void UASParameterCommsMgr::clearRetransmissionLists(int& missingReadCount, int& qDebug() << __FILE__ << __LINE__ << "clearRetransmissionLists"; missingReadCount = 0; - QList readKeys = missingReadPackets.keys(); - foreach (int compId, readKeys) { - missingReadCount += missingReadPackets.value(compId)->count(); - missingReadPackets.value(compId)->clear(); + QList compIds = readsWaiting.keys(); + foreach (int compId, compIds) { + missingReadCount += readsWaiting.value(compId)->count(); + readsWaiting.value(compId)->clear(); } missingWriteCount = 0; - QList writeKeys = missingWriteAckPackets.keys(); - foreach (int compId, writeKeys) { - missingWriteCount += missingWriteAckPackets.value(compId)->count(); - missingWriteAckPackets.value(compId)->clear(); + compIds = writesWaiting.keys(); + foreach (int compId, compIds) { + missingWriteCount += writesWaiting.value(compId)->count(); + writesWaiting.value(compId)->clear(); } } @@ -170,16 +179,21 @@ void UASParameterCommsMgr::resendReadWriteRequests() int compId; QList compIds; - // Re-request at maximum retransmissionBurstRequestSize parameters at once + // Re-request at maximum retransmitBurstLimit parameters at once // to prevent link flooding' int requestedReadCount = 0; - compIds = missingReadPackets.keys(); + compIds = readsWaiting.keys(); foreach (compId, compIds) { // Request n parameters from this component (at maximum) - QList* missingReadParams = missingReadPackets.value(compId, NULL); - qDebug() << "missingReadParams:" << missingReadParams->count(); + QSet* missingReadParams = readsWaiting.value(compId, NULL); + qDebug() << "compId " << compId << "readsWaiting:" << missingReadParams->count(); foreach (int paramId, *missingReadParams) { - if (requestedReadCount < retransmissionBurstRequestSize) { + if (0 == paramId && 0 == compId) { + mav->requestParameters(); + //don't request any other params individually for this component + break; + } + if (requestedReadCount < retransmitBurstLimit) { //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << paramId << "FROM COMPONENT #" << compId; emit parameterUpdateRequestedById(compId, paramId); setParameterStatusMsg(tr("Requested retransmission of #%1").arg(paramId+1)); @@ -192,16 +206,16 @@ void UASParameterCommsMgr::resendReadWriteRequests() } } - // Re-request at maximum retransmissionBurstRequestSize parameters at once + // Re-request at maximum retransmitBurstLimit parameters at once // to prevent write-request link flooding int requestedWriteCount = 0; - compIds = missingWriteAckPackets.keys(); + compIds = writesWaiting.keys(); foreach (compId, compIds) { - QMap * missingParams = missingWriteAckPackets.value(compId); - foreach (QString key, missingParams->keys()) { - if (requestedWriteCount < retransmissionBurstRequestSize) { + QMap * missingWriteParams = writesWaiting.value(compId); + foreach (QString key, missingWriteParams->keys()) { + if (requestedWriteCount < retransmitBurstLimit) { // Re-request write operation - QVariant value = missingParams->value(key); + QVariant value = missingWriteParams->value(key); emitPendingParameterCommit(compId, key, value); requestedWriteCount++; } @@ -212,106 +226,47 @@ void UASParameterCommsMgr::resendReadWriteRequests() } } - if ((0 == requestedWriteCount) && (0 == requestedReadCount) ) { - qDebug() << __FILE__ << __LINE__ << "No pending re-read or rewrite requests"; - if (!transmissionListMode) { - setRetransmissionGuardEnabled(false); - transmissionActive = false; - } - } - else { - //restart the timer now that we've sent - setRetransmissionGuardEnabled(true); - } + updateSilenceTimer(); + } void UASParameterCommsMgr::resetAfterListReceive() { transmissionListMode = false; - transmissionListSizeKnown.clear(); - - //We shouldn't clear missingPackets because other transactions might be using them? - + knownParamListSize.clear(); } -void UASParameterCommsMgr::retransmissionGuardTick() +void UASParameterCommsMgr::silenceTimerExpired() { quint64 curTime = QGC::groundTimeMilliseconds(); + int elapsed = (int)(curTime - lastSilenceTimerReset); + qDebug() << "silenceTimerExpired elapsed:" << elapsed; - //Workaround for an apparent Qt bug that causes retransmission guard timer to fire prematurely (350ms) - int elapsed = (int)(curTime - lastTimerReset); - if (elapsed < retransmissionTimeout) { - qDebug() << "retransmissionGuardTick elapsed:" << (curTime - lastTimerReset); + if (elapsed < silenceTimeout) { //reset the guard timer: it fired prematurely - setRetransmissionGuardEnabled(true); + updateSilenceTimer(); return; } - qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE after" << elapsed; - - - if (transmissionActive) { - - if (transmissionListMode && transmissionListSizeKnown.isEmpty() ) { - //we are still waitin for the first parameter list response - if (curTime > this->listRecvTimeout) { - //re-request parameters - setParameterStatusMsg(tr("TIMEOUT: Re-requesting param list"),ParamCommsStatusLevel_Warning); - listRecvTimeout = curTime + 10000; - mav->requestParameters(); - //reset the timer - setRetransmissionGuardEnabled(true); - } - return; - } - // Check for timeout - // stop retransmission attempts on timeout - if (curTime > transmissionTimeout) { - setRetransmissionGuardEnabled(false); - resetAfterListReceive(); - - int missingReadCount, missingWriteCount; - clearRetransmissionLists(missingReadCount,missingWriteCount); - if ((missingReadCount > 0) || (missingWriteCount > 0)) { - setParameterStatusMsg(tr("TIMEOUT! MISSING: %1 read, %2 write.").arg(missingReadCount).arg(missingWriteCount), - ParamCommsStatusLevel_Warning); - } - - return; - } - - resendReadWriteRequests(); + int totalElapsed = (int)(curTime - lastReceiveTime); + if (totalElapsed > maxSilenceTimeout) { + qDebug() << "Max silence time exceeded: " + totalElapsed; + int missingReads, missingWrites; + clearRetransmissionLists(missingReads,missingWrites); + //TODO notify user! } else { - qDebug() << __FILE__ << __LINE__ << "STOPPING RETRANSMISSION GUARD GRACEFULLY"; - setRetransmissionGuardEnabled(false); + resendReadWriteRequests(); } } - -/** - * Enabling the retransmission guard enables the parameter widget to track - * dropped parameters and to re-request them. This works for both individual - * parameter reads as well for whole list requests. - * - * @param enabled True if retransmission checking should be enabled, false else - */ - -void UASParameterCommsMgr::setRetransmissionGuardEnabled(bool enabled) -{ - if (enabled) { - retransmissionTimer.start(retransmissionTimeout); - lastTimerReset = QGC::groundTimeMilliseconds() ; - } else { - retransmissionTimer.stop(); - } -} - -void UASParameterCommsMgr::requestParameterUpdate(int component, const QString& parameter) +void UASParameterCommsMgr::requestParameterUpdate(int compId, const QString& paramName) { if (mav) { - mav->requestParameter(component, parameter); + mav->requestParameter(compId, paramName); + //TODO track these read requests with a paramName but no param ID : use index in getOnboardParamsForComponent? + //ensure we keep track of every single read request } } @@ -326,15 +281,14 @@ void UASParameterCommsMgr::requestRcCalibrationParamsUpdate() // Do not request the RC type, as these values depend on this // active onboard parameter + + int defCompId = paramDataModel->getDefaultComponentId(); for (unsigned int i = 1; i < (RC_CAL_CHAN_MAX+1); ++i) { qDebug() << "Request RC " << i; - mav->requestParameter(0, minTpl.arg(i)); - QGC::SLEEP::usleep(5000); - mav->requestParameter(0, trimTpl.arg(i)); - QGC::SLEEP::usleep(5000); - mav->requestParameter(0, maxTpl.arg(i)); - QGC::SLEEP::usleep(5000); - mav->requestParameter(0, revTpl.arg(i)); + requestParameterUpdate(defCompId, minTpl.arg(i)); + requestParameterUpdate(defCompId, trimTpl.arg(i)); + requestParameterUpdate(defCompId, maxTpl.arg(i)); + requestParameterUpdate(defCompId, revTpl.arg(i)); QGC::SLEEP::usleep(5000); } } @@ -349,158 +303,152 @@ void UASParameterCommsMgr::requestRcCalibrationParamsUpdate() * @param parameterName name of the parameter, as delivered by the system * @param value value of the parameter */ -void UASParameterCommsMgr::setParameter(int component, QString parameterName, QVariant value) +void UASParameterCommsMgr::setParameter(int compId, QString paramName, QVariant value) { - if (parameterName.isEmpty()) { + if (paramName.isEmpty()) { return; } double dblValue = value.toDouble(); - if (paramDataModel->isValueLessThanParamMin(parameterName,dblValue)) { - setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(parameterName).arg(dblValue), + if (paramDataModel->isValueLessThanParamMin(paramName,dblValue)) { + setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(paramName).arg(dblValue), ParamCommsStatusLevel_Error ); return; } - if (paramDataModel->isValueGreaterThanParamMax(parameterName,dblValue)) { - setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(parameterName).arg(dblValue), + if (paramDataModel->isValueGreaterThanParamMax(paramName,dblValue)) { + setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(paramName).arg(dblValue), ParamCommsStatusLevel_Error ); return; } QVariant onboardVal; - paramDataModel->getOnboardParamValue(component,parameterName,onboardVal); + paramDataModel->getOnboardParamValue(compId,paramName,onboardVal); if (onboardVal == value) { - setParameterStatusMsg(tr("REJ. %1 already %2").arg(parameterName).arg(dblValue), + setParameterStatusMsg(tr("REJ. %1 already %2").arg(paramName).arg(dblValue), ParamCommsStatusLevel_Warning ); return; } - emitPendingParameterCommit(component, parameterName, value); + emitPendingParameterCommit(compId, paramName, value); + + //Add this request to list of writes not yet ack'd - // Wait for parameter to be written back - // mark it therefore as missing - if (!missingWriteAckPackets.contains(component)) { - missingWriteAckPackets.insert(component, new QMap()); + markWriteParamWaiting( compId, paramName, value); + updateSilenceTimer(); + + +} + +void UASParameterCommsMgr::updateSilenceTimer() +{ + //if there are pending reads or writes, ensure we timeout in a little while + //if we hear nothing but silence from our partner + + int missReadCount = 0; + foreach (int key, readsWaiting.keys()) { + missReadCount += readsWaiting.value(key)->count(); + } + + int missWriteCount = 0; + foreach (int key, writesWaiting.keys()) { + missWriteCount += writesWaiting.value(key)->count(); } - // Insert it in missing write ACK list - missingWriteAckPackets.value(component)->insert(parameterName, value); - // Set timeouts - if (transmissionActive) { - transmissionTimeout += rewriteTimeout; + if (missReadCount > 0 || missWriteCount > 0) { + silenceTimer.start(silenceTimeout); //TODO configurable silence timeout + lastSilenceTimerReset = QGC::groundTimeMilliseconds(); } else { - quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + rewriteTimeout; - if (newTransmissionTimeout > transmissionTimeout) { - transmissionTimeout = newTransmissionTimeout; - } - transmissionActive = true; + //all parameters have been received, broadcast to UI + emit parameterListUpToDate(); + resetAfterListReceive(); + silenceTimer.stop(); } - // Enable guard / reset timeouts - setRetransmissionGuardEnabled(true); + + } + void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level) { qDebug() << "parameterStatusMsg: " << msg; - parameterStatusMsg = msg; - emit parameterStatusMsgUpdated(msg,level); } - -/** - * @param uas System which has the component - * @param component id of the component - * @param parameterName human friendly name of the parameter - */ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value) { Q_UNUSED(uas); //this object is assigned to one UAS only - + lastReceiveTime = QGC::groundTimeMilliseconds(); qDebug() << "compId" << compId << "receivedParameterUpdate:" << paramName; //notify the data model that we have an updated param paramDataModel->handleParamUpdate(compId,paramName,value); - // Missing packets list has to be instantiated for all components - if (!missingReadPackets.contains(compId)) { - missingReadPackets.insert(compId, new QList()); - } - QList* compMissReadPackets = missingReadPackets.value(compId); + // Ensure we have missing read/write lists for this compId + if (!readsWaiting.contains(compId)) { + readsWaiting.insert(compId, new QSet()); + } + if (!writesWaiting.contains(compId) ) { + writesWaiting.insert(compId,new QMap()); + } + QSet* compMissingReads = readsWaiting.value(compId); // List mode is different from single parameter transfers if (transmissionListMode) { - // Only accept the list size once on the first packet from - // each component - if (!transmissionListSizeKnown.contains(compId)) { + // Only accept the list size once on the first packet from each component + if (!knownParamListSize.contains(compId)) { // Mark list size as known - transmissionListSizeKnown.insert(compId, true); + knownParamListSize.insert(compId,paramCount); - qDebug() << "Mark all parameters as missing: " << paramCount; - for (int i = 1; i < paramCount; ++i) { //TODO check: param Id 0 is "all parameters" and not valid ? - if (!compMissReadPackets->contains(i)) { - compMissReadPackets->append(i); - } - } + //remove our placeholder read request for all params + readsWaiting.value(0)->remove(0); - // There is only one transmission timeout for all components - // since components do not manage their transmission, - // the longest timeout is safe for all components. - quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + (paramCount*retransmissionTimeout); - if (thisTransmissionTimeout > transmissionTimeout) { - transmissionTimeout = thisTransmissionTimeout; + qDebug() << "Mark all parameters as missing: " << paramCount; + for (int i = 1; i < paramCount; ++i) { //param Id 0 is "all parameters" and not valid + compMissingReads->insert(i); } } - } + // Mark this parameter as received in read list - int index = compMissReadPackets->indexOf(paramId); - if (index != -1) { - compMissReadPackets->removeAt(index); - } + compMissingReads->remove(paramId); + bool justWritten = false; bool writeMismatch = false; // Mark this parameter as received in write ACK list - QMap* compMissWritePackets = missingWriteAckPackets.value(compId); - if (!compMissWritePackets) { + QMap* compMissingWrites = writesWaiting.value(compId); + if (!compMissingWrites) { //we sometimes send a write request on compId 0 and get a response on a nonzero compId eg 50 - compMissWritePackets = missingWriteAckPackets.value(0); + compMissingWrites = writesWaiting.value(0); } - if (compMissWritePackets && compMissWritePackets->contains(paramName)) { + if (compMissingWrites && compMissingWrites->contains(paramName)) { justWritten = true; - if (compMissWritePackets->value(paramName) != value) { + if (compMissingWrites->value(paramName) != value) { writeMismatch = true; } - compMissWritePackets->remove(paramName); + compMissingWrites->remove(paramName); } - int missReadCount = 0; - foreach (int key, missingReadPackets.keys()) { - missReadCount += missingReadPackets.value(key)->count(); - } - - int missWriteCount = 0; - foreach (int key, missingWriteAckPackets.keys()) { - missWriteCount += missingWriteAckPackets.value(key)->count(); - } - //TODO simplify this if-else tree if (justWritten) { + int waitingWritesCount = compMissingWrites->count(); if (!writeMismatch) { - setParameterStatusMsg(tr("SUCCESS: Wrote %2 (#%1/%4): %3 [%5]").arg(paramId+1).arg(paramName).arg(value.toDouble()).arg(paramCount).arg(missWriteCount)); - if (0 == missWriteCount) { - setParameterStatusMsg(tr("SUCCESS: WROTE ALL PARAMETERS")); + setParameterStatusMsg(tr("SUCCESS: Wrote %2 (#%1): %3").arg(paramId+1).arg(paramName).arg(value.toDouble())); + } + + if (!writeMismatch) { + if (0 == waitingWritesCount) { + setParameterStatusMsg(tr("SUCCESS: Wrote all params for component %1").arg(compId)); if (persistParamsAfterSend) { writeParamsToPersistentStorage(); persistParamsAfterSend = false; @@ -509,60 +457,30 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para } else { // Mismatch, tell user - setParameterStatusMsg(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(paramName).arg(compMissWritePackets->value(paramName).toDouble()).arg(value.toDouble()), + setParameterStatusMsg(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(paramName).arg(compMissingWrites->value(paramName).toDouble()).arg(value.toDouble()), ParamCommsStatusLevel_Warning); } } else { - if (missReadCount == 0) { + int waitingReadsCount = compMissingReads->count(); + + if (0 == waitingReadsCount) { // Transmission done QTime time = QTime::currentTime(); QString timeString = time.toString(); setParameterStatusMsg(tr("All received. (updated at %1)").arg(timeString)); } else { - // Transmission in progress + // Waiting to receive more QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' ')); - setParameterStatusMsg(tr("OK: %1 %2 (%3/%4)").arg(paramName).arg(val).arg(paramCount-missReadCount).arg(paramCount), - ParamCommsStatusLevel_Warning); - //transmissionMissingPackets + setParameterStatusMsg(tr("OK: %1 %2 (%3/%4)").arg(paramName).arg(val).arg(paramCount-waitingReadsCount).arg(paramCount), + ParamCommsStatusLevel_OK); } } - // Check if last parameter was received - if (missReadCount == 0 && missWriteCount == 0) { - resetAfterListReceive(); - setRetransmissionGuardEnabled(false); - //all parameters have been received, broadcast to UI - emit parameterListUpToDate(); - } - else { - //reset the timeout timer since we received one - setRetransmissionGuardEnabled(true); - - //qDebug() << "missCount:" << missCount << "missWriteCount:" << missWriteCount; - //if (missCount < 4) { - // foreach (int key, transmissionMissingPackets.keys()) { - // QList* list = transmissionMissingPackets.value(key); - - // if (list && list->count()) { - // QString yazza = QString().sprintf("Component %d missing %d: ",key,list->count()); - - // for (int i = 0; i < list->count(); i++) { - // int val = list->at(i); - // yazza.append( QString().sprintf("%d,",val) ); - // } - - // qDebug() << yazza; - // } - // else { - // //very suspicious...no actual missing items?? - // transmissionMissingPackets.remove(key); - // break; - // } - // } - //} - } + updateSilenceTimer(); + + } @@ -591,42 +509,27 @@ void UASParameterCommsMgr::sendPendingParameters(bool copyToPersistent) setParameterStatusMsg(tr("%1 pending params for component %2").arg(paramList->count()).arg(compId)); for (j = paramList->begin(); j != paramList->end(); ++j) { - //TODO mavlink command for "set parameter list" ? setParameter(compId, j.key(), j.value()); parametersSent++; } } // Change transmission status if necessary - if (parametersSent == 0) { + if (0 == parametersSent) { setParameterStatusMsg(tr("No transmission: No changed values."),ParamCommsStatusLevel_Warning); - if (persistParamsAfterSend) { - writeParamsToPersistentStorage(); - } } else { setParameterStatusMsg(tr("Transmitting %1 parameters.").arg(parametersSent)); - // Set timeouts - if (transmissionActive) { - transmissionTimeout += parametersSent*rewriteTimeout; - } - else { - transmissionActive = true; - quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + parametersSent*rewriteTimeout; - if (newTransmissionTimeout > transmissionTimeout) { - transmissionTimeout = newTransmissionTimeout; - } - } - // Enable guard - setRetransmissionGuardEnabled(true); - qDebug() << "Pending parameters now:" << paramDataModel->countPendingParams(); } + + + updateSilenceTimer(); } UASParameterCommsMgr::~UASParameterCommsMgr() { - setRetransmissionGuardEnabled(false); + silenceTimer.stop(); QString ptrStr; ptrStr.sprintf("%8p", this); diff --git a/src/uas/UASParameterCommsMgr.h b/src/uas/UASParameterCommsMgr.h index 958fa4211..0f240c5e1 100644 --- a/src/uas/UASParameterCommsMgr.h +++ b/src/uas/UASParameterCommsMgr.h @@ -32,8 +32,9 @@ public: protected: - /** @brief Activate / deactivate parameter retransmission */ - virtual void setRetransmissionGuardEnabled(bool enabled); + + /** @brief activate the silence timer if there are unack'd reads or writes */ + virtual void updateSilenceTimer(); virtual void setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level=ParamCommsStatusLevel_OK); @@ -43,6 +44,12 @@ protected: /** @brief clear transmissionMissingPackets and transmissionMissingWriteAckPackets */ void clearRetransmissionLists(int& missingReadCount, int& missingWriteCount ); + /** @brief we are waiting for a response to this read param request */ + virtual void markReadParamWaiting(int compId, int paramId); + + /** @brief we are waiting for a response to this write param request */ + void markWriteParamWaiting(int compId, QString paramName, QVariant value); + void resendReadWriteRequests(); void resetAfterListReceive(); @@ -75,8 +82,8 @@ public slots: /** @brief Request list of parameters from MAV */ virtual void requestParameterList(); - /** @brief Check for missing parameters */ - virtual void retransmissionGuardTick(); + /** @brief The max silence time expired */ + virtual void silenceTimerExpired(); /** @brief Request a single parameter update by name */ virtual void requestParameterUpdate(int component, const QString& parameter); @@ -88,28 +95,24 @@ public slots: protected: + + QMap knownParamListSize;///< The known param list size for each component, by component ID + quint64 lastReceiveTime; ///< The last time we received anything from our partner + quint64 lastSilenceTimerReset; UASInterface* mav; ///< The MAV we're talking to + int maxSilenceTimeout; ///< If nothing received within this period of time, abandon resends + UASParameterDataModel* paramDataModel; - // Communications management - QVector receivedParamsList; ///< Successfully received parameters - QMap* > missingReadPackets; ///< Missing packets - QMap* > missingWriteAckPackets; ///< Missing write ACK packets - bool transmissionListMode; ///< Currently requesting list - QMap transmissionListSizeKnown; ///< List size initialized? - bool transmissionActive; ///< Missing packets, working on list? bool persistParamsAfterSend; ///< Copy all parameters to persistent storage after sending - quint64 transmissionTimeout; ///< Timeout - QTimer retransmissionTimer; ///< Timer handling parameter retransmission - quint64 lastTimerReset; ///< Last time the guard timer was reset, to prevent premature firing - int retransmissionTimeout; ///< Retransmission request timeout, in milliseconds - int rewriteTimeout; ///< Write request timeout, in milliseconds - int retransmissionBurstRequestSize; ///< Number of packets requested for retransmission per burst - quint64 listRecvTimeout; ///< How long to wait for first parameter list response before re-requesting - - // Status - QString parameterStatusMsg; + QMap*> readsWaiting; ///< All reads that have not yet been received, by component ID + int retransmitBurstLimit; ///< Number of packets requested for retransmission per burst + int silenceTimeout; ///< If nothing received within this period of time, start resends + QTimer silenceTimer; ///< Timer handling parameter retransmission + bool transmissionListMode; ///< Currently requesting list + QMap* > writesWaiting; ///< All writes that have not yet been ack'd, by component ID + }; diff --git a/src/uas/UASParameterDataModel.cc b/src/uas/UASParameterDataModel.cc index 4493758a3..0a16a3ad6 100644 --- a/src/uas/UASParameterDataModel.cc +++ b/src/uas/UASParameterDataModel.cc @@ -9,7 +9,8 @@ #include "QGCMAVLink.h" UASParameterDataModel::UASParameterDataModel(QObject *parent) : - QObject(parent) + QObject(parent), + defaultComponentId(-1) { onboardParameters.clear(); pendingParameters.clear(); @@ -199,6 +200,26 @@ bool UASParameterDataModel::getOnboardParamValue(int componentId, const QString& return false; } +int UASParameterDataModel::getDefaultComponentId() +{ + int result = 0; + + if (-1 != defaultComponentId) + return defaultComponentId; + + QList components = getComponentForOnboardParam("SYS_AUTOSTART");//TODO is this the best way to find the right component? + + // Guard against multiple components responding - this will never show in practice + if (1 == components.count()) { + result = components.first(); + defaultComponentId = result; + } + + qDebug() << "Default compId: " << result; + + return result; +} + QList UASParameterDataModel::getComponentForOnboardParam(const QString& parameter) const { QList components; diff --git a/src/uas/UASParameterDataModel.h b/src/uas/UASParameterDataModel.h index 84761a968..501f488e8 100644 --- a/src/uas/UASParameterDataModel.h +++ b/src/uas/UASParameterDataModel.h @@ -28,6 +28,9 @@ public: virtual QString getParamDescription(const QString& param) { return paramDescriptions.value(param, ""); } virtual void setParamDescriptions(const QMap& paramInfo); + /** @brief Get the default component ID for the UAS */ + virtual int getDefaultComponentId(); + //TODO make this method protected? /** @brief Ensure that the data model is aware of this component * @param compId Id of the component @@ -113,6 +116,8 @@ public slots: virtual void clearAllPendingParams(); protected: + int defaultComponentId; ///< Cached default component ID + int uasId; ///< The UAS / MAV to which this data model pertains QMap* > pendingParameters; ///< Changed values that have not yet been transmitted to the UAS, by component ID QMap* > onboardParameters; ///< All parameters confirmed to be stored onboard the UAS, by component ID -- 2.22.0