MAVLinkLogManager.cc 32.3 KB
Newer Older
Gus Grubba's avatar
Gus Grubba committed
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

Gus Grubba's avatar
Gus Grubba committed
10
#include "MAVLinkLogManager.h"
Gus Grubba's avatar
Gus Grubba committed
11
#include "QGCApplication.h"
12 13
#include "SettingsManager.h"

Gus Grubba's avatar
Gus Grubba committed
14 15 16 17 18 19 20 21 22 23
#include <QQmlContext>
#include <QQmlProperty>
#include <QQmlEngine>
#include <QtQml>
#include <QSettings>
#include <QHttpPart>
#include <QNetworkReply>
#include <QFile>
#include <QFileInfo>

Gus Grubba's avatar
Gus Grubba committed
24
QGC_LOGGING_CATEGORY(MAVLinkLogManagerLog, "MAVLinkLogManagerLog")
Gus Grubba's avatar
Gus Grubba committed
25

26 27 28
static const char* kMAVLinkLogGroup         = "MAVLinkLogGroup";
static const char* kEmailAddressKey         = "Email";
static const char* kDescriptionsKey         = "Description";
29
static const char* kDefaultDescr            = "QGroundControl Session";
30
static const char* kPx4URLKey               = "LogURL";
31
static const char* kDefaultPx4URL           = "https://logs.px4.io/upload";
32 33 34
static const char* kEnableAutoUploadKey     = "EnableAutoUpload";
static const char* kEnableAutoStartKey      = "EnableAutoStart";
static const char* kEnableDeletetKey        = "EnableDelete";
Gus Grubba's avatar
Gus Grubba committed
35
static const char* kSidecarExtension        = ".uploaded";
36 37 38 39 40 41
static const char* kVideoURLKey             = "VideoURL";
static const char* kWindSpeedKey            = "WindSpeed";
static const char* kRateKey                 = "RateKey";
static const char* kPublicLogKey            = "PublicLog";
static const char* kFeedback                = "feedback";
static const char* kVideoURL                = "videoUrl";
Gus Grubba's avatar
Gus Grubba committed
42 43

//-----------------------------------------------------------------------------
Gus Grubba's avatar
Gus Grubba committed
44
MAVLinkLogFiles::MAVLinkLogFiles(MAVLinkLogManager* manager, const QString& filePath, bool newFile)
Gus Grubba's avatar
Gus Grubba committed
45 46 47 48 49
    : _manager(manager)
    , _size(0)
    , _selected(false)
    , _uploading(false)
    , _progress(0)
Gus Grubba's avatar
Gus Grubba committed
50
    , _writing(false)
Gus Grubba's avatar
Gus Grubba committed
51
    , _uploaded(false)
Gus Grubba's avatar
Gus Grubba committed
52 53 54
{
    QFileInfo fi(filePath);
    _name = fi.baseName();
Gus Grubba's avatar
Gus Grubba committed
55 56 57
    if(!newFile) {
        _size = (quint32)fi.size();
        QString sideCar = filePath;
58
        sideCar.replace(manager->logExtension(), kSidecarExtension);
Gus Grubba's avatar
Gus Grubba committed
59 60 61
        QFileInfo sc(sideCar);
        _uploaded = sc.exists();
    }
Gus Grubba's avatar
Gus Grubba committed
62 63
}

64 65
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
66
MAVLinkLogFiles::setSize(quint32 size)
67 68 69 70 71
{
    _size = size;
    emit sizeChanged();
}

Gus Grubba's avatar
Gus Grubba committed
72 73
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
74
MAVLinkLogFiles::setSelected(bool selected)
Gus Grubba's avatar
Gus Grubba committed
75 76 77 78 79 80 81 82
{
    _selected = selected;
    emit selectedChanged();
    emit _manager->selectedCountChanged();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
83
MAVLinkLogFiles::setUploading(bool uploading)
Gus Grubba's avatar
Gus Grubba committed
84 85 86 87 88 89 90
{
    _uploading = uploading;
    emit uploadingChanged();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
91
MAVLinkLogFiles::setProgress(qreal progress)
Gus Grubba's avatar
Gus Grubba committed
92 93 94 95 96
{
    _progress = progress;
    emit progressChanged();
}

97 98
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
99
MAVLinkLogFiles::setWriting(bool writing)
100 101 102 103 104
{
    _writing = writing;
    emit writingChanged();
}

Gus Grubba's avatar
Gus Grubba committed
105 106
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
107
MAVLinkLogFiles::setUploaded(bool uploaded)
Gus Grubba's avatar
Gus Grubba committed
108 109 110 111 112
{
    _uploaded = uploaded;
    emit uploadedChanged();
}

Gus Grubba's avatar
Gus Grubba committed
113 114
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
Gus Grubba's avatar
Gus Grubba committed
115
MAVLinkLogProcessor::MAVLinkLogProcessor()
116
    : _fd(nullptr)
Gus Grubba's avatar
Gus Grubba committed
117 118 119 120 121
    , _written(0)
    , _sequence(-1)
    , _numDrops(0)
    , _gotHeader(false)
    , _error(false)
122
    , _record(nullptr)
Gus Grubba's avatar
Gus Grubba committed
123
{
Gus Grubba's avatar
Gus Grubba committed
124 125 126
}

//-----------------------------------------------------------------------------
Gus Grubba's avatar
Gus Grubba committed
127
MAVLinkLogProcessor::~MAVLinkLogProcessor()
Gus Grubba's avatar
Gus Grubba committed
128 129 130 131 132 133
{
    close();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
134
MAVLinkLogProcessor::close()
Gus Grubba's avatar
Gus Grubba committed
135 136 137
{
    if(_fd) {
        fclose(_fd);
138
        _fd = nullptr;
Gus Grubba's avatar
Gus Grubba committed
139 140 141
    }
}

Gus Grubba's avatar
Gus Grubba committed
142 143
//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
144
MAVLinkLogProcessor::valid()
Gus Grubba's avatar
Gus Grubba committed
145
{
146
    return (_fd != nullptr) && (_record != nullptr);
Gus Grubba's avatar
Gus Grubba committed
147 148 149 150
}

//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
151
MAVLinkLogProcessor::create(MAVLinkLogManager* manager, const QString path, uint8_t id)
Gus Grubba's avatar
Gus Grubba committed
152 153
{
    _fileName.sprintf("%s/%03d-%s%s",
DonLakeFlyer's avatar
DonLakeFlyer committed
154 155
                      path.toLatin1().data(),
                      id,
156 157 158
                      QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz").toLocal8Bit().data(),
                      manager->logExtension().toLocal8Bit().data());
    _fd = fopen(_fileName.toLocal8Bit().data(), "wb");
Gus Grubba's avatar
Gus Grubba committed
159
    if(_fd) {
Gus Grubba's avatar
Gus Grubba committed
160
        _record = new MAVLinkLogFiles(manager, _fileName, true);
Gus Grubba's avatar
Gus Grubba committed
161 162 163 164 165 166 167 168 169
        _record->setWriting(true);
        _sequence = -1;
        return true;
    }
    return false;
}

//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
170
MAVLinkLogProcessor::_checkSequence(uint16_t seq, int& num_drops)
Gus Grubba's avatar
Gus Grubba committed
171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203
{
    num_drops = 0;
    //-- Check if a sequence is newer than the one previously received and if
    //   there were dropped messages between the last one and this.
    if(_sequence == -1) {
        _sequence = seq;
        return true;
    }
    if((uint16_t)_sequence == seq) {
        return false;
    }
    if(seq > (uint16_t)_sequence) {
        // Account for wrap-arounds, sequence is 2 bytes
        if((seq - _sequence) > (1 << 15)) { // Assume reordered
            return false;
        }
        num_drops = seq - _sequence - 1;
        _numDrops += num_drops;
        _sequence = seq;
        return true;
    } else {
        if((_sequence - seq) > (1 << 15)) {
            num_drops = (1 << 16) - _sequence - 1 + seq;
            _numDrops += num_drops;
            _sequence = seq;
            return true;
        }
        return false;
    }
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
204
MAVLinkLogProcessor::_writeData(void* data, int len)
Gus Grubba's avatar
Gus Grubba committed
205 206 207 208 209 210 211 212 213
{
    if(!_error) {
        _error = fwrite(data, 1, len, _fd) != (size_t)len;
        if(!_error) {
            _written += len;
            if(_record) {
                _record->setSize(_written);
            }
        } else {
Gus Grubba's avatar
Gus Grubba committed
214
            qCDebug(MAVLinkLogManagerLog) << "File IO error:" << len << "bytes into" << _fileName;
Gus Grubba's avatar
Gus Grubba committed
215 216 217 218 219 220
        }
    }
}

//-----------------------------------------------------------------------------
QByteArray
Gus Grubba's avatar
Gus Grubba committed
221
MAVLinkLogProcessor::_writeUlogMessage(QByteArray& data)
Gus Grubba's avatar
Gus Grubba committed
222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237
{
    //-- Write ulog data w/o integrity checking, assuming data starts with a
    //   valid ulog message. returns the remaining data at the end.
    while(data.length() > 2) {
        uint8_t* ptr = (uint8_t*)data.data();
        int message_length = ptr[0] + (ptr[1] * 256) + 3; // 3 = ULog msg header
        if(message_length > data.length())
            break;
        _writeData(data.data(), message_length);
        data.remove(0, message_length);
    }
    return data;
}

//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
238
MAVLinkLogProcessor::processStreamData(uint16_t sequence, uint8_t first_message, QByteArray data)
Gus Grubba's avatar
Gus Grubba committed
239 240 241 242 243 244 245 246
{
    int num_drops = 0;
    _error = false;
    while(_checkSequence(sequence, num_drops)) {
        //-- The first 16 bytes need special treatment (this sounds awfully brittle)
        if(!_gotHeader) {
            if(data.size() < 16) {
                //-- Shouldn't happen but if it does, we might as well close shop.
247
                qCWarning(MAVLinkLogManagerLog) << "Corrupt log header. Canceling log download.";
Gus Grubba's avatar
Gus Grubba committed
248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267
                return false;
            }
            //-- Write header
            _writeData(data.data(), 16);
            data.remove(0, 16);
            _gotHeader = true;
            // What about data start offset now that we removed 16 bytes off the start?
        }
        if(_gotHeader && num_drops > 0) {
            if(num_drops > 25) num_drops = 25;
            //-- Hocus Pocus
            //   Write a dropout message. We don't really know the actual duration,
            //   so just use the number of drops * 10 ms
            uint8_t bogus[] = {2, 0, 79, 0, 0};
            bogus[3] = num_drops * 10;
            _writeData(bogus, sizeof(bogus));
        }
        if(num_drops > 0) {
            _writeUlogMessage(_ulogMessage);
            _ulogMessage.clear();
Patrick José Pereira's avatar
Patrick José Pereira committed
268
            //-- If no useful information in this message. Drop it.
Gus Grubba's avatar
Gus Grubba committed
269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296
            if(first_message == 255) {
                break;
            }
            if(first_message > 0) {
                data.remove(0, first_message);
                first_message = 0;
            }
        }
        if(first_message == 255 && _ulogMessage.length() > 0) {
            _ulogMessage.append(data);
            break;
        }
        if(_ulogMessage.length()) {
            _writeData(_ulogMessage.data(), _ulogMessage.length());
            if(first_message) {
                _writeData(data.left(first_message).data(), first_message);
            }
            _ulogMessage.clear();
        }
        if(first_message) {
            data.remove(0, first_message);
        }
        _ulogMessage = _writeUlogMessage(data);
        break;
    }
    return !_error;
}

Gus Grubba's avatar
Gus Grubba committed
297
//-----------------------------------------------------------------------------
Gus Grubba's avatar
Gus Grubba committed
298
//-----------------------------------------------------------------------------
299 300
MAVLinkLogManager::MAVLinkLogManager(QGCApplication* app, QGCToolbox* toolbox)
    : QGCTool(app, toolbox)
301
    , _enableAutoUpload(true)
302
    , _enableAutoStart(false)
303 304 305
    , _nam(nullptr)
    , _currentLogfile(nullptr)
    , _vehicle(nullptr)
306
    , _logRunning(false)
Gus Grubba's avatar
Gus Grubba committed
307
    , _loggingDisabled(false)
308
    , _logProcessor(nullptr)
309
    , _deleteAfterUpload(false)
310 311
    , _windSpeed(-1)
    , _publicLog(false)
312
    , _logginDenied(false)
Gus Grubba's avatar
Gus Grubba committed
313 314 315
{
    //-- Get saved settings
    QSettings settings;
316
    settings.beginGroup(kMAVLinkLogGroup);
Gus Grubba's avatar
Gus Grubba committed
317 318 319
    setEmailAddress(settings.value(kEmailAddressKey, QString()).toString());
    setDescription(settings.value(kDescriptionsKey, QString(kDefaultDescr)).toString());
    setUploadURL(settings.value(kPx4URLKey, QString(kDefaultPx4URL)).toString());
320
    setVideoURL(settings.value(kVideoURLKey, QString()).toString());
321
    setEnableAutoUpload(settings.value(kEnableAutoUploadKey, true).toBool());
322
    setEnableAutoStart(settings.value(kEnableAutoStartKey, false).toBool());
323
    setDeleteAfterUpload(settings.value(kEnableDeletetKey, false).toBool());
324 325 326
    setWindSpeed(settings.value(kWindSpeedKey, -1).toInt());
    setRating(settings.value(kRateKey, "notset").toString());
    setPublicLog(settings.value(kPublicLogKey, true).toBool());
Gus Grubba's avatar
Gus Grubba committed
327 328 329
}

//-----------------------------------------------------------------------------
Gus Grubba's avatar
Gus Grubba committed
330
MAVLinkLogManager::~MAVLinkLogManager()
Gus Grubba's avatar
Gus Grubba committed
331 332 333 334 335 336
{
    _logFiles.clear();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
337
MAVLinkLogManager::setToolbox(QGCToolbox* toolbox)
Gus Grubba's avatar
Gus Grubba committed
338
{
Gus Grubba's avatar
Gus Grubba committed
339 340
    QGCTool::setToolbox(toolbox);
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
Gus Grubba's avatar
Gus Grubba committed
341
    qmlRegisterUncreatableType<MAVLinkLogManager>("QGroundControl.MAVLinkLogManager", 1, 0, "MAVLinkLogManager", "Reference only");
342 343 344 345 346 347 348 349 350 351
    //-- Logging location
    _ulogExtension  = ".";
    _ulogExtension += qgcApp()->toolbox()->settingsManager()->appSettings()->logFileExtension;
    _logPath = qgcApp()->toolbox()->settingsManager()->appSettings()->logSavePath();
    if(!QDir(_logPath).exists()) {
        if(!QDir().mkpath(_logPath)) {
            qCWarning(MAVLinkLogManagerLog) << "Could not create MAVLink log download path:" << _logPath;
            _loggingDisabled = true;
        }
    }
Gus Grubba's avatar
Gus Grubba committed
352
    if(!_loggingDisabled) {
353 354 355 356 357 358 359 360
        //-- Load current list of logs
        QString filter = "*";
        filter += _ulogExtension;
        QDirIterator it(_logPath, QStringList() << filter, QDir::Files);
        while(it.hasNext()) {
            _insertNewLog(new MAVLinkLogFiles(this, it.next()));
        }
        qCDebug(MAVLinkLogManagerLog) << "MAVLink logs directory:" << _logPath;
Gus Grubba's avatar
Gus Grubba committed
361
        connect(toolbox->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &MAVLinkLogManager::_activeVehicleChanged);
Gus Grubba's avatar
Gus Grubba committed
362
    }
Gus Grubba's avatar
Gus Grubba committed
363 364 365 366
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
367
MAVLinkLogManager::setEmailAddress(QString email)
Gus Grubba's avatar
Gus Grubba committed
368 369 370
{
    _emailAddress = email;
    QSettings settings;
371
    settings.beginGroup(kMAVLinkLogGroup);
Gus Grubba's avatar
Gus Grubba committed
372 373 374 375 376 377
    settings.setValue(kEmailAddressKey, email);
    emit emailAddressChanged();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
378
MAVLinkLogManager::setDescription(QString description)
Gus Grubba's avatar
Gus Grubba committed
379 380 381
{
    _description = description;
    QSettings settings;
382
    settings.beginGroup(kMAVLinkLogGroup);
Gus Grubba's avatar
Gus Grubba committed
383 384 385 386 387 388
    settings.setValue(kDescriptionsKey, description);
    emit descriptionChanged();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
389
MAVLinkLogManager::setUploadURL(QString url)
Gus Grubba's avatar
Gus Grubba committed
390 391 392 393 394 395
{
    _uploadURL = url;
    if(_uploadURL.isEmpty()) {
        _uploadURL = kDefaultPx4URL;
    }
    QSettings settings;
396
    settings.beginGroup(kMAVLinkLogGroup);
Gus Grubba's avatar
Gus Grubba committed
397 398 399 400
    settings.setValue(kPx4URLKey, _uploadURL);
    emit uploadURLChanged();
}

401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419
//-----------------------------------------------------------------------------
void
MAVLinkLogManager::setFeedback(QString fb)
{
    _feedback = fb;
    emit feedbackChanged();
}

//-----------------------------------------------------------------------------
void
MAVLinkLogManager::setVideoURL(QString url)
{
    _videoURL = url;
    QSettings settings;
    settings.beginGroup(kMAVLinkLogGroup);
    settings.setValue(kVideoURLKey, url);
    emit videoURLChanged();
}

Gus Grubba's avatar
Gus Grubba committed
420 421
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
422
MAVLinkLogManager::setEnableAutoUpload(bool enable)
423 424 425
{
    _enableAutoUpload = enable;
    QSettings settings;
426
    settings.beginGroup(kMAVLinkLogGroup);
427 428 429 430 431 432
    settings.setValue(kEnableAutoUploadKey, enable);
    emit enableAutoUploadChanged();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
433
MAVLinkLogManager::setEnableAutoStart(bool enable)
Gus Grubba's avatar
Gus Grubba committed
434
{
435
    _enableAutoStart = enable;
Gus Grubba's avatar
Gus Grubba committed
436
    QSettings settings;
437
    settings.beginGroup(kMAVLinkLogGroup);
438 439
    settings.setValue(kEnableAutoStartKey, enable);
    emit enableAutoStartChanged();
Gus Grubba's avatar
Gus Grubba committed
440 441
}

442 443
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
444
MAVLinkLogManager::setDeleteAfterUpload(bool enable)
445 446 447
{
    _deleteAfterUpload = enable;
    QSettings settings;
448
    settings.beginGroup(kMAVLinkLogGroup);
449 450 451 452
    settings.setValue(kEnableDeletetKey, enable);
    emit deleteAfterUploadChanged();
}

453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485
//-----------------------------------------------------------------------------
void
MAVLinkLogManager::setWindSpeed(int speed)
{
    _windSpeed = speed;
    QSettings settings;
    settings.beginGroup(kMAVLinkLogGroup);
    settings.setValue(kWindSpeedKey, speed);
    emit windSpeedChanged();
}

//-----------------------------------------------------------------------------
void
MAVLinkLogManager::setRating(QString rate)
{
    _rating = rate;
    QSettings settings;
    settings.beginGroup(kMAVLinkLogGroup);
    settings.setValue(kRateKey, rate);
    emit ratingChanged();
}

//-----------------------------------------------------------------------------
void
MAVLinkLogManager::setPublicLog(bool pub)
{
    _publicLog = pub;
    QSettings settings;
    settings.beginGroup(kMAVLinkLogGroup);
    settings.setValue(kPublicLogKey, pub);
    emit publicLogChanged();
}

Gus Grubba's avatar
Gus Grubba committed
486 487
//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
488
MAVLinkLogManager::uploading()
Gus Grubba's avatar
Gus Grubba committed
489
{
490
    return _currentLogfile != nullptr;
Gus Grubba's avatar
Gus Grubba committed
491 492 493 494
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
495
MAVLinkLogManager::uploadLog()
Gus Grubba's avatar
Gus Grubba committed
496 497 498 499
{
    if(_currentLogfile) {
        _currentLogfile->setUploading(false);
    }
Gus Grubba's avatar
Gus Grubba committed
500
    for(int i = 0; i < _logFiles.count(); i++) {
Gus Grubba's avatar
Gus Grubba committed
501
        _currentLogfile = qobject_cast<MAVLinkLogFiles*>(_logFiles.get(i));
DonLakeFlyer's avatar
DonLakeFlyer committed
502 503 504 505 506 507 508 509 510 511 512
        if (_currentLogfile) {
            if(_currentLogfile->selected()) {
                _currentLogfile->setSelected(false);
                if(!_currentLogfile->uploaded() && !_emailAddress.isEmpty() && !_uploadURL.isEmpty()) {
                    _currentLogfile->setUploading(true);
                    _currentLogfile->setProgress(0.0);
                    QString filePath = _makeFilename(_currentLogfile->name());
                    _sendLog(filePath);
                    emit uploadingChanged();
                    return;
                }
Gus Grubba's avatar
Gus Grubba committed
513
            }
DonLakeFlyer's avatar
DonLakeFlyer committed
514 515
        } else {
            qWarning() << "Internal error";
Gus Grubba's avatar
Gus Grubba committed
516 517
        }
    }
518
    _currentLogfile = nullptr;
519 520 521 522 523
    emit uploadingChanged();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
524
MAVLinkLogManager::_insertNewLog(MAVLinkLogFiles* newLog)
525 526 527 528 529 530 531
{
    //-- Simpler than trying to sort this thing
    int count = _logFiles.count();
    if(!count) {
        _logFiles.append(newLog);
    } else {
        for(int i = 0; i < count; i++) {
Gus Grubba's avatar
Gus Grubba committed
532
            MAVLinkLogFiles* f = qobject_cast<MAVLinkLogFiles*>(_logFiles.get(i));
533 534 535 536 537 538 539
            if(newLog->name() < f->name()) {
                _logFiles.insert(i, newLog);
                return;
            }
        }
        _logFiles.append(newLog);
    }
Gus Grubba's avatar
Gus Grubba committed
540 541
}

Gus Grubba's avatar
Gus Grubba committed
542 543
//-----------------------------------------------------------------------------
int
Gus Grubba's avatar
Gus Grubba committed
544
MAVLinkLogManager::_getFirstSelected()
Gus Grubba's avatar
Gus Grubba committed
545 546
{
    for(int i = 0; i < _logFiles.count(); i++) {
Gus Grubba's avatar
Gus Grubba committed
547
        MAVLinkLogFiles* f = qobject_cast<MAVLinkLogFiles*>(_logFiles.get(i));
DonLakeFlyer's avatar
DonLakeFlyer committed
548 549 550 551 552 553
        if (f) {
            if(f->selected()) {
                return i;
            }
        } else {
            qWarning() << "Internal error";
Gus Grubba's avatar
Gus Grubba committed
554 555 556 557 558
        }
    }
    return -1;
}

Gus Grubba's avatar
Gus Grubba committed
559 560
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
561
MAVLinkLogManager::deleteLog()
Gus Grubba's avatar
Gus Grubba committed
562
{
Gus Grubba's avatar
Gus Grubba committed
563 564 565 566 567
    while (true) {
        int idx = _getFirstSelected();
        if(idx < 0) {
            break;
        }
Gus Grubba's avatar
Gus Grubba committed
568
        MAVLinkLogFiles* log = qobject_cast<MAVLinkLogFiles*>(_logFiles.get(idx));
569
        _deleteLog(log);
Gus Grubba's avatar
Gus Grubba committed
570
    }
Gus Grubba's avatar
Gus Grubba committed
571 572
}

573 574
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
575
MAVLinkLogManager::_deleteLog(MAVLinkLogFiles* log)
576
{
Gus Grubba's avatar
Gus Grubba committed
577
    QString filePath = _makeFilename(log->name());
578 579
    QFile gone(filePath);
    if(!gone.remove()) {
Gus Grubba's avatar
Gus Grubba committed
580
        qCWarning(MAVLinkLogManagerLog) << "Could not delete MAVLink log file:" << _logPath;
581
    }
Gus Grubba's avatar
Gus Grubba committed
582
    //-- Remove sidecar file (if any)
583
    filePath.replace(_ulogExtension, kSidecarExtension);
Gus Grubba's avatar
Gus Grubba committed
584 585 586 587 588
    QFile sgone(filePath);
    if(sgone.exists()) {
        sgone.remove();
    }
    //-- Remove file from list and delete record
589 590 591 592 593
    _logFiles.removeOne(log);
    delete log;
    emit logFilesChanged();
}

Gus Grubba's avatar
Gus Grubba committed
594 595
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
596
MAVLinkLogManager::cancelUpload()
Gus Grubba's avatar
Gus Grubba committed
597
{
Gus Grubba's avatar
Gus Grubba committed
598
    for(int i = 0; i < _logFiles.count(); i++) {
Gus Grubba's avatar
Gus Grubba committed
599
        MAVLinkLogFiles* pLogFile = qobject_cast<MAVLinkLogFiles*>(_logFiles.get(i));
DonLakeFlyer's avatar
DonLakeFlyer committed
600 601 602 603 604 605
        if (pLogFile) {
            if(pLogFile->selected() && pLogFile != _currentLogfile) {
                pLogFile->setSelected(false);
            }
        } else {
            qWarning() << "Internal error";
Gus Grubba's avatar
Gus Grubba committed
606 607 608 609 610 611 612
        }
    }
    if(_currentLogfile) {
        emit abortUpload();
    }
}

613 614
//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
615
MAVLinkLogManager::startLogging()
616
{
617 618 619 620 621 622 623 624 625
    //-- If we are allowed to persist data
    AppSettings* appSettings = qgcApp()->toolbox()->settingsManager()->appSettings();
    if(!appSettings->disableAllPersistence()->rawValue().toBool()) {
        if(_vehicle && _vehicle->px4Firmware() && !_logginDenied) {
            if(_createNewLog()) {
                _vehicle->startMavlinkLog();
                _logRunning = true;
                emit logRunningChanged();
            }
Gus Grubba's avatar
Gus Grubba committed
626
        }
627 628 629 630 631
    }
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
632
MAVLinkLogManager::stopLogging()
633
{
634
    if(_vehicle && _vehicle->px4Firmware()) {
635
        //-- Tell vehicle to stop sending logs
636
        _vehicle->stopMavlinkLog();
637
    }
Gus Grubba's avatar
Gus Grubba committed
638 639 640 641
    if(_logProcessor) {
        _logProcessor->close();
        if(_logProcessor->record()) {
            _logProcessor->record()->setWriting(false);
642 643
            if(_enableAutoUpload) {
                //-- Queue log for auto upload (set selected flag)
Gus Grubba's avatar
Gus Grubba committed
644
                _logProcessor->record()->setSelected(true);
645 646
                if(!uploading()) {
                    uploadLog();
647
                }
Gus Grubba's avatar
Gus Grubba committed
648 649
            }
        }
Gus Grubba's avatar
Gus Grubba committed
650
        delete _logProcessor;
651
        _logProcessor = nullptr;
652 653
        _logRunning = false;
        emit logRunningChanged();
654 655 656
    }
}

Gus Grubba's avatar
Gus Grubba committed
657 658 659 660 661 662 663 664 665 666 667 668
//-----------------------------------------------------------------------------
QHttpPart
create_form_part(const QString& name, const QString& value)
{
    QHttpPart formPart;
    formPart.setHeader(QNetworkRequest::ContentDispositionHeader, QString("form-data; name=\"%1\"").arg(name));
    formPart.setBody(value.toUtf8());
    return formPart;
}

//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
669
MAVLinkLogManager::_sendLog(const QString& logFile)
Gus Grubba's avatar
Gus Grubba committed
670 671 672
{
    QString defaultDescription = _description;
    if(_description.isEmpty()) {
Gus Grubba's avatar
Gus Grubba committed
673
        qCWarning(MAVLinkLogManagerLog) << "Log description missing. Using defaults.";
Gus Grubba's avatar
Gus Grubba committed
674 675 676
        defaultDescription = kDefaultDescr;
    }
    if(_emailAddress.isEmpty()) {
677
        qCWarning(MAVLinkLogManagerLog) << "User email missing.";
Gus Grubba's avatar
Gus Grubba committed
678 679 680
        return false;
    }
    if(_uploadURL.isEmpty()) {
681
        qCWarning(MAVLinkLogManagerLog) << "Upload URL missing.";
Gus Grubba's avatar
Gus Grubba committed
682 683 684 685
        return false;
    }
    QFileInfo fi(logFile);
    if(!fi.exists()) {
686
        qCWarning(MAVLinkLogManagerLog) << "Log file missing:" << logFile;
Gus Grubba's avatar
Gus Grubba committed
687 688
        return false;
    }
Gus Grubba's avatar
Gus Grubba committed
689
    QFile* file = new QFile(logFile);
Gus Grubba's avatar
Gus Grubba committed
690
    if(!file || !file->open(QIODevice::ReadOnly)) {
Gus Grubba's avatar
Gus Grubba committed
691 692 693
        if(file) {
            delete file;
        }
694
        qCWarning(MAVLinkLogManagerLog) << "Could not open log file:" << logFile;
Gus Grubba's avatar
Gus Grubba committed
695 696 697
        return false;
    }
    if(!_nam) {
Gus Grubba's avatar
Gus Grubba committed
698
        _nam = new QNetworkAccessManager(this);
Gus Grubba's avatar
Gus Grubba committed
699 700 701 702 703 704
    }
    QNetworkProxy savedProxy = _nam->proxy();
    QNetworkProxy tempProxy;
    tempProxy.setType(QNetworkProxy::DefaultProxy);
    _nam->setProxy(tempProxy);
    //-- Build POST request
Gus Grubba's avatar
Gus Grubba committed
705
    QHttpMultiPart* multiPart = new QHttpMultiPart(QHttpMultiPart::FormDataType);
Gus Grubba's avatar
Gus Grubba committed
706 707
    QHttpPart emailPart = create_form_part("email", _emailAddress);
    QHttpPart descriptionPart = create_form_part("description", _description);
708 709 710 711 712 713
    QHttpPart sourcePart      = create_form_part("source", "QGroundControl");
    QHttpPart versionPart     = create_form_part("version", _app->applicationVersion());
    QHttpPart typePart        = create_form_part("type", "flightreport");
    QHttpPart windPart        = create_form_part("windSpeed", QString::number(_windSpeed));
    QHttpPart ratingPart      = create_form_part("rating", _rating);
    QHttpPart publicPart      = create_form_part("public", _publicLog ? "true" : "false");
Gus Grubba's avatar
Gus Grubba committed
714 715 716 717 718
    //-- Assemble request and POST it
    multiPart->append(emailPart);
    multiPart->append(descriptionPart);
    multiPart->append(sourcePart);
    multiPart->append(versionPart);
719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742
    multiPart->append(typePart);
    multiPart->append(windPart);
    multiPart->append(ratingPart);
    multiPart->append(publicPart);
    //-- Optional
    QHttpPart feedbackPart;
    if(_feedback.isEmpty()) {
        feedbackPart = create_form_part(kFeedback, "None Given");
    } else {
        feedbackPart = create_form_part(kFeedback, _feedback);
    }
    multiPart->append(feedbackPart);
    QHttpPart videoPart;
    if(_videoURL.isEmpty()) {
        videoPart = create_form_part(kVideoURL, "None");
    } else {
        videoPart = create_form_part(kVideoURL, _videoURL);
    }
    multiPart->append(videoPart);
    //-- Actual Log File
    QHttpPart logPart;
    logPart.setHeader(QNetworkRequest::ContentTypeHeader, "application/octet-stream");
    logPart.setHeader(QNetworkRequest::ContentDispositionHeader, QString("form-data; name=\"filearg\"; filename=\"%1\"").arg(fi.fileName()));
    logPart.setBodyDevice(file);
Gus Grubba's avatar
Gus Grubba committed
743 744 745
    multiPart->append(logPart);
    file->setParent(multiPart);
    QNetworkRequest request(_uploadURL);
746
#if QT_VERSION > 0x050600
Gus Grubba's avatar
Gus Grubba committed
747
    request.setAttribute(QNetworkRequest::FollowRedirectsAttribute, true);
748
#endif
Gus Grubba's avatar
Gus Grubba committed
749
    QNetworkReply* reply = _nam->post(request, multiPart);
Gus Grubba's avatar
Gus Grubba committed
750 751 752 753
    connect(reply, &QNetworkReply::finished,  this, &MAVLinkLogManager::_uploadFinished);
    connect(this, &MAVLinkLogManager::abortUpload, reply, &QNetworkReply::abort);
    //connect(reply, &QNetworkReply::readyRead, this, &MAVLinkLogManager::_dataAvailable);
    connect(reply, &QNetworkReply::uploadProgress, this, &MAVLinkLogManager::_uploadProgress);
Gus Grubba's avatar
Gus Grubba committed
754
    multiPart->setParent(reply);
Gus Grubba's avatar
Gus Grubba committed
755
    qCDebug(MAVLinkLogManagerLog) << "Log" << fi.baseName() << "Uploading." << fi.size() << "bytes.";
Gus Grubba's avatar
Gus Grubba committed
756 757 758 759 760 761
    _nam->setProxy(savedProxy);
    return true;
}

//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
762
MAVLinkLogManager::_processUploadResponse(int http_code, QByteArray& data)
Gus Grubba's avatar
Gus Grubba committed
763
{
Gus Grubba's avatar
Gus Grubba committed
764
    qCDebug(MAVLinkLogManagerLog) << "Uploaded response:" << QString::fromUtf8(data);
Gus Grubba's avatar
Gus Grubba committed
765 766 767 768 769 770
    emit readyRead(data);
    return http_code == 200;
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
771
MAVLinkLogManager::_dataAvailable()
Gus Grubba's avatar
Gus Grubba committed
772
{
Gus Grubba's avatar
Gus Grubba committed
773
    QNetworkReply* reply = qobject_cast<QNetworkReply*>(sender());
Gus Grubba's avatar
Gus Grubba committed
774 775 776 777
    if(!reply) {
        return;
    }
    QByteArray data = reply->readAll();
Gus Grubba's avatar
Gus Grubba committed
778
    qCDebug(MAVLinkLogManagerLog) << "Uploaded response data:" << QString::fromUtf8(data);
Gus Grubba's avatar
Gus Grubba committed
779 780 781 782
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
783
MAVLinkLogManager::_uploadFinished()
Gus Grubba's avatar
Gus Grubba committed
784
{
Gus Grubba's avatar
Gus Grubba committed
785
    QNetworkReply* reply = qobject_cast<QNetworkReply*>(sender());
Gus Grubba's avatar
Gus Grubba committed
786 787 788 789 790 791
    if(!reply) {
        return;
    }
    const int http_code = reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toInt();
    QByteArray data = reply->readAll();
    if(_processUploadResponse(http_code, data)) {
Gus Grubba's avatar
Gus Grubba committed
792
        qCDebug(MAVLinkLogManagerLog) << "Log uploaded.";
Gus Grubba's avatar
Gus Grubba committed
793
        emit succeed();
794 795 796
        if(_deleteAfterUpload) {
            if(_currentLogfile) {
                _deleteLog(_currentLogfile);
797
                _currentLogfile = nullptr;
798
            }
Gus Grubba's avatar
Gus Grubba committed
799 800 801 802 803
        } else {
            if(_currentLogfile) {
                _currentLogfile->setUploaded(true);
                //-- Write side-car file to flag it as uploaded
                QString sideCar = _makeFilename(_currentLogfile->name());
804
                sideCar.replace(_ulogExtension, kSidecarExtension);
Gus Grubba's avatar
Gus Grubba committed
805 806 807 808 809
                FILE* f = fopen(sideCar.toLatin1().data(), "wb");
                if(f) {
                    fclose(f);
                }
            }
810
        }
Gus Grubba's avatar
Gus Grubba committed
811
    } else {
Gus Grubba's avatar
Gus Grubba committed
812
        qCWarning(MAVLinkLogManagerLog) << QString("Log Upload Error: %1 status: %2").arg(reply->errorString(), reply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toString());
Gus Grubba's avatar
Gus Grubba committed
813 814 815 816 817 818 819 820 821
        emit failed();
    }
    reply->deleteLater();
    //-- Next (if any)
    uploadLog();
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
822
MAVLinkLogManager::_uploadProgress(qint64 bytesSent, qint64 bytesTotal)
Gus Grubba's avatar
Gus Grubba committed
823 824
{
    if(bytesTotal) {
825
        qreal progress = static_cast<qreal>(bytesSent) / static_cast<qreal>(bytesTotal);
Gus Grubba's avatar
Gus Grubba committed
826
        if(_currentLogfile) {
Gus Grubba's avatar
Gus Grubba committed
827
            _currentLogfile->setProgress(progress);
Gus Grubba's avatar
Gus Grubba committed
828
        }
Gus Grubba's avatar
Gus Grubba committed
829
    }
Gus Grubba's avatar
Gus Grubba committed
830
    qCDebug(MAVLinkLogManagerLog) << bytesSent << "of" << bytesTotal;
Gus Grubba's avatar
Gus Grubba committed
831
}
832 833 834

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
835
MAVLinkLogManager::_activeVehicleChanged(Vehicle* vehicle)
836 837 838 839 840
{
    //-- TODO: This is not quite right. This is being used to detect when a vehicle
    //   connects/disconnects. In reality, if QGC is connected to multiple vehicles,
    //   this is called each time the user switches from one vehicle to another. So
    //   far, I'm working on the assumption that multiple vehicles is a rare exception.
841 842
    //   For now, we only handle one log download at a time. The proper way is to have
    //   each vehicle with their own instance of this "log manager".
843
    // Disconnect the previous one (if any)
844
    if(_vehicle && _vehicle->px4Firmware()) {
845 846 847
        disconnect(_vehicle, &Vehicle::armedChanged,        this, &MAVLinkLogManager::_armedChanged);
        disconnect(_vehicle, &Vehicle::mavlinkLogData,      this, &MAVLinkLogManager::_mavlinkLogData);
        disconnect(_vehicle, &Vehicle::mavCommandResult,    this, &MAVLinkLogManager::_mavCommandResult);
848
        _vehicle = nullptr;
849 850
        //-- Stop logging (if that's the case)
        stopLogging();
851 852 853
        emit canStartLogChanged();
    }
    // Connect new system
854
    if(vehicle && vehicle->px4Firmware()) {
855
        _vehicle = vehicle;
856 857
        //-- Reset logging denied flag as well
        _logginDenied = false;
858 859 860
        connect(_vehicle, &Vehicle::armedChanged,       this, &MAVLinkLogManager::_armedChanged);
        connect(_vehicle, &Vehicle::mavlinkLogData,     this, &MAVLinkLogManager::_mavlinkLogData);
        connect(_vehicle, &Vehicle::mavCommandResult,   this, &MAVLinkLogManager::_mavCommandResult);
861 862 863 864 865 866
        emit canStartLogChanged();
    }
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
867
MAVLinkLogManager::_mavlinkLogData(Vehicle* /*vehicle*/, uint8_t /*target_system*/, uint8_t /*target_component*/, uint16_t sequence, uint8_t first_message, QByteArray data, bool /*acked*/)
Gus Grubba's avatar
Gus Grubba committed
868
{
Gus Grubba's avatar
Gus Grubba committed
869 870
    if(_logProcessor && _logProcessor->valid()) {
        if(!_logProcessor->processStreamData(sequence, first_message, data)) {
871
            qCWarning(MAVLinkLogManagerLog) << "Error writing MAVLink log file:" << _logProcessor->fileName();
Gus Grubba's avatar
Gus Grubba committed
872
            delete _logProcessor;
873
            _logProcessor = nullptr;
Gus Grubba's avatar
Gus Grubba committed
874 875 876 877 878
            _logRunning = false;
            _vehicle->stopMavlinkLog();
            emit logRunningChanged();
        }
    } else {
Gus Grubba's avatar
Gus Grubba committed
879
        qCWarning(MAVLinkLogManagerLog) << "MAVLink log data received when not expected.";
Gus Grubba's avatar
Gus Grubba committed
880 881 882
    }
}

883 884
//-----------------------------------------------------------------------------
void
885
MAVLinkLogManager::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
886
{
887 888 889 890
    Q_UNUSED(vehicleId);
    Q_UNUSED(component);
    Q_UNUSED(noReponseFromVehicle)

891 892
    if(command == MAV_CMD_LOGGING_START || command == MAV_CMD_LOGGING_STOP) {
        //-- Did it fail?
893
        if(result != MAV_RESULT_ACCEPTED) {
894 895
            if(command == MAV_CMD_LOGGING_STOP) {
                //-- Not that it could happen but...
Gus Grubba's avatar
Gus Grubba committed
896
                qCWarning(MAVLinkLogManagerLog) << "Stop MAVLink log command failed.";
897 898
            } else {
                //-- Could not start logging for some reason.
899 900 901 902 903 904
                if(result == MAV_RESULT_DENIED) {
                    _logginDenied = true;
                    qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command denied.";
                } else {
                    qCWarning(MAVLinkLogManagerLog) << "Start MAVLink log command failed.";
                }
905 906 907 908 909 910 911 912
                _discardLog();
            }
        }
    }
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
913
MAVLinkLogManager::_discardLog()
914 915
{
    //-- Delete (empty) log file (and record)
Gus Grubba's avatar
Gus Grubba committed
916 917 918 919
    if(_logProcessor) {
        _logProcessor->close();
        if(_logProcessor->record()) {
            _deleteLog(_logProcessor->record());
920
        }
Gus Grubba's avatar
Gus Grubba committed
921
        delete _logProcessor;
922
        _logProcessor = nullptr;
923 924 925 926 927
    }
    _logRunning = false;
    emit logRunningChanged();
}

Gus Grubba's avatar
Gus Grubba committed
928 929
//-----------------------------------------------------------------------------
bool
Gus Grubba's avatar
Gus Grubba committed
930
MAVLinkLogManager::_createNewLog()
931
{
Gus Grubba's avatar
Gus Grubba committed
932 933
    if(_logProcessor) {
        delete _logProcessor;
934
        _logProcessor = nullptr;
Gus Grubba's avatar
Gus Grubba committed
935
    }
Gus Grubba's avatar
Gus Grubba committed
936
    _logProcessor = new MAVLinkLogProcessor;
937
    if(_logProcessor->create(this, _logPath, static_cast<uint8_t>(_vehicle->id()))) {
Gus Grubba's avatar
Gus Grubba committed
938
        _insertNewLog(_logProcessor->record());
939 940
        emit logFilesChanged();
    } else {
941
        qCWarning(MAVLinkLogManagerLog) << "Could not create MAVLink log file:" << _logProcessor->fileName();
Gus Grubba's avatar
Gus Grubba committed
942
        delete _logProcessor;
943
        _logProcessor = nullptr;
Gus Grubba's avatar
Gus Grubba committed
944
    }
945
    return _logProcessor != nullptr;
946 947 948 949
}

//-----------------------------------------------------------------------------
void
Gus Grubba's avatar
Gus Grubba committed
950
MAVLinkLogManager::_armedChanged(bool armed)
951
{
952
    if(_vehicle && _vehicle->px4Firmware()) {
953 954
        if(armed) {
            if(_enableAutoStart) {
Gus Grubba's avatar
Gus Grubba committed
955
                startLogging();
956 957 958
            }
        } else {
            if(_logRunning && _enableAutoStart) {
Gus Grubba's avatar
Gus Grubba committed
959
                stopLogging();
960 961 962 963
            }
        }
    }
}
Gus Grubba's avatar
Gus Grubba committed
964 965 966

//-----------------------------------------------------------------------------
QString
Gus Grubba's avatar
Gus Grubba committed
967
MAVLinkLogManager::_makeFilename(const QString& baseName)
Gus Grubba's avatar
Gus Grubba committed
968 969 970 971
{
    QString filePath = _logPath;
    filePath += "/";
    filePath += baseName;
972
    filePath += _ulogExtension;
Gus Grubba's avatar
Gus Grubba committed
973 974
    return filePath;
}