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

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

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

Bill Bonney's avatar
Bill Bonney committed
232
    return true; // successful connection
pixhawk's avatar
pixhawk committed
233
}
234

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

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

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

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

277
    return isConnected;
pixhawk's avatar
pixhawk committed
278 279
}

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

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

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

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

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

355 356
//--------------------------------------------------------------------------
//-- SerialConfiguration
pixhawk's avatar
pixhawk committed
357

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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