From 1d0bf6270960570bfe408f478fa411f3065c4799 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Fri, 25 Mar 2016 14:20:00 -0400 Subject: [PATCH 1/5] Force link writes to occur via a signal/slot connection. This mechanism automatically queues data into the correct thread where necessary, otherwise the method is invoked directly with low overhead. At the same time remove the UDP socket event workaround. It is not a problem. --- src/Vehicle/Vehicle.cc | 2 +- src/comm/BluetoothLink.cc | 9 ++---- src/comm/BluetoothLink.h | 8 +++-- src/comm/LinkInterface.h | 13 ++++++-- src/comm/LinkManager.cc | 2 +- src/comm/LogReplayLink.cc | 3 +- src/comm/LogReplayLink.h | 4 +-- src/comm/MAVLinkProtocol.cc | 4 +-- src/comm/MockLink.cc | 12 +------- src/comm/MockLink.h | 9 ++---- src/comm/QGCFlightGearLink.cc | 8 ++--- src/comm/QGCFlightGearLink.h | 6 +++- src/comm/QGCHilLink.h | 18 ++++++++++- src/comm/QGCJSBSimLink.cc | 8 ++--- src/comm/QGCJSBSimLink.h | 6 +++- src/comm/QGCXPlaneLink.cc | 20 ++++++------ src/comm/QGCXPlaneLink.h | 6 +++- src/comm/SerialLink.cc | 6 ++-- src/comm/SerialLink.h | 5 +-- src/comm/TCPLink.cc | 12 ++++---- src/comm/TCPLink.h | 9 +++--- src/comm/UDPLink.cc | 70 +++--------------------------------------- src/comm/UDPLink.h | 9 ++---- src/comm/XbeeLink.cpp | 12 ++------ src/comm/XbeeLink.h | 4 +-- src/qgcunittest/TCPLinkTest.cc | 2 +- 26 files changed, 108 insertions(+), 159 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index ab953ef..3e71ab9 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -733,7 +733,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int len = mavlink_msg_to_send_buffer(buffer, &message); - link->writeBytes((const char*)buffer, len); + link->writeBytesSafe((const char*)buffer, len); _messagesSent++; emit messagesSentChanged(); } diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc index 3f814b9..f1645e2 100644 --- a/src/comm/BluetoothLink.cc +++ b/src/comm/BluetoothLink.cc @@ -89,18 +89,13 @@ QString BluetoothLink::getName() const return _config->name(); } -void BluetoothLink::writeBytes(const char* data, qint64 size) -{ - _sendBytes(data, size); -} - -void BluetoothLink::_sendBytes(const char* data, qint64 size) +void BluetoothLink::writeBytes(const QByteArray bytes) { if(_targetSocket) { if(_targetSocket->isWritable()) { - if(_targetSocket->write(data, size) > 0) { + if(_targetSocket->write(bytes) > 0) { _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch()); } else diff --git a/src/comm/BluetoothLink.h b/src/comm/BluetoothLink.h index 1767cc1..3fe6540 100644 --- a/src/comm/BluetoothLink.h +++ b/src/comm/BluetoothLink.h @@ -165,9 +165,7 @@ public: LinkConfiguration* getLinkConfiguration() { return _config; } public slots: - void readBytes (); - void writeBytes (const char* data, qint64 length); void deviceConnected (); void deviceDisconnected (); void deviceError (QBluetoothSocket::SocketError error); @@ -193,7 +191,11 @@ private: bool _hardwareConnect (); void _restartConnection (); - void _sendBytes (const char* data, qint64 size); + +private slots: + void writeBytes (const QByteArray bytes); + +private: void _createSocket (); private: diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index ff73934..dbd2d3b 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -151,16 +151,24 @@ public slots: * * If the underlying communication is packet oriented, * one write command equals a datagram. In case of serial - * communication arbitrary byte lengths can be written + * communication arbitrary byte lengths can be written. The method ensures + * thread safety regardless of the underlying LinkInterface implementation. * * @param bytes The pointer to the byte array containing the data * @param length The length of the data array **/ - virtual void writeBytes(const char *bytes, qint64 length) = 0; + void writeBytesSafe(const char *bytes, int length) + { + emit _invokeWriteBytes(QByteArray(bytes, length)); + } + +private slots: + virtual void writeBytes(const QByteArray) = 0; signals: void autoconnectChanged(bool autoconnect); void activeChanged(bool active); + void _invokeWriteBytes(QByteArray); /** * @brief New data arrived @@ -212,6 +220,7 @@ protected: memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts)); memset(_outDataWriteTimes, 0, sizeof(_outDataWriteTimes)); + QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::writeBytes); qRegisterMetaType("LinkInterface*"); } diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 6840343..e1310ba 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -881,7 +881,7 @@ void LinkManager::_activeLinkCheck(void) if (!found && link) { // See if we can get an NSH prompt on this link bool foundNSHPrompt = false; - link->writeBytes("\r", 1); + link->writeBytesSafe("\r", 1); QSignalSpy spy(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray))); if (spy.wait(100)) { QList arguments = spy.takeFirst(); diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index f3ba81e..439e594 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -151,10 +151,9 @@ void LogReplayLink::_replayError(const QString& errorMsg) } /// Since this is log replay, we just drops writes on the floor -void LogReplayLink::writeBytes(const char* bytes, qint64 cBytes) +void LogReplayLink::writeBytes(const QByteArray bytes) { Q_UNUSED(bytes); - Q_UNUSED(cBytes); } /// Parses a BigEndian quint64 timestamp diff --git a/src/comm/LogReplayLink.h b/src/comm/LogReplayLink.h index 034e5ae..1c28bd8 100644 --- a/src/comm/LogReplayLink.h +++ b/src/comm/LogReplayLink.h @@ -98,8 +98,8 @@ public: bool connect(void); bool disconnect(void); -public slots: - virtual void writeBytes(const char *bytes, qint64 cBytes); +private slots: + virtual void writeBytes(const QByteArray bytes); signals: void logFileStats(bool logTimestamped, int logDurationSecs, int binaryBaudRate); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 453ab1b..ebde1f1 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -438,7 +438,7 @@ void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t messag if (link->isConnected()) { // Send the portion of the buffer now occupied by the message - link->writeBytes((const char*)buffer, len); + link->writeBytesSafe((const char*)buffer, len); } } @@ -461,7 +461,7 @@ void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t messag if (link->isConnected()) { // Send the portion of the buffer now occupied by the message - link->writeBytes((const char*)buffer, len); + link->writeBytesSafe((const char*)buffer, len); } } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 64780a1..d06d56d 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -115,7 +115,6 @@ MockLink::MockLink(MockConfiguration* config) moveToThread(this); _loadParams(); - QObject::connect(this, &MockLink::_incomingBytes, this, &MockLink::_handleIncomingBytes); } MockLink::~MockLink(void) @@ -314,16 +313,7 @@ void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg) } /// @brief Called when QGC wants to write bytes to the MAV -void MockLink::writeBytes(const char* bytes, qint64 cBytes) -{ - // Package up the data so we can signal it over to the right thread - QByteArray byteArray(bytes, cBytes); - - emit _incomingBytes(byteArray); -} - -/// @brief Handles bytes from QGC on the thread -void MockLink::_handleIncomingBytes(const QByteArray bytes) +void MockLink::writeBytes(const QByteArray bytes) { if (_inNSH) { _handleIncomingNSHBytes(bytes.constData(), bytes.count()); diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 304b941..49ef8f0 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -150,12 +150,8 @@ public: static MockLink* startAPMArduCopterMockLink (bool sendStatusText); static MockLink* startAPMArduPlaneMockLink (bool sendStatusText); -signals: - /// @brief Used internally to move data to the thread. - void _incomingBytes(const QByteArray bytes); - -public slots: - virtual void writeBytes(const char *bytes, qint64 cBytes); +private slots: + virtual void writeBytes(const QByteArray bytes); private slots: void _run1HzTasks(void); @@ -172,7 +168,6 @@ private: // MockLink methods void _sendHeartBeat(void); - void _handleIncomingBytes(const QByteArray bytes); void _handleIncomingNSHBytes(const char* bytes, int cBytes); void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes); void _loadParams(void); diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 16d2ed2..9b8773f 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -234,7 +234,7 @@ void QGCFlightGearLink::updateControls(quint64 time, float rollAilerons, float p { QString state("%1\t%2\t%3\t%4\t%5\n"); state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle); - writeBytes(state.toLatin1().constData(), state.length()); + emit _invokeWriteBytes(state.toLatin1()); //qDebug() << "Updated controls" << rollAilerons << pitchElevator << yawRudder << throttle; //qDebug() << "Updated controls" << state; } @@ -244,13 +244,13 @@ void QGCFlightGearLink::updateControls(quint64 time, float rollAilerons, float p } } -void QGCFlightGearLink::writeBytes(const char* data, qint64 size) +void QGCFlightGearLink::writeBytes(const QByteArray data) { //#define QGCFlightGearLink_DEBUG #ifdef QGCFlightGearLink_DEBUG QString bytes; QString ascii; - for (int i=0; iwriteDatagram(data, size, currentHost, currentPort); + if (connectState && _udpCommSocket) _udpCommSocket->writeDatagram(data, currentHost, currentPort); } /** diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h index 8945b20..5db87ba 100644 --- a/src/comm/QGCFlightGearLink.h +++ b/src/comm/QGCFlightGearLink.h @@ -121,13 +121,17 @@ public slots: } void readBytes(); + +private slots: /** * @brief Write a number of bytes to the interface. * * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const char* data, qint64 length); + void writeBytes(const QByteArray data); + +public slots: bool connectSimulation(); bool disconnectSimulation(); diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h index abc1d36..19163e7 100644 --- a/src/comm/QGCHilLink.h +++ b/src/comm/QGCHilLink.h @@ -64,13 +64,26 @@ public slots: * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - virtual void writeBytes(const char* data, qint64 length) = 0; + void writeBytesSafe(const char* data, int length) + { + emit _invokeWriteBytes(QByteArray(data, length)); + } + virtual bool connectSimulation() = 0; virtual bool disconnectSimulation() = 0; +private slots: + virtual void writeBytes(const QByteArray) = 0; + protected: virtual void setName(QString name) = 0; + QGCHilLink() : + QThread() + { + connect(this, &QGCHilLink::_invokeWriteBytes, this, &QGCHilLink::writeBytes); + } + signals: /** * @brief This signal is emitted instantly when the link is connected @@ -130,6 +143,9 @@ signals: /** @brief Sensor leve HIL state changed */ void sensorHilChanged(bool enabled); + + /** @brief Helper signal to force execution on the correct thread */ + void _invokeWriteBytes(QByteArray); }; #endif // QGCHILLINK_H diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc index 487240c..0f31dbb 100644 --- a/src/comm/QGCJSBSimLink.cc +++ b/src/comm/QGCJSBSimLink.cc @@ -246,7 +246,7 @@ void QGCJSBSimLink::updateControls(quint64 time, float rollAilerons, float pitch { QString state("%1\t%2\t%3\t%4\t%5\n"); state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle); - writeBytes(state.toLatin1().constData(), state.length()); + emit _invokeWriteBytes(state.toLatin1()); } else { @@ -255,13 +255,13 @@ void QGCJSBSimLink::updateControls(quint64 time, float rollAilerons, float pitch //qDebug() << "Updated controls" << state; } -void QGCJSBSimLink::writeBytes(const char* data, qint64 size) +void QGCJSBSimLink::writeBytes(const QByteArray data) { //#define QGCJSBSimLink_DEBUG #ifdef QGCJSBSimLink_DEBUG QString bytes; QString ascii; - for (int i=0; iwriteDatagram(data, size, currentHost, currentPort); + if (connectState && socket) socket->writeDatagram(data, currentHost, currentPort); } /** diff --git a/src/comm/QGCJSBSimLink.h b/src/comm/QGCJSBSimLink.h index 41bc446..cd4e4b4 100644 --- a/src/comm/QGCJSBSimLink.h +++ b/src/comm/QGCJSBSimLink.h @@ -114,13 +114,17 @@ public slots: } void readBytes(); + +private slots: /** * @brief Write a number of bytes to the interface. * * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const char* data, qint64 length); + void writeBytes(const QByteArray data); + +public slots: bool connectSimulation(); bool disconnectSimulation(); diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 487f5b2..16b0c1c 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -229,7 +229,7 @@ void QGCXPlaneLink::run() strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6)); ip.use_ip = 1; - writeBytes((const char*)&ip, sizeof(ip)); + writeBytesSafe((const char*)&ip, sizeof(ip)); _should_exit = false; @@ -395,10 +395,10 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch { // Ail / Elevon / Rudder p.index = 12; // XPlane, wing sweep - writeBytes((const char*)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); p.index = 8; // XPlane, joystick? why? - writeBytes((const char*)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); p.index = 25; // Thrust memset(p.f, 0, sizeof(p.f)); @@ -408,13 +408,13 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch p.f[3] = throttle; // Throttle - writeBytes((const char*)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); } else { qDebug() << "Transmitting p.index = 25"; p.index = 25; // XPlane, throttle command. - writeBytes((const char*)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); } } @@ -448,14 +448,14 @@ Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) { return wRo; } -void QGCXPlaneLink::writeBytes(const char* data, qint64 size) +void QGCXPlaneLink::writeBytes(const QByteArray data) { - if (!data) return; + if (data.isEmpty()) return; // If socket exists and is connected, transmit the data if (socket && connectState) { - socket->writeDatagram(data, size, remoteHost, remotePort); + socket->writeDatagram(data, remoteHost, remotePort); } } @@ -899,7 +899,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub pos.gear_flap_vect[1] = 0.0f; pos.gear_flap_vect[2] = 0.0f; - writeBytes((const char*)&pos, sizeof(pos)); + writeBytesSafe((const char*)&pos, sizeof(pos)); // pos.header[0] = 'V'; // pos.header[1] = 'E'; @@ -917,7 +917,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub // pos.gear_flap_vect[1] = -999; // pos.gear_flap_vect[2] = -999; -// writeBytes((const char*)&pos, sizeof(pos)); +// writeBytesSafe((const char*)&pos, sizeof(pos)); } /** diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index 3ead098..4b234d0 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -126,13 +126,17 @@ public slots: void processError(QProcess::ProcessError err); void readBytes(); + +private slots: /** * @brief Write a number of bytes to the interface. * * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const char* data, qint64 length); + void writeBytes(const QByteArray data); + +public slots: bool connectSimulation(); bool disconnectSimulation(); /** diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index e20499a..4633d16 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -82,11 +82,11 @@ bool SerialLink::_isBootloader() return false; } -void SerialLink::writeBytes(const char* data, qint64 size) +void SerialLink::writeBytes(const QByteArray data) { if(_port && _port->isOpen()) { - _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch()); - _port->write(data, size); + _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); + _port->write(data); } else { // Error occured _emitLinkError(tr("Could not send data - link %1 is disconnected!").arg(getName())); diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 011dfb8..bdd5e07 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -157,15 +157,16 @@ public: bool connect(void); bool disconnect(void); -public slots: +private slots: /** * @brief Write a number of bytes to the interface. * * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const char* data, qint64 length); + void writeBytes(const QByteArray data); +public slots: void linkError(QSerialPort::SerialPortError error); protected: diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index 19935e7..529bfc5 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -65,11 +65,11 @@ void TCPLink::run() } #ifdef TCPLINK_READWRITE_DEBUG -void TCPLink::_writeDebugBytes(const char *data, qint16 size) +void TCPLink::_writeDebugBytes(const QByteArray data) { QString bytes; QString ascii; - for (int i=0; iwrite(data, size); - _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch()); + _socket->write(data); + _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); } /** diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h index ff9ac78..3644531 100644 --- a/src/comm/TCPLink.h +++ b/src/comm/TCPLink.h @@ -152,10 +152,11 @@ public: bool connect(void); bool disconnect(void); -public slots: - +private slots: // From LinkInterface - void writeBytes(const char* data, qint64 length); + void writeBytes(const QByteArray data); + +public slots: void waitForBytesWritten(int msecs); void waitForReadyRead(int msecs); @@ -182,7 +183,7 @@ private: void _restartConnection(); #ifdef TCPLINK_READWRITE_DEBUG - void _writeDebugBytes(const char *data, qint16 size); + void _writeDebugBytes(const QByteArray data); #endif TCPConfiguration* _config; diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index c3cedf5..726511f 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -29,12 +29,6 @@ This file is part of the QGROUNDCONTROL project */ #include -#if QT_VERSION > 0x050401 -#define UDP_BROKEN_SIGNAL 1 -#else -#define UDP_BROKEN_SIGNAL 0 -#endif - #include #include #include @@ -111,9 +105,6 @@ UDPLink::~UDPLink() quit(); // Wait for it to exit wait(); - while(_outQueue.count() > 0) { - delete _outQueue.dequeue(); - } this->deleteLater(); } @@ -124,25 +115,7 @@ UDPLink::~UDPLink() void UDPLink::run() { if(_hardwareConnect()) { - if(UDP_BROKEN_SIGNAL) { - bool loop = false; - while(true) { - //-- Anything to read? - loop = _socket->hasPendingDatagrams(); - if(loop) { - readBytes(); - } - //-- Loop right away if busy - if((_dequeBytes() || loop) && _running) - continue; - if(!_running) - break; - //-- Settle down (it gets here if there is nothing to read or write) - _socket->waitForReadyRead(5); - } - } else { - exec(); - } + exec(); } if (_socket) { _deregisterZeroconf(); @@ -174,35 +147,7 @@ void UDPLink::removeHost(const QString& host) _config->removeHost(host); } -void UDPLink::writeBytes(const char* data, qint64 size) -{ - if (!_socket) { - return; - } - if(UDP_BROKEN_SIGNAL) { - QByteArray* qdata = new QByteArray(data, size); - QMutexLocker lock(&_mutex); - _outQueue.enqueue(qdata); - } else { - _sendBytes(data, size); - } -} - - -bool UDPLink::_dequeBytes() -{ - QMutexLocker lock(&_mutex); - if(_outQueue.count() > 0) { - QByteArray* qdata = _outQueue.dequeue(); - lock.unlock(); - _sendBytes(qdata->data(), qdata->size()); - delete qdata; - lock.relock(); - } - return (_outQueue.count() > 0); -} - -void UDPLink::_sendBytes(const char* data, qint64 size) +void UDPLink::writeBytes(const QByteArray data) { QStringList goneHosts; // Send to all connected systems @@ -211,7 +156,7 @@ void UDPLink::_sendBytes(const char* data, qint64 size) if(_config->firstHost(host, port)) { do { QHostAddress currentHost(host); - if(_socket->writeDatagram(data, size, currentHost, (quint16)port) < 0) { + if(_socket->writeDatagram(data, currentHost, (quint16)port) < 0) { // This host is gone. Add to list to be removed // We should keep track of hosts that were manually added (static) and // hosts that were added because we heard from them (dynamic). Only @@ -225,7 +170,7 @@ void UDPLink::_sendBytes(const char* data, qint64 size) // "host not there" takes time too regardless of size of data. In fact, // 1 byte or "UDP frame size" bytes are the same as that's the data // unit sent by UDP. - _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch()); + _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); } } while (_config->nextHost(host, port)); //-- Remove hosts that are no longer there @@ -260,8 +205,6 @@ void UDPLink::readBytes() // would trigger this. // Add host to broadcast list if not yet present, or update its port _config->addHost(sender.toString(), (int)senderPort); - if(UDP_BROKEN_SIGNAL && !_running) - break; } //-- Send whatever is left if(databuffer.size()) { @@ -326,10 +269,7 @@ bool UDPLink::_hardwareConnect() _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 512 * 1024); #endif _registerZeroconf(_config->localPort(), kZeroconfRegistration); - //-- Connect signal if this version of Qt is not broken - if(!UDP_BROKEN_SIGNAL) { - QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes); - } + QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes); emit connected(); } else { emit communicationError("UDP Link Error", "Error binding UDP port"); diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index e354d1a..cb22923 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -201,13 +201,14 @@ public slots: void readBytes(); +private slots: /*! * @brief Write a number of bytes to the interface. * * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const char* data, qint64 length); + void writeBytes(const QByteArray data); protected: @@ -235,12 +236,6 @@ private: #endif bool _running; - QMutex _mutex; - QQueue _outQueue; - - bool _dequeBytes (); - void _sendBytes (const char* data, qint64 size); - }; #endif // UDPLINK_H diff --git a/src/comm/XbeeLink.cpp b/src/comm/XbeeLink.cpp index d365eb8..8331a1b 100644 --- a/src/comm/XbeeLink.cpp +++ b/src/comm/XbeeLink.cpp @@ -172,17 +172,11 @@ void XbeeLink::_disconnect(void) emit disconnected(); } -void XbeeLink::writeBytes(const char *bytes, qint64 length) // TO DO: delete the data array +void XbeeLink::writeBytes(const QByteArray bytes) { - char *data; - data = new char[length]; - for(long i=0;im_xbeeCon,const_cast(bytes.data()),bytes.size())) // return value of 0 is successful written { - data[i] = bytes[i]; - } - if(!xbee_nsenddata(this->m_xbeeCon,data,length)) // return value of 0 is successful written - { - _logOutputDataRate(length, QDateTime::currentMSecsSinceEpoch()); + _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch()); } else { diff --git a/src/comm/XbeeLink.h b/src/comm/XbeeLink.h index 78048be..a8d2d34 100644 --- a/src/comm/XbeeLink.h +++ b/src/comm/XbeeLink.h @@ -45,8 +45,8 @@ public: qint64 getCurrentOutDataRate() const; qint64 getCurrentInDataRate() const; -public slots: // virtual functions from LinkInterface - void writeBytes(const char *bytes, qint64 length); +private slots: // virtual functions from LinkInterface + void writeBytes(const QByteArray bytes); protected slots: // virtual functions from LinkInterface void readBytes(); diff --git a/src/qgcunittest/TCPLinkTest.cc b/src/qgcunittest/TCPLinkTest.cc index 7d2c99f..45319d9 100644 --- a/src/qgcunittest/TCPLinkTest.cc +++ b/src/qgcunittest/TCPLinkTest.cc @@ -154,7 +154,7 @@ void TCPLinkUnitTest::_connectSucceed_test(void) const char* bytesWrittenSignal = SIGNAL(bytesWritten(qint64)); MultiSignalSpy bytesWrittenSpy; QCOMPARE(bytesWrittenSpy.init(_link->getSocket(), &bytesWrittenSignal, 1), true); - _link->writeBytes(bytesOut.data(), bytesOut.size()); + _link->writeBytesSafe(bytesOut.data(), bytesOut.size()); _multiSpy->clearAllSignals(); // We emit this signal such that it will be queued and run on the TCPLink thread. This in turn From 7f5726e76f1f52a7e6e182f7f33ca749fa6c439e Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Fri, 25 Mar 2016 14:25:50 -0400 Subject: [PATCH 2/5] Check for socket nullptr in UDPLink::writeBytes --- src/comm/UDPLink.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 726511f..380e1f4 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -149,6 +149,9 @@ void UDPLink::removeHost(const QString& host) void UDPLink::writeBytes(const QByteArray data) { + if (!_socket) + return; + QStringList goneHosts; // Send to all connected systems QString host; From 0aaca176660cc6e0a99b5feeee685e1617707037 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Fri, 25 Mar 2016 14:26:29 -0400 Subject: [PATCH 3/5] Check for socket nullptr in TCPLink::writeBytes --- src/comm/TCPLink.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index 529bfc5..7554a49 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -93,6 +93,9 @@ void TCPLink::writeBytes(const QByteArray data) #ifdef TCPLINK_READWRITE_DEBUG _writeDebugBytes(data); #endif + if (!_socket) + return; + _socket->write(data); _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); } From a2a203530e514a56c58e45349b8e6385c0ca57a1 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Fri, 25 Mar 2016 14:53:31 -0400 Subject: [PATCH 4/5] Fix BluetoothLink::writeBytes reference to size --- src/comm/BluetoothLink.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc index f1645e2..5062d12 100644 --- a/src/comm/BluetoothLink.cc +++ b/src/comm/BluetoothLink.cc @@ -96,7 +96,7 @@ void BluetoothLink::writeBytes(const QByteArray bytes) if(_targetSocket->isWritable()) { if(_targetSocket->write(bytes) > 0) { - _logOutputDataRate(size, QDateTime::currentMSecsSinceEpoch()); + _logOutputDataRate(bytes.size(), QDateTime::currentMSecsSinceEpoch()); } else qWarning() << "Bluetooth write error"; From fe8ff69e24fc78917ff1d94a446c70cd939a55e7 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Fri, 25 Mar 2016 16:36:00 -0400 Subject: [PATCH 5/5] Refactor writeBytes to _writeBytes --- src/comm/BluetoothLink.cc | 2 +- src/comm/BluetoothLink.h | 2 +- src/comm/LinkInterface.h | 4 ++-- src/comm/LogReplayLink.cc | 2 +- src/comm/LogReplayLink.h | 2 +- src/comm/MockLink.cc | 2 +- src/comm/MockLink.h | 2 +- src/comm/QGCFlightGearLink.cc | 2 +- src/comm/QGCFlightGearLink.h | 2 +- src/comm/QGCHilLink.h | 4 ++-- src/comm/QGCJSBSimLink.cc | 2 +- src/comm/QGCJSBSimLink.h | 2 +- src/comm/QGCXPlaneLink.cc | 2 +- src/comm/QGCXPlaneLink.h | 2 +- src/comm/SerialLink.cc | 2 +- src/comm/SerialLink.h | 2 +- src/comm/TCPLink.cc | 2 +- src/comm/TCPLink.h | 2 +- src/comm/UDPLink.cc | 2 +- src/comm/UDPLink.h | 2 +- src/comm/XbeeLink.cpp | 2 +- src/comm/XbeeLink.h | 2 +- 22 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/comm/BluetoothLink.cc b/src/comm/BluetoothLink.cc index 5062d12..f10161e 100644 --- a/src/comm/BluetoothLink.cc +++ b/src/comm/BluetoothLink.cc @@ -89,7 +89,7 @@ QString BluetoothLink::getName() const return _config->name(); } -void BluetoothLink::writeBytes(const QByteArray bytes) +void BluetoothLink::_writeBytes(const QByteArray bytes) { if(_targetSocket) { diff --git a/src/comm/BluetoothLink.h b/src/comm/BluetoothLink.h index 3fe6540..c2a83f4 100644 --- a/src/comm/BluetoothLink.h +++ b/src/comm/BluetoothLink.h @@ -193,7 +193,7 @@ private: void _restartConnection (); private slots: - void writeBytes (const QByteArray bytes); + void _writeBytes (const QByteArray bytes); private: void _createSocket (); diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index dbd2d3b..bd0b48e 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -163,7 +163,7 @@ public slots: } private slots: - virtual void writeBytes(const QByteArray) = 0; + virtual void _writeBytes(const QByteArray) = 0; signals: void autoconnectChanged(bool autoconnect); @@ -220,7 +220,7 @@ protected: memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts)); memset(_outDataWriteTimes, 0, sizeof(_outDataWriteTimes)); - QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::writeBytes); + QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes); qRegisterMetaType("LinkInterface*"); } diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 439e594..81ac1cc 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -151,7 +151,7 @@ void LogReplayLink::_replayError(const QString& errorMsg) } /// Since this is log replay, we just drops writes on the floor -void LogReplayLink::writeBytes(const QByteArray bytes) +void LogReplayLink::_writeBytes(const QByteArray bytes) { Q_UNUSED(bytes); } diff --git a/src/comm/LogReplayLink.h b/src/comm/LogReplayLink.h index 1c28bd8..e54d27e 100644 --- a/src/comm/LogReplayLink.h +++ b/src/comm/LogReplayLink.h @@ -99,7 +99,7 @@ public: bool disconnect(void); private slots: - virtual void writeBytes(const QByteArray bytes); + virtual void _writeBytes(const QByteArray bytes); signals: void logFileStats(bool logTimestamped, int logDurationSecs, int binaryBaudRate); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index d06d56d..9d161c6 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -313,7 +313,7 @@ void MockLink::respondWithMavlinkMessage(const mavlink_message_t& msg) } /// @brief Called when QGC wants to write bytes to the MAV -void MockLink::writeBytes(const QByteArray bytes) +void MockLink::_writeBytes(const QByteArray bytes) { if (_inNSH) { _handleIncomingNSHBytes(bytes.constData(), bytes.count()); diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 49ef8f0..395907e 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -151,7 +151,7 @@ public: static MockLink* startAPMArduPlaneMockLink (bool sendStatusText); private slots: - virtual void writeBytes(const QByteArray bytes); + virtual void _writeBytes(const QByteArray bytes); private slots: void _run1HzTasks(void); diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 9b8773f..5f0ec84 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -244,7 +244,7 @@ void QGCFlightGearLink::updateControls(quint64 time, float rollAilerons, float p } } -void QGCFlightGearLink::writeBytes(const QByteArray data) +void QGCFlightGearLink::_writeBytes(const QByteArray data) { //#define QGCFlightGearLink_DEBUG #ifdef QGCFlightGearLink_DEBUG diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h index 5db87ba..b9887ef 100644 --- a/src/comm/QGCFlightGearLink.h +++ b/src/comm/QGCFlightGearLink.h @@ -129,7 +129,7 @@ private slots: * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const QByteArray data); + void _writeBytes(const QByteArray data); public slots: bool connectSimulation(); diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h index 19163e7..9ac6981 100644 --- a/src/comm/QGCHilLink.h +++ b/src/comm/QGCHilLink.h @@ -73,7 +73,7 @@ public slots: virtual bool disconnectSimulation() = 0; private slots: - virtual void writeBytes(const QByteArray) = 0; + virtual void _writeBytes(const QByteArray) = 0; protected: virtual void setName(QString name) = 0; @@ -81,7 +81,7 @@ protected: QGCHilLink() : QThread() { - connect(this, &QGCHilLink::_invokeWriteBytes, this, &QGCHilLink::writeBytes); + connect(this, &QGCHilLink::_invokeWriteBytes, this, &QGCHilLink::_writeBytes); } signals: diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc index 0f31dbb..fe82547 100644 --- a/src/comm/QGCJSBSimLink.cc +++ b/src/comm/QGCJSBSimLink.cc @@ -255,7 +255,7 @@ void QGCJSBSimLink::updateControls(quint64 time, float rollAilerons, float pitch //qDebug() << "Updated controls" << state; } -void QGCJSBSimLink::writeBytes(const QByteArray data) +void QGCJSBSimLink::_writeBytes(const QByteArray data) { //#define QGCJSBSimLink_DEBUG #ifdef QGCJSBSimLink_DEBUG diff --git a/src/comm/QGCJSBSimLink.h b/src/comm/QGCJSBSimLink.h index cd4e4b4..e09d43b 100644 --- a/src/comm/QGCJSBSimLink.h +++ b/src/comm/QGCJSBSimLink.h @@ -122,7 +122,7 @@ private slots: * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const QByteArray data); + void _writeBytes(const QByteArray data); public slots: bool connectSimulation(); diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 16b0c1c..244b143 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -448,7 +448,7 @@ Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) { return wRo; } -void QGCXPlaneLink::writeBytes(const QByteArray data) +void QGCXPlaneLink::_writeBytes(const QByteArray data) { if (data.isEmpty()) return; diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index 4b234d0..03ebf12 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -134,7 +134,7 @@ private slots: * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const QByteArray data); + void _writeBytes(const QByteArray data); public slots: bool connectSimulation(); diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 4633d16..ae702b5 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -82,7 +82,7 @@ bool SerialLink::_isBootloader() return false; } -void SerialLink::writeBytes(const QByteArray data) +void SerialLink::_writeBytes(const QByteArray data) { if(_port && _port->isOpen()) { _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index bdd5e07..f787b81 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -164,7 +164,7 @@ private slots: * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const QByteArray data); + void _writeBytes(const QByteArray data); public slots: void linkError(QSerialPort::SerialPortError error); diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index 7554a49..75114d3 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -88,7 +88,7 @@ void TCPLink::_writeDebugBytes(const QByteArray data) } #endif -void TCPLink::writeBytes(const QByteArray data) +void TCPLink::_writeBytes(const QByteArray data) { #ifdef TCPLINK_READWRITE_DEBUG _writeDebugBytes(data); diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h index 3644531..593e8e9 100644 --- a/src/comm/TCPLink.h +++ b/src/comm/TCPLink.h @@ -154,7 +154,7 @@ public: private slots: // From LinkInterface - void writeBytes(const QByteArray data); + void _writeBytes(const QByteArray data); public slots: void waitForBytesWritten(int msecs); diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 380e1f4..7f24c38 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -147,7 +147,7 @@ void UDPLink::removeHost(const QString& host) _config->removeHost(host); } -void UDPLink::writeBytes(const QByteArray data) +void UDPLink::_writeBytes(const QByteArray data) { if (!_socket) return; diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index cb22923..23c1151 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -208,7 +208,7 @@ private slots: * @param data Pointer to the data byte array * @param size The size of the bytes array **/ - void writeBytes(const QByteArray data); + void _writeBytes(const QByteArray data); protected: diff --git a/src/comm/XbeeLink.cpp b/src/comm/XbeeLink.cpp index 8331a1b..c3a7b15 100644 --- a/src/comm/XbeeLink.cpp +++ b/src/comm/XbeeLink.cpp @@ -172,7 +172,7 @@ void XbeeLink::_disconnect(void) emit disconnected(); } -void XbeeLink::writeBytes(const QByteArray bytes) +void XbeeLink::_writeBytes(const QByteArray bytes) { if(!xbee_nsenddata(this->m_xbeeCon,const_cast(bytes.data()),bytes.size())) // return value of 0 is successful written { diff --git a/src/comm/XbeeLink.h b/src/comm/XbeeLink.h index a8d2d34..02180dd 100644 --- a/src/comm/XbeeLink.h +++ b/src/comm/XbeeLink.h @@ -46,7 +46,7 @@ public: qint64 getCurrentInDataRate() const; private slots: // virtual functions from LinkInterface - void writeBytes(const QByteArray bytes); + void _writeBytes(const QByteArray bytes); protected slots: // virtual functions from LinkInterface void readBytes();