Skip to content

Q
qgroundcontrol
Commit 50052f1c authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Merge pull request #351 from tstellanova/fix_retransmit 

Fix retransmit
parents 4d5aa0df de5fc6a5
Hide whitespace changes
Inline Side-by-side
Showing with 230 additions and 316 deletions
src/uas/QGCUASParamManager.cc
View file @ 50052f1c
......@@ -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<int> 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<int> 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);
}
......
src/uas/QGCUASParamManager.h
View file @ 50052f1c
......@@ -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
};
......
src/uas/UASParameterCommsMgr.cc
View file @ 50052f1c
......@@ -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<int>());
}
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<QString, QVariant>());
}
// 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<int> readKeys = missingReadPackets.keys();
foreach (int compId, readKeys) {
missingReadCount += missingReadPackets.value(compId)->count();
missingReadPackets.value(compId)->clear();
QList<int> compIds = readsWaiting.keys();
foreach (int compId, compIds) {
missingReadCount += readsWaiting.value(compId)->count();
readsWaiting.value(compId)->clear();
}
missingWriteCount = 0;
QList<int> 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<int> 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<int>* missingReadParams = missingReadPackets.value(compId, NULL);
qDebug() << "missingReadParams:" << missingReadParams->count();
QSet<int>* 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 <QString, QVariant>* missingParams = missingWriteAckPackets.value(compId);
foreach (QString key, missingParams->keys()) {
if (requestedWriteCount < retransmissionBurstRequestSize) {
QMap <QString, QVariant>* 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<QString, QVariant>());
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<int>());
}
QList<int>* compMissReadPackets = missingReadPackets.value(compId);
// Ensure we have missing read/write lists for this compId
if (!readsWaiting.contains(compId)) {
readsWaiting.insert(compId, new QSet<int>());
}
if (!writesWaiting.contains(compId) ) {
writesWaiting.insert(compId,new QMap<QString,QVariant>());
}
QSet<int>* 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<QString, QVariant>* compMissWritePackets = missingWriteAckPackets.value(compId);
if (!compMissWritePackets) {
QMap<QString, QVariant>* 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<int>* 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);
......
src/uas/UASParameterCommsMgr.h
View file @ 50052f1c
......@@ -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<int, int> 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<bool> receivedParamsList; ///< Successfully received parameters
QMap<int, QList<int>* > missingReadPackets; ///< Missing packets
QMap<int, QMap<QString, QVariant>* > missingWriteAckPackets; ///< Missing write ACK packets
bool transmissionListMode; ///< Currently requesting list
QMap<int, bool> 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<int, QSet<int>*> 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<int, QMap<QString, QVariant>* > writesWaiting; ///< All writes that have not yet been ack'd, by component ID
};
......
src/uas/UASParameterDataModel.cc
View file @ 50052f1c
......@@ -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<int> 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<int> UASParameterDataModel::getComponentForOnboardParam(const QString& parameter) const
{
QList<int> components;
......
src/uas/UASParameterDataModel.h
View file @ 50052f1c
......@@ -28,6 +28,9 @@ public:
virtual QString getParamDescription(const QString& param) { return paramDescriptions.value(param, ""); }
virtual void setParamDescriptions(const QMap<QString,QString>& 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<int, QMap<QString, QVariant>* > pendingParameters; ///< Changed values that have not yet been transmitted to the UAS, by component ID
QMap<int, QMap<QString, QVariant>* > onboardParameters; ///< All parameters confirmed to be stored onboard the UAS, by component ID
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment