SerialLink.cc 17.5 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 36 37 38 39
    , _port(NULL)
    , _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 68 69
    if( portList.count() == 0){
        return false;
    }
    foreach (const QSerialPortInfo &info, portList)
    {
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 88
        _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch());
        _port->write(data);
89
    } else {
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
90
        // Error occurred
91
        qWarning() << "Serial port not writeable";
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
    if (_port) {
        _port->close();
105
        _port->deleteLater();
106
        _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(tr("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 158 159 160 161 162

        // 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);
        }
163 164
        delete _port;
        _port = NULL;
165
    }
pixhawk's avatar
pixhawk committed
166

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

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

175
        for (retries = 0; retries < retry_limit; retries++) {
176
            if (!_isBootloader()) {
177 178 179 180 181
                // 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);
                }
182 183
                break;
            }
184 185 186 187 188 189

            // 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);
            }
190 191 192 193
        }
        // Check limit
        if (retries == retry_limit) {
            // bail out
194
            qWarning() << "Timeout waiting for something other than booloader";
195 196 197 198
            return false;
        }
    }

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

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

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

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

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

    // Try to open the port three times
    for (int openRetries = 0; openRetries < 3; openRetries++) {
217
        if (!_port->open(QIODevice::ReadWrite)) {
218
            qCDebug(SerialLinkLog) << "Port open failed, retrying";
219 220 221 222 223 224
            // 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);
225 226 227
        } else {
            break;
        }
228
    }
dogmaphobic's avatar
dogmaphobic committed
229
#endif
230
    if (!_port->isOpen() ) {
231 232 233
        qDebug() << "open failed" << _port->errorString() << _port->error() << getName() << qgcApp()->toolbox()->linkManager()->isAutoconnectLink(this);
        error = _port->error();
        errorString = _port->errorString();
234
        emit communicationUpdate(getName(), tr("Error opening port: %1").arg(_port->errorString()));
235
        _port->close();
236 237
        delete _port;
        _port = NULL;
238
        return false; // couldn't open serial port
239 240
    }

241 242
    _port->setDataTerminalReady(true);

243
    qCDebug(SerialLinkLog) << "Configuring port";
244 245 246 247 248
    _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()));
249

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

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

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

259 260
void SerialLink::_readBytes(void)
{
261 262 263 264 265 266 267 268 269 270 271 272
    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()));
273 274 275
    }
}

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

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

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

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

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

315 316 317 318
/**
  * This function maps baud rate constants to numerical equivalents.
  * It relies on the mapping given in qportsettings.h from the QSerialPort library.
  */
319
qint64 SerialLink::getConnectionSpeed() const
320
{
Bill Bonney's avatar
Bill Bonney committed
321
    int baudRate;
322 323
    if (_port) {
        baudRate = _port->baudRate();
Bill Bonney's avatar
Bill Bonney committed
324
    } else {
325
        baudRate = _serialConfig->baud();
Bill Bonney's avatar
Bill Bonney committed
326 327 328 329
    }
    qint64 dataRate;
    switch (baudRate)
    {
DonLakeFlyer's avatar
DonLakeFlyer committed
330 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
    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
358 359 360 361
    }
    return dataRate;
}

362
void SerialLink::_resetConfiguration()
363
{
364
    if (_port) {
365 366 367 368 369
        _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()));
370
    }
pixhawk's avatar
pixhawk committed
371 372
}

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

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

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

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

405
void SerialConfiguration::copyFrom(LinkConfiguration *source)
406
{
407 408
    LinkConfiguration::copyFrom(source);
    SerialConfiguration* ssource = dynamic_cast<SerialConfiguration*>(source);
DonLakeFlyer's avatar
DonLakeFlyer committed
409 410 411 412 413 414 415 416 417 418 419 420
    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";
    }
421 422
}

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

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

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

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

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

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

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

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

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

493
void SerialConfiguration::loadSettings(QSettings& settings, const QString& root)
494
{
495
    settings.beginGroup(root);
496 497 498 499 500 501 502
    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();
503
    settings.endGroup();
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 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555

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";
    kSupportedBaudRates << "500000";
556
#if defined(Q_OS_LINUX)
557 558 559 560 561
    kSupportedBaudRates << "576000";
#endif
    kSupportedBaudRates << "921600";
}

562 563 564 565 566 567 568
void SerialConfiguration::setUsbDirect(bool usbDirect)
{
    if (_usbDirect != usbDirect) {
        _usbDirect = usbDirect;
        emit usbDirectChanged(_usbDirect);
    }
}