From 406f9cc48e063346c6c6c72a2d4ccf64b30ec7dd Mon Sep 17 00:00:00 2001 From: Holden Date: Fri, 2 Aug 2024 03:16:54 -0400 Subject: [PATCH] Comms: Convert Serial to Signals/Slots --- .../APM/APMAutoPilotPlugin.cc | 2 +- src/Comms/LinkManager.cc | 16 +- src/Comms/LinkManager.h | 3 - src/Comms/SerialLink.cc | 457 ++++++------------ src/Comms/SerialLink.h | 171 +++---- 5 files changed, 233 insertions(+), 416 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc b/src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc index 9ee688a589bb..0c67b9d6962c 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 70adeb3bb59d..c6e5d5a3630f 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -71,7 +71,9 @@ LinkManager::LinkManager(QGCApplication* app, QGCToolbox* toolbox) qRegisterMetaType(); qRegisterMetaType("LinkInterface*"); +#ifndef NO_SERIAL_LINK qRegisterMetaType("QGCSerialPortInfo"); +#endif } LinkManager::~LinkManager() @@ -257,11 +259,6 @@ void LinkManager::setConnectionsSuspended(QString reason) _connectionsSuspendedReason = reason; } -void LinkManager::suspendConfigurationUpdates(bool suspend) -{ - _configUpdateSuspended = suspend; -} - void LinkManager::saveLinkConfigurationList() { QSettings settings; @@ -915,7 +912,8 @@ bool LinkManager::_allowAutoConnectToBoard(QGCSerialPortInfo::BoardType_t boardT bool LinkManager::_portAlreadyConnected(const QString &portName) { const QString searchPort = portName.trimmed(); - for (const SharedLinkInterfacePtr &linkConfig : _rgLinks) { + for (const SharedLinkInterfacePtr &linkInterface : _rgLinks) { + const SharedLinkConfigurationPtr linkConfig = linkInterface->linkConfiguration(); const SerialConfiguration* const serialConfig = qobject_cast(linkConfig.get()); if (serialConfig && (serialConfig->portName() == searchPort)) { return true; @@ -933,13 +931,13 @@ void LinkManager::_updateSerialPorts() for (const QGCSerialPortInfo &info: portList) { const QString port = info.systemLocation().trimmed(); // + " " + info.description(); _commPortList += port; - _commPortDisplayList += SerialConfiguration::cleanPortDisplayname(port); + _commPortDisplayList += SerialConfiguration::cleanPortDisplayName(port); } } QStringList LinkManager::serialPortStrings() { - if (!_commPortDisplayList.size()) { + if (_commPortDisplayList.isEmpty()) { _updateSerialPorts(); } @@ -948,7 +946,7 @@ QStringList LinkManager::serialPortStrings() QStringList LinkManager::serialPorts() { - if (!_commPortList.size()) { + if (_commPortList.isEmpty()) { _updateSerialPorts(); } diff --git a/src/Comms/LinkManager.h b/src/Comms/LinkManager.h index 2ff3411f7086..6fe24df38c31 100644 --- a/src/Comms/LinkManager.h +++ b/src/Comms/LinkManager.h @@ -81,9 +81,6 @@ class LinkManager : public QGCTool void loadLinkConfigurationList(); void saveLinkConfigurationList(); - /// Suspend automatic confguration updates (during link maintenance for instance) - void suspendConfigurationUpdates(bool suspend); - /// Sets the flag to suspend the all new connections /// @param reason User visible reason to suspend connections void setConnectionsSuspended(QString reason); diff --git a/src/Comms/SerialLink.cc b/src/Comms/SerialLink.cc index 5a584c71c0e4..b1e90ab80085 100644 --- a/src/Comms/SerialLink.cc +++ b/src/Comms/SerialLink.cc @@ -8,408 +8,245 @@ ****************************************************************************/ #include "SerialLink.h" -#include "QGC.h" -#include "QGCApplication.h" + #include "QGCLoggingCategory.h" -#ifdef Q_OS_ANDROID -#include "LinkManager.h" -#include "qserialportinfo.h" -#else -#include -#endif +#include "QGCSerialPortInfo.h" + #include -QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog") +QGC_LOGGING_CATEGORY(SerialLinkLog, "qgc.comms.seriallink") -SerialLink::SerialLink(SharedLinkConfigurationPtr& config, bool isPX4Flow) - : LinkInterface(config, isPX4Flow) - , _serialConfig(qobject_cast(config.get())) -{ - qRegisterMetaType(); - qCDebug(SerialLinkLog) << "Create SerialLink portName:baud:flowControl:parity:dataButs:stopBits" << _serialConfig->portName() << _serialConfig->baud() << _serialConfig->flowControl() - << _serialConfig->parity() << _serialConfig->dataBits() << _serialConfig->stopBits(); -} +const QString SerialLink::kTitle = QStringLiteral("Serial Link Error"); +const QString SerialLink::kError = QStringLiteral("Link %1: %2."); -SerialLink::~SerialLink() +SerialLink::SerialLink(SharedLinkConfigurationPtr &config, bool isPX4Flow, QObject *parent) + : LinkInterface(config, isPX4Flow, parent) + , _serialConfig(qobject_cast(config.get())) + , _port(new QSerialPort(_serialConfig->portName(), this)) { - disconnect(); -} + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; -bool SerialLink::_isBootloader() -{ - 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; + (void) qRegisterMetaType("QSerialPort::SerialPortError"); + + (void) connect(_port, &QIODevice::aboutToClose, this, &SerialLink::disconnected, Qt::AutoConnection); + + (void) QObject::connect(_port, &QSerialPort::errorOccurred, this, [this](QSerialPort::SerialPortError error) { + switch (error) { + case QSerialPort::NoError: + return; + case QSerialPort::ResourceError: + _connectionRemoved(); + break; + default: + break; } - } - // Not found - return false; + qCWarning(SerialLinkLog) << "Serial Link Error:" << error; + emit communicationError(kTitle, kError.arg(_serialConfig->name(), _port->errorString())); + }, Qt::AutoConnection); } -void SerialLink::_writeBytes(const QByteArray &data) +SerialLink::~SerialLink() { - 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())); - } + SerialLink::disconnect(); + + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; } -void SerialLink::disconnect(void) +void SerialLink::disconnect() { - 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(); - } - -#ifdef Q_OS_ANDROID - qgcApp()->toolbox()->linkManager()->suspendConfigurationUpdates(false); -#endif + (void) QObject::disconnect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes); + _port->close(); } -bool SerialLink::_connect(void) +bool SerialLink::_connect() { - qCDebug(SerialLinkLog) << "CONNECT CALLED"; - - if (_port) { - qCWarning(SerialLinkLog) << "connect called while already connected"; + if (isConnected()) { return true; } -#ifdef Q_OS_ANDROID - qgcApp()->toolbox()->linkManager()->suspendConfigurationUpdates(true); -#endif - - 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; - } - } + if (_hardwareConnect()) { + emit connected(); + return true; + } - _emitLinkError(tr("Error connecting: Could not create port. %1").arg(errorString)); - return false; + if (!_serialConfig->isAutoConnect() || (_port->error() != QSerialPort::PermissionError)) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Failed to Connect."))); } - return true; + + return false; } -/// 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) +bool SerialLink::_hardwareConnect() { - if (_port) { - qCDebug(SerialLinkLog) << "SerialLink:" << QString::number((qulonglong)this, 16) << "closing port"; - _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); + const QGCSerialPortInfo portInfo(*_port); + if (portInfo.isBootloader()) { + if (!_serialConfig->isAutoConnect()) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Not Connecting to a Bootloader."))); } - delete _port; - _port = nullptr; + return false; } - 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; + if (!_port->open(QIODevice::ReadWrite)) { + if (!_serialConfig->isAutoConnect()) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Could Not Open Port."))); } + return false; } - _port = new QSerialPort(_serialConfig->portName(), this); - - QObject::connect(_port, &QSerialPort::errorOccurred, this, &SerialLink::linkError); - QObject::connect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes); - - // 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 - - // 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 { - 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 - } + (void) QObject::connect(_port, &QIODevice::readyRead, this, &SerialLink::_readBytes, Qt::AutoConnection); _port->setDataTerminalReady(true); - 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())); + _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())); - emit connected(); - - qCDebug(SerialLinkLog) << "Connection SeriaLink: " << "with settings" << _serialConfig->portName() - << _serialConfig->baud() << _serialConfig->dataBits() << _serialConfig->parity() << _serialConfig->stopBits(); - - return true; // successful connection + return true; } -void SerialLink::_readBytes(void) +void SerialLink::_writeBytes(const QByteArray &data) { - 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 (!isConnected()) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Could Not Send Data - Link is Disconnected!"))); + return; } -} -void SerialLink::linkError(QSerialPort::SerialPortError error) -{ - 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; + if (_port->write(data) <= 0) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Could Not Send Data - Write Failed!"))); + return; } + + emit bytesSent(this, data); } -bool SerialLink::isConnected() const +void SerialLink::_readBytes() { - bool isConnected = false; + if (!isConnected()) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Could Not Read Data - link is Disconnected!"))); + return; + } - if (_port) { - isConnected = _port->isOpen(); + const qint64 byteCount = _port->bytesAvailable(); + if (byteCount <= 0) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Could Not Read Data - No Data Available!"))); + return; } - return isConnected; + QByteArray buffer(byteCount, Qt::Initialization::Uninitialized); + if (_port->read(buffer.data(), buffer.size()) < 0) { + emit communicationError(kTitle, kError.arg(_serialConfig->name(), QStringLiteral("Could Not Read Data - Read Failed!"))); + return; + } + + emit bytesReceived(this, buffer); } -void SerialLink::_emitLinkError(const QString& errorMsg) +/*===========================================================================*/ + +SerialConfiguration::SerialConfiguration(const QString &name, QObject *parent) + : LinkConfiguration(name, parent) { - QString msg("Error on link %1. %2"); - qDebug() << errorMsg; - emit communicationError(tr("Link Error"), msg.arg(_config->name()).arg(errorMsg)); + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; } -bool SerialLink::isSecureConnection() +SerialConfiguration::SerialConfiguration(SerialConfiguration *copy, QObject *parent) + : LinkConfiguration(copy, parent) { - return _serialConfig && _serialConfig->usbDirect(); -} + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; -//-------------------------------------------------------------------------- -//-- SerialConfiguration + Q_CHECK_PTR(copy); -SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguration(name) -{ - _baud = 57600; - _flowControl= QSerialPort::NoFlowControl; - _parity = QSerialPort::NoParity; - _dataBits = 8; - _stopBits = 1; - _usbDirect = false; + SerialConfiguration::copyFrom(copy); } -SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfiguration(copy) +SerialConfiguration::~SerialConfiguration() { - _baud = copy->baud(); - _flowControl = copy->flowControl(); - _parity = copy->parity(); - _dataBits = copy->dataBits(); - _stopBits = copy->stopBits(); - _portName = copy->portName(); - _portDisplayName = copy->portDisplayName(); - _usbDirect = copy->_usbDirect; + // qCDebug(SerialLinkLog) << Q_FUNC_INFO << this; } void SerialConfiguration::copyFrom(LinkConfiguration *source) { + Q_CHECK_PTR(source); LinkConfiguration::copyFrom(source); - auto* 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"; - } -} - -void SerialConfiguration::setBaud(int baud) -{ - _baud = baud; -} -void SerialConfiguration::setDataBits(int databits) -{ - _dataBits = databits; -} + const SerialConfiguration* const serialSource = qobject_cast(source); + Q_CHECK_PTR(serialSource); -void SerialConfiguration::setFlowControl(int flowControl) -{ - _flowControl = flowControl; + 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::setStopBits(int stopBits) +void SerialConfiguration::setPortName(const QString &name) { - _stopBits = stopBits; -} - -void SerialConfiguration::setParity(int parity) -{ - _parity = parity; -} + const QString portName = name.trimmed(); + if (portName.isEmpty()) { + return; + } -void SerialConfiguration::setPortName(const QString& portName) -{ - // No effect on a running connection - QString pname = portName.trimmed(); - if (!pname.isEmpty() && pname != _portName) { - _portName = pname; - _portDisplayName = cleanPortDisplayname(pname); + if (portName != _portName) { + _portName = portName; + emit portNameChanged(); } -} -QString SerialConfiguration::cleanPortDisplayname(const QString name) -{ - QString pname = name.trimmed(); -#ifdef Q_OS_WIN - pname.replace("\\\\.\\", ""); -#else - pname.replace("/dev/cu.", ""); - pname.replace("/dev/", ""); -#endif - return pname; + const QString portDisplayName = cleanPortDisplayName(portName); + setPortDisplayName(portDisplayName); } -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()); + _portDisplayName = 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()) { + for (qint32 rate : QSerialPortInfo::standardBaudRates()) { (void) 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); + for (const QSerialPortInfo &info : QSerialPortInfo::availablePorts()) { + if (info.systemLocation() == name) { + return info.portName(); + } } + + return QString(); } diff --git a/src/Comms/SerialLink.h b/src/Comms/SerialLink.h index 1d1f3f710509..2a78846eb64d 100644 --- a/src/Comms/SerialLink.h +++ b/src/Comms/SerialLink.h @@ -9,136 +9,121 @@ #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) - -// We use QSerialPort::SerialPortError in a signal so we must declare it as a meta type -Q_DECLARE_METATYPE(QSerialPort::SerialPortError) +#include "LinkConfiguration.h" +#include "LinkInterface.h" -class LinkManager; +Q_DECLARE_LOGGING_CATEGORY(SerialLinkLog) -/// 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(SerialConfiguration *copy, QObject *parent = nullptr); + virtual ~SerialConfiguration(); - SerialConfiguration(const QString& name); - SerialConfiguration(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 () { return LinkConfiguration::TypeSerial; } - void copyFrom (LinkConfiguration* source); - void loadSettings (QSettings& settings, const QString& root); - void saveSettings (QSettings& settings, const QString& root); - void updateSettings (); - QString settingsURL () { return "SerialSettings.qml"; } - QString settingsTitle () { 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() override { return LinkConfiguration::TypeSerial; } + void copyFrom(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 QStringLiteral("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 { Q_OBJECT public: - SerialLink(SharedLinkConfigurationPtr& config, bool isPX4Flow = false); + explicit SerialLink(SharedLinkConfigurationPtr &config, bool isPX4Flow = false, QObject *parent = nullptr); virtual ~SerialLink(); - // LinkInterface overrides - bool isConnected (void) const override; - void disconnect (void) override; - bool isSecureConnection (void) override; + void run() override {} + bool isConnected() const override { return _port->isOpen(); } + void disconnect() override; + bool isSecureConnection() override { return (_serialConfig && _serialConfig->usbDirect()); } - /// Don't even think of calling this method! - QSerialPort* _hackAccessToPort(void) { return _port; } + const QSerialPort *port() const { return _port; } private slots: void _writeBytes(const QByteArray &data) override; + void _readBytes(); -public slots: - void linkError(QSerialPort::SerialPortError error); +private: + bool _connect() override; + bool _hardwareConnect(); -private slots: - void _readBytes (void); + const SerialConfiguration *_serialConfig = nullptr; + QSerialPort *_port = nullptr; -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. - SerialConfiguration* _serialConfig = nullptr; + static const QString kTitle; + static const QString kError; };