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
void SerialLink::_writeBytes(const QByteArray data)
86
{
87
    if(_port && _port->isOpen()) {
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
        _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
    _port = new QSerialPort(_config->portName());
Bill Bonney's avatar
Bill Bonney committed
185

186 187
    QObject::connect(_port, static_cast<void (QSerialPort::*)(QSerialPort::SerialPortError)>(&QSerialPort::error),
                     this, &SerialLink::linkError);
188
    QObject::connect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes);
Bill Bonney's avatar
Bill Bonney committed
189

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

192
    // TODO This needs a bit of TLC still...
193

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

219 220
    _port->setDataTerminalReady(true);

221
    qCDebug(SerialLinkLog) << "Configuring port";
222 223 224 225 226
    _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()));
227

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

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

Bill Bonney's avatar
Bill Bonney committed
234
    return true; // successful connection
pixhawk's avatar
pixhawk committed
235
}
236

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

248
void SerialLink::linkError(QSerialPort::SerialPortError error)
249
{
250 251 252 253 254 255 256
    switch (error) {
    case QSerialPort::NoError:
        break;
    case QSerialPort::ResourceError:
        emit connectionRemoved(this);
        break;
    default:
257 258 259 260
        // 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.
261
        //qCDebug(SerialLinkLog) << "SerialLink::linkError" << error;
262
        break;
263
    }
264 265
}

pixhawk's avatar
pixhawk committed
266 267 268 269 270
/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
271
bool SerialLink::isConnected() const
272
{
273
    bool isConnected = false;
Bill Bonney's avatar
Bill Bonney committed
274

275
    if (_port) {
276
        isConnected = _port->isOpen();
lm's avatar
lm committed
277
    }
278

279
    return isConnected;
pixhawk's avatar
pixhawk committed
280 281
}

282
QString SerialLink::getName() const
pixhawk's avatar
pixhawk committed
283
{
284
    return _config->portName();
pixhawk's avatar
pixhawk committed
285 286
}

287 288 289 290
/**
  * This function maps baud rate constants to numerical equivalents.
  * It relies on the mapping given in qportsettings.h from the QSerialPort library.
  */
291
qint64 SerialLink::getConnectionSpeed() const
292
{
Bill Bonney's avatar
Bill Bonney committed
293
    int baudRate;
294 295
    if (_port) {
        baudRate = _port->baudRate();
Bill Bonney's avatar
Bill Bonney committed
296
    } else {
297
        baudRate = _config->baud();
Bill Bonney's avatar
Bill Bonney committed
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 325 326 327 328 329
    }
    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
330 331 332 333
    }
    return dataRate;
}

334
void SerialLink::_resetConfiguration()
335
{
336
    if (_port) {
Don Gagne's avatar
Don Gagne committed
337 338 339 340 341
        _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()));
342
    }
pixhawk's avatar
pixhawk committed
343 344
}

345
void SerialLink::_emitLinkError(const QString& errorMsg)
346
{
347
    QString msg("Error on link %1. %2");
348
    qDebug() << errorMsg;
349
    emit communicationError(tr("Link Error"), msg.arg(getName()).arg(errorMsg));
pixhawk's avatar
pixhawk committed
350 351
}

352
LinkConfiguration* SerialLink::getLinkConfiguration()
353
{
354
    return _config;
pixhawk's avatar
pixhawk committed
355 356
}

357 358
//--------------------------------------------------------------------------
//-- SerialConfiguration
pixhawk's avatar
pixhawk committed
359

360
SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguration(name)
361
{
362 363 364 365 366
    _baud       = 57600;
    _flowControl= QSerialPort::NoFlowControl;
    _parity     = QSerialPort::NoParity;
    _dataBits   = 8;
    _stopBits   = 1;
367
    _usbDirect  = false;
pixhawk's avatar
pixhawk committed
368 369
}

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

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

397
void SerialConfiguration::updateSettings()
398
{
399 400 401 402 403
    if(_link) {
        SerialLink* serialLink = dynamic_cast<SerialLink*>(_link);
        if(serialLink) {
            serialLink->_resetConfiguration();
        }
404
    }
405 406
}

407
void SerialConfiguration::setBaud(int baud)
pixhawk's avatar
pixhawk committed
408
{
409
    _baud = baud;
pixhawk's avatar
pixhawk committed
410 411
}

412
void SerialConfiguration::setDataBits(int databits)
pixhawk's avatar
pixhawk committed
413
{
414
    _dataBits = databits;
pixhawk's avatar
pixhawk committed
415 416
}

417
void SerialConfiguration::setFlowControl(int flowControl)
pixhawk's avatar
pixhawk committed
418
{
419
    _flowControl = flowControl;
pixhawk's avatar
pixhawk committed
420 421
}

422
void SerialConfiguration::setStopBits(int stopBits)
423
{
424
    _stopBits = stopBits;
pixhawk's avatar
pixhawk committed
425 426
}

427
void SerialConfiguration::setParity(int parity)
428
{
429
    _parity = parity;
pixhawk's avatar
pixhawk committed
430 431
}

432
void SerialConfiguration::setPortName(const QString& portName)
433
{
434 435 436 437
    // No effect on a running connection
    QString pname = portName.trimmed();
    if (!pname.isEmpty() && pname != _portName) {
        _portName = pname;
438
        _portDisplayName = cleanPortDisplayname(pname);
439 440 441
    }
}

442 443 444
QString SerialConfiguration::cleanPortDisplayname(const QString name)
{
    QString pname = name.trimmed();
445
#ifdef Q_OS_WIN
446 447 448 449 450 451 452 453
    pname.replace("\\\\.\\", "");
#else
    pname.replace("/dev/cu.", "");
    pname.replace("/dev/", "");
#endif
    return pname;
}

454
void SerialConfiguration::saveSettings(QSettings& settings, const QString& root)
455
{
456
    settings.beginGroup(root);
457 458 459 460 461 462 463
    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);
464
    settings.endGroup();
465
}
466

467
void SerialConfiguration::loadSettings(QSettings& settings, const QString& root)
468
{
469
    settings.beginGroup(root);
470 471 472 473 474 475 476
    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();
477
    settings.endGroup();
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 531 532 533 534 535

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";
}

536 537 538 539 540 541 542
void SerialConfiguration::setUsbDirect(bool usbDirect)
{
    if (_usbDirect != usbDirect) {
        _usbDirect = usbDirect;
        emit usbDirectChanged(_usbDirect);
    }
}