From f727f89d5c7eb91a4822bf231505b95052ba6e08 Mon Sep 17 00:00:00 2001 From: Holden Date: Fri, 2 Aug 2024 03:16:54 -0400 Subject: [PATCH] Comms: Update Serial Threading --- .../qserialport_android.cpp | 42 +- .../APM/APMAutoPilotPlugin.cc | 2 +- src/Comms/LinkManager.cc | 2 +- src/Comms/LinkManager.h | 3 - src/Comms/QGCSerialPortInfo.cc | 30 +- src/Comms/QGCSerialPortInfo.h | 19 +- src/Comms/SerialLink.cc | 502 ++++++++---------- src/Comms/SerialLink.h | 208 ++++---- 8 files changed, 388 insertions(+), 420 deletions(-) diff --git a/android/libs/qtandroidserialport/qserialport_android.cpp b/android/libs/qtandroidserialport/qserialport_android.cpp index 7ea4847daa3..e7a2b106f9e 100644 --- a/android/libs/qtandroidserialport/qserialport_android.cpp +++ b/android/libs/qtandroidserialport/qserialport_android.cpp @@ -41,19 +41,19 @@ bool QSerialPortPrivate::open(QIODevice::OpenMode mode) return false; } - if (mode & QIODevice::ReadOnly) { + if (mode & QIODevice::ReadWrite == QIODevice::ReadOnly) { if (!startAsyncRead()) { qCWarning(AndroidSerialPortLog) << "Failed to start async read for" << systemLocation.toLatin1().constData(); close(); return false; } - } else if (mode & QIODevice::WriteOnly) { + } else if (mode & QIODevice::ReadWrite == QIODevice::WriteOnly) { if (!_stopAsyncRead()) { qCWarning(AndroidSerialPortLog) << "Failed to stop async read for" << systemLocation.toLatin1().constData(); } } - clear(QSerialPort::AllDirections); + (void) clear(QSerialPort::AllDirections); return true; } @@ -109,6 +109,9 @@ void QSerialPortPrivate::newDataArrived(const char *bytes, int length) qCDebug(AndroidSerialPortLog) << "newDataArrived" << length; + // qint64 newBytes = buffer.size(); + // qint64 bytesToRead = QSERIALPORT_BUFFERSIZE; + int bytesToRead = length; if (readBufferMaxSize && (bytesToRead > (readBufferMaxSize - buffer.size()))) { bytesToRead = static_cast(readBufferMaxSize - buffer.size()); @@ -122,6 +125,17 @@ void QSerialPortPrivate::newDataArrived(const char *bytes, int length) char* const ptr = buffer.reserve(bytesToRead); (void) memcpy(ptr, bytes, static_cast(bytesToRead)); + /*buffer.chop(bytesToRead - qMax(length, qint64(0))); + if (length < 0) { + setError(QSerialPortErrorInfo(QSerialPort::ReadError, QSerialPort::tr("Failed to Read New Data"))); + if (QSerialPort::ResourceError) { + (void) _stopAsyncRead(); + } + return; + } else if (length == 0) { + return; + }*/ + // TODO: Limit signals as one read can handle multiple signals emit q->readyRead(); } @@ -169,26 +183,30 @@ bool QSerialPortPrivate::_writeDataOneShot(int msecs) { Q_Q(QSerialPort); - qint64 pendingBytesWritten = -1; - + qint64 pendingBytesWritten = 0; while (!writeBuffer.isEmpty()) { const char *dataPtr = writeBuffer.readPointer(); const qint64 dataSize = writeBuffer.nextDataBlockSize(); - pendingBytesWritten = _writeToPort(dataPtr, dataSize, msecs); + const qint64 written = _writeToPort(dataPtr, dataSize, msecs); - if (pendingBytesWritten <= 0) { + if (written < 0) { qCWarning(AndroidSerialPortLog) << "Failed to write data one shot on device ID" << m_deviceId; setError(QSerialPortErrorInfo(QSerialPort::WriteError, QSerialPort::tr("Failed to write data one shot"))); return false; } - writeBuffer.free(pendingBytesWritten); + writeBuffer.free(written); + pendingBytesWritten += written; + } + + const bool result = (pendingBytesWritten > 0); + if (result) { emit q->bytesWritten(pendingBytesWritten); } - return (pendingBytesWritten >= 0); + return result; } qint64 QSerialPortPrivate::_writeToPort(const char *data, qint64 maxSize, int timeout, bool async) @@ -204,12 +222,14 @@ qint64 QSerialPortPrivate::_writeToPort(const char *data, qint64 maxSize, int ti qint64 QSerialPortPrivate::writeData(const char *data, qint64 maxSize) { - if (!data || maxSize <= 0) { + if (!data || (maxSize < 0)) { qCWarning(AndroidSerialPortLog) << "Invalid data or size in writeData for device ID" << m_deviceId; setError(QSerialPortErrorInfo(QSerialPort::WriteError, QSerialPort::tr("Invalid data or size"))); return -1; } + // writeBuffer.append(data, maxSize); + const qint64 result = _writeToPort(data, maxSize); if (result < 0) { setError(QSerialPortErrorInfo(QSerialPort::WriteError, QSerialPort::tr("Failed to write data"))); @@ -249,7 +269,7 @@ bool QSerialPortPrivate::clear(QSerialPort::Directions directions) const bool result = AndroidSerial::purgeBuffers(m_deviceId, input, output); if (!result) { qCWarning(AndroidSerialPortLog) << "Failed to purge buffers for device ID" << m_deviceId; - setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to purge buffers"))); + // setError(QSerialPortErrorInfo(QSerialPort::UnknownError, QSerialPort::tr("Failed to purge buffers"))); } return result; diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc index 434f54fc15a..589eb3fce27 100644 --- a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc @@ -204,7 +204,7 @@ void APMAutoPilotPlugin::_checkForBadCubeBlack(void) // FIXME: Put back for (const QVariant& varLink: _vehicle->links()) { SerialLink* serialLink = varLink.value(); - if (serialLink && QSerialPortInfo(*serialLink->_hackAccessToPort()).description().contains(QStringLiteral("CubeBlack"))) { + if (serialLink && QSerialPortInfo(*serialLink->port()).description().contains(QStringLiteral("CubeBlack"))) { cubeBlackFound = true; } diff --git a/src/Comms/LinkManager.cc b/src/Comms/LinkManager.cc index 9c0981c89ef..3a2cdfebd81 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -945,7 +945,7 @@ void LinkManager::_updateSerialPorts() for (const QGCSerialPortInfo &info: portList) { const QString port = info.systemLocation().trimmed(); _commPortList += port; - _commPortDisplayList += SerialConfiguration::cleanPortDisplayname(port); + _commPortDisplayList += SerialConfiguration::cleanPortDisplayName(port); } } diff --git a/src/Comms/LinkManager.h b/src/Comms/LinkManager.h index 7889d9339fb..568b8bce7b6 100644 --- a/src/Comms/LinkManager.h +++ b/src/Comms/LinkManager.h @@ -76,9 +76,6 @@ class LinkManager : public QObject void loadLinkConfigurationList(); void saveLinkConfigurationList(); - /// Suspend automatic confguration updates (during link maintenance for instance) - void suspendConfigurationUpdates(bool suspend) { _configUpdateSuspended = suspend; } - /// Sets the flag to suspend the all new connections /// @param reason User visible reason to suspend connections void setConnectionsSuspended(const QString &reason) { _connectionsSuspended = true; _connectionsSuspendedReason = reason; } diff --git a/src/Comms/QGCSerialPortInfo.cc b/src/Comms/QGCSerialPortInfo.cc index c9283a847f1..beef48b45de 100644 --- a/src/Comms/QGCSerialPortInfo.cc +++ b/src/Comms/QGCSerialPortInfo.cc @@ -52,7 +52,7 @@ bool QGCSerialPortInfo::_loadJsonData() QString errorString; int version; - const QJsonObject json = JsonHelper::openInternalQGCJsonFile(":/json/USBBoardInfo.json", _jsonFileTypeValue, 1, 1, version, errorString); + const QJsonObject json = JsonHelper::openInternalQGCJsonFile(QStringLiteral(":/json/USBBoardInfo.json"), QString(_jsonFileTypeValue), 1, 1, version, errorString); if (!errorString.isEmpty()) { qCWarning(QGCSerialPortInfoLog) << "Internal Error:" << errorString; return false; @@ -165,7 +165,14 @@ bool QGCSerialPortInfo::_loadJsonData() QGCSerialPortInfo::BoardType_t QGCSerialPortInfo::_boardClassStringToType(const QString &boardClass) { - for (const BoardClassString2BoardType_t &board : _rgBoardClass2BoardType) { + static const BoardClassString2BoardType_t rgBoardClass2BoardType[BoardTypeUnknown] = { + { _boardTypeToString(BoardTypePixhawk), BoardTypePixhawk }, + { _boardTypeToString(BoardTypeRTKGPS), BoardTypeRTKGPS }, + { _boardTypeToString(BoardTypeSiKRadio), BoardTypeSiKRadio }, + { _boardTypeToString(BoardTypeOpenPilot), BoardTypeOpenPilot }, + }; + + for (const BoardClassString2BoardType_t &board : rgBoardClass2BoardType) { if (boardClass == board.classString) { return board.boardType; } @@ -245,12 +252,14 @@ QString QGCSerialPortInfo::_boardTypeToString(BoardType_t boardType) QList QGCSerialPortInfo::availablePorts() { QList list; - for (const QSerialPortInfo &portInfo : QSerialPortInfo::availablePorts()) { + + const QList availablePorts = QSerialPortInfo::availablePorts(); + for (const QSerialPortInfo &portInfo : availablePorts) { if (isSystemPort(portInfo)) { continue; } - const QGCSerialPortInfo* const qgcPortInfo = reinterpret_cast(&portInfo); + const QGCSerialPortInfo *const qgcPortInfo = reinterpret_cast(&portInfo); list << *qgcPortInfo; } @@ -298,11 +307,10 @@ bool QGCSerialPortInfo::canFlash() const return false; } - switch(boardType) { - case QGCSerialPortInfo::BoardTypePixhawk: - case QGCSerialPortInfo::BoardTypeSiKRadio: - return true; - default: - return false; - } + static const QList flashable = { + BoardTypePixhawk, + BoardTypeSiKRadio + }; + + return flashable.contains(boardType); } diff --git a/src/Comms/QGCSerialPortInfo.h b/src/Comms/QGCSerialPortInfo.h index ce83f4713d1..3888fcc05e2 100644 --- a/src/Comms/QGCSerialPortInfo.h +++ b/src/Comms/QGCSerialPortInfo.h @@ -33,12 +33,13 @@ class QGCSerialPortInfo : public QSerialPortInfo ~QGCSerialPortInfo(); enum BoardType_t { - BoardTypePixhawk, + BoardTypePixhawk = 0, BoardTypeSiKRadio, BoardTypeOpenPilot, BoardTypeRTKGPS, BoardTypeUnknown }; + bool getBoardInfo(BoardType_t &boardType, QString &name) const; /// @return true: we can flash this board type @@ -55,6 +56,11 @@ class QGCSerialPortInfo : public QSerialPortInfo static QList availablePorts(); private: + struct BoardClassString2BoardType_t { + const QString classString; + const BoardType_t boardType = BoardTypeUnknown; + }; + static bool _loadJsonData(); static BoardType_t _boardClassStringToType(const QString &boardClass); static QString _boardTypeToString(BoardType_t boardType); @@ -62,17 +68,6 @@ class QGCSerialPortInfo : public QSerialPortInfo static bool _jsonLoaded; static bool _jsonDataValid; - struct BoardClassString2BoardType_t { - const char *classString; - BoardType_t boardType; - }; - static constexpr const BoardClassString2BoardType_t _rgBoardClass2BoardType[BoardTypeUnknown] = { - { "Pixhawk", QGCSerialPortInfo::BoardTypePixhawk }, - { "RTK GPS", QGCSerialPortInfo::BoardTypeRTKGPS }, - { "SiK Radio", QGCSerialPortInfo::BoardTypeSiKRadio }, - { "OpenPilot", QGCSerialPortInfo::BoardTypeOpenPilot }, - }; - struct BoardInfo_t { int vendorId; int productId; diff --git a/src/Comms/SerialLink.cc b/src/Comms/SerialLink.cc index 18de72d747e..264cf25099c 100644 --- a/src/Comms/SerialLink.cc +++ b/src/Comms/SerialLink.cc @@ -8,410 +8,334 @@ ****************************************************************************/ #include "SerialLink.h" -#include "QGC.h" #include "QGCLoggingCategory.h" +#include "QGCSerialPortInfo.h" #ifdef Q_OS_ANDROID -#include "QGCApplication.h" -#include "LinkManager.h" #include "qserialportinfo.h" #else #include #endif -#include #include -QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog") +QGC_LOGGING_CATEGORY(SerialLinkLog, "qgc.comms.seriallink") -SerialLink::SerialLink(SharedLinkConfigurationPtr& config) - : LinkInterface(config) - , _serialConfig(qobject_cast(config.get())) +SerialWorker::SerialWorker(const SerialConfiguration *config, QObject *parent) + : QObject(parent) + , _config(config) + , _port(new QSerialPort(this)) { - qRegisterMetaType(); - qCDebug(SerialLinkLog) << "Create SerialLink portName:baud:flowControl:parity:dataButs:stopBits" << _serialConfig->portName() << _serialConfig->baud() << _serialConfig->flowControl() - << _serialConfig->parity() << _serialConfig->dataBits() << _serialConfig->stopBits(); -} + Q_CHECK_PTR(_config); -SerialLink::~SerialLink() -{ - disconnect(); + (void) qRegisterMetaType("QSerialPort::SerialPortError"); + + (void) connect(_port, &QSerialPort::aboutToClose, this, &SerialWorker::_onDisconnected); + (void) connect(_port, &QSerialPort::readyRead, this, &SerialWorker::_onReadyRead); + // (void) connect(_port, &QSerialPort::bytesWritten, this, &SerialWorker::_onBytesWritten); + (void) connect(_port, &QSerialPort::errorOccurred, this, &SerialWorker::_onErrorOccurred); } -bool SerialLink::_isBootloader() +SerialWorker::~SerialWorker() { - QList portList = QSerialPortInfo::availablePorts(); - if( portList.count() == 0){ - return false; - } - for (const QSerialPortInfo &info: portList) - { - qCDebug(SerialLinkLog) << "PortName : " << info.portName() << "Description : " << info.description(); - qCDebug(SerialLinkLog) << "Manufacturer: " << info.manufacturer(); - if (info.portName().trimmed() == _serialConfig->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; - } - } - // Not found - return false; + disconnectFromPort(); } -void SerialLink::_writeBytes(const QByteArray &data) +bool SerialWorker::isConnected() const { - if(_port && _port->isOpen()) { - emit bytesSent(this, data); - _port->write(data); - } else { - // Error occurred - qWarning() << "Serial port not writeable"; - _emitLinkError(tr("Could not send data - link %1 is disconnected!").arg(_config->name())); - } + return _port->isOpen(); } -void SerialLink::disconnect(void) +void SerialWorker::connectToPort() { - if (_port) { - // This prevents stale signals from calling the link after it has been deleted - QObject::disconnect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes); - _port->close(); - _port->deleteLater(); - _port = nullptr; - emit disconnected(); + if (isConnected()) { + qCWarning(SerialLinkLog) << "Already connected to" << _port->portName(); + emit connected(); + return; } -#ifdef Q_OS_ANDROID - LinkManager::instance()->suspendConfigurationUpdates(false); -#endif -} + _port->setPortName(_config->portName()); -bool SerialLink::_connect(void) -{ - qCDebug(SerialLinkLog) << "CONNECT CALLED"; - - if (_port) { - qCWarning(SerialLinkLog) << "connect called while already connected"; - return true; + const QGCSerialPortInfo portInfo(*_port); + if (portInfo.isBootloader()) { + qCWarning(SerialLinkLog) << "Not connecting to bootloader" << _port->portName(); + emit errorOccurred(tr("Not connecting to a bootloader")); + _onDisconnected(); + return; } -#ifdef Q_OS_ANDROID - LinkManager::instance()->suspendConfigurationUpdates(true); -#endif + qCDebug(SerialLinkLog) << "Attempting to open port" << _port->portName(); + if (!_port->open(QIODevice::ReadWrite)) { + qCWarning(SerialLinkLog) << "Opening port" << _port->portName() << "failed:" << _port->errorString(); + emit errorOccurred(tr("Could not open port: %1").arg(_port->errorString())); + _onDisconnected(); + return; + } - QSerialPort::SerialPortError error; - QString errorString; - - // Initialize the connection - if (!_hardwareConnect(error, errorString)) { - if (_config->isAutoConnect()) { - // 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; - } - } + _port->setBaudRate(_config->baud()); + _port->setDataBits(static_cast(_config->dataBits())); + _port->setFlowControl(static_cast(_config->flowControl())); + _port->setStopBits(static_cast(_config->stopBits())); + _port->setParity(static_cast(_config->parity())); + _port->setDataTerminalReady(true); - _emitLinkError(tr("Error connecting: Could not create port. %1").arg(errorString)); - return false; - } - return true; + qCDebug(SerialLinkLog) << "Successfully connected to" << _port->portName(); + emit connected(); } -/// 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) +void SerialWorker::disconnectFromPort() { - if (_port) { - qCDebug(SerialLinkLog) << "SerialLink:" << QString::number((qulonglong)this, 16) << "closing port"; + if (isConnected()) { _port->close(); - - // Wait 50 ms while continuing to run the event queue - for (unsigned i = 0; i < 10; i++) { - QGC::SLEEP::usleep(5000); - QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents); - } - delete _port; - _port = nullptr; } +} - qCDebug(SerialLinkLog) << "SerialLink: hardwareConnect to " << _serialConfig->portName(); - - // If we are in the Pixhawk bootloader code wait for it to timeout - if (_isBootloader()) { - qCDebug(SerialLinkLog) << "Not connecting to a bootloader, waiting for 2nd chance"; - const unsigned retry_limit = 12; - unsigned retries; - - for (retries = 0; retries < retry_limit; retries++) { - if (!_isBootloader()) { - // Wait 500 ms while continuing to run the event loop - for (unsigned i = 0; i < 100; i++) { - QGC::SLEEP::msleep(5); - QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents); - } - break; - } - - // Wait 500 ms while continuing to run the event loop - for (unsigned i = 0; i < 100; i++) { - QGC::SLEEP::msleep(5); - QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents); - } - } - // Check limit - if (retries == retry_limit) { - // bail out - qWarning() << "Timeout waiting for something other than booloader"; - return false; - } +void SerialWorker::writeData(const QByteArray &data) +{ + if (data.isEmpty()) { + emit errorOccurred(tr("Data to Send is Empty")); + return; } - _port = new QSerialPort(_serialConfig->portName(), this); - + if (!isConnected()) { + emit errorOccurred(tr("Port is not Connected")); + return; + } - // After the bootloader times out, it still can take a second or so for the Pixhawk USB driver to come up and make - // the port available for open. So we retry a few times to wait for it. -#ifdef Q_OS_ANDROID - _port->open(QIODevice::ReadWrite); -#else + if (!_port->isWritable()) { + emit errorOccurred(tr("Port is not Writable")); + return; + } - // Try to open the port three times - for (int openRetries = 0; openRetries < 3; openRetries++) { - if (!_port->open(QIODevice::ReadWrite)) { - qCDebug(SerialLinkLog) << "Port open failed, retrying"; - // Wait 250 ms while continuing to run the event loop - for (unsigned i = 0; i < 50; i++) { - QGC::SLEEP::msleep(5); - QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents); - } - QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents); - } else { + qint64 totalBytesWritten = 0; + while (totalBytesWritten < data.size()) { + const qint64 bytesWritten = _port->write(data.constData() + totalBytesWritten, data.size() - totalBytesWritten); + if (bytesWritten <= 0) { break; } - } -#endif - if (!_port->isOpen() ) { - qDebug() << "open failed" << _port->errorString() << _port->error() << _config->name() << "autconnect:" << _config->isAutoConnect(); - error = _port->error(); - errorString = _port->errorString(); - _port->close(); - delete _port; - _port = nullptr; - return false; // couldn't open serial port - } - _port->setDataTerminalReady(true); + totalBytesWritten += bytesWritten; + } - qCDebug(SerialLinkLog) << "Configuring port"; - _port->setBaudRate (_serialConfig->baud()); - _port->setDataBits (static_cast (_serialConfig->dataBits())); - _port->setFlowControl (static_cast (_serialConfig->flowControl())); - _port->setStopBits (static_cast (_serialConfig->stopBits())); - _port->setParity (static_cast (_serialConfig->parity())); + if (totalBytesWritten < 0) { + emit errorOccurred(tr("Could Not Send Data - Write Failed: %1").arg(_port->errorString())); + return; + } - emit connected(); + const QByteArray sent = data.first(totalBytesWritten); + emit dataSent(sent); +} - QObject::connect(_port, &QSerialPort::errorOccurred, this, &SerialLink::linkError); - QObject::connect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes); +void SerialWorker::_onDisconnected() +{ + emit disconnected(); +} - qCDebug(SerialLinkLog) << "Connection SeriaLink: " << "with settings" << _serialConfig->portName() - << _serialConfig->baud() << _serialConfig->dataBits() << _serialConfig->parity() << _serialConfig->stopBits(); +void SerialWorker::_onReadyRead() +{ + const QByteArray data = _port->readAll(); + emit dataReceived(data); +} - return true; // successful connection +void SerialWorker::_onBytesWritten(qint64 bytes) const +{ + qCDebug(SerialLinkLog) << _port->portName() << "Wrote" << bytes << "bytes"; } -void SerialLink::_readBytes(void) +void SerialWorker::_onErrorOccurred(QSerialPort::SerialPortError error) { - 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(_config->name())); + if (error == QSerialPort::NoError) { + return; } + + emit errorOccurred(_port->errorString()); } -void SerialLink::linkError(QSerialPort::SerialPortError error) +/*===========================================================================*/ + +SerialLink::SerialLink(SharedLinkConfigurationPtr &config, QObject *parent) + : LinkInterface(config, parent) + , _serialConfig(qobject_cast(config.get())) + , _worker(new SerialWorker(_serialConfig)) + , _workerThread(new QThread(this)) { - switch (error) { - case QSerialPort::NoError: - break; - case QSerialPort::ResourceError: - // This indicates the hardware was pulled from the computer. For example usb cable unplugged. - _connectionRemoved(); - break; - default: - // 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. - // qCDebug(SerialLinkLog) << "SerialLink::linkError" << error; - break; - } + Q_CHECK_PTR(_serialConfig); + + _worker->moveToThread(_workerThread); + + (void) connect(_workerThread, &QThread::finished, _worker, &QObject::deleteLater); + + (void) connect(_worker, &SerialWorker::connected, this, &SerialLink::_onConnected, Qt::QueuedConnection); + (void) connect(_worker, &SerialWorker::disconnected, this, &SerialLink::_onDisconnected, Qt::QueuedConnection); + (void) connect(_worker, &SerialWorker::dataReceived, this, &SerialLink::_onDataReceived, Qt::QueuedConnection); + (void) connect(_worker, &SerialWorker::dataSent, this, &SerialLink::_onDataSent, Qt::QueuedConnection); + (void) connect(_worker, &SerialWorker::errorOccurred, this, &SerialLink::_onErrorOccurred, Qt::QueuedConnection); + +#ifdef QT_DEBUG + _workerThread->setObjectName(QStringLiteral("Serial_%1").arg(config->name())); +#endif + + _workerThread->start(); } -bool SerialLink::isConnected() const +SerialLink::~SerialLink() { - bool isConnected = false; + disconnect(); - if (_port) { - isConnected = _port->isOpen(); - } + _workerThread->quit(); + (void) _workerThread->wait(); +} - return isConnected; +bool SerialLink::isConnected() const +{ + return _worker->isConnected(); } -void SerialLink::_emitLinkError(const QString& errorMsg) +bool SerialLink::_connect() { - QString msg("Error on link %1. %2"); - qDebug() << errorMsg; - emit communicationError(tr("Link Error"), msg.arg(_config->name()).arg(errorMsg)); + return QMetaObject::invokeMethod(_worker, "connectToPort", Qt::QueuedConnection); } -bool SerialLink::isSecureConnection() +void SerialLink::disconnect() { - return _serialConfig && _serialConfig->usbDirect(); + (void) QMetaObject::invokeMethod(_worker, "disconnectFromPort", Qt::QueuedConnection); } -//-------------------------------------------------------------------------- -//-- SerialConfiguration +void SerialLink::_onConnected() +{ + emit connected(); +} -SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguration(name) +void SerialLink::_onDisconnected() { - _baud = 57600; - _flowControl= QSerialPort::NoFlowControl; - _parity = QSerialPort::NoParity; - _dataBits = 8; - _stopBits = 1; - _usbDirect = false; + emit disconnected(); } -SerialConfiguration::SerialConfiguration(const SerialConfiguration* copy) : LinkConfiguration(copy) +void SerialLink::_onErrorOccurred(const QString &errorString) { - _baud = copy->baud(); - _flowControl = copy->flowControl(); - _parity = copy->parity(); - _dataBits = copy->dataBits(); - _stopBits = copy->stopBits(); - _portName = copy->portName(); - _portDisplayName = copy->portDisplayName(); - _usbDirect = copy->_usbDirect; + emit communicationError(tr("Serial Link Error"), tr("Link %1: %2").arg(_serialConfig->name(), errorString)); } -void SerialConfiguration::copyFrom(const LinkConfiguration *source) +void SerialLink::_onDataReceived(const QByteArray &data) { - LinkConfiguration::copyFrom(source); - const SerialConfiguration* ssource = qobject_cast(source); - 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"; - } + emit bytesReceived(this, data); } -void SerialConfiguration::setBaud(int baud) +void SerialLink::_onDataSent(const QByteArray &data) { - _baud = baud; + emit bytesSent(this, data); } -void SerialConfiguration::setDataBits(int databits) +void SerialLink::_writeBytes(const QByteArray &data) { - _dataBits = databits; + (void) QMetaObject::invokeMethod(_worker, "writeData", Qt::QueuedConnection, Q_ARG(QByteArray, data)); } -void SerialConfiguration::setFlowControl(int flowControl) +/*===========================================================================*/ + +SerialConfiguration::SerialConfiguration(const QString &name, QObject *parent) + : LinkConfiguration(name, parent) { - _flowControl = flowControl; + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; } -void SerialConfiguration::setStopBits(int stopBits) +SerialConfiguration::SerialConfiguration(const SerialConfiguration *source, QObject *parent) + : LinkConfiguration(source, parent) { - _stopBits = stopBits; + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; + + Q_CHECK_PTR(source); + + SerialConfiguration::copyFrom(source); } -void SerialConfiguration::setParity(int parity) +SerialConfiguration::~SerialConfiguration() { - _parity = parity; + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; } -void SerialConfiguration::setPortName(const QString& portName) +void SerialConfiguration::setPortName(const QString &name) { - // No effect on a running connection - QString pname = portName.trimmed(); - if (!pname.isEmpty() && pname != _portName) { - _portName = pname; - _portDisplayName = cleanPortDisplayname(pname); + const QString portName = name.trimmed(); + if (portName.isEmpty()) { + return; + } + + if (portName != _portName) { + _portName = portName; + emit portNameChanged(); } + + const QString portDisplayName = cleanPortDisplayName(portName); + setPortDisplayName(portDisplayName); } -QString SerialConfiguration::cleanPortDisplayname(const QString name) +void SerialConfiguration::copyFrom(const LinkConfiguration *source) { - QString pname = name.trimmed(); -#ifdef Q_OS_WIN - pname.replace("\\\\.\\", ""); -#else - pname.replace("/dev/cu.", ""); - pname.replace("/dev/", ""); -#endif - return pname; + Q_CHECK_PTR(source); + LinkConfiguration::copyFrom(source); + + const SerialConfiguration* const serialSource = qobject_cast(source); + Q_CHECK_PTR(serialSource); + + setBaud(serialSource->baud()); + setDataBits(serialSource->dataBits()); + setFlowControl(serialSource->flowControl()); + setStopBits(serialSource->stopBits()); + setParity(serialSource->parity()); + setPortName(serialSource->portName()); + setPortDisplayName(serialSource->portDisplayName()); + setUsbDirect(serialSource->usbDirect()); } -void SerialConfiguration::saveSettings(QSettings& settings, const QString& root) +void SerialConfiguration::loadSettings(QSettings &settings, const QString &root) { 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.setValue("portDisplayName",_portDisplayName); + + setBaud(settings.value("baud", _baud).toInt()); + setDataBits(static_cast(settings.value("dataBits", _dataBits).toInt())); + setFlowControl(static_cast(settings.value("flowControl", _flowControl).toInt())); + setStopBits(static_cast(settings.value("stopBits", _stopBits).toInt())); + setParity(static_cast(settings.value("parity", _parity).toInt())); + setPortName(settings.value("portName", _portName).toString()); + setPortDisplayName(settings.value("portDisplayName", _portDisplayName).toString()); + settings.endGroup(); } -void SerialConfiguration::loadSettings(QSettings& settings, const QString& root) +void SerialConfiguration::saveSettings(QSettings &settings, const QString &root) { 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(); - if(settings.contains("portDisplayName"))_portDisplayName= settings.value("portDisplayName").toString(); + + 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); + settings.endGroup(); } QStringList SerialConfiguration::supportedBaudRates() { QStringList supportBaudRateStrings; - for (int rate : QSerialPortInfo::standardBaudRates()) { - (void) supportBaudRateStrings.append(QString::number(rate)); + + const QList rates = QSerialPortInfo::standardBaudRates(); + for (qint32 rate : rates) { + supportBaudRateStrings.append(QString::number(rate)); } return supportBaudRateStrings; } -void SerialConfiguration::setUsbDirect(bool usbDirect) +QString SerialConfiguration::cleanPortDisplayName(const QString &name) { - if (_usbDirect != usbDirect) { - _usbDirect = usbDirect; - emit usbDirectChanged(_usbDirect); + const QList availablePorts = QSerialPortInfo::availablePorts(); + for (const QSerialPortInfo &portInfo : availablePorts) { + if (portInfo.systemLocation() == name) { + return portInfo.portName(); + } } + + return QString(); } diff --git a/src/Comms/SerialLink.h b/src/Comms/SerialLink.h index 3f4e4ef066e..0ccc6b8b42a 100644 --- a/src/Comms/SerialLink.h +++ b/src/Comms/SerialLink.h @@ -9,136 +9,160 @@ #pragma once -#include "LinkConfiguration.h" -#include "LinkInterface.h" - -#include +#include #include -#include + #ifdef Q_OS_ANDROID #include "qserialport.h" #else #include #endif -#include -Q_DECLARE_LOGGING_CATEGORY(SerialLinkLog) +#include "LinkConfiguration.h" +#include "LinkInterface.h" -// We use QSerialPort::SerialPortError in a signal so we must declare it as a meta type -Q_DECLARE_METATYPE(QSerialPort::SerialPortError) +Q_DECLARE_LOGGING_CATEGORY(SerialLinkLog) -class LinkManager; +/*===========================================================================*/ -/// SerialLink configuration class SerialConfiguration : public LinkConfiguration { Q_OBJECT + Q_PROPERTY(qint32 baud READ baud WRITE setBaud NOTIFY baudChanged) + Q_PROPERTY(QSerialPort::DataBits dataBits READ dataBits WRITE setDataBits NOTIFY dataBitsChanged) + Q_PROPERTY(QSerialPort::FlowControl flowControl READ flowControl WRITE setFlowControl NOTIFY flowControlChanged) + Q_PROPERTY(QSerialPort::StopBits stopBits READ stopBits WRITE setStopBits NOTIFY stopBitsChanged) + Q_PROPERTY(QSerialPort::Parity parity READ parity WRITE setParity NOTIFY parityChanged) + Q_PROPERTY(QString portName READ portName WRITE setPortName NOTIFY portNameChanged) + Q_PROPERTY(QString portDisplayName READ portDisplayName NOTIFY portDisplayNameChanged) + Q_PROPERTY(bool usbDirect READ usbDirect WRITE setUsbDirect NOTIFY usbDirectChanged) public: + explicit SerialConfiguration(const QString &name, QObject *parent = nullptr); + explicit SerialConfiguration(const SerialConfiguration *copy, QObject *parent = nullptr); + virtual ~SerialConfiguration(); - SerialConfiguration(const QString& name); - SerialConfiguration(const SerialConfiguration* copy); - - Q_PROPERTY(int baud READ baud WRITE setBaud NOTIFY baudChanged) - Q_PROPERTY(int dataBits READ dataBits WRITE setDataBits NOTIFY dataBitsChanged) - Q_PROPERTY(int flowControl READ flowControl WRITE setFlowControl NOTIFY flowControlChanged) - Q_PROPERTY(int stopBits READ stopBits WRITE setStopBits NOTIFY stopBitsChanged) - Q_PROPERTY(int parity READ parity WRITE setParity NOTIFY parityChanged) - Q_PROPERTY(QString portName READ portName WRITE setPortName NOTIFY portNameChanged) - Q_PROPERTY(QString portDisplayName READ portDisplayName NOTIFY portDisplayNameChanged) - Q_PROPERTY(bool usbDirect READ usbDirect WRITE setUsbDirect NOTIFY usbDirectChanged) ///< true: direct usb connection to board - - int baud() const { return _baud; } - int dataBits() const { return _dataBits; } - int flowControl() const { return _flowControl; } ///< QSerialPort Enums - int stopBits() const { return _stopBits; } - int parity() const { return _parity; } ///< QSerialPort Enums - bool usbDirect() const { return _usbDirect; } - - const QString portName () const { return _portName; } - const QString portDisplayName () const { return _portDisplayName; } - - void setBaud (int baud); - void setDataBits (int databits); - void setFlowControl (int flowControl); ///< QSerialPort Enums - void setStopBits (int stopBits); - void setParity (int parity); ///< QSerialPort Enums - void setPortName (const QString& portName); - void setUsbDirect (bool usbDirect); + qint32 baud() const { return _baud; } + void setBaud(qint32 baud) { if (baud != _baud) { _baud = baud; emit baudChanged(); } } - static QStringList supportedBaudRates(); - static QString cleanPortDisplayname(const QString name); + QSerialPort::DataBits dataBits() const { return _dataBits; } + void setDataBits(QSerialPort::DataBits databits) { if (databits != _dataBits) { _dataBits = databits; emit dataBitsChanged(); } } - /// From LinkConfiguration - LinkType type () const override { return LinkConfiguration::TypeSerial; } - void copyFrom (const LinkConfiguration* source) override; - void loadSettings (QSettings& settings, const QString& root) override; - void saveSettings (QSettings& settings, const QString& root) override; - void updateSettings (); - QString settingsURL () override { return "SerialSettings.qml"; } - QString settingsTitle () override { return tr("Serial Link Settings"); } + QSerialPort::FlowControl flowControl() const { return _flowControl; } + void setFlowControl(QSerialPort::FlowControl flowControl) { if (flowControl != _flowControl) { _flowControl = flowControl; emit flowControlChanged(); } } + + QSerialPort::StopBits stopBits() const { return _stopBits; } + void setStopBits(QSerialPort::StopBits stopBits) { if (stopBits != _stopBits) { _stopBits = stopBits; emit stopBitsChanged(); } } + + QSerialPort::Parity parity() const { return _parity; } + void setParity(QSerialPort::Parity parity) { if (parity != _parity) { _parity = parity; emit parityChanged(); } } + + QString portName() const { return _portName; } + void setPortName(const QString &name); + + QString portDisplayName() const { return _portDisplayName; } + void setPortDisplayName(const QString &portDisplayName) { if (portDisplayName != _portDisplayName) { _portDisplayName = portDisplayName; emit portDisplayNameChanged(); } } + + bool usbDirect() const { return _usbDirect; } + void setUsbDirect(bool usbDirect) { if (usbDirect != _usbDirect) { _usbDirect = usbDirect; emit usbDirectChanged(); } } + + LinkType type() const override { return LinkConfiguration::TypeSerial; } + void copyFrom(const LinkConfiguration *source) override; + void loadSettings(QSettings &settings, const QString &root) override; + void saveSettings(QSettings &settings, const QString &root) override; + QString settingsURL() override { return QStringLiteral("SerialSettings.qml"); } + QString settingsTitle() override { return tr("Serial Link Settings"); } + + static QStringList supportedBaudRates(); + static QString cleanPortDisplayName(const QString &name); signals: - void baudChanged (); - void dataBitsChanged (); - void flowControlChanged (); - void stopBitsChanged (); - void parityChanged (); - void portNameChanged (); - void portDisplayNameChanged (); - void usbDirectChanged (bool usbDirect); + void baudChanged(); + void dataBitsChanged(); + void flowControlChanged(); + void stopBitsChanged(); + void parityChanged(); + void portNameChanged(); + void portDisplayNameChanged(); + void usbDirectChanged(); private: - int _baud; - int _dataBits; - int _flowControl; - int _stopBits; - int _parity; + qint32 _baud = QSerialPort::Baud57600; + QSerialPort::DataBits _dataBits = QSerialPort::Data8; + QSerialPort::FlowControl _flowControl = QSerialPort::NoFlowControl; + QSerialPort::StopBits _stopBits = QSerialPort::OneStop; + QSerialPort::Parity _parity = QSerialPort::NoParity; QString _portName; QString _portDisplayName; - bool _usbDirect; + bool _usbDirect = false; }; -class SerialLink : public LinkInterface +/*===========================================================================*/ + +class SerialWorker : public QObject { Q_OBJECT public: - SerialLink(SharedLinkConfigurationPtr& config); - virtual ~SerialLink(); + explicit SerialWorker(const SerialConfiguration *config, QObject *parent = nullptr); + ~SerialWorker(); - // LinkInterface overrides - bool isConnected (void) const override; - void disconnect (void) override; - bool isSecureConnection (void) override; + bool isConnected() const; + const QSerialPort *port() const { return _port; } - /// Don't even think of calling this method! - QSerialPort* _hackAccessToPort(void) { return _port; } +signals: + void connected(); + void disconnected(); + void dataReceived(const QByteArray &data); + void dataSent(const QByteArray &data); + void errorOccurred(const QString &errorString); + +public slots: + void connectToPort(); + void disconnectFromPort(); + void writeData(const QByteArray &data); private slots: - void _writeBytes(const QByteArray &data) override; + void _onDisconnected(); + void _onReadyRead(); + void _onBytesWritten(qint64 bytes) const; + void _onErrorOccurred(QSerialPort::SerialPortError error); + +private: + const SerialConfiguration *_config = nullptr; + QSerialPort *_port = nullptr; +}; + +/*===========================================================================*/ + +class SerialLink : public LinkInterface +{ + Q_OBJECT + +public: + explicit SerialLink(SharedLinkConfigurationPtr &config, QObject *parent = nullptr); + virtual ~SerialLink(); + + bool isConnected() const override; + bool isSecureConnection() override { return _serialConfig->usbDirect(); } + + const QSerialPort *port() const { return _worker->port(); } public slots: - void linkError(QSerialPort::SerialPortError error); + void disconnect() override; private slots: - void _readBytes (void); + void _onConnected(); + void _onDisconnected(); + void _onDataReceived(const QByteArray &data); + void _onDataSent(const QByteArray &data); + void _onErrorOccurred(const QString &errorString); private: - // LinkInterface overrides - bool _connect(void) override; - - void _emitLinkError (const QString& errorMsg); - bool _hardwareConnect (QSerialPort::SerialPortError& error, QString& errorString); - bool _isBootloader (void); - - QSerialPort* _port = nullptr; - quint64 _bytesRead = 0; - int _timeout; - QMutex _dataMutex; ///< Mutex for reading data from _port - QMutex _writeMutex; ///< Mutex for accessing the _transmitBuffer. - volatile bool _stopp = false; - QMutex _stoppMutex; ///< Mutex for accessing _stopp - QByteArray _transmitBuffer; ///< An internal buffer for receiving data from member functions and actually transmitting them via the serial port. - const SerialConfiguration* _serialConfig = nullptr; + bool _connect() override; + void _writeBytes(const QByteArray &data) override; + + const SerialConfiguration *_serialConfig = nullptr; + SerialWorker *_worker = nullptr; + QThread *_workerThread = nullptr; };