UASParameterCommsMgr.cc 17 KB
Newer Older
1 2
#include "UASParameterCommsMgr.h"

tstellanova's avatar
tstellanova committed
3 4
#include <QSettings>

5 6 7
#include "QGCUASParamManager.h"
#include "UASInterface.h"

8 9


10 11
#define RC_CAL_CHAN_MAX 8

12
UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent) :
tstellanova's avatar
tstellanova committed
13
    QObject(parent),
tstellanova's avatar
tstellanova committed
14
    lastReceiveTime(0),
15
    mav(NULL),
16
    maxSilenceTimeout(30000),
17
    paramDataModel(NULL),
18 19 20
    retransmitBurstLimit(5),
    silenceTimeout(1000),
    transmissionListMode(false)
21
{
22 23 24 25 26 27 28


}

UASParameterCommsMgr* UASParameterCommsMgr::initWithUAS(UASInterface* uas)
{
    mav = uas;
29
    paramDataModel = mav->getParamManager()->dataModel();
tstellanova's avatar
tstellanova committed
30 31 32 33 34
    loadParamCommsSettings();

    //Requesting parameters one-by-one from mav
    connect(this, SIGNAL(parameterUpdateRequestedById(int,int)),
            mav, SLOT(requestParameter(int,int)));
35 36

    // Sending params to the UAS
37
    connect(this, SIGNAL(commitPendingParameter(int,QString,QVariant)),
38
            mav, SLOT(setParameter(int,QString,QVariant)));
39

40
    // Received parameter updates from UAS
41 42
    connect(mav, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)),
            this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant)));
43

44 45
    connect(&silenceTimer, SIGNAL(timeout()),
            this,SLOT(silenceTimerExpired()));
tstellanova's avatar
tstellanova committed
46

47
    return this;
48 49 50
}


tstellanova's avatar
tstellanova committed
51

52

tstellanova's avatar
tstellanova committed
53 54 55
void UASParameterCommsMgr::loadParamCommsSettings()
{
    QSettings settings;
56
    //TODO these are duplicates of MAVLinkProtocol settings...seems wrong to use them in two places
tstellanova's avatar
tstellanova committed
57
    settings.beginGroup("QGC_MAVLINK_PROTOCOL");
tstellanova's avatar
tstellanova committed
58
    bool ok;
59
    int val = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", 1000).toInt(&ok);
tstellanova's avatar
tstellanova committed
60
    if (ok) {
61 62
        silenceTimeout = val;
        qDebug() << "silenceTimeout" << silenceTimeout;
tstellanova's avatar
tstellanova committed
63
    }
64

tstellanova's avatar
tstellanova committed
65 66 67
    settings.endGroup();
}

68 69


70 71 72 73 74 75 76 77 78 79
/**
 * Send a request to deliver the list of onboard parameters
 * from the MAV.
 */
void UASParameterCommsMgr::requestParameterList()
{
    if (!mav) {
        return;
    }

80
    if (!transmissionListMode) {
81 82 83
        transmissionListMode = true;//TODO eliminate?
        //we use (compId 0, paramId 0) as  indicating all params for the system
        markReadParamWaiting(0,0);
84
        mav->requestParameters();
85
        updateSilenceTimer();
86 87 88
    }
    else {
        qDebug() << __FILE__ << __LINE__ << "Ignoring requestParameterList because we're receiving params list";
89 90 91 92 93
    }

}


94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113
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);
}

114 115 116 117
/*
 Empty read retransmission list
 Empty write retransmission list
*/
tstellanova's avatar
tstellanova committed
118
void UASParameterCommsMgr::clearRetransmissionLists(int& missingReadCount, int& missingWriteCount )
119 120 121
{
    qDebug() << __FILE__ << __LINE__ << "clearRetransmissionLists";

tstellanova's avatar
tstellanova committed
122
    missingReadCount = 0;
123 124 125 126
    QList<int> compIds = readsWaiting.keys();
    foreach (int compId, compIds) {
        missingReadCount += readsWaiting.value(compId)->count();
        readsWaiting.value(compId)->clear();
127 128
    }

tstellanova's avatar
tstellanova committed
129
    missingWriteCount = 0;
130 131 132 133
    compIds = writesWaiting.keys();
    foreach (int compId, compIds) {
        missingWriteCount += writesWaiting.value(compId)->count();
        writesWaiting.value(compId)->clear();
134 135 136 137 138
    }

}


139
void UASParameterCommsMgr::emitPendingParameterCommit(int compId, const QString& key, QVariant& value)
140 141 142 143 144 145 146
{
    int paramType = (int)value.type();
    switch (paramType)
    {
    case QVariant::Char:
    {
        QVariant fixedValue(QChar((unsigned char)value.toInt()));
147
        emit commitPendingParameter(compId, key, fixedValue);
148 149 150 151 152
    }
        break;
    case QVariant::Int:
    {
        QVariant fixedValue(value.toInt());
153
        emit commitPendingParameter(compId, key, fixedValue);
154 155 156 157 158
    }
        break;
    case QVariant::UInt:
    {
        QVariant fixedValue(value.toUInt());
159
        emit commitPendingParameter(compId, key, fixedValue);
160 161 162 163 164
    }
        break;
    case QMetaType::Float:
    {
        QVariant fixedValue(value.toFloat());
165
        emit commitPendingParameter(compId, key, fixedValue);
166 167 168
    }
        break;
    default:
169
        qCritical() << "ABORTED PARAM SEND, INVALID QVARIANT TYPE" << paramType;
170 171 172
        return;
    }

173
    setParameterStatusMsg(tr("Writing %1: %2 for comp. %3").arg(key).arg(value.toDouble()).arg(compId));
174 175 176 177 178 179 180 181 182

}


void UASParameterCommsMgr::resendReadWriteRequests()
{
    int compId;
    QList<int> compIds;

183
    // Re-request at maximum retransmitBurstLimit parameters at once
184 185
    // to prevent link flooding'
    int requestedReadCount = 0;
186
    compIds = readsWaiting.keys();
187 188
    foreach (compId, compIds) {
        // Request n parameters from this component (at maximum)
189 190
        QSet<int>* missingReadParams = readsWaiting.value(compId, NULL);
        qDebug() << "compId " << compId << "readsWaiting:" << missingReadParams->count();
191
        foreach (int paramId, *missingReadParams) {
192 193 194 195 196 197
            if (0 == paramId && 0 == compId) {
                mav->requestParameters();
                //don't request any other params individually for this component
                break;
            }
            if (requestedReadCount < retransmitBurstLimit) {
198
                //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << paramId << "FROM COMPONENT #" << compId;
199 200 201 202 203 204 205 206 207 208 209
                emit parameterUpdateRequestedById(compId, paramId);
                setParameterStatusMsg(tr("Requested retransmission of #%1").arg(paramId+1));
                requestedReadCount++;
            }
            else {
                qDebug() << "Throttling read retransmit requests at" << requestedReadCount;
                break;
            }
        }
    }

210
    // Re-request at maximum retransmitBurstLimit parameters at once
211 212
    // to prevent write-request link flooding
    int requestedWriteCount = 0;
213
    compIds = writesWaiting.keys();
214
    foreach (compId, compIds) {
215 216 217
        QMap <QString, QVariant>* missingWriteParams = writesWaiting.value(compId);
        foreach (QString key, missingWriteParams->keys()) {
            if (requestedWriteCount < retransmitBurstLimit) {
218
                // Re-request write operation
219
                QVariant value = missingWriteParams->value(key);
220
                emitPendingParameterCommit(compId, key, value);
221 222 223 224 225 226 227 228 229
                requestedWriteCount++;
            }
            else {
                qDebug() << "Throttling write retransmit requests at" << requestedWriteCount;
                break;
            }
        }
    }

230 231
    updateSilenceTimer();

232 233
}

234 235 236
void UASParameterCommsMgr::resetAfterListReceive()
{
    transmissionListMode = false;
237
    knownParamListSize.clear();
238 239
}

240
void UASParameterCommsMgr::silenceTimerExpired()
241
{
242
    quint64 curTime = QGC::groundTimeMilliseconds();
243 244
    int elapsed = (int)(curTime - lastSilenceTimerReset);
    qDebug() << "silenceTimerExpired elapsed:" << elapsed;
245

246
    if (elapsed < silenceTimeout) {
247
        //reset the guard timer: it fired prematurely
248
        updateSilenceTimer();
249 250
        return;
    }
251

252 253
    int totalElapsed = (int)(curTime - lastReceiveTime);
    if (totalElapsed > maxSilenceTimeout) {
254
        qDebug() << "maxSilenceTimeout exceeded: " << totalElapsed;
255 256
        int missingReads, missingWrites;
        clearRetransmissionLists(missingReads,missingWrites);
257
        silenceTimer.stop();
tstellanova's avatar
tstellanova committed
258
        lastReceiveTime = 0;
259
        lastSilenceTimerReset = curTime;
260
        setParameterStatusMsg(tr("TIMEOUT: Abandoning %1 reads %2 writes after %3 seconds").arg(missingReads).arg(missingWrites).arg(totalElapsed/1000));
261 262
    }
    else {
263
        resendReadWriteRequests();
264 265 266 267
    }
}


268
void UASParameterCommsMgr::requestParameterUpdate(int compId, const QString& paramName)
269 270
{
    if (mav) {
271 272 273
        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
274 275 276
    }
}

277 278 279 280 281 282 283 284 285 286 287
void UASParameterCommsMgr::requestRcCalibrationParamsUpdate()
{
    if (!transmissionListMode) {
        QString minTpl("RC%1_MIN");
        QString maxTpl("RC%1_MAX");
        QString trimTpl("RC%1_TRIM");
        QString revTpl("RC%1_REV");

        // Do not request the RC type, as these values depend on this
        // active onboard parameter

288 289

        int defCompId = paramDataModel->getDefaultComponentId();
290 291
        for (unsigned int i = 1; i < (RC_CAL_CHAN_MAX+1); ++i)  {
            qDebug() << "Request RC " << i;
292 293 294 295
            requestParameterUpdate(defCompId, minTpl.arg(i));
            requestParameterUpdate(defCompId, trimTpl.arg(i));
            requestParameterUpdate(defCompId, maxTpl.arg(i));
            requestParameterUpdate(defCompId, revTpl.arg(i));
296 297 298 299 300 301 302 303 304
            QGC::SLEEP::usleep(5000);
        }
    }
    else {
        qDebug() << __FILE__ << __LINE__ << "Ignoring requestRcCalibrationParamsUpdate because we're receiving params list";
    }
}


305 306 307 308 309
/**
 * @param component the subsystem which has the parameter
 * @param parameterName name of the parameter, as delivered by the system
 * @param value value of the parameter
 */
310
void UASParameterCommsMgr::setParameter(int compId, QString paramName, QVariant value, bool forceSend)
311
{
312
    if (paramName.isEmpty()) {
313 314 315 316 317
        return;
    }

    double dblValue = value.toDouble();

318 319
    if (paramDataModel->isValueLessThanParamMin(paramName,dblValue)) {
        setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(paramName).arg(dblValue),
tstellanova's avatar
tstellanova committed
320 321
                              ParamCommsStatusLevel_Error
                              );
322 323
        return;
    }
324 325
    if (paramDataModel->isValueGreaterThanParamMax(paramName,dblValue)) {
        setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(paramName).arg(dblValue),
tstellanova's avatar
tstellanova committed
326 327
                              ParamCommsStatusLevel_Error
                              );
328 329
        return;
    }
330

331 332 333 334 335 336 337 338 339 340
	if (!forceSend) {
		QVariant onboardVal;
		paramDataModel->getOnboardParamValue(compId,paramName,onboardVal);
		if (onboardVal == value) {
			setParameterStatusMsg(tr("REJ. %1 already %2").arg(paramName).arg(dblValue),
				ParamCommsStatusLevel_Warning
				);
			return;
		}
	}
341

342 343 344
    emitPendingParameterCommit(compId, paramName, value);

    //Add this request to list of writes not yet ack'd
345

346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364
    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();
365 366 367
    }


368 369
    if (missReadCount > 0 || missWriteCount > 0) {
        lastSilenceTimerReset = QGC::groundTimeMilliseconds();
tstellanova's avatar
tstellanova committed
370 371 372 373
        if (0 == lastReceiveTime) {
            lastReceiveTime = lastSilenceTimerReset;
        }
        silenceTimer.start(silenceTimeout);
374
    }
375
    else {
376 377 378 379
        //all parameters have been received, broadcast to UI
        emit parameterListUpToDate();
        resetAfterListReceive();
        silenceTimer.stop();
tstellanova's avatar
tstellanova committed
380
        lastReceiveTime = 0;
381 382
    }

383 384


385 386
}

387

tstellanova's avatar
tstellanova committed
388
void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level)
389
{
Lorenz Meier's avatar
Lorenz Meier committed
390
    //qDebug() << "parameterStatusMsg: " << msg;
tstellanova's avatar
tstellanova committed
391
    emit parameterStatusMsgUpdated(msg,level);
392
}
tstellanova's avatar
tstellanova committed
393 394 395

void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value)
{
396
    Q_UNUSED(uas); //this object is assigned to one UAS only
397
    lastReceiveTime = QGC::groundTimeMilliseconds();
Lorenz Meier's avatar
Lorenz Meier committed
398
    // qDebug() << "compId" << compId << "receivedParameterUpdate:" << paramName;
399

400
    //notify the data model that we have an updated param
401
    paramDataModel->handleParamUpdate(compId,paramName,value);
tstellanova's avatar
tstellanova committed
402 403


404 405 406 407 408 409 410
    // 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>());
    }
411

412
    QSet<int>* compMissingReads =  readsWaiting.value(compId);
tstellanova's avatar
tstellanova committed
413 414
    // List mode is different from single parameter transfers
    if (transmissionListMode) {
415 416
        // Only accept the list size once on the first packet from each component
        if (!knownParamListSize.contains(compId)) {
tstellanova's avatar
tstellanova committed
417
            // Mark list size as known
418
            knownParamListSize.insert(compId,paramCount);
tstellanova's avatar
tstellanova committed
419

420 421
            //remove our placeholder read request for all params
            readsWaiting.value(0)->remove(0);
tstellanova's avatar
tstellanova committed
422

423 424 425
            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);
tstellanova's avatar
tstellanova committed
426 427 428 429
            }
        }
    }

430

tstellanova's avatar
tstellanova committed
431
    // Mark this parameter as received in read list
432 433
    compMissingReads->remove(paramId);

tstellanova's avatar
tstellanova committed
434 435 436

    bool justWritten = false;
    bool writeMismatch = false;
437

tstellanova's avatar
tstellanova committed
438
    // Mark this parameter as received in write ACK list
439 440
    QMap<QString, QVariant>* compMissingWrites = writesWaiting.value(compId);
    if (!compMissingWrites) {
441
        //we sometimes send a write request on compId 0 and get a response on a nonzero compId eg 50
442
        compMissingWrites = writesWaiting.value(0);
443
    }
444
    if (compMissingWrites && compMissingWrites->contains(paramName)) {
tstellanova's avatar
tstellanova committed
445
        justWritten = true;
446
        if (compMissingWrites->value(paramName) != value) {
tstellanova's avatar
tstellanova committed
447 448
            writeMismatch = true;
        }
449
        compMissingWrites->remove(paramName);
tstellanova's avatar
tstellanova committed
450 451 452
    }


453
    if (justWritten) {
454
        int waitingWritesCount = compMissingWrites->count();
455
        if (!writeMismatch) {
456 457 458 459 460 461
            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));
462 463 464 465
                if (persistParamsAfterSend) {
                    writeParamsToPersistentStorage();
                    persistParamsAfterSend = false;
                }
466 467 468 469
            }
        }
        else  {
            // Mismatch, tell user
470
            setParameterStatusMsg(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(paramName).arg(compMissingWrites->value(paramName).toDouble()).arg(value.toDouble()),
471 472
                                  ParamCommsStatusLevel_Warning);
        }
tstellanova's avatar
tstellanova committed
473 474
    }
    else {
475 476 477
        int waitingReadsCount = compMissingReads->count();

        if (0 == waitingReadsCount) {
tstellanova's avatar
tstellanova committed
478 479 480 481 482 483
            // Transmission done
            QTime time = QTime::currentTime();
            QString timeString = time.toString();
            setParameterStatusMsg(tr("All received. (updated at %1)").arg(timeString));
        }
        else {
484
            // Waiting to receive more
485
            QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' '));
486 487
            setParameterStatusMsg(tr("OK: %1 %2 (%3/%4)").arg(paramName).arg(val).arg(paramCount-waitingReadsCount).arg(paramCount),
                                  ParamCommsStatusLevel_OK);
tstellanova's avatar
tstellanova committed
488 489 490
        }
    }

491 492 493
    updateSilenceTimer();


tstellanova's avatar
tstellanova committed
494 495 496 497 498 499 500
}


void UASParameterCommsMgr::writeParamsToPersistentStorage()
{
    if (mav) {
        mav->writeParametersToStorage(); //TODO track timeout, retransmit etc?
501
        persistParamsAfterSend = false; //done
tstellanova's avatar
tstellanova committed
502 503 504 505
    }
}


506
void UASParameterCommsMgr::sendPendingParameters(bool copyToPersistent, bool forceSend)
tstellanova's avatar
tstellanova committed
507
{
508 509
    persistParamsAfterSend |= copyToPersistent;

tstellanova's avatar
tstellanova committed
510 511
    // Iterate through all components, through all pending parameters and send them to UAS
    int parametersSent = 0;
512
    QMap<int, QMap<QString, QVariant>*>* changedValues = paramDataModel->getAllPendingParams();
tstellanova's avatar
tstellanova committed
513
    QMap<int, QMap<QString, QVariant>*>::iterator i;
514
    for (i = changedValues->begin(); i != changedValues->end(); ++i) {
tstellanova's avatar
tstellanova committed
515
        // Iterate through the parameters of the component
tstellanova's avatar
tstellanova committed
516 517 518 519 520 521
        int compId = i.key();
        QMap<QString, QVariant>* paramList = i.value();
        QMap<QString, QVariant>::iterator j;
        setParameterStatusMsg(tr("%1 pending params for component %2").arg(paramList->count()).arg(compId));

        for (j = paramList->begin(); j != paramList->end(); ++j) {
522
            setParameter(compId, j.key(), j.value(), forceSend);
tstellanova's avatar
tstellanova committed
523
            parametersSent++;
tstellanova's avatar
tstellanova committed
524 525 526 527
        }
    }

    // Change transmission status if necessary
528
    if (0 == parametersSent) {
tstellanova's avatar
tstellanova committed
529
        setParameterStatusMsg(tr("No transmission: No changed values."),ParamCommsStatusLevel_Warning);
530 531
    }
    else {
tstellanova's avatar
tstellanova committed
532
        setParameterStatusMsg(tr("Transmitting %1 parameters.").arg(parametersSent));
533
        qDebug() << "Pending parameters now:" << paramDataModel->countPendingParams();
tstellanova's avatar
tstellanova committed
534
    }
535 536 537


    updateSilenceTimer();
tstellanova's avatar
tstellanova committed
538 539
}

540 541
UASParameterCommsMgr::~UASParameterCommsMgr()
{
542
    silenceTimer.stop();
543 544 545 546 547

    QString ptrStr;
    ptrStr.sprintf("%8p", this);
    qDebug() <<  "UASParameterCommsMgr destructor: " << ptrStr ;

548 549
}