PX4FirmwareUpgradeThread.cc 13.1 KB
Newer Older
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.
 *
 ****************************************************************************/

10 11 12 13 14 15

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

#include "PX4FirmwareUpgradeThread.h"
16 17 18
#include "Bootloader.h"
#include "QGCLoggingCategory.h"
#include "QGC.h"
19 20 21

#include <QTimer>
#include <QDebug>
Don Gagne's avatar
Don Gagne committed
22
#include <QSerialPort>
23

24 25
PX4FirmwareUpgradeThreadWorker::PX4FirmwareUpgradeThreadWorker(PX4FirmwareUpgradeThreadController* controller) :
    _controller(controller),
26 27
    _bootloader(NULL),
    _bootloaderPort(NULL),
28 29 30
    _timerRetry(NULL),
    _foundBoard(false),
    _findBoardFirstAttempt(true)
31
{
32
    Q_ASSERT(_controller);
33
    
34 35 36 37 38
    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);
39 40 41 42 43 44 45 46 47 48 49 50
}

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.
51
void PX4FirmwareUpgradeThreadWorker::_init(void)
52 53 54 55 56 57 58 59
{
    // 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);
60
    connect(_timerRetry, &QTimer::timeout, this, &PX4FirmwareUpgradeThreadWorker::_findBoardOnce);
61 62
    
    Q_ASSERT(_bootloader == NULL);
63 64
    _bootloader = new Bootloader(this);
    connect(_bootloader, &Bootloader::updateProgress, this, &PX4FirmwareUpgradeThreadWorker::_updateProgress);
65 66
}

67
void PX4FirmwareUpgradeThreadWorker::_cancel(void)
68
{
69 70 71 72 73 74 75 76 77 78
    if (_bootloaderPort) {
        _bootloaderPort->close();
        _bootloaderPort->deleteLater();
        _bootloaderPort = NULL;
    }
}

void PX4FirmwareUpgradeThreadWorker::_startFindBoardLoop(void)
{
    _foundBoard = false;
Don Gagne's avatar
Don Gagne committed
79
    _findBoardFirstAttempt = true;
80 81 82 83 84
    _findBoardOnce();
}

void PX4FirmwareUpgradeThreadWorker::_findBoardOnce(void)
{
Don Gagne's avatar
Don Gagne committed
85
    qCDebug(FirmwareUpgradeVerboseLog) << "_findBoardOnce";
86
    
Don Gagne's avatar
Don Gagne committed
87 88
    QGCSerialPortInfo               portInfo;
    QGCSerialPortInfo::BoardType_t  boardType;
89
    
90 91 92 93 94 95
    if (_findBoardFromPorts(portInfo, boardType)) {
        if (!_foundBoard) {
            _foundBoard = true;
            _foundBoardPortInfo = portInfo;
            emit foundBoard(_findBoardFirstAttempt, portInfo, boardType);
            if (!_findBoardFirstAttempt) {
Don Gagne's avatar
Don Gagne committed
96
                if (boardType == QGCSerialPortInfo::BoardTypeSikRadio) {
97 98 99 100 101 102 103 104 105 106 107 108 109 110 111
                    _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();
112 113 114
        }
    }
    
Don Gagne's avatar
Don Gagne committed
115
    _findBoardFirstAttempt = false;
116 117 118
    _timerRetry->start();
}

Don Gagne's avatar
Don Gagne committed
119
bool PX4FirmwareUpgradeThreadWorker::_findBoardFromPorts(QGCSerialPortInfo& portInfo, QGCSerialPortInfo::BoardType_t& boardType)
120
{
Don Gagne's avatar
Don Gagne committed
121
    foreach (QGCSerialPortInfo info, QGCSerialPortInfo::availablePorts()) {
Don Gagne's avatar
Don Gagne committed
122 123 124 125 126 127 128
        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();
129
        
Don Gagne's avatar
Don Gagne committed
130
        boardType = info.boardType();
131
        if (info.canFlash()) {
132
            portInfo = info;
133 134 135 136 137 138 139
            return true;
        }
    }
    
    return false;
}

Don Gagne's avatar
Don Gagne committed
140
void PX4FirmwareUpgradeThreadWorker::_3drRadioForceBootloader(const QGCSerialPortInfo& portInfo)
141
{
142 143 144 145 146 147 148 149 150
    // 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
151
    
152 153 154 155 156
    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
157 158
    QGC::SLEEP::msleep(2000);
    port.open(QIODevice::ReadWrite);
159 160 161 162 163 164 165
    
    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
166
    QGC::SLEEP::msleep(2000);
167 168 169 170 171 172 173
    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
174
        qCDebug(FirmwareUpgradeLog) << bytes;
175 176 177 178 179 180 181 182
        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
183
        emit error("Unable to reboot radio (bytes written)");
184 185
        return;
    }
Don Gagne's avatar
Don Gagne committed
186 187 188 189
    if (!port.waitForReadyRead(1500)) {
        emit error("Unable to reboot radio (ready read)");
        return;
    }
190
    port.close();
Don Gagne's avatar
Don Gagne committed
191 192
    QGC::SLEEP::msleep(2000);

193 194 195
    // The bootloader should be waiting for us now
    
    _findBootloader(portInfo, true /* radio mode */, true /* errorOnNotFound */);
196 197
}

Don Gagne's avatar
Don Gagne committed
198
bool PX4FirmwareUpgradeThreadWorker::_findBootloader(const QGCSerialPortInfo& portInfo, bool radioMode, bool errorOnNotFound)
199
{
Don Gagne's avatar
Don Gagne committed
200
    qCDebug(FirmwareUpgradeLog) << "_findBootloader" << portInfo.systemLocation();
201
    
202 203 204
    uint32_t bootloaderVersion = 0;
    uint32_t boardID;
    uint32_t flashSize = 0;
205

206
    
207
    _bootloaderPort = new QextSerialPort(QextSerialPort::Polling);
Don Gagne's avatar
Don Gagne committed
208 209 210 211
    if (radioMode) {
        _bootloaderPort->setBaudRate(BAUD115200);
    }

212
    // Wait a little while for the USB port to initialize.
213
    bool openFailed = true;
214 215
    for (int i=0; i<10; i++) {
        if (_bootloader->open(_bootloaderPort, portInfo.systemLocation())) {
216
            openFailed = false;
217 218 219 220 221 222
            break;
        } else {
            QGC::SLEEP::msleep(100);
        }
    }
    
Don Gagne's avatar
Don Gagne committed
223 224 225 226
    if (radioMode) {
        QGC::SLEEP::msleep(2000);
    }

227 228 229 230 231 232 233 234 235 236
    if (openFailed) {
        qCDebug(FirmwareUpgradeLog) << "Bootloader open port failed:" << _bootloader->errorString();
        if (errorOnNotFound) {
            emit error(_bootloader->errorString());
        }
        _bootloaderPort->deleteLater();
        _bootloaderPort = NULL;
        return false;
    }

237 238 239 240 241 242 243 244 245 246 247 248
    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;
249 250 251
        }
    }
    
Don Gagne's avatar
Don Gagne committed
252 253 254
    _bootloaderPort->close();
    _bootloaderPort->deleteLater();
    _bootloaderPort = NULL;
255 256 257 258 259 260
    qCDebug(FirmwareUpgradeLog) << "Bootloader error:" << _bootloader->errorString();
    if (errorOnNotFound) {
        emit error(_bootloader->errorString());
    }
    
    return false;
261 262
}

263
void PX4FirmwareUpgradeThreadWorker::_reboot(void)
264
{
265 266
    if (_bootloaderPort) {
        if (_bootloaderPort->isOpen()) {
267
            _bootloader->reboot(_bootloaderPort);
268 269 270 271
        }
        _bootloaderPort->deleteLater();
        _bootloaderPort = NULL;
    }
272 273
}

274
void PX4FirmwareUpgradeThreadWorker::_flash(void)
275
{
276 277 278 279 280 281 282 283 284 285 286 287 288
    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
289
            return;
290 291 292 293 294 295 296 297 298 299
        }
        
        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
300
            return;
301
        }
302
    }
303 304 305 306
    
    emit _reboot();
    
    emit flashComplete();
307 308
}

309
bool PX4FirmwareUpgradeThreadWorker::_erase(void)
310
{
311 312 313 314 315 316 317 318 319 320
    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;
321
    } else {
322 323 324
        qCDebug(FirmwareUpgradeLog) << "Erase failed:" << _bootloader->errorString();
        emit error(_bootloader->errorString());
        return false;
325 326 327 328 329 330
    }
}

PX4FirmwareUpgradeThreadController::PX4FirmwareUpgradeThreadController(QObject* parent) :
    QObject(parent)
{
331
    _worker = new PX4FirmwareUpgradeThreadWorker(this);
332 333 334 335 336 337
    Q_CHECK_PTR(_worker);
    
    _workerThread = new QThread(this);
    Q_CHECK_PTR(_workerThread);
    _worker->moveToThread(_workerThread);
    
338 339 340 341 342
    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);
343
    connect(_worker, &PX4FirmwareUpgradeThreadWorker::bootloaderSyncFailed, this, &PX4FirmwareUpgradeThreadController::_bootloaderSyncFailed);
344 345 346 347 348
    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);
349 350 351 352 353 354 355 356 357 358

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

PX4FirmwareUpgradeThreadController::~PX4FirmwareUpgradeThreadController()
{
    _workerThread->quit();
    _workerThread->wait();
359 360
    
    delete _workerThread;
361 362
}

363
void PX4FirmwareUpgradeThreadController::startFindBoardLoop(void)
364
{
365 366
    qCDebug(FirmwareUpgradeLog) << "PX4FirmwareUpgradeThreadController::findBoard";
    emit _startFindBoardLoopOnThread();
367 368
}

369
void PX4FirmwareUpgradeThreadController::cancel(void)
370
{
371 372
    qCDebug(FirmwareUpgradeLog) << "PX4FirmwareUpgradeThreadController::cancel";
    emit _cancel();
373 374
}

375
void PX4FirmwareUpgradeThreadController::flash(const FirmwareImage* image)
376
{
377 378
    _image = image;
    emit _flashOnThread();
379
}