SerialLink.cc 17.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
SerialLink::SerialLink(SharedLinkConfigurationPointer& config, bool isPX4Flow)
    : LinkInterface(config, isPX4Flow)
35
    , _port(nullptr)
36 37 38 39
    , _bytesRead(0)
    , _stopp(false)
    , _reqReset(false)
    , _serialConfig(qobject_cast<SerialConfiguration*>(config.data()))
40
{
DonLakeFlyer's avatar
DonLakeFlyer committed
41 42 43 44
    if (!_serialConfig) {
        qWarning() << "Internal error";
        return;
    }
45 46

    qCDebug(SerialLinkLog) << "Create SerialLink " << _serialConfig->portName() << _serialConfig->baud() << _serialConfig->flowControl()
DonLakeFlyer's avatar
DonLakeFlyer committed
47
                           << _serialConfig->parity() << _serialConfig->dataBits() << _serialConfig->stopBits();
48
    qCDebug(SerialLinkLog) << "portName: " << _serialConfig->portName();
pixhawk's avatar
pixhawk committed
49
}
50

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

SerialLink::~SerialLink()
{
59
    _disconnect();
60 61
}

62
bool SerialLink::_isBootloader()
63
{
64
    QList<QSerialPortInfo> portList = QSerialPortInfo::availablePorts();
65 66 67
    if( portList.count() == 0){
        return false;
    }
68
    for (const QSerialPortInfo &info: portList)
69
    {
70 71
        qCDebug(SerialLinkLog) << "PortName    : " << info.portName() << "Description : " << info.description();
        qCDebug(SerialLinkLog) << "Manufacturer: " << info.manufacturer();
72
        if (info.portName().trimmed() == _serialConfig->portName() &&
73 74 75 76 77
                (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;
DonLakeFlyer's avatar
DonLakeFlyer committed
78
        }
79 80 81 82 83
    }
    // Not found
    return false;
}

84
void SerialLink::_writeBytes(const QByteArray data)
85
{
86
    if(_port && _port->isOpen()) {
87
        emit bytesSent(this, data);
88 89
        _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
        _port->write(data);
90
    } else {
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
91
        // Error occurred
92
        qWarning() << "Serial port not writeable";
93
        _emitLinkError(tr("Could not send data - link %1 is disconnected!").arg(getName()));
pixhawk's avatar
pixhawk committed
94 95 96 97 98 99 100 101
    }
}

/**
 * @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
102
void SerialLink::_disconnect(void)
103
{
104 105
    if (_port) {
        _port->close();
106
        _port->deleteLater();
107
        _port = nullptr;
108
    }
109

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

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

    _disconnect();
125

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

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

dogmaphobic's avatar
dogmaphobic committed
133
    // Initialize the connection
134 135 136 137 138 139 140
    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
141
        }
142

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

149 150 151 152 153
/// 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)
154
{
155
    if (_port) {
DonLakeFlyer's avatar
DonLakeFlyer committed
156
        qCDebug(SerialLinkLog) << "SerialLink:" << QString::number((qulonglong)this, 16) << "closing port";
157
        _port->close();
158 159 160 161 162 163

        // Wait 50 ms while continuing to run the event queue
        for (unsigned i = 0; i < 10; i++) {
            QGC::SLEEP::usleep(5000);
            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
        }
164
        delete _port;
165
        _port = nullptr;
166
    }
pixhawk's avatar
pixhawk committed
167

168
    qCDebug(SerialLinkLog) << "SerialLink: hardwareConnect to " << _serialConfig->portName();
169

170
    // If we are in the Pixhawk bootloader code wait for it to timeout
171
    if (_isBootloader()) {
172
        qCDebug(SerialLinkLog) << "Not connecting to a bootloader, waiting for 2nd chance";
173 174
        const unsigned retry_limit = 12;
        unsigned retries;
175

176
        for (retries = 0; retries < retry_limit; retries++) {
177
            if (!_isBootloader()) {
178 179 180 181 182
                // Wait 500 ms while continuing to run the event loop
                for (unsigned i = 0; i < 100; i++) {
                    QGC::SLEEP::msleep(5);
                    qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
                }
183 184
                break;
            }
185 186 187 188 189 190

            // Wait 500 ms while continuing to run the event loop
            for (unsigned i = 0; i < 100; i++) {
                QGC::SLEEP::msleep(5);
                qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
            }
191 192 193 194
        }
        // Check limit
        if (retries == retry_limit) {
            // bail out
195
            qWarning() << "Timeout waiting for something other than booloader";
196 197 198 199
            return false;
        }
    }

200
    _port = new QSerialPort(_serialConfig->portName(), this);
Bill Bonney's avatar
Bill Bonney committed
201

202 203
    QObject::connect(_port, static_cast<void (QSerialPort::*)(QSerialPort::SerialPortError)>(&QSerialPort::error),
                     this, &SerialLink::linkError);
204
    QObject::connect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes);
Bill Bonney's avatar
Bill Bonney committed
205

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

208
    // TODO This needs a bit of TLC still...
209

210
    // After the bootloader times out, it still can take a second or so for the Pixhawk USB driver to come up and make
211
    // the port available for open. So we retry a few times to wait for it.
dogmaphobic's avatar
dogmaphobic committed
212 213 214
#ifdef __android__
    _port->open(QIODevice::ReadWrite);
#else
215 216 217

    // Try to open the port three times
    for (int openRetries = 0; openRetries < 3; openRetries++) {
218
        if (!_port->open(QIODevice::ReadWrite)) {
219
            qCDebug(SerialLinkLog) << "Port open failed, retrying";
220 221 222 223 224 225
            // Wait 250 ms while continuing to run the event loop
            for (unsigned i = 0; i < 50; i++) {
                QGC::SLEEP::msleep(5);
                qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
            }
            qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
226 227 228
        } else {
            break;
        }
229
    }
dogmaphobic's avatar
dogmaphobic committed
230
#endif
231
    if (!_port->isOpen() ) {
232 233 234
        qDebug() << "open failed" << _port->errorString() << _port->error() << getName() << qgcApp()->toolbox()->linkManager()->isAutoconnectLink(this);
        error = _port->error();
        errorString = _port->errorString();
235
        emit communicationUpdate(getName(), tr("Error opening port: %1").arg(_port->errorString()));
236
        _port->close();
237
        delete _port;
238
        _port = nullptr;
239
        return false; // couldn't open serial port
240 241
    }

242 243
    _port->setDataTerminalReady(true);

244
    qCDebug(SerialLinkLog) << "Configuring port";
245 246 247 248 249
    _port->setBaudRate     (_serialConfig->baud());
    _port->setDataBits     (static_cast<QSerialPort::DataBits>     (_serialConfig->dataBits()));
    _port->setFlowControl  (static_cast<QSerialPort::FlowControl>  (_serialConfig->flowControl()));
    _port->setStopBits     (static_cast<QSerialPort::StopBits>     (_serialConfig->stopBits()));
    _port->setParity       (static_cast<QSerialPort::Parity>       (_serialConfig->parity()));
250

251
    emit communicationUpdate(getName(), "Opened port!");
Bill Bonney's avatar
Bill Bonney committed
252
    emit connected();
253

254
    qCDebug(SerialLinkLog) << "Connection SeriaLink: " << "with settings" << _serialConfig->portName()
DonLakeFlyer's avatar
DonLakeFlyer committed
255
                           << _serialConfig->baud() << _serialConfig->dataBits() << _serialConfig->parity() << _serialConfig->stopBits();
256

Bill Bonney's avatar
Bill Bonney committed
257
    return true; // successful connection
pixhawk's avatar
pixhawk committed
258
}
259

260 261
void SerialLink::_readBytes(void)
{
262 263 264 265 266 267 268 269 270 271 272 273
    if (_port && _port->isOpen()) {
        qint64 byteCount = _port->bytesAvailable();
        if (byteCount) {
            QByteArray buffer;
            buffer.resize(byteCount);
            _port->read(buffer.data(), buffer.size());
            emit bytesReceived(this, buffer);
        }
    } else {
        // Error occurred
        qWarning() << "Serial port not readable";
        _emitLinkError(tr("Could not read data - link %1 is disconnected!").arg(getName()));
274 275 276
    }
}

277
void SerialLink::linkError(QSerialPort::SerialPortError error)
278
{
279 280 281 282 283 284 285
    switch (error) {
    case QSerialPort::NoError:
        break;
    case QSerialPort::ResourceError:
        emit connectionRemoved(this);
        break;
    default:
286 287 288 289
        // 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.
290
        //qCDebug(SerialLinkLog) << "SerialLink::linkError" << error;
291
        break;
292
    }
293 294
}

pixhawk's avatar
pixhawk committed
295 296 297 298 299
/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
300
bool SerialLink::isConnected() const
301
{
302
    bool isConnected = false;
Bill Bonney's avatar
Bill Bonney committed
303

304
    if (_port) {
305
        isConnected = _port->isOpen();
lm's avatar
lm committed
306
    }
307

308
    return isConnected;
pixhawk's avatar
pixhawk committed
309 310
}

311
QString SerialLink::getName() const
pixhawk's avatar
pixhawk committed
312
{
313
    return _serialConfig->name();
pixhawk's avatar
pixhawk committed
314 315
}

316 317 318 319
/**
  * This function maps baud rate constants to numerical equivalents.
  * It relies on the mapping given in qportsettings.h from the QSerialPort library.
  */
320
qint64 SerialLink::getConnectionSpeed() const
321
{
Bill Bonney's avatar
Bill Bonney committed
322
    int baudRate;
323 324
    if (_port) {
        baudRate = _port->baudRate();
Bill Bonney's avatar
Bill Bonney committed
325
    } else {
326
        baudRate = _serialConfig->baud();
Bill Bonney's avatar
Bill Bonney committed
327 328 329 330
    }
    qint64 dataRate;
    switch (baudRate)
    {
DonLakeFlyer's avatar
DonLakeFlyer committed
331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358
    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
359 360 361 362
    }
    return dataRate;
}

363
void SerialLink::_resetConfiguration()
364
{
365
    if (_port) {
366 367 368 369 370
        _port->setBaudRate      (_serialConfig->baud());
        _port->setDataBits      (static_cast<QSerialPort::DataBits>    (_serialConfig->dataBits()));
        _port->setFlowControl   (static_cast<QSerialPort::FlowControl> (_serialConfig->flowControl()));
        _port->setStopBits      (static_cast<QSerialPort::StopBits>    (_serialConfig->stopBits()));
        _port->setParity        (static_cast<QSerialPort::Parity>      (_serialConfig->parity()));
371
    }
pixhawk's avatar
pixhawk committed
372 373
}

374
void SerialLink::_emitLinkError(const QString& errorMsg)
375
{
376
    QString msg("Error on link %1. %2");
377
    qDebug() << errorMsg;
378
    emit communicationError(tr("Link Error"), msg.arg(getName()).arg(errorMsg));
pixhawk's avatar
pixhawk committed
379 380
}

381 382
//--------------------------------------------------------------------------
//-- SerialConfiguration
pixhawk's avatar
pixhawk committed
383

384
SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguration(name)
385
{
386 387 388 389 390
    _baud       = 57600;
    _flowControl= QSerialPort::NoFlowControl;
    _parity     = QSerialPort::NoParity;
    _dataBits   = 8;
    _stopBits   = 1;
391
    _usbDirect  = false;
pixhawk's avatar
pixhawk committed
392 393
}

394
SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfiguration(copy)
395
{
396 397 398 399 400 401 402
    _baud               = copy->baud();
    _flowControl        = copy->flowControl();
    _parity             = copy->parity();
    _dataBits           = copy->dataBits();
    _stopBits           = copy->stopBits();
    _portName           = copy->portName();
    _portDisplayName    = copy->portDisplayName();
403
    _usbDirect          = copy->_usbDirect;
pixhawk's avatar
pixhawk committed
404 405
}

406
void SerialConfiguration::copyFrom(LinkConfiguration *source)
407
{
408
    LinkConfiguration::copyFrom(source);
409
    auto* ssource = qobject_cast<SerialConfiguration*>(source);
DonLakeFlyer's avatar
DonLakeFlyer committed
410 411 412 413 414 415 416 417 418 419 420 421
    if (ssource) {
        _baud               = ssource->baud();
        _flowControl        = ssource->flowControl();
        _parity             = ssource->parity();
        _dataBits           = ssource->dataBits();
        _stopBits           = ssource->stopBits();
        _portName           = ssource->portName();
        _portDisplayName    = ssource->portDisplayName();
        _usbDirect          = ssource->_usbDirect;
    } else {
        qWarning() << "Internal error";
    }
422 423
}

424
void SerialConfiguration::updateSettings()
425
{
426
    if(_link) {
427
        auto* serialLink = qobject_cast<SerialLink*>(_link);
428 429 430
        if(serialLink) {
            serialLink->_resetConfiguration();
        }
431
    }
432 433
}

434
void SerialConfiguration::setBaud(int baud)
pixhawk's avatar
pixhawk committed
435
{
436
    _baud = baud;
pixhawk's avatar
pixhawk committed
437 438
}

439
void SerialConfiguration::setDataBits(int databits)
pixhawk's avatar
pixhawk committed
440
{
441
    _dataBits = databits;
pixhawk's avatar
pixhawk committed
442 443
}

444
void SerialConfiguration::setFlowControl(int flowControl)
pixhawk's avatar
pixhawk committed
445
{
446
    _flowControl = flowControl;
pixhawk's avatar
pixhawk committed
447 448
}

449
void SerialConfiguration::setStopBits(int stopBits)
450
{
451
    _stopBits = stopBits;
pixhawk's avatar
pixhawk committed
452 453
}

454
void SerialConfiguration::setParity(int parity)
455
{
456
    _parity = parity;
pixhawk's avatar
pixhawk committed
457 458
}

459
void SerialConfiguration::setPortName(const QString& portName)
460
{
461 462 463 464
    // No effect on a running connection
    QString pname = portName.trimmed();
    if (!pname.isEmpty() && pname != _portName) {
        _portName = pname;
465
        _portDisplayName = cleanPortDisplayname(pname);
466 467 468
    }
}

469 470 471
QString SerialConfiguration::cleanPortDisplayname(const QString name)
{
    QString pname = name.trimmed();
472
#ifdef Q_OS_WIN
473 474 475 476 477 478 479 480
    pname.replace("\\\\.\\", "");
#else
    pname.replace("/dev/cu.", "");
    pname.replace("/dev/", "");
#endif
    return pname;
}

481
void SerialConfiguration::saveSettings(QSettings& settings, const QString& root)
482
{
483
    settings.beginGroup(root);
484 485 486 487 488 489 490
    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);
491
    settings.endGroup();
492
}
493

494
void SerialConfiguration::loadSettings(QSettings& settings, const QString& root)
495
{
496
    settings.beginGroup(root);
497 498 499 500 501 502 503
    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();
504
    settings.endGroup();
505
}
506 507 508 509 510 511 512 513 514 515 516

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

void SerialConfiguration::_initBaudRates()
{
    kSupportedBaudRates.clear();
517
    kSupportedBaudRates = QStringList({
518 519
#if USE_ANCIENT_RATES
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
520 521
        "50",
        "75",
522
#endif
523
        "110",
524
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
525 526 527
        "150",
        "200" ,
        "134"  ,
528
#endif
529 530 531
        "300",
        "600",
        "1200",
532
#if defined(Q_OS_UNIX) || defined(Q_OS_LINUX) || defined(Q_OS_DARWIN)
533
        "1800",
534 535
#endif
#endif
536 537 538
        "2400",
        "4800",
        "9600",
539
#if defined(Q_OS_WIN)
540
        "14400",
541
#endif
542 543
        "19200",
        "38400",
544
#if defined(Q_OS_WIN)
545
        "56000",
546
#endif
547 548
        "57600",
        "115200",
549
#if defined(Q_OS_WIN)
550
        "128000",
551
#endif
552
        "230400",
553
#if defined(Q_OS_WIN)
554
        "256000",
555
#endif
556 557
        "460800",
        "500000",
558
#if defined(Q_OS_LINUX)
559
        "576000",
560
#endif
561 562
        "921600",
    });
563 564
}

565 566 567 568 569 570 571
void SerialConfiguration::setUsbDirect(bool usbDirect)
{
    if (_usbDirect != usbDirect) {
        _usbDirect = usbDirect;
        emit usbDirectChanged(_usbDirect);
    }
}