SerialLink.cc 14.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 19

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

pixhawk's avatar
pixhawk committed
24
#include "SerialLink.h"
25
#include "QGC.h"
26
#include "MG.h"
27
#include "QGCLoggingCategory.h"
28

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

31 32 33 34 35 36 37 38 39
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
40

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

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

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

61
bool SerialLink::_isBootloader()
62
{
63
    QList<QSerialPortInfo> portList = QSerialPortInfo::availablePorts();
64 65 66 67 68
    if( portList.count() == 0){
        return false;
    }
    foreach (const QSerialPortInfo &info, portList)
    {
69 70 71 72 73 74 75 76
        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;
77 78 79 80 81 82
       }
    }
    // Not found
    return false;
}

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

/**
 * @brief Read a number of bytes from the interface.
 *
 * @param data Pointer to the data byte array to write the bytes to
 * @param maxLength The maximum number of bytes to write
 **/
100 101
void SerialLink::readBytes()
{
102
    if(_port && _port->isOpen()) {
103 104
        const qint64 maxLength = 2048;
        char data[maxLength];
105 106
        _dataMutex.lock();
        qint64 numBytes = _port->bytesAvailable();
107

108
        if (numBytes > 0) {
pixhawk's avatar
pixhawk committed
109 110 111
            /* Read as much data in buffer as possible without overflow */
            if(maxLength < numBytes) numBytes = maxLength;

112 113
            _logInputDataRate(numBytes, QDateTime::currentMSecsSinceEpoch());
            
114
            _port->read(data, numBytes);
pixhawk's avatar
pixhawk committed
115 116 117
            QByteArray b(data, numBytes);
            emit bytesReceived(this, b);
        }
118
        _dataMutex.unlock();
pixhawk's avatar
pixhawk committed
119 120 121 122 123 124 125 126
    }
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
127
bool SerialLink::_disconnect(void)
128
{
129 130 131 132
    if (_port) {
        _port->close();
        delete _port;
        _port = NULL;
133
    }
134
    
dogmaphobic's avatar
dogmaphobic committed
135 136 137
#ifdef __android__
    LinkManager::instance()->suspendConfigurationUpdates(false);
#endif
138
    return true;
pixhawk's avatar
pixhawk committed
139 140 141 142 143 144 145
}

/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
146
bool SerialLink::_connect(void)
147
{
148
    qCDebug(SerialLinkLog) << "CONNECT CALLED";
149 150 151

    _disconnect();
    
dogmaphobic's avatar
dogmaphobic committed
152 153
#ifdef __android__
    LinkManager::instance()->suspendConfigurationUpdates(true);
154 155
#endif
    
dogmaphobic's avatar
dogmaphobic committed
156 157 158 159 160 161 162 163 164 165
    // Initialize the connection
    if (!_hardwareConnect(_type)) {
        // Need to error out here.
        QString err("Could not create port.");
        if (_port) {
            err = _port->errorString();
        }
        _emitLinkError("Error connecting: " + err);
        return false;
    }
166
    return true;
pixhawk's avatar
pixhawk committed
167 168 169
}

/**
170
 * @brief This function is called indirectly by the _connect() call.
pixhawk's avatar
pixhawk committed
171
 *
172
 * The _connect() function starts the thread and indirectly calls this method.
pixhawk's avatar
pixhawk committed
173 174
 *
 * @return True if the connection could be established, false otherwise
175
 * @see _connect() For the right function to establish the connection.
pixhawk's avatar
pixhawk committed
176
 **/
177
bool SerialLink::_hardwareConnect(QString &type)
178
{
179
    if (_port) {
180
        qCDebug(SerialLinkLog) << "SerialLink:" << QString::number((long)this, 16) << "closing port";
181
        _port->close();
182
        QGC::SLEEP::usleep(50000);
183 184
        delete _port;
        _port = NULL;
185
    }
pixhawk's avatar
pixhawk committed
186

187
    qCDebug(SerialLinkLog) << "SerialLink: hardwareConnect to " << _config->portName();
188

189
    // If we are in the Pixhawk bootloader code wait for it to timeout
190
    if (_isBootloader()) {
191
        qCDebug(SerialLinkLog) << "Not connecting to a bootloader, waiting for 2nd chance";
192 193 194
        const unsigned retry_limit = 12;
        unsigned retries;
        for (retries = 0; retries < retry_limit; retries++) {
195 196
            if (!_isBootloader()) {
                QGC::SLEEP::msleep(500);
197 198 199 200 201 202 203
                break;
            }
            QGC::SLEEP::msleep(500);
        }
        // Check limit
        if (retries == retry_limit) {
            // bail out
204
            qWarning() << "Timeout waiting for something other than booloader";
205 206 207 208
            return false;
        }
    }

209 210 211
    _port = new QSerialPort(_config->portName());
    if (!_port) {
        emit communicationUpdate(getName(),"Error opening port: " + _config->portName());
Bill Bonney's avatar
Bill Bonney committed
212
        return false; // couldn't create serial port.
213
    }
Bill Bonney's avatar
Bill Bonney committed
214

215 216
    // We need to catch this signal and then emit disconnected. You can't connect
    // signal to signal otherwise disonnected will have the wrong QObject::Sender
217 218
    QObject::connect(_port, SIGNAL(aboutToClose()), this, SLOT(_rerouteDisconnected()));
    QObject::connect(_port, SIGNAL(error(QSerialPort::SerialPortError)), this, SLOT(linkError(QSerialPort::SerialPortError)));
219
    QObject::connect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes);
Bill Bonney's avatar
Bill Bonney committed
220

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

223
    // TODO This needs a bit of TLC still...
224

225
    // After the bootloader times out, it still can take a second or so for the Pixhawk USB driver to come up and make
226
    // the port available for open. So we retry a few times to wait for it.
dogmaphobic's avatar
dogmaphobic committed
227 228 229
#ifdef __android__
    _port->open(QIODevice::ReadWrite);
#else
230 231
    for (int openRetries = 0; openRetries < 4; openRetries++) {
        if (!_port->open(QIODevice::ReadWrite)) {
232
            qCDebug(SerialLinkLog) << "Port open failed, retrying";
233 234 235 236
            QGC::SLEEP::msleep(500);
        } else {
            break;
        }
237
    }
dogmaphobic's avatar
dogmaphobic committed
238
#endif
239 240 241
    if (!_port->isOpen() ) {
        emit communicationUpdate(getName(),"Error opening port: " + _port->errorString());
        _port->close();
242 243
        delete _port;
        _port = NULL;
244
        return false; // couldn't open serial port
245 246
    }

247
    qCDebug(SerialLinkLog) << "Configuring port";
248 249 250 251 252
    _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()));
253

254
    emit communicationUpdate(getName(), "Opened port!");
Bill Bonney's avatar
Bill Bonney committed
255
    emit connected();
256

257
    qCDebug(SerialLinkLog) << "CONNECTING LINK: " << __FILE__ << __LINE__ << "type:" << type << "with settings" << _config->portName()
258
             << _config->baud() << _config->dataBits() << _config->parity() << _config->stopBits();
259

Bill Bonney's avatar
Bill Bonney committed
260
    return true; // successful connection
pixhawk's avatar
pixhawk committed
261
}
262

263 264 265 266 267 268 269 270 271 272 273 274
void SerialLink::_readBytes(void)
{
    qint64 byteCount = _port->bytesAvailable();
    if (byteCount)
    {
        QByteArray buffer;
        buffer.resize(byteCount);
        _port->read(buffer.data(), buffer.size());
        emit bytesReceived(this, buffer);
    }
}

275
void SerialLink::linkError(QSerialPort::SerialPortError error)
276
{
277
    if (error != QSerialPort::NoError) {
278 279 280 281
        // 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.
282
        //qCDebug(SerialLinkLog) << "SerialLink::linkError" << error;
283
    }
284 285
}

pixhawk's avatar
pixhawk committed
286 287 288 289 290
/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
291
bool SerialLink::isConnected() const
292
{
293
    bool isConnected = false;
Bill Bonney's avatar
Bill Bonney committed
294

295
    if (_port) {
296
        isConnected = _port->isOpen();
lm's avatar
lm committed
297
    }
298 299
    
    return isConnected;
pixhawk's avatar
pixhawk committed
300 301
}

302
QString SerialLink::getName() const
pixhawk's avatar
pixhawk committed
303
{
304
    return _config->portName();
pixhawk's avatar
pixhawk committed
305 306
}

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

354
void SerialLink::_resetConfiguration()
355
{
356 357 358 359 360 361 362 363 364
    bool somethingChanged = false;
    if (_port) {
        somethingChanged = _port->setBaudRate     (_config->baud());
        somethingChanged |= _port->setDataBits    (static_cast<QSerialPort::DataBits>    (_config->dataBits()));
        somethingChanged |= _port->setFlowControl (static_cast<QSerialPort::FlowControl> (_config->flowControl()));
        somethingChanged |= _port->setStopBits    (static_cast<QSerialPort::StopBits>    (_config->stopBits()));
        somethingChanged |= _port->setParity      (static_cast<QSerialPort::Parity>      (_config->parity()));
    }
    if(somethingChanged) {
365
        qCDebug(SerialLinkLog) << "Reconfiguring port";
366 367
        emit updateLink(this);
    }
pixhawk's avatar
pixhawk committed
368 369
}

370
void SerialLink::_rerouteDisconnected(void)
371
{
372
    emit disconnected();
pixhawk's avatar
pixhawk committed
373 374
}

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

382
LinkConfiguration* SerialLink::getLinkConfiguration()
383
{
384
    return _config;
pixhawk's avatar
pixhawk committed
385 386
}

387 388
//--------------------------------------------------------------------------
//-- SerialConfiguration
pixhawk's avatar
pixhawk committed
389

390
SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguration(name)
391
{
392 393 394 395 396
    _baud       = 57600;
    _flowControl= QSerialPort::NoFlowControl;
    _parity     = QSerialPort::NoParity;
    _dataBits   = 8;
    _stopBits   = 1;
pixhawk's avatar
pixhawk committed
397 398
}

399
SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfiguration(copy)
400
{
401 402 403 404 405 406
    _baud       = copy->baud();
    _flowControl= copy->flowControl();
    _parity     = copy->parity();
    _dataBits   = copy->dataBits();
    _stopBits   = copy->stopBits();
    _portName   = copy->portName();
pixhawk's avatar
pixhawk committed
407 408
}

409
void SerialConfiguration::copyFrom(LinkConfiguration *source)
410
{
411 412 413 414 415 416 417 418 419
    LinkConfiguration::copyFrom(source);
    SerialConfiguration* ssource = dynamic_cast<SerialConfiguration*>(source);
    Q_ASSERT(ssource != NULL);
    _baud       = ssource->baud();
    _flowControl= ssource->flowControl();
    _parity     = ssource->parity();
    _dataBits   = ssource->dataBits();
    _stopBits   = ssource->stopBits();
    _portName   = ssource->portName();
420 421
}

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

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

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

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

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

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

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

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

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