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

tstellanova's avatar
tstellanova committed
3
#include <QSettings>
Don Gagne's avatar
Don Gagne committed
4
#include <QDebug>
tstellanova's avatar
tstellanova committed
5

6
#include "QGCUASParamManagerInterface.h"
7
#include "UASInterface.h"
8 9
#include "MAVLinkProtocol.h"
#include "MainWindow.h"
10

11 12
#define RC_CAL_CHAN_MAX 8

13 14
Q_LOGGING_CATEGORY(UASParameterCommsMgrLog, "UASParameterCommsMgrLog")

15
UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent) :
tstellanova's avatar
tstellanova committed
16
    QObject(parent),
tstellanova's avatar
tstellanova committed
17
    lastReceiveTime(0),
18
    mav(NULL),
19
    maxSilenceTimeout(30000),
20
    paramDataModel(NULL),
21 22 23
    retransmitBurstLimit(5),
    silenceTimeout(1000),
    transmissionListMode(false)
24
{
25 26 27
    // We signal to ourselves to start/stop timer on our own thread
    connect(this, SIGNAL(_startSilenceTimer(void)), this, SLOT(_startSilenceTimerOnThisThread(void)));
    connect(this, SIGNAL(_stopSilenceTimer(void)), this, SLOT(_stopSilenceTimerOnThisThread(void)));
28 29 30 31 32
}

UASParameterCommsMgr* UASParameterCommsMgr::initWithUAS(UASInterface* uas)
{
    mav = uas;
33
    paramDataModel = mav->getParamManager()->dataModel();
tstellanova's avatar
tstellanova committed
34 35 36 37 38
    loadParamCommsSettings();

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

    // Sending params to the UAS
41
    connect(this, SIGNAL(commitPendingParameter(int,QString,QVariant)),
42
            mav, SLOT(setParameter(int,QString,QVariant)));
43

44
    // Received parameter updates from UAS
45 46
    connect(mav, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)),
            this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant)));
47

48 49
    connect(&silenceTimer, SIGNAL(timeout()),
            this,SLOT(silenceTimerExpired()));
tstellanova's avatar
tstellanova committed
50

51
    return this;
52 53 54
}


tstellanova's avatar
tstellanova committed
55

56

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

tstellanova's avatar
tstellanova committed
69 70 71
    settings.endGroup();
}

72 73
void UASParameterCommsMgr::_sendParamRequestListMsg(void)
{
74
    MAVLinkProtocol* mavlink = MAVLinkProtocol::instance();
75 76 77 78 79 80
    Q_ASSERT(mavlink);
    
    mavlink_message_t msg;
    mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, mav->getUASID(), MAV_COMP_ID_ALL);
    mav->sendMessage(msg);
}
81

82 83 84 85 86 87 88 89 90
/**
 * Send a request to deliver the list of onboard parameters
 * from the MAV.
 */
void UASParameterCommsMgr::requestParameterList()
{
    if (!mav) {
        return;
    }
91
    
92

93
    if (!transmissionListMode) {
94
        qCDebug(UASParameterCommsMgrLog) << "Requesting full parameter list";
95 96 97
        transmissionListMode = true;//TODO eliminate?
        //we use (compId 0, paramId 0) as  indicating all params for the system
        markReadParamWaiting(0,0);
98 99 100
        
        _sendParamRequestListMsg();
        
101
        updateSilenceTimer();
102 103
    }
    else {
104
        qCDebug(UASParameterCommsMgrLog) << "Ignoring requestParameterList because we're receiving params list";
105 106 107 108 109
    }

}


110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129
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);
}

130 131 132 133
/*
 Empty read retransmission list
 Empty write retransmission list
*/
tstellanova's avatar
tstellanova committed
134
void UASParameterCommsMgr::clearRetransmissionLists(int& missingReadCount, int& missingWriteCount )
135
{
136
    qCDebug(UASParameterCommsMgrLog) << "Clearing re-transmission lists";
137

tstellanova's avatar
tstellanova committed
138
    missingReadCount = 0;
139 140 141 142
    QList<int> compIds = readsWaiting.keys();
    foreach (int compId, compIds) {
        missingReadCount += readsWaiting.value(compId)->count();
        readsWaiting.value(compId)->clear();
143 144
    }

tstellanova's avatar
tstellanova committed
145
    missingWriteCount = 0;
146 147 148 149
    compIds = writesWaiting.keys();
    foreach (int compId, compIds) {
        missingWriteCount += writesWaiting.value(compId)->count();
        writesWaiting.value(compId)->clear();
150 151 152 153 154
    }

}


155
void UASParameterCommsMgr::emitPendingParameterCommit(int compId, const QString& key, QVariant& value)
156 157 158 159 160 161 162
{
    int paramType = (int)value.type();
    switch (paramType)
    {
    case QVariant::Char:
    {
        QVariant fixedValue(QChar((unsigned char)value.toInt()));
163
        emit commitPendingParameter(compId, key, fixedValue);
164 165 166 167 168
    }
        break;
    case QVariant::Int:
    {
        QVariant fixedValue(value.toInt());
169
        emit commitPendingParameter(compId, key, fixedValue);
170 171 172 173 174
    }
        break;
    case QVariant::UInt:
    {
        QVariant fixedValue(value.toUInt());
175
        emit commitPendingParameter(compId, key, fixedValue);
176 177 178 179 180
    }
        break;
    case QMetaType::Float:
    {
        QVariant fixedValue(value.toFloat());
181
        emit commitPendingParameter(compId, key, fixedValue);
182 183 184
    }
        break;
    default:
185
        qCritical() << "ABORTED PARAM SEND, INVALID QVARIANT TYPE" << paramType;
186 187 188
        return;
    }

189
    setParameterStatusMsg(tr("Writing %1: %2 for comp. %3").arg(key).arg(value.toDouble()).arg(compId));
190 191 192 193 194 195 196 197 198

}


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

199
    // Re-request at maximum retransmitBurstLimit parameters at once
200 201
    // to prevent link flooding'
    int requestedReadCount = 0;
202
    compIds = readsWaiting.keys();
203 204
    foreach (compId, compIds) {
        // Request n parameters from this component (at maximum)
205 206
        QSet<int>* missingReadParams = readsWaiting.value(compId, NULL);
        qDebug() << "compId " << compId << "readsWaiting:" << missingReadParams->count();
207
        foreach (int paramId, *missingReadParams) {
208
            if (0 == paramId && 0 == compId) {
209
                _sendParamRequestListMsg();
210 211 212 213
                //don't request any other params individually for this component
                break;
            }
            if (requestedReadCount < retransmitBurstLimit) {
214
                //qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << paramId << "FROM COMPONENT #" << compId;
215 216 217 218 219
                emit parameterUpdateRequestedById(compId, paramId);
                setParameterStatusMsg(tr("Requested retransmission of #%1").arg(paramId+1));
                requestedReadCount++;
            }
            else {
220
                qCDebug(UASParameterCommsMgrLog) << "Throttling read retransmit requests at" << requestedReadCount;
221 222 223 224 225
                break;
            }
        }
    }

226
    // Re-request at maximum retransmitBurstLimit parameters at once
227 228
    // to prevent write-request link flooding
    int requestedWriteCount = 0;
229
    compIds = writesWaiting.keys();
230
    foreach (compId, compIds) {
231 232 233
        QMap <QString, QVariant>* missingWriteParams = writesWaiting.value(compId);
        foreach (QString key, missingWriteParams->keys()) {
            if (requestedWriteCount < retransmitBurstLimit) {
234
                // Re-request write operation
235
                QVariant value = missingWriteParams->value(key);
236
                emitPendingParameterCommit(compId, key, value);
237 238 239
                requestedWriteCount++;
            }
            else {
240
                qCDebug(UASParameterCommsMgrLog) << "Throttling write retransmit requests at" << requestedWriteCount;
241 242 243 244 245
                break;
            }
        }
    }

246 247
    updateSilenceTimer();

248 249
}

250 251 252
void UASParameterCommsMgr::resetAfterListReceive()
{
    transmissionListMode = false;
253
    knownParamListSize.clear();
254 255
}

256
void UASParameterCommsMgr::silenceTimerExpired()
257
{
258
    quint64 curTime = QGC::groundTimeMilliseconds();
259
    int elapsed = (int)(curTime - lastSilenceTimerReset);
260
    qCDebug(UASParameterCommsMgrLog) << "silenceTimerExpired elapsed:" << elapsed;
261

262
    if (elapsed < silenceTimeout) {
263
        //reset the guard timer: it fired prematurely
264
        updateSilenceTimer();
265 266
        return;
    }
267

268 269
    int totalElapsed = (int)(curTime - lastReceiveTime);
    if (totalElapsed > maxSilenceTimeout) {
270
        qCDebug(UASParameterCommsMgrLog) << "maxSilenceTimeout exceeded: " << totalElapsed;
271 272
        int missingReads, missingWrites;
        clearRetransmissionLists(missingReads,missingWrites);
273
        emit _stopSilenceTimer(); // Stop timer on our thread;
tstellanova's avatar
tstellanova committed
274
        lastReceiveTime = 0;
275
        lastSilenceTimerReset = curTime;
276
        setParameterStatusMsg(tr("TIMEOUT: Abandoning %1 reads %2 writes after %3 seconds").arg(missingReads).arg(missingWrites).arg(totalElapsed/1000));
277 278
    }
    else {
279
        resendReadWriteRequests();
280 281 282 283
    }
}


284
void UASParameterCommsMgr::requestParameterUpdate(int compId, const QString& paramName)
285 286
{
    if (mav) {
287 288 289
        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
290 291 292
    }
}

293 294 295 296 297 298 299 300 301 302 303
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

304 305

        int defCompId = paramDataModel->getDefaultComponentId();
306 307
        for (unsigned int i = 1; i < (RC_CAL_CHAN_MAX+1); ++i)  {
            qDebug() << "Request RC " << i;
308 309 310 311
            requestParameterUpdate(defCompId, minTpl.arg(i));
            requestParameterUpdate(defCompId, trimTpl.arg(i));
            requestParameterUpdate(defCompId, maxTpl.arg(i));
            requestParameterUpdate(defCompId, revTpl.arg(i));
312 313 314 315
            QGC::SLEEP::usleep(5000);
        }
    }
    else {
316
        qCDebug(UASParameterCommsMgrLog) << "Ignoring requestRcCalibrationParamsUpdate because we're receiving params list";
317 318 319 320
    }
}


321 322 323 324 325
/**
 * @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
 */
326
void UASParameterCommsMgr::setParameter(int compId, QString paramName, QVariant value, bool forceSend)
327
{
328
    if (paramName.isEmpty()) {
329 330 331 332 333
        return;
    }

    double dblValue = value.toDouble();

334 335
    if (paramDataModel->isValueLessThanParamMin(paramName,dblValue)) {
        setParameterStatusMsg(tr("REJ. %1, %2 < min").arg(paramName).arg(dblValue),
tstellanova's avatar
tstellanova committed
336 337
                              ParamCommsStatusLevel_Error
                              );
338 339
        return;
    }
340 341
    if (paramDataModel->isValueGreaterThanParamMax(paramName,dblValue)) {
        setParameterStatusMsg(tr("REJ. %1, %2 > max").arg(paramName).arg(dblValue),
tstellanova's avatar
tstellanova committed
342 343
                              ParamCommsStatusLevel_Error
                              );
344 345
        return;
    }
346

347 348 349 350 351 352 353 354 355 356
	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;
		}
	}
357

358 359 360
    emitPendingParameterCommit(compId, paramName, value);

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

362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380
    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();
381 382 383
    }


384 385
    if (missReadCount > 0 || missWriteCount > 0) {
        lastSilenceTimerReset = QGC::groundTimeMilliseconds();
tstellanova's avatar
tstellanova committed
386 387 388
        if (0 == lastReceiveTime) {
            lastReceiveTime = lastSilenceTimerReset;
        }
389 390
        // We signal this to ourselves so timer is started on the right thread
        emit _startSilenceTimer();
391
    }
392
    else {
393
        //all parameters have been received, broadcast to UI
394
        qCDebug(UASParameterCommsMgrLog) << "emitting parameterListUpToDate";
395 396
        emit parameterListUpToDate();
        resetAfterListReceive();
397
        emit _stopSilenceTimer(); // Stop timer on our thread;
tstellanova's avatar
tstellanova committed
398
        lastReceiveTime = 0;
399 400
    }

401 402


403 404
}

405

tstellanova's avatar
tstellanova committed
406
void UASParameterCommsMgr::setParameterStatusMsg(const QString& msg, ParamCommsStatusLevel_t level)
407
{
Lorenz Meier's avatar
Lorenz Meier committed
408
    //qDebug() << "parameterStatusMsg: " << msg;
tstellanova's avatar
tstellanova committed
409
    emit parameterStatusMsgUpdated(msg,level);
410
}
tstellanova's avatar
tstellanova committed
411 412 413

void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int paramCount, int paramId, QString paramName, QVariant value)
{
414
    qCDebug(UASParameterCommsMgrLog) << "Received parameter update for:" << paramName << "count" << paramCount << "index" << paramId << "value" << value;
415

416
    Q_UNUSED(uas); //this object is assigned to one UAS only
417
    lastReceiveTime = QGC::groundTimeMilliseconds();
Lorenz Meier's avatar
Lorenz Meier committed
418
    // qDebug() << "compId" << compId << "receivedParameterUpdate:" << paramName;
419

420
    //notify the data model that we have an updated param
421
    paramDataModel->handleParamUpdate(compId,paramName,value);
tstellanova's avatar
tstellanova committed
422 423


424 425 426 427 428 429 430
    // 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>());
    }
431

432
    QSet<int>* compMissingReads =  readsWaiting.value(compId);
tstellanova's avatar
tstellanova committed
433 434
    // List mode is different from single parameter transfers
    if (transmissionListMode) {
435 436
        // Only accept the list size once on the first packet from each component
        if (!knownParamListSize.contains(compId)) {
tstellanova's avatar
tstellanova committed
437
            // Mark list size as known
438
            knownParamListSize.insert(compId,paramCount);
tstellanova's avatar
tstellanova committed
439

440 441
            //remove our placeholder read request for all params
            readsWaiting.value(0)->remove(0);
tstellanova's avatar
tstellanova committed
442

443
            qCDebug(UASParameterCommsMgrLog) << "receivedParameterUpdate: Mark all parameters as missing: " << paramCount;
444 445
            for (int i = 1; i < paramCount; ++i) { //param Id 0 is  "all parameters" and not valid
                compMissingReads->insert(i);
tstellanova's avatar
tstellanova committed
446 447 448 449
            }
        }
    }

450

tstellanova's avatar
tstellanova committed
451
    // Mark this parameter as received in read list
452 453
    compMissingReads->remove(paramId);

tstellanova's avatar
tstellanova committed
454 455 456

    bool justWritten = false;
    bool writeMismatch = false;
457

tstellanova's avatar
tstellanova committed
458
    // Mark this parameter as received in write ACK list
459 460
    QMap<QString, QVariant>* compMissingWrites = writesWaiting.value(compId);
    if (!compMissingWrites) {
461
        //we sometimes send a write request on compId 0 and get a response on a nonzero compId eg 50
462
        compMissingWrites = writesWaiting.value(0);
463
    }
464
    if (compMissingWrites && compMissingWrites->contains(paramName)) {
tstellanova's avatar
tstellanova committed
465
        justWritten = true;
466
        if (compMissingWrites->value(paramName) != value) {
tstellanova's avatar
tstellanova committed
467 468
            writeMismatch = true;
        }
469
        compMissingWrites->remove(paramName);
tstellanova's avatar
tstellanova committed
470 471 472
    }


473
    if (justWritten) {
474
        int waitingWritesCount = compMissingWrites->count();
475
        if (!writeMismatch) {
476 477 478 479 480 481
            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));
482 483 484 485
                if (persistParamsAfterSend) {
                    writeParamsToPersistentStorage();
                    persistParamsAfterSend = false;
                }
486 487 488 489
            }
        }
        else  {
            // Mismatch, tell user
490
            setParameterStatusMsg(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(paramName).arg(compMissingWrites->value(paramName).toDouble()).arg(value.toDouble()),
491 492
                                  ParamCommsStatusLevel_Warning);
        }
tstellanova's avatar
tstellanova committed
493 494
    }
    else {
495 496 497
        int waitingReadsCount = compMissingReads->count();

        if (0 == waitingReadsCount) {
tstellanova's avatar
tstellanova committed
498 499 500 501 502 503
            // Transmission done
            QTime time = QTime::currentTime();
            QString timeString = time.toString();
            setParameterStatusMsg(tr("All received. (updated at %1)").arg(timeString));
        }
        else {
504
            // Waiting to receive more
505
            QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' '));
506 507
            setParameterStatusMsg(tr("OK: %1 %2 (%3/%4)").arg(paramName).arg(val).arg(paramCount-waitingReadsCount).arg(paramCount),
                                  ParamCommsStatusLevel_OK);
tstellanova's avatar
tstellanova committed
508 509 510
        }
    }

511 512 513
    updateSilenceTimer();


tstellanova's avatar
tstellanova committed
514 515 516 517 518 519 520
}


void UASParameterCommsMgr::writeParamsToPersistentStorage()
{
    if (mav) {
        mav->writeParametersToStorage(); //TODO track timeout, retransmit etc?
521
        persistParamsAfterSend = false; //done
tstellanova's avatar
tstellanova committed
522 523 524 525
    }
}


526
void UASParameterCommsMgr::sendPendingParameters(bool copyToPersistent, bool forceSend)
tstellanova's avatar
tstellanova committed
527
{
528 529
    persistParamsAfterSend |= copyToPersistent;

tstellanova's avatar
tstellanova committed
530 531
    // Iterate through all components, through all pending parameters and send them to UAS
    int parametersSent = 0;
532
    QMap<int, QMap<QString, QVariant>*>* changedValues = paramDataModel->getAllPendingParams();
tstellanova's avatar
tstellanova committed
533
    QMap<int, QMap<QString, QVariant>*>::iterator i;
534
    for (i = changedValues->begin(); i != changedValues->end(); ++i) {
tstellanova's avatar
tstellanova committed
535
        // Iterate through the parameters of the component
tstellanova's avatar
tstellanova committed
536 537 538 539 540 541
        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) {
542
            setParameter(compId, j.key(), j.value(), forceSend);
tstellanova's avatar
tstellanova committed
543
            parametersSent++;
tstellanova's avatar
tstellanova committed
544 545 546 547
        }
    }

    // Change transmission status if necessary
548
    if (0 == parametersSent) {
tstellanova's avatar
tstellanova committed
549
        setParameterStatusMsg(tr("No transmission: No changed values."),ParamCommsStatusLevel_Warning);
550 551
    }
    else {
tstellanova's avatar
tstellanova committed
552
        setParameterStatusMsg(tr("Transmitting %1 parameters.").arg(parametersSent));
553
        qCDebug(UASParameterCommsMgrLog) << "Pending parameters now:" << paramDataModel->countPendingParams();
tstellanova's avatar
tstellanova committed
554
    }
555 556 557


    updateSilenceTimer();
tstellanova's avatar
tstellanova committed
558 559
}

560 561
UASParameterCommsMgr::~UASParameterCommsMgr()
{
562
    silenceTimer.stop();
563 564 565

    QString ptrStr;
    ptrStr.sprintf("%8p", this);
566
    qCDebug(UASParameterCommsMgrLog) <<  "UASParameterCommsMgr destructor: " << ptrStr ;
567

568 569
}

570 571 572 573 574 575 576 577 578
void UASParameterCommsMgr::_startSilenceTimerOnThisThread(void)
{
    silenceTimer.start(silenceTimeout);
}

void UASParameterCommsMgr::_stopSilenceTimerOnThisThread(void)
{
    silenceTimer.stop();
}