PX4FirmwareUpgradeThread.cc 13.7 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009, 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

/// @file
///     @brief PX4 Firmware Upgrade operations which occur on a seperate thread.
///     @author Don Gagne <don@thegagnes.com>

#include "PX4FirmwareUpgradeThread.h"
29 30 31
#include "Bootloader.h"
#include "QGCLoggingCategory.h"
#include "QGC.h"
32 33 34

#include <QTimer>
#include <QDebug>
Don Gagne's avatar
Don Gagne committed
35
#include <QSerialPort>
36

37 38
PX4FirmwareUpgradeThreadWorker::PX4FirmwareUpgradeThreadWorker(PX4FirmwareUpgradeThreadController* controller) :
    _controller(controller),
39 40
    _bootloader(NULL),
    _bootloaderPort(NULL),
41 42 43
    _timerRetry(NULL),
    _foundBoard(false),
    _findBoardFirstAttempt(true)
44
{
45
    Q_ASSERT(_controller);
46
    
47 48 49 50 51
    connect(_controller, &PX4FirmwareUpgradeThreadController::_initThreadWorker,            this, &PX4FirmwareUpgradeThreadWorker::_init);
    connect(_controller, &PX4FirmwareUpgradeThreadController::_startFindBoardLoopOnThread,  this, &PX4FirmwareUpgradeThreadWorker::_startFindBoardLoop);
    connect(_controller, &PX4FirmwareUpgradeThreadController::_flashOnThread,               this, &PX4FirmwareUpgradeThreadWorker::_flash);
    connect(_controller, &PX4FirmwareUpgradeThreadController::_rebootOnThread,              this, &PX4FirmwareUpgradeThreadWorker::_reboot);
    connect(_controller, &PX4FirmwareUpgradeThreadController::_cancel,                      this, &PX4FirmwareUpgradeThreadWorker::_cancel);
52 53 54 55 56 57 58 59 60 61 62 63
}

PX4FirmwareUpgradeThreadWorker::~PX4FirmwareUpgradeThreadWorker()
{
    if (_bootloaderPort) {
        // deleteLater so delete happens on correct thread
        _bootloaderPort->deleteLater();
    }
}

/// @brief Initializes the PX4FirmwareUpgradeThreadWorker with the various child objects which must be created
///         on the worker thread.
64
void PX4FirmwareUpgradeThreadWorker::_init(void)
65 66 67 68 69 70 71 72
{
    // We create the timers here so that they are on the right thread
    
    Q_ASSERT(_timerRetry == NULL);
    _timerRetry = new QTimer(this);
    Q_CHECK_PTR(_timerRetry);
    _timerRetry->setSingleShot(true);
    _timerRetry->setInterval(_retryTimeout);
73
    connect(_timerRetry, &QTimer::timeout, this, &PX4FirmwareUpgradeThreadWorker::_findBoardOnce);
74 75
    
    Q_ASSERT(_bootloader == NULL);
76 77
    _bootloader = new Bootloader(this);
    connect(_bootloader, &Bootloader::updateProgress, this, &PX4FirmwareUpgradeThreadWorker::_updateProgress);
78 79
}

80
void PX4FirmwareUpgradeThreadWorker::_cancel(void)
81
{
82 83 84 85 86 87 88 89 90 91
    if (_bootloaderPort) {
        _bootloaderPort->close();
        _bootloaderPort->deleteLater();
        _bootloaderPort = NULL;
    }
}

void PX4FirmwareUpgradeThreadWorker::_startFindBoardLoop(void)
{
    _foundBoard = false;
Don Gagne's avatar
Don Gagne committed
92
    _findBoardFirstAttempt = true;
93 94 95 96 97
    _findBoardOnce();
}

void PX4FirmwareUpgradeThreadWorker::_findBoardOnce(void)
{
Don Gagne's avatar
Don Gagne committed
98
    qCDebug(FirmwareUpgradeVerboseLog) << "_findBoardOnce";
99
    
Don Gagne's avatar
Don Gagne committed
100 101
    QGCSerialPortInfo               portInfo;
    QGCSerialPortInfo::BoardType_t  boardType;
102
    
103 104 105 106 107 108
    if (_findBoardFromPorts(portInfo, boardType)) {
        if (!_foundBoard) {
            _foundBoard = true;
            _foundBoardPortInfo = portInfo;
            emit foundBoard(_findBoardFirstAttempt, portInfo, boardType);
            if (!_findBoardFirstAttempt) {
Don Gagne's avatar
Don Gagne committed
109
                if (boardType == QGCSerialPortInfo::BoardType3drRadio) {
110 111 112 113 114 115 116 117 118 119 120 121 122 123 124
                    _3drRadioForceBootloader(portInfo);
                    return;
                } else {
                    _findBootloader(portInfo, false /* radio mode */, true /* errorOnNotFound */);
                    return;
                }
            }
        }
    } else {
        if (_foundBoard) {
            _foundBoard = false;
            qCDebug(FirmwareUpgradeLog) << "Board gone";
            emit boardGone();
        } else if (_findBoardFirstAttempt) {
            emit noBoardFound();
125 126 127
        }
    }
    
Don Gagne's avatar
Don Gagne committed
128
    _findBoardFirstAttempt = false;
129 130 131
    _timerRetry->start();
}

Don Gagne's avatar
Don Gagne committed
132
bool PX4FirmwareUpgradeThreadWorker::_findBoardFromPorts(QGCSerialPortInfo& portInfo, QGCSerialPortInfo::BoardType_t& boardType)
133
{
Don Gagne's avatar
Don Gagne committed
134
    foreach (QGCSerialPortInfo info, QGCSerialPortInfo::availablePorts()) {
Don Gagne's avatar
Don Gagne committed
135 136 137 138 139 140 141
        qCDebug(FirmwareUpgradeVerboseLog) << "Serial Port --------------";
        qCDebug(FirmwareUpgradeVerboseLog) << "\tboard type" << info.boardType();
        qCDebug(FirmwareUpgradeVerboseLog) << "\tport name:" << info.portName();
        qCDebug(FirmwareUpgradeVerboseLog) << "\tdescription:" << info.description();
        qCDebug(FirmwareUpgradeVerboseLog) << "\tsystem location:" << info.systemLocation();
        qCDebug(FirmwareUpgradeVerboseLog) << "\tvendor ID:" << info.vendorIdentifier();
        qCDebug(FirmwareUpgradeVerboseLog) << "\tproduct ID:" << info.productIdentifier();
142
        
Don Gagne's avatar
Don Gagne committed
143 144
        boardType = info.boardType();
        if (boardType != QGCSerialPortInfo::BoardTypeUnknown) {
145
            portInfo = info;
146 147 148 149 150 151 152
            return true;
        }
    }
    
    return false;
}

Don Gagne's avatar
Don Gagne committed
153
void PX4FirmwareUpgradeThreadWorker::_3drRadioForceBootloader(const QGCSerialPortInfo& portInfo)
154
{
155 156 157 158 159 160 161 162 163
    // First make sure we can't get the bootloader
    
    if (_findBootloader(portInfo, true /* radio Mode */, false /* errorOnNotFound */)) {
        return;
    }

    // Couldn't find the bootloader. We'll need to reboot the radio into bootloader.
    
    QSerialPort port(portInfo);
Don Gagne's avatar
Don Gagne committed
164
    
165 166 167 168 169
    port.setBaudRate(QSerialPort::Baud57600);
    
    emit status("Putting radio into command mode");
    
    // Wait a little while for the USB port to initialize. 3DR Radio boot is really slow.
Don Gagne's avatar
Don Gagne committed
170 171
    QGC::SLEEP::msleep(2000);
    port.open(QIODevice::ReadWrite);
172 173 174 175 176 177 178
    
    if (!port.isOpen()) {
        emit error(QString("Unable to open port: %1 error: %2").arg(portInfo.systemLocation()).arg(port.errorString()));
        return;
    }

    // Put radio into command mode
Don Gagne's avatar
Don Gagne committed
179
    QGC::SLEEP::msleep(2000);
180 181 182 183 184 185 186
    port.write("+++", 3);
    if (!port.waitForReadyRead(1500)) {
        emit error("Unable to put radio into command mode");
        return;
    }
    QByteArray bytes = port.readAll();
    if (!bytes.contains("OK")) {
Don Gagne's avatar
Don Gagne committed
187
        qCDebug(FirmwareUpgradeLog) << bytes;
188 189 190 191 192 193 194 195
        emit error("Unable to put radio into command mode");
        return;
    }

    emit status("Rebooting radio to bootloader");
    
    port.write("AT&UPDATE\r\n");
    if (!port.waitForBytesWritten(1500)) {
Don Gagne's avatar
Don Gagne committed
196
        emit error("Unable to reboot radio (bytes written)");
197 198
        return;
    }
Don Gagne's avatar
Don Gagne committed
199 200 201 202
    if (!port.waitForReadyRead(1500)) {
        emit error("Unable to reboot radio (ready read)");
        return;
    }
203
    port.close();
Don Gagne's avatar
Don Gagne committed
204 205
    QGC::SLEEP::msleep(2000);

206 207 208
    // The bootloader should be waiting for us now
    
    _findBootloader(portInfo, true /* radio mode */, true /* errorOnNotFound */);
209 210
}

Don Gagne's avatar
Don Gagne committed
211
bool PX4FirmwareUpgradeThreadWorker::_findBootloader(const QGCSerialPortInfo& portInfo, bool radioMode, bool errorOnNotFound)
212
{
Don Gagne's avatar
Don Gagne committed
213
    qCDebug(FirmwareUpgradeLog) << "_findBootloader" << portInfo.systemLocation();
214
    
215 216 217
    uint32_t bootloaderVersion = 0;
    uint32_t boardID;
    uint32_t flashSize = 0;
218

219
    
220
    _bootloaderPort = new QextSerialPort(QextSerialPort::Polling);
Don Gagne's avatar
Don Gagne committed
221 222 223 224
    if (radioMode) {
        _bootloaderPort->setBaudRate(BAUD115200);
    }

225
    // Wait a little while for the USB port to initialize.
226
    bool openFailed = true;
227 228
    for (int i=0; i<10; i++) {
        if (_bootloader->open(_bootloaderPort, portInfo.systemLocation())) {
229
            openFailed = false;
230 231 232 233 234 235
            break;
        } else {
            QGC::SLEEP::msleep(100);
        }
    }
    
Don Gagne's avatar
Don Gagne committed
236 237 238 239
    if (radioMode) {
        QGC::SLEEP::msleep(2000);
    }

240 241 242 243 244 245 246 247 248 249
    if (openFailed) {
        qCDebug(FirmwareUpgradeLog) << "Bootloader open port failed:" << _bootloader->errorString();
        if (errorOnNotFound) {
            emit error(_bootloader->errorString());
        }
        _bootloaderPort->deleteLater();
        _bootloaderPort = NULL;
        return false;
    }

250 251 252 253 254 255 256 257 258 259 260 261
    if (_bootloader->sync(_bootloaderPort)) {
        bool success;
        
        if (radioMode) {
            success = _bootloader->get3DRRadioBoardId(_bootloaderPort, boardID);
        } else {
            success = _bootloader->getPX4BoardInfo(_bootloaderPort, bootloaderVersion, boardID, flashSize);
        }
        if (success) {
            qCDebug(FirmwareUpgradeLog) << "Found bootloader";
            emit foundBootloader(bootloaderVersion, boardID, flashSize);
            return true;
262 263 264
        }
    }
    
Don Gagne's avatar
Don Gagne committed
265 266 267
    _bootloaderPort->close();
    _bootloaderPort->deleteLater();
    _bootloaderPort = NULL;
268 269 270 271 272 273
    qCDebug(FirmwareUpgradeLog) << "Bootloader error:" << _bootloader->errorString();
    if (errorOnNotFound) {
        emit error(_bootloader->errorString());
    }
    
    return false;
274 275
}

276
void PX4FirmwareUpgradeThreadWorker::_reboot(void)
277
{
278 279
    if (_bootloaderPort) {
        if (_bootloaderPort->isOpen()) {
280
            _bootloader->reboot(_bootloaderPort);
281 282 283 284
        }
        _bootloaderPort->deleteLater();
        _bootloaderPort = NULL;
    }
285 286
}

287
void PX4FirmwareUpgradeThreadWorker::_flash(void)
288
{
289 290 291 292 293 294 295 296 297 298 299 300 301
    qCDebug(FirmwareUpgradeLog) << "PX4FirmwareUpgradeThreadWorker::_flash";
    
    if (_erase()) {
        emit status("Programming new version...");
        
        if (_bootloader->program(_bootloaderPort, _controller->image())) {
            qCDebug(FirmwareUpgradeLog) << "Program complete";
            emit status("Program complete");
        } else {
            _bootloaderPort->deleteLater();
            _bootloaderPort = NULL;
            qCDebug(FirmwareUpgradeLog) << "Program failed:" << _bootloader->errorString();
            emit error(_bootloader->errorString());
Don Gagne's avatar
Don Gagne committed
302
            return;
303 304 305 306 307 308 309 310 311 312
        }
        
        emit status("Verifying program...");
        
        if (_bootloader->verify(_bootloaderPort, _controller->image())) {
            qCDebug(FirmwareUpgradeLog) << "Verify complete";
            emit status("Verify complete");
        } else {
            qCDebug(FirmwareUpgradeLog) << "Verify failed:" << _bootloader->errorString();
            emit error(_bootloader->errorString());
Don Gagne's avatar
Don Gagne committed
313
            return;
314
        }
315
    }
316 317 318 319
    
    emit _reboot();
    
    emit flashComplete();
320 321
}

322
bool PX4FirmwareUpgradeThreadWorker::_erase(void)
323
{
324 325 326 327 328 329 330 331 332 333
    qCDebug(FirmwareUpgradeLog) << "PX4FirmwareUpgradeThreadWorker::_erase";
    
    emit eraseStarted();
    emit status("Erasing previous program...");
    
    if (_bootloader->erase(_bootloaderPort)) {
        qCDebug(FirmwareUpgradeLog) << "Erase complete";
        emit status("Erase complete");
        emit eraseComplete();
        return true;
334
    } else {
335 336 337
        qCDebug(FirmwareUpgradeLog) << "Erase failed:" << _bootloader->errorString();
        emit error(_bootloader->errorString());
        return false;
338 339 340 341 342 343
    }
}

PX4FirmwareUpgradeThreadController::PX4FirmwareUpgradeThreadController(QObject* parent) :
    QObject(parent)
{
344
    _worker = new PX4FirmwareUpgradeThreadWorker(this);
345 346 347 348 349 350
    Q_CHECK_PTR(_worker);
    
    _workerThread = new QThread(this);
    Q_CHECK_PTR(_workerThread);
    _worker->moveToThread(_workerThread);
    
351 352 353 354 355
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::updateProgress,       this, &PX4FirmwareUpgradeThreadController::_updateProgress);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::foundBoard,           this, &PX4FirmwareUpgradeThreadController::_foundBoard);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::noBoardFound,         this, &PX4FirmwareUpgradeThreadController::_noBoardFound);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::boardGone,            this, &PX4FirmwareUpgradeThreadController::_boardGone);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::foundBootloader,      this, &PX4FirmwareUpgradeThreadController::_foundBootloader);
356
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::bootloaderSyncFailed, this, &PX4FirmwareUpgradeThreadController::_bootloaderSyncFailed);
357 358 359 360 361
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::error,                this, &PX4FirmwareUpgradeThreadController::_error);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::status,               this, &PX4FirmwareUpgradeThreadController::_status);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::eraseStarted,         this, &PX4FirmwareUpgradeThreadController::_eraseStarted);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::eraseComplete,        this, &PX4FirmwareUpgradeThreadController::_eraseComplete);
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::flashComplete,        this, &PX4FirmwareUpgradeThreadController::_flashComplete);
362 363 364 365 366 367 368 369 370 371

    _workerThread->start();
    
    emit _initThreadWorker();
}

PX4FirmwareUpgradeThreadController::~PX4FirmwareUpgradeThreadController()
{
    _workerThread->quit();
    _workerThread->wait();
372 373
    
    delete _workerThread;
374 375
}

376
void PX4FirmwareUpgradeThreadController::startFindBoardLoop(void)
377
{
378 379
    qCDebug(FirmwareUpgradeLog) << "PX4FirmwareUpgradeThreadController::findBoard";
    emit _startFindBoardLoopOnThread();
380 381
}

382
void PX4FirmwareUpgradeThreadController::cancel(void)
383
{
384 385
    qCDebug(FirmwareUpgradeLog) << "PX4FirmwareUpgradeThreadController::cancel";
    emit _cancel();
386 387
}

388
void PX4FirmwareUpgradeThreadController::flash(const FirmwareImage* image)
389
{
390 391
    _image = image;
    emit _flashOnThread();
392
}