SerialLink.cc 16.1 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9 10 11 12
/*=====================================================================
======================================================================*/
/**
 * @file
 *   @brief Cross-platform support for serial ports
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QTimer>
#include <QDebug>
13
#include <QSettings>
pixhawk's avatar
pixhawk committed
14
#include <QMutexLocker>
dogmaphobic's avatar
dogmaphobic committed
15 16 17 18

#ifdef __android__
#include "qserialport.h"
#else
19
#include <QSerialPort>
dogmaphobic's avatar
dogmaphobic committed
20
#endif
21

pixhawk's avatar
pixhawk committed
22
#include "SerialLink.h"
23
#include "QGC.h"
24
#include "MG.h"
25
#include "QGCLoggingCategory.h"
Don Gagne's avatar
Don Gagne committed
26
#include "QGCApplication.h"
27
#include "QGCSerialPortInfo.h"
28

29
QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog")
pixhawk's avatar
pixhawk committed
30

31 32
static QStringList kSupportedBaudRates;

33 34 35 36 37 38 39 40 41
SerialLink::SerialLink(SerialConfiguration* config)
{
    _bytesRead = 0;
    _port     = Q_NULLPTR;
    _stopp    = false;
    _reqReset = false;
    Q_ASSERT(config != NULL);
    _config = config;
    _config->setLink(this);
Bill Bonney's avatar
Bill Bonney committed
42

43
    qCDebug(SerialLinkLog) << "Create SerialLink " << config->portName() << config->baud() << config->flowControl()
44
             << config->parity() << config->dataBits() << config->stopBits();
45
    qCDebug(SerialLinkLog) << "portName: " << config->portName();
pixhawk's avatar
pixhawk committed
46
}
47

48 49
void SerialLink::requestReset()
{
50 51
    QMutexLocker locker(&this->_stoppMutex);
    _reqReset = true;
52
}
pixhawk's avatar
pixhawk committed
53 54 55

SerialLink::~SerialLink()
{
56 57
    // Disconnect link from configuration
    _config->setLink(NULL);
58
    _disconnect();
59 60
    if(_port) delete _port;
    _port = NULL;
61 62
}

63
bool SerialLink::_isBootloader()
64
{
65
    QList<QSerialPortInfo> portList = QSerialPortInfo::availablePorts();
66 67 68 69 70
    if( portList.count() == 0){
        return false;
    }
    foreach (const QSerialPortInfo &info, portList)
    {
71 72 73 74 75 76 77 78
        qCDebug(SerialLinkLog) << "PortName    : " << info.portName() << "Description : " << info.description();
        qCDebug(SerialLinkLog) << "Manufacturer: " << info.manufacturer();
        if (info.portName().trimmed() == _config->portName() &&
                (info.description().toLower().contains("bootloader") ||
                 info.description().toLower().contains("px4 bl") ||
                 info.description().toLower().contains("px4 fmu v1.6"))) {
            qCDebug(SerialLinkLog) << "BOOTLOADER FOUND";
            return true;
79 80 81 82 83 84
       }
    }
    // Not found
    return false;
}

85 86
void SerialLink::writeBytes(const char* data, qint64 size)
{
87
    if(_port && _port->isOpen()) {
88 89
        _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch());
        _port->write(data, size);
90 91
    } else {
        // Error occured
92
        _emitLinkError(tr("Could not send data - link %1 is disconnected!").arg(getName()));
pixhawk's avatar
pixhawk committed
93 94 95 96 97 98 99 100
    }
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
Don Gagne's avatar
Don Gagne committed
101
void SerialLink::_disconnect(void)
102
{
103 104 105 106
    if (_port) {
        _port->close();
        delete _port;
        _port = NULL;
107
    }
108

dogmaphobic's avatar
dogmaphobic committed
109
#ifdef __android__
110
    qgcApp()->toolbox()->linkManager()->suspendConfigurationUpdates(false);
dogmaphobic's avatar
dogmaphobic committed
111
#endif
pixhawk's avatar
pixhawk committed
112 113 114 115 116 117 118
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
119
bool SerialLink::_connect(void)
120
{
121
    qCDebug(SerialLinkLog) << "CONNECT CALLED";
122 123

    _disconnect();
124

dogmaphobic's avatar
dogmaphobic committed
125
#ifdef __android__
126
    qgcApp()->toolbox()->linkManager()->suspendConfigurationUpdates(true);
127
#endif
128

129 130 131
    QSerialPort::SerialPortError    error;
    QString                         errorString;

dogmaphobic's avatar
dogmaphobic committed
132
    // Initialize the connection
133 134 135 136 137 138 139
    if (!_hardwareConnect(error, errorString)) {
        if (qgcApp()->toolbox()->linkManager()->isAutoconnectLink(this)) {
            // Be careful with spitting out open error related to trying to open a busy port using autoconnect
            if (error == QSerialPort::PermissionError) {
                // Device already open, ignore and fail connect
                return false;
            }
dogmaphobic's avatar
dogmaphobic committed
140
        }
141 142

        _emitLinkError(QString("Error connecting: Could not create port. %1").arg(errorString));
dogmaphobic's avatar
dogmaphobic committed
143 144
        return false;
    }
145
    return true;
pixhawk's avatar
pixhawk committed
146 147
}

148 149 150 151 152
/// Performs the actual hardware port connection.
///     @param[out] error if failed
///     @param[out] error string if failed
/// @return success/fail
bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& errorString)
153
{
154
    if (_port) {
155
        qCDebug(SerialLinkLog) << "SerialLink:" << QString::number((long)this, 16) << "closing port";
156
        _port->close();
157
        QGC::SLEEP::usleep(50000);
158 159
        delete _port;
        _port = NULL;
160
    }
pixhawk's avatar
pixhawk committed
161

162
    qCDebug(SerialLinkLog) << "SerialLink: hardwareConnect to " << _config->portName();
163

164
    // If we are in the Pixhawk bootloader code wait for it to timeout
165
    if (_isBootloader()) {
166
        qCDebug(SerialLinkLog) << "Not connecting to a bootloader, waiting for 2nd chance";
167 168 169
        const unsigned retry_limit = 12;
        unsigned retries;
        for (retries = 0; retries < retry_limit; retries++) {
170 171
            if (!_isBootloader()) {
                QGC::SLEEP::msleep(500);
172 173 174 175 176 177 178
                break;
            }
            QGC::SLEEP::msleep(500);
        }
        // Check limit
        if (retries == retry_limit) {
            // bail out
179
            qWarning() << "Timeout waiting for something other than booloader";
180 181 182 183
            return false;
        }
    }

184 185 186
    _port = new QSerialPort(_config->portName());
    if (!_port) {
        emit communicationUpdate(getName(),"Error opening port: " + _config->portName());
Bill Bonney's avatar
Bill Bonney committed
187
        return false; // couldn't create serial port.
188
    }
Bill Bonney's avatar
Bill Bonney committed
189

190 191
    QObject::connect(_port, static_cast<void (QSerialPort::*)(QSerialPort::SerialPortError)>(&QSerialPort::error),
                     this, &SerialLink::linkError);
192
    QObject::connect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes);
Bill Bonney's avatar
Bill Bonney committed
193

194
    //  port->setCommTimeouts(QSerialPort::CtScheme_NonBlockingRead);
pixhawk's avatar
pixhawk committed
195

196
    // TODO This needs a bit of TLC still...
197

198
    // After the bootloader times out, it still can take a second or so for the Pixhawk USB driver to come up and make
199
    // the port available for open. So we retry a few times to wait for it.
dogmaphobic's avatar
dogmaphobic committed
200 201 202
#ifdef __android__
    _port->open(QIODevice::ReadWrite);
#else
203 204
    for (int openRetries = 0; openRetries < 4; openRetries++) {
        if (!_port->open(QIODevice::ReadWrite)) {
205
            qCDebug(SerialLinkLog) << "Port open failed, retrying";
206 207 208 209
            QGC::SLEEP::msleep(500);
        } else {
            break;
        }
210
    }
dogmaphobic's avatar
dogmaphobic committed
211
#endif
212
    if (!_port->isOpen() ) {
213 214 215
        qDebug() << "open failed" << _port->errorString() << _port->error() << getName() << qgcApp()->toolbox()->linkManager()->isAutoconnectLink(this);
        error = _port->error();
        errorString = _port->errorString();
216 217
        emit communicationUpdate(getName(),"Error opening port: " + _port->errorString());
        _port->close();
218 219
        delete _port;
        _port = NULL;
220
        return false; // couldn't open serial port
221 222
    }

223
    qCDebug(SerialLinkLog) << "Configuring port";
224 225 226 227 228
    _port->setBaudRate     (_config->baud());
    _port->setDataBits     (static_cast<QSerialPort::DataBits>     (_config->dataBits()));
    _port->setFlowControl  (static_cast<QSerialPort::FlowControl>  (_config->flowControl()));
    _port->setStopBits     (static_cast<QSerialPort::StopBits>     (_config->stopBits()));
    _port->setParity       (static_cast<QSerialPort::Parity>       (_config->parity()));
229

230
    emit communicationUpdate(getName(), "Opened port!");
Bill Bonney's avatar
Bill Bonney committed
231
    emit connected();
232

233
    qCDebug(SerialLinkLog) << "Connection SeriaLink: " << "with settings" << _config->portName()
234
             << _config->baud() << _config->dataBits() << _config->parity() << _config->stopBits();
235

Bill Bonney's avatar
Bill Bonney committed
236
    return true; // successful connection
pixhawk's avatar
pixhawk committed
237
}
238

239 240 241
void SerialLink::_readBytes(void)
{
    qint64 byteCount = _port->bytesAvailable();
242
    if (byteCount) {
243 244 245 246 247 248 249
        QByteArray buffer;
        buffer.resize(byteCount);
        _port->read(buffer.data(), buffer.size());
        emit bytesReceived(this, buffer);
    }
}

250
void SerialLink::linkError(QSerialPort::SerialPortError error)
251
{
252
    if (error != QSerialPort::NoError) {
253 254 255 256
        // You can use the following qDebug output as needed during development. Make sure to comment it back out
        // when you are done. The reason for this is that this signal is very noisy. For example if you try to
        // connect to a PixHawk before it is ready to accept the connection it will output a continuous stream
        // of errors until the Pixhawk responds.
257
        //qCDebug(SerialLinkLog) << "SerialLink::linkError" << error;
258
    }
259 260
}

pixhawk's avatar
pixhawk committed
261 262 263 264 265
/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
266
bool SerialLink::isConnected() const
267
{
268
    bool isConnected = false;
Bill Bonney's avatar
Bill Bonney committed
269

270
    if (_port) {
271
        isConnected = _port->isOpen();
lm's avatar
lm committed
272
    }
273

274
    return isConnected;
pixhawk's avatar
pixhawk committed
275 276
}

277
QString SerialLink::getName() const
pixhawk's avatar
pixhawk committed
278
{
279
    return _config->portName();
pixhawk's avatar
pixhawk committed
280 281
}

282 283 284 285
/**
  * This function maps baud rate constants to numerical equivalents.
  * It relies on the mapping given in qportsettings.h from the QSerialPort library.
  */
286
qint64 SerialLink::getConnectionSpeed() const
287
{
Bill Bonney's avatar
Bill Bonney committed
288
    int baudRate;
289 290
    if (_port) {
        baudRate = _port->baudRate();
Bill Bonney's avatar
Bill Bonney committed
291
    } else {
292
        baudRate = _config->baud();
Bill Bonney's avatar
Bill Bonney committed
293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324
    }
    qint64 dataRate;
    switch (baudRate)
    {
        case QSerialPort::Baud1200:
            dataRate = 1200;
            break;
        case QSerialPort::Baud2400:
            dataRate = 2400;
            break;
        case QSerialPort::Baud4800:
            dataRate = 4800;
            break;
        case QSerialPort::Baud9600:
            dataRate = 9600;
            break;
        case QSerialPort::Baud19200:
            dataRate = 19200;
            break;
        case QSerialPort::Baud38400:
            dataRate = 38400;
            break;
        case QSerialPort::Baud57600:
            dataRate = 57600;
            break;
        case QSerialPort::Baud115200:
            dataRate = 115200;
            break;
            // Otherwise do nothing.
        default:
            dataRate = -1;
            break;
pixhawk's avatar
pixhawk committed
325 326 327 328
    }
    return dataRate;
}

329
void SerialLink::_resetConfiguration()
330
{
331
    if (_port) {
Don Gagne's avatar
Don Gagne committed
332 333 334 335 336
        _port->setBaudRate      (_config->baud());
        _port->setDataBits      (static_cast<QSerialPort::DataBits>    (_config->dataBits()));
        _port->setFlowControl   (static_cast<QSerialPort::FlowControl> (_config->flowControl()));
        _port->setStopBits      (static_cast<QSerialPort::StopBits>    (_config->stopBits()));
        _port->setParity        (static_cast<QSerialPort::Parity>      (_config->parity()));
337
    }
pixhawk's avatar
pixhawk committed
338 339
}

340
void SerialLink::_emitLinkError(const QString& errorMsg)
341
{
342
    QString msg("Error on link %1. %2");
343
    qDebug() << errorMsg;
344
    emit communicationError(tr("Link Error"), msg.arg(getName()).arg(errorMsg));
pixhawk's avatar
pixhawk committed
345 346
}

347
LinkConfiguration* SerialLink::getLinkConfiguration()
348
{
349
    return _config;
pixhawk's avatar
pixhawk committed
350 351
}

352 353
//--------------------------------------------------------------------------
//-- SerialConfiguration
pixhawk's avatar
pixhawk committed
354

355
SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguration(name)
356
{
357 358 359 360 361
    _baud       = 57600;
    _flowControl= QSerialPort::NoFlowControl;
    _parity     = QSerialPort::NoParity;
    _dataBits   = 8;
    _stopBits   = 1;
362
    _usbDirect  = false;
pixhawk's avatar
pixhawk committed
363 364
}

365
SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfiguration(copy)
366
{
367 368 369 370 371 372 373
    _baud               = copy->baud();
    _flowControl        = copy->flowControl();
    _parity             = copy->parity();
    _dataBits           = copy->dataBits();
    _stopBits           = copy->stopBits();
    _portName           = copy->portName();
    _portDisplayName    = copy->portDisplayName();
374
    _usbDirect          = copy->_usbDirect;
pixhawk's avatar
pixhawk committed
375 376
}

377
void SerialConfiguration::copyFrom(LinkConfiguration *source)
378
{
379 380 381
    LinkConfiguration::copyFrom(source);
    SerialConfiguration* ssource = dynamic_cast<SerialConfiguration*>(source);
    Q_ASSERT(ssource != NULL);
382 383 384 385 386 387 388
    _baud               = ssource->baud();
    _flowControl        = ssource->flowControl();
    _parity             = ssource->parity();
    _dataBits           = ssource->dataBits();
    _stopBits           = ssource->stopBits();
    _portName           = ssource->portName();
    _portDisplayName    = ssource->portDisplayName();
389
    _usbDirect          = ssource->_usbDirect;
390 391
}

392
void SerialConfiguration::updateSettings()
393
{
394 395 396 397 398
    if(_link) {
        SerialLink* serialLink = dynamic_cast<SerialLink*>(_link);
        if(serialLink) {
            serialLink->_resetConfiguration();
        }
399
    }
400 401
}

402
void SerialConfiguration::setBaud(int baud)
pixhawk's avatar
pixhawk committed
403
{
404
    _baud = baud;
pixhawk's avatar
pixhawk committed
405 406
}

407
void SerialConfiguration::setDataBits(int databits)
pixhawk's avatar
pixhawk committed
408
{
409
    _dataBits = databits;
pixhawk's avatar
pixhawk committed
410 411
}

412
void SerialConfiguration::setFlowControl(int flowControl)
pixhawk's avatar
pixhawk committed
413
{
414
    _flowControl = flowControl;
pixhawk's avatar
pixhawk committed
415 416
}

417
void SerialConfiguration::setStopBits(int stopBits)
418
{
419
    _stopBits = stopBits;
pixhawk's avatar
pixhawk committed
420 421
}

422
void SerialConfiguration::setParity(int parity)
423
{
424
    _parity = parity;
pixhawk's avatar
pixhawk committed
425 426
}

427
void SerialConfiguration::setPortName(const QString& portName)
428
{
429 430 431 432
    // No effect on a running connection
    QString pname = portName.trimmed();
    if (!pname.isEmpty() && pname != _portName) {
        _portName = pname;
433
        _portDisplayName = cleanPortDisplayname(pname);
434 435 436
    }
}

437 438 439
QString SerialConfiguration::cleanPortDisplayname(const QString name)
{
    QString pname = name.trimmed();
440
#ifdef Q_OS_WIN
441 442 443 444 445 446 447 448
    pname.replace("\\\\.\\", "");
#else
    pname.replace("/dev/cu.", "");
    pname.replace("/dev/", "");
#endif
    return pname;
}

449
void SerialConfiguration::saveSettings(QSettings& settings, const QString& root)
450
{
451
    settings.beginGroup(root);
452 453 454 455 456 457 458
    settings.setValue("baud",           _baud);
    settings.setValue("dataBits",       _dataBits);
    settings.setValue("flowControl",    _flowControl);
    settings.setValue("stopBits",       _stopBits);
    settings.setValue("parity",         _parity);
    settings.setValue("portName",       _portName);
    settings.setValue("portDisplayName",_portDisplayName);
459
    settings.endGroup();
460
}
461

462
void SerialConfiguration::loadSettings(QSettings& settings, const QString& root)
463
{
464
    settings.beginGroup(root);
465 466 467 468 469 470 471
    if(settings.contains("baud"))           _baud           = settings.value("baud").toInt();
    if(settings.contains("dataBits"))       _dataBits       = settings.value("dataBits").toInt();
    if(settings.contains("flowControl"))    _flowControl    = settings.value("flowControl").toInt();
    if(settings.contains("stopBits"))       _stopBits       = settings.value("stopBits").toInt();
    if(settings.contains("parity"))         _parity         = settings.value("parity").toInt();
    if(settings.contains("portName"))       _portName       = settings.value("portName").toString();
    if(settings.contains("portDisplayName"))_portDisplayName= settings.value("portDisplayName").toString();
472
    settings.endGroup();
473
}
474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530

QStringList SerialConfiguration::supportedBaudRates()
{
    if(!kSupportedBaudRates.size())
        _initBaudRates();
    return kSupportedBaudRates;
}

void SerialConfiguration::_initBaudRates()
{
    kSupportedBaudRates.clear();
#if USE_ANCIENT_RATES
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
    kSupportedBaudRates << "50";
    kSupportedBaudRates << "75";
#endif
    kSupportedBaudRates << "110";
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
    kSupportedBaudRates << "134";
    kSupportedBaudRates << "150";
    kSupportedBaudRates << "200";
#endif
    kSupportedBaudRates << "300";
    kSupportedBaudRates << "600";
    kSupportedBaudRates << "1200";
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
    kSupportedBaudRates << "1800";
#endif
#endif
    kSupportedBaudRates << "2400";
    kSupportedBaudRates << "4800";
    kSupportedBaudRates << "9600";
#if defined(Q_OS_WIN)
    kSupportedBaudRates << "14400";
#endif
    kSupportedBaudRates << "19200";
    kSupportedBaudRates << "38400";
#if defined(Q_OS_WIN)
    kSupportedBaudRates << "56000";
#endif
    kSupportedBaudRates << "57600";
    kSupportedBaudRates << "115200";
#if defined(Q_OS_WIN)
    kSupportedBaudRates << "128000";
#endif
    kSupportedBaudRates << "230400";
#if defined(Q_OS_WIN)
    kSupportedBaudRates << "256000";
#endif
    kSupportedBaudRates << "460800";
#if defined(Q_OS_LINUX)
    kSupportedBaudRates << "500000";
    kSupportedBaudRates << "576000";
#endif
    kSupportedBaudRates << "921600";
}

531 532 533 534 535 536 537
void SerialConfiguration::setUsbDirect(bool usbDirect)
{
    if (_usbDirect != usbDirect) {
        _usbDirect = usbDirect;
        emit usbDirectChanged(_usbDirect);
    }
}