diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index c0cd800d16faac75e43e3856bcafa2beab6cac18..ce9cd426e25aad33e02e7ea7d8a87634f83221cb 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -641,6 +641,7 @@ SOURCES += \ src/VehicleSetup/JoystickConfigController.cc \ src/audio/QGCAudioWorker.cpp \ src/comm/LinkConfiguration.cc \ + src/comm/LinkInterface.cc \ src/comm/LinkManager.cc \ src/comm/MAVLinkProtocol.cc \ src/comm/QGCMAVLink.cc \ diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index 2a8168db32061e1c2cccb1a2d6733f0a5a02fc93..27966dd365445c63dde0887f526ba79f3f81eea3 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -155,8 +155,8 @@ void QGroundControlQmlGlobal::stopAllMockLinks(void) #ifdef QT_DEBUG LinkManager* linkManager = qgcApp()->toolbox()->linkManager(); - for (int i=0; ilinks()->count(); i++) { - LinkInterface* link = linkManager->links()->value(i); + for (int i=0; ilinks().count(); i++) { + LinkInterface* link = linkManager->links()[i]; MockLink* mockLink = qobject_cast(link); if (mockLink) { diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 5baa4fab65451ac1d53554284e4c3fac91b5218e..eae33e12bf7f898b754a20268f360e9cd4617392 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -306,9 +306,9 @@ void MultiVehicleManager::setGcsHeartbeatEnabled(bool gcsHeartBeatEnabled) void MultiVehicleManager::_sendGCSHeartbeat(void) { // Send a heartbeat out on each link - QmlObjectListModel* links = _toolbox->linkManager()->links(); - for (int i=0; icount(); i++) { - LinkInterface* link = links->value(i); + LinkManager* linkMgr = _toolbox->linkManager(); + for (int i=0; ilinks().count(); i++) { + LinkInterface* link = linkMgr->links()[i]; if (link->isConnected()) { mavlink_message_t message; mavlink_msg_heartbeat_pack_chan(_mavlinkProtocol->getSystemId(), diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 52a7183f9d26b2861758c3edb4bab21204fcf876..4b8387c0387e89ffcb4977cf253a762ed4b74e13 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -926,8 +926,9 @@ bool Vehicle::_containsLink(LinkInterface* link) void Vehicle::_addLink(LinkInterface* link) { if (!_containsLink(link)) { - _links += link; qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16); + _links += link; + _updatePriorityLink(); connect(qgcApp()->toolbox()->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted); connect(qgcApp()->toolbox()->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted); } @@ -938,6 +939,7 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link) qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count(); _links.removeOne(link); + _updatePriorityLink(); if (_links.count() == 0 && !_allLinksInactiveSent) { qCDebug(VehicleLog) << "All links inactive"; @@ -983,26 +985,42 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) emit messagesSentChanged(); } -/// @return Direct usb connection link to board if one, NULL if none -LinkInterface* Vehicle::priorityLink(void) +void Vehicle::_updatePriorityLink(void) { #ifndef NO_SERIAL_LINK - foreach (LinkInterface* link, _links) { + LinkInterface* newPriorityLink = NULL; + + // Note that this routine specificallty does not clear _priorityLink when there are no links remaining. + // By doing this we hold a reference on the last link as the Vehicle shuts down. Thus preventing shutdown + // ordering NULL pointer crashes where priorityLink() is still called during shutdown sequence. + for (int i=0; i<_links.count(); i++) { + LinkInterface* link = _links[i]; if (link->isConnected()) { SerialLink* pSerialLink = qobject_cast(link); if (pSerialLink) { - LinkConfiguration* pLinkConfig = pSerialLink->getLinkConfiguration(); - if (pLinkConfig) { - SerialConfiguration* pSerialConfig = qobject_cast(pLinkConfig); + LinkConfiguration* config = pSerialLink->getLinkConfiguration(); + if (config) { + SerialConfiguration* pSerialConfig = qobject_cast(config); if (pSerialConfig && pSerialConfig->usbDirect()) { - return link; + if (_priorityLink.data() != link) { + newPriorityLink = link; + break; + } + return; } } } } } + + if (!newPriorityLink && !_priorityLink.data() && _links.count()) { + newPriorityLink = _links[0]; + } + + if (newPriorityLink) { + _priorityLink = qgcApp()->toolbox()->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); + } #endif - return _links.count() ? _links[0] : NULL; } void Vehicle::setLatitude(double latitude) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2d6bdcd4e64fb1e2e49f5fcc81c038bf6c478bff..aadbf51b9e764db2eff269d587e364f5c2474bbb 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -416,8 +416,9 @@ public: MAV_TYPE vehicleType(void) const { return _vehicleType; } Q_INVOKABLE QString vehicleTypeName(void) const; - /// Returns the highest quality link available to the Vehicle - LinkInterface* priorityLink(void); + /// Returns the highest quality link available to the Vehicle. If you need to hold a refernce to this link use + /// LinkManager::sharedLinkInterfaceForGet to get QSharedPointer for link. + LinkInterface* priorityLink(void) { return _priorityLink.data(); } /// Sends a message to the specified link /// @return true: message sent, false: Link no longer connected @@ -720,6 +721,7 @@ private: void _handleMavlinkLoggingDataAcked(mavlink_message_t& message); void _ackMavlinkLogData(uint16_t sequence); void _sendNextQueuedMavCommand(void); + void _updatePriorityLink(void); private: int _id; ///< Mavlink system id @@ -851,6 +853,8 @@ private: static const int _lowBatteryAnnounceRepeatMSecs; // Amount of time in between each low battery announcement QElapsedTimer _lowBatteryAnnounceTimer; + SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering + // FactGroup facts Fact _rollFact; diff --git a/src/comm/LinkConfiguration.h b/src/comm/LinkConfiguration.h index 59fa8f36ffc7ec07adbfb3f9bf4eae0f2f879c52..af6b79b4b347596f307e949397cdd1f557a18d61 100644 --- a/src/comm/LinkConfiguration.h +++ b/src/comm/LinkConfiguration.h @@ -7,7 +7,6 @@ * ****************************************************************************/ - #ifndef LINKCONFIGURATION_H #define LINKCONFIGURATION_H @@ -37,7 +36,7 @@ public: // Property accessors - const QString name(void) { return _name; } + QString name(void) const { return _name; } LinkInterface* link(void) { return _link; } void setName(const QString name); @@ -190,4 +189,6 @@ private: bool _autoConnect; ///< This connection is started automatically at boot }; +typedef QSharedPointer SharedLinkConfigurationPointer; + #endif // LINKCONFIGURATION_H diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc new file mode 100644 index 0000000000000000000000000000000000000000..a65d3fb3150000c7e08f0b4257f02a1cd6066b37 --- /dev/null +++ b/src/comm/LinkInterface.cc @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "LinkInterface.h" +#include "QGCApplication.h" + +/// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only +/// set into the link when it is added to LinkManager +uint8_t LinkInterface::mavlinkChannel(void) const +{ + if (!_mavlinkChannelSet) { + qWarning() << "Call to LinkInterface::mavlinkChannel with _mavlinkChannelSet == false"; + } + return _mavlinkChannel; +} +// Links are only created by LinkManager so constructor is not public +LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config) + : QThread(0) + , _config(config) + , _mavlinkChannelSet(false) + , _active(false) + , _enableRateCollection(false) +{ + _config->setLink(this); + + // Initialize everything for the data rate calculation buffers. + _inDataIndex = 0; + _outDataIndex = 0; + + // Initialize our data rate buffers. + memset(_inDataWriteAmounts, 0, sizeof(_inDataWriteAmounts)); + memset(_inDataWriteTimes, 0, sizeof(_inDataWriteTimes)); + memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts)); + memset(_outDataWriteTimes, 0, sizeof(_outDataWriteTimes)); + + QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes); + qRegisterMetaType("LinkInterface*"); +} + +/// This function logs the send times and amounts of datas for input. Data is used for calculating +/// the transmission rate. +/// @param byteCount Number of bytes received +/// @param time Time in ms send occurred +void LinkInterface::_logInputDataRate(quint64 byteCount, qint64 time) { + if(_enableRateCollection) + _logDataRateToBuffer(_inDataWriteAmounts, _inDataWriteTimes, &_inDataIndex, byteCount, time); +} + +/// This function logs the send times and amounts of datas for output. Data is used for calculating +/// the transmission rate. +/// @param byteCount Number of bytes sent +/// @param time Time in ms receive occurred +void LinkInterface::_logOutputDataRate(quint64 byteCount, qint64 time) { + if(_enableRateCollection) + _logDataRateToBuffer(_outDataWriteAmounts, _outDataWriteTimes, &_outDataIndex, byteCount, time); +} + +/** + * @brief logDataRateToBuffer Stores transmission times/amounts for statistics + * + * This function logs the send times and amounts of datas to the given circular buffers. + * This data is used for calculating the transmission rate. + * + * @param bytesBuffer[out] The buffer to write the bytes value into. + * @param timeBuffer[out] The buffer to write the time value into + * @param writeIndex[out] The write index used for this buffer. + * @param bytes The amount of bytes transmit. + * @param time The time (in ms) this transmission occurred. + */ +void LinkInterface::_logDataRateToBuffer(quint64 *bytesBuffer, qint64 *timeBuffer, int *writeIndex, quint64 bytes, qint64 time) +{ + QMutexLocker dataRateLocker(&_dataRateMutex); + + int i = *writeIndex; + + // Now write into the buffer, if there's no room, we just overwrite the first data point. + bytesBuffer[i] = bytes; + timeBuffer[i] = time; + + // Increment and wrap the write index + ++i; + if (i == _dataRateBufferSize) + { + i = 0; + } + *writeIndex = i; +} + +/** + * @brief getCurrentDataRate Get the current data rate given a data rate buffer. + * + * This function attempts to use the times and number of bytes transmit into a current data rate + * estimation. Since it needs to use timestamps to get the timeperiods over when the data was sent, + * this is effectively a global data rate over the last _dataRateBufferSize - 1 data points. Also note + * that data points older than NOW - dataRateCurrentTimespan are ignored. + * + * @param index The first valid sample in the data rate buffer. Refers to the oldest time sample. + * @param dataWriteTimes The time, in ms since epoch, that each data sample took place. + * @param dataWriteAmounts The amount of data (in bits) that was transferred. + * @return The bits per second of data transferrence of the interface over the last [-statsCurrentTimespan, 0] timespan. + */ +qint64 LinkInterface::_getCurrentDataRate(int index, const qint64 dataWriteTimes[], const quint64 dataWriteAmounts[]) const +{ + const qint64 now = QDateTime::currentMSecsSinceEpoch(); + + // Limit the time we calculate to the recent past + const qint64 cutoff = now - _dataRateCurrentTimespan; + + // Grab the mutex for working with the stats variables + QMutexLocker dataRateLocker(&_dataRateMutex); + + // Now iterate through the buffer of all received data packets adding up all values + // within now and our cutof. + qint64 totalBytes = 0; + qint64 totalTime = 0; + qint64 lastTime = 0; + int size = _dataRateBufferSize; + while (size-- > 0) + { + // If this data is within our cutoff time, include it in our calculations. + // This also accounts for when the buffer is empty and filled with 0-times. + if (dataWriteTimes[index] > cutoff && lastTime > 0) { + // Track the total time, using the previous time as our timeperiod. + totalTime += dataWriteTimes[index] - lastTime; + totalBytes += dataWriteAmounts[index]; + } + + // Track the last time sample for doing timespan calculations + lastTime = dataWriteTimes[index]; + + // Increment and wrap the index if necessary. + if (++index == _dataRateBufferSize) + { + index = 0; + } + } + + // Return the final calculated value in bits / s, converted from bytes/ms. + qint64 dataRate = (totalTime != 0)?(qint64)((float)totalBytes * 8.0f / ((float)totalTime / 1000.0f)):0; + + // Finally return our calculated data rate. + return dataRate; +} + +/// Sets the mavlink channel to use for this link +void LinkInterface::_setMavlinkChannel(uint8_t channel) +{ + Q_ASSERT(!_mavlinkChannelSet); + _mavlinkChannelSet = true; + _mavlinkChannel = channel; +} diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index ecb8947406e07b8ad0d8a2cdaaac0a14df26969e..63ce682571b03177e969e6d1cbd214e4e511f31d 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -19,9 +19,9 @@ #include #include "QGCMAVLink.h" +#include "LinkConfiguration.h" class LinkManager; -class LinkConfiguration; /** * The link interface defines the interface for all links used to communicate @@ -35,18 +35,16 @@ class LinkInterface : public QThread // Only LinkManager is allowed to create/delete or _connect/_disconnect a link friend class LinkManager; -public: +public: + ~LinkInterface() { _config->setLink(NULL); } + Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) // Property accessors - bool active(void) { return _active; } - void setActive(bool active) { _active = active; emit activeChanged(active); } + bool active(void) { return _active; } + void setActive(bool active) { _active = active; emit activeChanged(active); } - /** - * @brief Get link configuration - * @return A pointer to the instance of LinkConfiguration - **/ - virtual LinkConfiguration* getLinkConfiguration() = 0; + LinkConfiguration* getLinkConfiguration(void) { return _config.data(); } /* Connection management */ @@ -116,13 +114,7 @@ public: /// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only /// set into the link when it is added to LinkManager - uint8_t mavlinkChannel(void) const - { - if (!_mavlinkChannelSet) { - qWarning() << "Call to LinkInterface::mavlinkChannel with _mavlinkChannelSet == false"; - } - return _mavlinkChannel; - } + uint8_t mavlinkChannel(void) const; // These are left unimplemented in order to cause linker errors which indicate incorrect usage of // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. @@ -192,43 +184,21 @@ signals: protected: // Links are only created by LinkManager so constructor is not public - LinkInterface() : - QThread(0) - , _mavlinkChannelSet(false) - , _active(false) - , _enableRateCollection(false) - { - // Initialize everything for the data rate calculation buffers. - _inDataIndex = 0; - _outDataIndex = 0; - - // Initialize our data rate buffers. - memset(_inDataWriteAmounts, 0, sizeof(_inDataWriteAmounts)); - memset(_inDataWriteTimes, 0, sizeof(_inDataWriteTimes)); - memset(_outDataWriteAmounts,0, sizeof(_outDataWriteAmounts)); - memset(_outDataWriteTimes, 0, sizeof(_outDataWriteTimes)); - - QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes); - qRegisterMetaType("LinkInterface*"); - } + LinkInterface(SharedLinkConfigurationPointer& config); /// This function logs the send times and amounts of datas for input. Data is used for calculating /// the transmission rate. /// @param byteCount Number of bytes received /// @param time Time in ms send occurred - void _logInputDataRate(quint64 byteCount, qint64 time) { - if(_enableRateCollection) - _logDataRateToBuffer(_inDataWriteAmounts, _inDataWriteTimes, &_inDataIndex, byteCount, time); - } + void _logInputDataRate(quint64 byteCount, qint64 time); /// This function logs the send times and amounts of datas for output. Data is used for calculating /// the transmission rate. /// @param byteCount Number of bytes sent /// @param time Time in ms receive occurred - void _logOutputDataRate(quint64 byteCount, qint64 time) { - if(_enableRateCollection) - _logDataRateToBuffer(_outDataWriteAmounts, _outDataWriteTimes, &_outDataIndex, byteCount, time); - } + void _logOutputDataRate(quint64 byteCount, qint64 time); + + SharedLinkConfigurationPointer _config; private: /** @@ -243,24 +213,7 @@ private: * @param bytes The amount of bytes transmit. * @param time The time (in ms) this transmission occurred. */ - void _logDataRateToBuffer(quint64 *bytesBuffer, qint64 *timeBuffer, int *writeIndex, quint64 bytes, qint64 time) - { - QMutexLocker dataRateLocker(&_dataRateMutex); - - int i = *writeIndex; - - // Now write into the buffer, if there's no room, we just overwrite the first data point. - bytesBuffer[i] = bytes; - timeBuffer[i] = time; - - // Increment and wrap the write index - ++i; - if (i == _dataRateBufferSize) - { - i = 0; - } - *writeIndex = i; - } + void _logDataRateToBuffer(quint64 *bytesBuffer, qint64 *timeBuffer, int *writeIndex, quint64 bytes, qint64 time); /** * @brief getCurrentDataRate Get the current data rate given a data rate buffer. @@ -275,48 +228,7 @@ private: * @param dataWriteAmounts The amount of data (in bits) that was transferred. * @return The bits per second of data transferrence of the interface over the last [-statsCurrentTimespan, 0] timespan. */ - qint64 _getCurrentDataRate(int index, const qint64 dataWriteTimes[], const quint64 dataWriteAmounts[]) const - { - const qint64 now = QDateTime::currentMSecsSinceEpoch(); - - // Limit the time we calculate to the recent past - const qint64 cutoff = now - _dataRateCurrentTimespan; - - // Grab the mutex for working with the stats variables - QMutexLocker dataRateLocker(&_dataRateMutex); - - // Now iterate through the buffer of all received data packets adding up all values - // within now and our cutof. - qint64 totalBytes = 0; - qint64 totalTime = 0; - qint64 lastTime = 0; - int size = _dataRateBufferSize; - while (size-- > 0) - { - // If this data is within our cutoff time, include it in our calculations. - // This also accounts for when the buffer is empty and filled with 0-times. - if (dataWriteTimes[index] > cutoff && lastTime > 0) { - // Track the total time, using the previous time as our timeperiod. - totalTime += dataWriteTimes[index] - lastTime; - totalBytes += dataWriteAmounts[index]; - } - - // Track the last time sample for doing timespan calculations - lastTime = dataWriteTimes[index]; - - // Increment and wrap the index if necessary. - if (++index == _dataRateBufferSize) - { - index = 0; - } - } - - // Return the final calculated value in bits / s, converted from bytes/ms. - qint64 dataRate = (totalTime != 0)?(qint64)((float)totalBytes * 8.0f / ((float)totalTime / 1000.0f)):0; - - // Finally return our calculated data rate. - return dataRate; - } + qint64 _getCurrentDataRate(int index, const qint64 dataWriteTimes[], const quint64 dataWriteAmounts[]) const; /** * @brief Connect this interface logically @@ -328,7 +240,7 @@ private: virtual void _disconnect(void) = 0; /// Sets the mavlink channel to use for this link - void _setMavlinkChannel(uint8_t channel) { Q_ASSERT(!_mavlinkChannelSet); _mavlinkChannelSet = true; _mavlinkChannel = channel; } + void _setMavlinkChannel(uint8_t channel); bool _mavlinkChannelSet; ///< true: _mavlinkChannel has been set uint8_t _mavlinkChannel; ///< mavlink channel to use for this link, as used by mavlink_parse_char @@ -355,6 +267,6 @@ private: bool _enableRateCollection; }; -typedef QSharedPointer SharedLinkInterface; +typedef QSharedPointer SharedLinkInterfacePointer; #endif // _LINKINTERFACE_H_ diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 8a400a95524849bfb6569d65559ce987a9d6340d..c4482009d7a96f5a7125f658b240b742093dff03 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -7,15 +7,6 @@ * ****************************************************************************/ - -/** - * @file - * @brief Brief Description - * - * @author Lorenz Meier - * - */ - #include #include #include @@ -108,17 +99,21 @@ void LinkManager::setToolbox(QGCToolbox *toolbox) } -LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config) +LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config) { - Q_ASSERT(config); + if (!config) { + qWarning() << "LinkManager::createConnectedLink called with NULL config"; + return NULL; + } + LinkInterface* pLink = NULL; switch(config->type()) { #ifndef NO_SERIAL_LINK case LinkConfiguration::TypeSerial: { - SerialConfiguration* serialConfig = dynamic_cast(config); + SerialConfiguration* serialConfig = dynamic_cast(config.data()); if (serialConfig) { - pLink = new SerialLink(serialConfig); + pLink = new SerialLink(config); if (serialConfig->usbDirect()) { _activeLinkCheckList.append((SerialLink*)pLink); if (!_activeLinkCheckTimer.isActive()) { @@ -130,43 +125,45 @@ LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config) break; #endif case LinkConfiguration::TypeUdp: - pLink = new UDPLink(dynamic_cast(config)); + pLink = new UDPLink(config); break; case LinkConfiguration::TypeTcp: - pLink = new TCPLink(dynamic_cast(config)); + pLink = new TCPLink(config); break; #ifdef QGC_ENABLE_BLUETOOTH case LinkConfiguration::TypeBluetooth: - pLink = new BluetoothLink(dynamic_cast(config)); + pLink = new BluetoothLink(config); break; #endif #ifndef __mobile__ case LinkConfiguration::TypeLogReplay: - pLink = new LogReplayLink(dynamic_cast(config)); + pLink = new LogReplayLink(config); break; #endif #ifdef QT_DEBUG case LinkConfiguration::TypeMock: - pLink = new MockLink(dynamic_cast(config)); + pLink = new MockLink(config); break; #endif case LinkConfiguration::TypeLast: default: break; } - if(pLink) { + + if (pLink) { _addLink(pLink); connectLink(pLink); } + return pLink; } LinkInterface* LinkManager::createConnectedLink(const QString& name) { Q_ASSERT(name.isEmpty() == false); - for(int i = 0; i < _linkConfigurations.count(); i++) { - LinkConfiguration* conf = _linkConfigurations.value(i); - if(conf && conf->name() == name) + for(int i = 0; i < _sharedConfigurations.count(); i++) { + SharedLinkConfigurationPointer& conf = _sharedConfigurations[i]; + if (conf->name() == name) return createConnectedLink(conf); } return NULL; @@ -183,7 +180,7 @@ void LinkManager::_addLink(LinkInterface* link) return; } - if (!_links.contains(link)) { + if (!containsLink(link)) { bool channelSet = false; // Find a mavlink channel to use for this link, Channel 0 is reserved for internal use. @@ -205,7 +202,7 @@ void LinkManager::_addLink(LinkInterface* link) qWarning() << "Ran out of mavlink channels"; } - _links.append(link); + _sharedLinks.append(SharedLinkInterfacePointer(link)); emit newLink(link); } @@ -225,8 +222,8 @@ void LinkManager::_addLink(LinkInterface* link) void LinkManager::disconnectAll(void) { // Walk list in reverse order to preserve indices during delete - for (int i=_links.count()-1; i>=0; i--) { - disconnectLink(_links.value(i)); + for (int i=_sharedLinks.count()-1; i>=0; i--) { + disconnectLink(_sharedLinks[i].data()); } } @@ -243,23 +240,22 @@ bool LinkManager::connectLink(LinkInterface* link) void LinkManager::disconnectLink(LinkInterface* link) { - if (!link || !_links.contains(link)) { + if (!link || !containsLink(link)) { return; } link->_disconnect(); + LinkConfiguration* config = link->getLinkConfiguration(); - if (config) { - if (_autoconnectConfigurations.contains(config)) { - config->setLink(NULL); + for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) { + if (_sharedAutoconnectConfigurations[i].data() == config) { + qCDebug(LinkManagerLog) << "Removing disconnected autoconnect config" << config->name(); + _sharedAutoconnectConfigurations.removeAt(i); + break; } } + _deleteLink(link); - if (_autoconnectConfigurations.contains(config)) { - qCDebug(LinkManagerLog) << "Removing disconnected autoconnect config" << config->name(); - _autoconnectConfigurations.removeOne(config); - delete config; - } } void LinkManager::_deleteLink(LinkInterface* link) @@ -276,13 +272,29 @@ void LinkManager::_deleteLink(LinkInterface* link) // Free up the mavlink channel associated with this link _mavlinkChannelsUsedBitMask &= ~(1 << link->mavlinkChannel()); - _links.removeOne(link); - delete link; + for (int i=0; i<_sharedLinks.count(); i++) { + if (_sharedLinks[i].data() == link) { + _sharedLinks.removeAt(i); + break; + } + } // Emit removal of link emit linkDeleted(link); } +SharedLinkInterfacePointer LinkManager::sharedLinkInterfacePointerForLink(LinkInterface* link) +{ + for (int i=0; i<_sharedLinks.count(); i++) { + if (_sharedLinks[i].data() == link) { + return _sharedLinks[i]; + } + } + + qWarning() << "LinkManager::sharedLinkInterfaceForLink returning NULL"; + return SharedLinkInterfacePointer(NULL); +} + /// @brief If all new connections should be suspended a message is displayed to the user and true /// is returned; bool LinkManager::_connectionsSuspendedMsg(void) @@ -328,11 +340,10 @@ void LinkManager::saveLinkConfigurationList() QSettings settings; settings.remove(LinkConfiguration::settingsRoot()); int trueCount = 0; - for (int i = 0; i < _linkConfigurations.count(); i++) { - LinkConfiguration* linkConfig = _linkConfigurations.value(i); + for (int i = 0; i < _sharedConfigurations.count(); i++) { + SharedLinkConfigurationPointer linkConfig = _sharedConfigurations[i]; if (linkConfig) { - if(!linkConfig->isDynamic()) - { + if (!linkConfig->isDynamic()) { QString root = LinkConfiguration::settingsRoot(); root += QString("/Link%1").arg(trueCount++); settings.setValue(root + "/name", linkConfig->name()); @@ -404,7 +415,7 @@ void LinkManager::loadLinkConfigurationList() //-- Have the instance load its own values pLink->setAutoConnect(autoConnect); pLink->loadSettings(settings, root); - _linkConfigurations.append(pLink); + addConfiguration(pLink); linksChanged = true; } } else { @@ -434,12 +445,12 @@ SerialConfiguration* LinkManager::_autoconnectConfigurationsContainsPort(const Q { QString searchPort = portName.trimmed(); - for (int i=0; i<_autoconnectConfigurations.count(); i++) { - SerialConfiguration* linkConfig = _autoconnectConfigurations.value(i); + for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) { + SerialConfiguration* serialConfig = qobject_cast(_sharedAutoconnectConfigurations[i].data()); - if (linkConfig) { - if (linkConfig->portName() == searchPort) { - return linkConfig; + if (serialConfig) { + if (serialConfig->portName() == searchPort) { + return serialConfig; } } else { qWarning() << "Internal error"; @@ -457,8 +468,8 @@ void LinkManager::_updateAutoConnectLinks(void) // Re-add UDP if we need to bool foundUDP = false; - for (int i=0; i<_links.count(); i++) { - LinkConfiguration* linkConfig = _links.value(i)->getLinkConfiguration(); + for (int i=0; i<_sharedLinks.count(); i++) { + LinkConfiguration* linkConfig = _sharedLinks[i]->getLinkConfiguration(); if (linkConfig->type() == LinkConfiguration::TypeUdp && linkConfig->name() == _defaultUPDLinkName) { foundUDP = true; break; @@ -468,9 +479,9 @@ void LinkManager::_updateAutoConnectLinks(void) qCDebug(LinkManagerLog) << "New auto-connect UDP port added"; UDPConfiguration* udpConfig = new UDPConfiguration(_defaultUPDLinkName); udpConfig->setLocalPort(QGC_UDP_LOCAL_PORT); - udpConfig->setDynamic(true); - _linkConfigurations.append(udpConfig); - createConnectedLink(udpConfig); + udpConfig->setDynamic(true); + SharedLinkConfigurationPointer config = addConfiguration(udpConfig); + createConnectedLink(config); emit linkConfigurationsChanged(); } @@ -591,8 +602,8 @@ void LinkManager::_updateAutoConnectLinks(void) pSerialConfig->setBaud(boardType == QGCSerialPortInfo::BoardTypeSikRadio ? 57600 : 115200); pSerialConfig->setDynamic(true); pSerialConfig->setPortName(portInfo.systemLocation()); - _autoconnectConfigurations.append(pSerialConfig); - createConnectedLink(pSerialConfig); + _sharedAutoconnectConfigurations.append(SharedLinkConfigurationPointer(pSerialConfig)); + createConnectedLink(_sharedAutoconnectConfigurations.last()); } } } @@ -607,13 +618,13 @@ void LinkManager::_updateAutoConnectLinks(void) // Now we go through the current configuration list and make sure any dynamic config has gone away QList _confToDelete; - for (int i=0; i<_autoconnectConfigurations.count(); i++) { - SerialConfiguration* linkConfig = _autoconnectConfigurations.value(i); - if (linkConfig) { - if (!currentPorts.contains(linkConfig->portName())) { - if (linkConfig->link()) { - if (linkConfig->link()->isConnected()) { - if (linkConfig->link()->active()) { + for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) { + SerialConfiguration* serialConfig = qobject_cast(_sharedAutoconnectConfigurations[i].data()); + if (serialConfig) { + if (!currentPorts.contains(serialConfig->portName())) { + if (serialConfig->link()) { + if (serialConfig->link()->isConnected()) { + if (serialConfig->link()->active()) { // We don't remove links which are still connected which have been active with a vehicle on them // even though at this point the cable may have been pulled. Instead we wait for the user to // Disconnect. Once the user disconnects, the link will be removed. @@ -621,7 +632,7 @@ void LinkManager::_updateAutoConnectLinks(void) } } } - _confToDelete.append(linkConfig); + _confToDelete.append(serialConfig); } } else { qWarning() << "Internal error"; @@ -631,7 +642,12 @@ void LinkManager::_updateAutoConnectLinks(void) // Now remove all configs that are gone foreach (LinkConfiguration* pDeleteConfig, _confToDelete) { qCDebug(LinkManagerLog) << "Removing unused autoconnect config" << pDeleteConfig->name(); - _autoconnectConfigurations.removeOne(pDeleteConfig); + for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) { + if (_sharedAutoconnectConfigurations[i].data() == pDeleteConfig) { + _sharedAutoconnectConfigurations.removeAt(i); + break; + } + } if (pDeleteConfig->link()) { disconnectLink(pDeleteConfig->link()); } @@ -789,7 +805,7 @@ bool LinkManager::endCreateConfiguration(LinkConfiguration* config) { Q_ASSERT(config != NULL); _fixUnnamed(config); - _linkConfigurations.append(config); + addConfiguration(config); saveLinkConfigurationList(); return true; } @@ -883,16 +899,19 @@ void LinkManager::removeConfiguration(LinkConfiguration* config) if(iface) { disconnectLink(iface); } - // Remove configuration - _linkConfigurations.removeOne(config); - delete config; - // Save list + + _removeConfiguration(config); saveLinkConfigurationList(); } bool LinkManager::isAutoconnectLink(LinkInterface* link) { - return _autoconnectConfigurations.contains(link->getLinkConfiguration()); + for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) { + if (_sharedAutoconnectConfigurations[i].data() == link->getLinkConfiguration()) { + return true; + } + } + return false; } bool LinkManager::isBluetoothAvailable(void) @@ -908,7 +927,7 @@ void LinkManager::_activeLinkCheck(void) if (_activeLinkCheckList.count() != 0) { link = _activeLinkCheckList.takeFirst(); - if (_links.contains(link) && link->isConnected()) { + if (containsLink(link) && link->isConnected()) { // Make sure there is a vehicle on the link QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles(); for (int i=0; icount(); i++) { @@ -945,3 +964,45 @@ void LinkManager::_activeLinkCheck(void) } } #endif + +bool LinkManager::containsLink(LinkInterface* link) +{ + for (int i=0; i<_sharedLinks.count(); i++) { + if (_sharedLinks[i].data() == link) { + return true; + } + } + return false; +} + +SharedLinkConfigurationPointer LinkManager::addConfiguration(LinkConfiguration* config) +{ + _qmlConfigurations.append(config); + _sharedConfigurations.append(SharedLinkConfigurationPointer(config)); + return _sharedConfigurations.last(); +} + +void LinkManager::_removeConfiguration(LinkConfiguration* config) +{ + _qmlConfigurations.removeOne(config); + + for (int i=0; i<_sharedConfigurations.count(); i++) { + if (_sharedConfigurations[i].data() == config) { + _sharedConfigurations.removeAt(i); + return; + } + } + + qWarning() << "LinkManager::_removeConfiguration called with unknown config"; +} + +QList LinkManager::links(void) +{ + QList rawLinks; + + for (int i=0; i<_sharedLinks.count(); i++) { + rawLinks.append(_sharedLinks[i].data()); + } + + return rawLinks; +} diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 23cddcbdf47a25e56bd5f7cd42df288ab4ec28e0..f75fc50f149cf3f0d74fcc5a4e0eed8c8e531b7f 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -67,19 +67,12 @@ public: Q_PROPERTY(bool autoconnectPX4Flow READ autoconnectPX4Flow WRITE setAutoconnectPX4Flow NOTIFY autoconnectPX4FlowChanged) Q_PROPERTY(bool autoconnectRTKGPS READ autoconnectRTKGPS WRITE setAutoconnectRTKGPS NOTIFY autoconnectRTKGPSChanged) Q_PROPERTY(bool autoconnectLibrePilot READ autoconnectLibrePilot WRITE setAutoconnectLibrePilot NOTIFY autoconnectLibrePilotChanged) - Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable CONSTANT) - - /// LinkInterface Accessor - Q_PROPERTY(QmlObjectListModel* links READ links CONSTANT) - /// LinkConfiguration Accessor - Q_PROPERTY(QmlObjectListModel* linkConfigurations READ linkConfigurations NOTIFY linkConfigurationsChanged) - /// List of comm type strings - Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT) - /// List of supported baud rates for serial links - Q_PROPERTY(QStringList serialBaudRates READ serialBaudRates CONSTANT) - /// List of comm ports display names + Q_PROPERTY(bool isBluetoothAvailable READ isBluetoothAvailable CONSTANT) + + Q_PROPERTY(QmlObjectListModel* linkConfigurations READ _qmlLinkConfigurations NOTIFY linkConfigurationsChanged) + Q_PROPERTY(QStringList linkTypeStrings READ linkTypeStrings CONSTANT) + Q_PROPERTY(QStringList serialBaudRates READ serialBaudRates CONSTANT) Q_PROPERTY(QStringList serialPortStrings READ serialPortStrings NOTIFY commPortStringsChanged) - /// List of comm ports Q_PROPERTY(QStringList serialPorts READ serialPorts NOTIFY commPortsChanged) // Create/Edit Link Configuration @@ -100,12 +93,11 @@ public: bool autoconnectLibrePilot (void) { return _autoconnectLibrePilot; } bool isBluetoothAvailable (void); - QmlObjectListModel* links (void) { return &_links; } - QmlObjectListModel* linkConfigurations (void) { return &_linkConfigurations; } - QStringList linkTypeStrings (void) const; - QStringList serialBaudRates (void); - QStringList serialPortStrings (void); - QStringList serialPorts (void); + QList links (void); + QStringList linkTypeStrings (void) const; + QStringList serialBaudRates (void); + QStringList serialPortStrings (void); + QStringList serialPorts (void); void setAutoconnectUDP (bool autoconnect); void setAutoconnectPixhawk (bool autoconnect); @@ -132,7 +124,7 @@ public: /// Creates, connects (and adds) a link based on the given configuration instance. /// Link takes ownership of config. - Q_INVOKABLE LinkInterface* createConnectedLink(LinkConfiguration* config); + Q_INVOKABLE LinkInterface* createConnectedLink(SharedLinkConfigurationPointer& config); /// Creates, connects (and adds) a link based on the given configuration name. LinkInterface* createConnectedLink(const QString& name); @@ -165,6 +157,17 @@ public: // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); + /// @return This mavlink channel is never assigned to a vehicle. + uint8_t reservedMavlinkChannel(void) { return 0; } + + /// If you are going to hold a reference to a LinkInterface* in your object you must reference count it + /// by using this method to get access to the shared pointer. + SharedLinkInterfacePointer sharedLinkInterfacePointerForLink(LinkInterface* link); + + bool containsLink(LinkInterface* link); + + SharedLinkConfigurationPointer addConfiguration(LinkConfiguration* config); + signals: void autoconnectUDPChanged (bool autoconnect); void autoconnectPixhawkChanged (bool autoconnect); @@ -204,11 +207,13 @@ private slots: #endif private: + QmlObjectListModel* _qmlLinkConfigurations (void) { return &_qmlConfigurations; } bool _connectionsSuspendedMsg(void); void _updateAutoConnectLinks(void); void _updateSerialPorts(); void _fixUnnamed(LinkConfiguration* config); bool _setAutoconnectWorker(bool& currentAutoconnect, bool newAutoconnect, const char* autoconnectKey); + void _removeConfiguration(LinkConfiguration* config); #ifndef NO_SERIAL_LINK SerialConfiguration* _autoconnectConfigurationsContainsPort(const QString& portName); @@ -223,9 +228,11 @@ private: MAVLinkProtocol* _mavlinkProtocol; - QmlObjectListModel _links; - QmlObjectListModel _linkConfigurations; - QmlObjectListModel _autoconnectConfigurations; + + QList _sharedLinks; + QList _sharedConfigurations; + QList _sharedAutoconnectConfigurations; + QmlObjectListModel _qmlConfigurations; QMap _autoconnectWaitList; ///< key: QGCSerialPortInfo.systemLocation, value: wait count QStringList _commPortList; diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 4a15dd8417a2038158390a5f108afe68f2c1f7e3..d6b818467c7fc1823d7fa9aa650260bc1dd42ffa 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -64,12 +64,13 @@ QString LogReplayLinkConfiguration::logFilenameShort(void) return fi.fileName(); } -LogReplayLink::LogReplayLink(LogReplayLinkConfiguration* config) : - _connected(false), - _replayAccelerationFactor(1.0f) +LogReplayLink::LogReplayLink(SharedLinkConfigurationPointer& config) + : LinkInterface(config) + , _logReplayConfig(qobject_cast(config.data())) + , _connected(false) + , _replayAccelerationFactor(1.0f) { - Q_ASSERT(config); - _config = config; + Q_ASSERT(_logReplayConfig); _readTickTimer.moveToThread(this); @@ -184,7 +185,7 @@ quint64 LogReplayLink::_seekToNextMavlinkMessage(mavlink_message_t* nextMsg) bool LogReplayLink::_loadLogFile(void) { QString errorMsg; - QString logFilename = _config->logFilename(); + QString logFilename = _logReplayConfig->logFilename(); QFileInfo logFileInfo; int logDurationSecondsTotal; diff --git a/src/comm/LogReplayLink.h b/src/comm/LogReplayLink.h index 923dfb6373cc6036645bd6f72048b7d162796ff0..2b78756266889b34b67e6c7fa7be7c94e39dec4e 100644 --- a/src/comm/LogReplayLink.h +++ b/src/comm/LogReplayLink.h @@ -57,8 +57,6 @@ class LogReplayLink : public LinkInterface friend class LinkManager; public: - virtual LinkConfiguration* getLinkConfiguration() { return _config; } - /// @return true: log is currently playing, false: log playback is paused bool isPlaying(void) { return _readTickTimer.isActive(); } @@ -111,7 +109,7 @@ private slots: private: // Links are only created/destroyed by LinkManager so constructor/destructor is not public - LogReplayLink(LogReplayLinkConfiguration* config); + LogReplayLink(SharedLinkConfigurationPointer& config); ~LogReplayLink(); void _replayError(const QString& errorMsg); @@ -129,7 +127,7 @@ private: // Virtuals from QThread virtual void run(void); - LogReplayLinkConfiguration* _config; + LogReplayLinkConfiguration* _logReplayConfig; bool _connected; QTimer _readTickTimer; ///< Timer which signals a read of next log record diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 49c1a1a38e57110eae946dc9b8c940e32a830abb..4fee754823bece8ff6dab81441264a857508ca18 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -159,7 +159,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Since receiveBytes signals cross threads we can end up with signals in the queue // that come through after the link is disconnected. For these we just drop the data // since the link is closed. - if (!_linkMgr->links()->contains(link)) { + if (!_linkMgr->containsLink(link)) { return; } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 411287c4d15b6861e47b82b9fe034c63b6b07ba6..f8dcb64c1a2e50c94f112fa4f1fd6eb8547066a8 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -44,8 +44,9 @@ const char* MockConfiguration::_vehicleTypeKey = "VehicleType"; const char* MockConfiguration::_sendStatusTextKey = "SendStatusText"; const char* MockConfiguration::_failureModeKey = "FailureMode"; -MockLink::MockLink(MockConfiguration* config) - : _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol()) +MockLink::MockLink(SharedLinkConfigurationPointer& config) + : LinkInterface(config) + , _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol()) , _name("MockLink") , _connected(false) , _vehicleSystemId(_nextVehicleSystemId++) @@ -67,14 +68,11 @@ MockLink::MockLink(MockConfiguration* config) , _logDownloadCurrentOffset(0) , _logDownloadBytesRemaining(0) { - _config = config; - if (_config) { - _firmwareType = config->firmwareType(); - _vehicleType = config->vehicleType(); - _sendStatusText = config->sendStatusText(); - _failureMode = config->failureMode(); - _config->setLink(this); - } + MockConfiguration* mockConfig = qobject_cast(_config.data()); + _firmwareType = mockConfig->firmwareType(); + _vehicleType = mockConfig->vehicleType(); + _sendStatusText = mockConfig->sendStatusText(); + _failureMode = mockConfig->failureMode(); union px4_custom_mode px4_cm; @@ -1041,12 +1039,12 @@ void MockConfiguration::updateSettings() MockLink* MockLink::_startMockLink(MockConfiguration* mockConfig) { - LinkManager* linkManager = qgcApp()->toolbox()->linkManager(); + LinkManager* linkMgr = qgcApp()->toolbox()->linkManager(); mockConfig->setDynamic(true); - linkManager->linkConfigurations()->append(mockConfig); + SharedLinkConfigurationPointer config = linkMgr->addConfiguration(mockConfig); - return qobject_cast(linkManager->createConnectedLink(mockConfig)); + return qobject_cast(linkMgr->createConnectedLink(config)); } MockLink* MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index cdb25373f8f98e3e4bf340f5437190701c9a4264..15e8a5eebdff9b9e37ced799ab0185bf93c24379 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -90,8 +90,7 @@ class MockLink : public LinkInterface Q_OBJECT public: - // LinkConfiguration is optional for MockLink - MockLink(MockConfiguration* config = NULL); + MockLink(SharedLinkConfigurationPointer& config); ~MockLink(void); // MockLink methods @@ -126,8 +125,6 @@ public: bool connect(void); bool disconnect(void); - LinkConfiguration* getLinkConfiguration() { return _config; } - /// Sets a failure mode for unit testing /// @param failureMode Type of failure to simulate void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode); @@ -216,7 +213,6 @@ private: uint32_t _mavCustomMode; uint8_t _mavState; - MockConfiguration* _config; MAV_AUTOPILOT _firmwareType; MAV_TYPE _vehicleType; diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 3e345f682a23e24e83eedd5e42ef124f11221236..9e600db18d1256be835e7a0af4e8dba02d3b96f7 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -30,19 +30,19 @@ QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog") static QStringList kSupportedBaudRates; -SerialLink::SerialLink(SerialConfiguration* config) +SerialLink::SerialLink(SharedLinkConfigurationPointer& config) + : LinkInterface(config) + , _port(NULL) + , _bytesRead(0) + , _stopp(false) + , _reqReset(false) + , _serialConfig(qobject_cast(config.data())) { - _bytesRead = 0; - _port = Q_NULLPTR; - _stopp = false; - _reqReset = false; - Q_ASSERT(config != NULL); - _config = config; - _config->setLink(this); - - qCDebug(SerialLinkLog) << "Create SerialLink " << config->portName() << config->baud() << config->flowControl() - << config->parity() << config->dataBits() << config->stopBits(); - qCDebug(SerialLinkLog) << "portName: " << config->portName(); + Q_ASSERT(_serialConfig); + + qCDebug(SerialLinkLog) << "Create SerialLink " << _serialConfig->portName() << _serialConfig->baud() << _serialConfig->flowControl() + << _serialConfig->parity() << _serialConfig->dataBits() << _serialConfig->stopBits(); + qCDebug(SerialLinkLog) << "portName: " << _serialConfig->portName(); } void SerialLink::requestReset() @@ -53,10 +53,10 @@ void SerialLink::requestReset() SerialLink::~SerialLink() { - // Disconnect link from configuration - _config->setLink(NULL); _disconnect(); - if(_port) delete _port; + if (_port) { + delete _port; + } _port = NULL; } @@ -70,7 +70,7 @@ bool SerialLink::_isBootloader() { qCDebug(SerialLinkLog) << "PortName : " << info.portName() << "Description : " << info.description(); qCDebug(SerialLinkLog) << "Manufacturer: " << info.manufacturer(); - if (info.portName().trimmed() == _config->portName() && + 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"))) { @@ -159,7 +159,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& _port = NULL; } - qCDebug(SerialLinkLog) << "SerialLink: hardwareConnect to " << _config->portName(); + qCDebug(SerialLinkLog) << "SerialLink: hardwareConnect to " << _serialConfig->portName(); // If we are in the Pixhawk bootloader code wait for it to timeout if (_isBootloader()) { @@ -181,7 +181,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& } } - _port = new QSerialPort(_config->portName()); + _port = new QSerialPort(_serialConfig->portName()); QObject::connect(_port, static_cast(&QSerialPort::error), this, &SerialLink::linkError); @@ -219,17 +219,17 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& _port->setDataTerminalReady(true); qCDebug(SerialLinkLog) << "Configuring port"; - _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->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 communicationUpdate(getName(), "Opened port!"); emit connected(); - qCDebug(SerialLinkLog) << "Connection SeriaLink: " << "with settings" << _config->portName() - << _config->baud() << _config->dataBits() << _config->parity() << _config->stopBits(); + qCDebug(SerialLinkLog) << "Connection SeriaLink: " << "with settings" << _serialConfig->portName() + << _serialConfig->baud() << _serialConfig->dataBits() << _serialConfig->parity() << _serialConfig->stopBits(); return true; // successful connection } @@ -281,7 +281,7 @@ bool SerialLink::isConnected() const QString SerialLink::getName() const { - return _config->portName(); + return _serialConfig->portName(); } /** @@ -294,7 +294,7 @@ qint64 SerialLink::getConnectionSpeed() const if (_port) { baudRate = _port->baudRate(); } else { - baudRate = _config->baud(); + baudRate = _serialConfig->baud(); } qint64 dataRate; switch (baudRate) @@ -334,11 +334,11 @@ qint64 SerialLink::getConnectionSpeed() const void SerialLink::_resetConfiguration() { if (_port) { - _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->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())); } } @@ -349,11 +349,6 @@ void SerialLink::_emitLinkError(const QString& errorMsg) emit communicationError(tr("Link Error"), msg.arg(getName()).arg(errorMsg)); } -LinkConfiguration* SerialLink::getLinkConfiguration() -{ - return _config; -} - //-------------------------------------------------------------------------- //-- SerialConfiguration diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 82b0edee71352da494cf6881d9774fbb75e5e11d..be46a06e93661e5ef2e60f6fa84a045ab47dfac6 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -133,7 +133,6 @@ class SerialLink : public LinkInterface public: // LinkInterface - LinkConfiguration* getLinkConfiguration(); QString getName() const; void requestReset(); bool isConnected() const; @@ -168,7 +167,7 @@ private slots: private: // Links are only created/destroyed by LinkManager so constructor/destructor is not public - SerialLink(SerialConfiguration* config); + SerialLink(SharedLinkConfigurationPointer& config); ~SerialLink(); // From LinkInterface @@ -186,7 +185,7 @@ private: volatile bool _reqReset; 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* _config; + SerialConfiguration* _serialConfig; signals: void aboutToCloseFlag(); diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index a57cb61d87483aad406898c7d822383dbf407be1..d540f644496f86160585f1a941f531f695f8fa77 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -24,16 +24,14 @@ /// /// @author Don Gagne -TCPLink::TCPLink(TCPConfiguration *config) - : _config(config) +TCPLink::TCPLink(SharedLinkConfigurationPointer& config) + : LinkInterface(config) + , _tcpConfig(qobject_cast(config.data())) , _socket(NULL) , _socketIsConnected(false) { - Q_ASSERT(_config != NULL); - // We're doing it wrong - because the Qt folks got the API wrong: - // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ + Q_ASSERT(_tcpConfig); moveToThread(this); - //qDebug() << "TCP Created " << _config->name(); } TCPLink::~TCPLink() @@ -69,7 +67,7 @@ void TCPLink::_writeDebugBytes(const QByteArray data) ascii.append(219); } } - qDebug() << "Sent" << size << "bytes to" << _config->address().toString() << ":" << _config->port() << "data:"; + qDebug() << "Sent" << size << "bytes to" << _tcpConfig->address().toString() << ":" << _tcpConfig->port() << "data:"; qDebug() << bytes; qDebug() << "ASCII:" << ascii; } @@ -148,7 +146,7 @@ bool TCPLink::_hardwareConnect() _socket = new QTcpSocket(); QSignalSpy errorSpy(_socket, static_cast(&QTcpSocket::error)); - _socket->connectToHost(_config->address(), _config->port()); + _socket->connectToHost(_tcpConfig->address(), _tcpConfig->port()); QObject::connect(_socket, &QTcpSocket::readyRead, this, &TCPLink::readBytes); QObject::connect(_socket,static_cast(&QTcpSocket::error), @@ -189,7 +187,7 @@ bool TCPLink::isConnected() const QString TCPLink::getName() const { - return _config->name(); + return _tcpConfig->name(); } qint64 TCPLink::getConnectionSpeed() const diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h index c67606f583420054e0243bada84fdc52a6b21cb5..9eb2827feb43a947f8ccec9e975e9dff23017050 100644 --- a/src/comm/TCPLink.h +++ b/src/comm/TCPLink.h @@ -121,7 +121,6 @@ class TCPLink : public LinkInterface public: QTcpSocket* getSocket(void) { return _socket; } - virtual LinkConfiguration* getLinkConfiguration() { return _config; } void signalBytesWritten(void); @@ -160,7 +159,7 @@ protected: private: // Links are only created/destroyed by LinkManager so constructor/destructor is not public - TCPLink(TCPConfiguration* config); + TCPLink(SharedLinkConfigurationPointer& config); ~TCPLink(); // From LinkInterface @@ -174,7 +173,7 @@ private: void _writeDebugBytes(const QByteArray data); #endif - TCPConfiguration* _config; + TCPConfiguration* _tcpConfig; QTcpSocket* _socket; bool _socketIsConnected; diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 627c6d282a8ba295c3a2fda584d57841339ae362..51faba4ad34affa7f34a263a8ec69bdfe8d60502 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -65,27 +65,22 @@ static QString get_ip_address(const QString& address) return QString(""); } -UDPLink::UDPLink(UDPConfiguration* config) - : _socket(NULL) - , _connectState(false) - #if defined(QGC_ZEROCONF_ENABLED) +UDPLink::UDPLink(SharedLinkConfigurationPointer& config) + : LinkInterface(config) +#if defined(QGC_ZEROCONF_ENABLED) , _dnssServiceRef(NULL) - #endif +#endif , _running(false) + , _socket(NULL) + , _udpConfig(qobject_cast(config.data())) + , _connectState(false) { - Q_ASSERT(config != NULL); - _config = config; - _config->setLink(this); - - // We're doing it wrong - because the Qt folks got the API wrong: - // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ + Q_ASSERT(_udpConfig); moveToThread(this); } UDPLink::~UDPLink() { - // Disconnect link from configuration - _config->setLink(NULL); _disconnect(); // Tell the thread to exit _running = false; @@ -121,17 +116,17 @@ void UDPLink::_restartConnection() QString UDPLink::getName() const { - return _config->name(); + return _udpConfig->name(); } void UDPLink::addHost(const QString& host) { - _config->addHost(host); + _udpConfig->addHost(host); } void UDPLink::removeHost(const QString& host) { - _config->removeHost(host); + _udpConfig->removeHost(host); } void UDPLink::_writeBytes(const QByteArray data) @@ -143,7 +138,7 @@ void UDPLink::_writeBytes(const QByteArray data) // Send to all connected systems QString host; int port; - if(_config->firstHost(host, port)) { + if(_udpConfig->firstHost(host, port)) { do { QHostAddress currentHost(host); if(_socket->writeDatagram(data, currentHost, (quint16)port) < 0) { @@ -162,10 +157,10 @@ void UDPLink::_writeBytes(const QByteArray data) // unit sent by UDP. _logOutputDataRate(data.size(), QDateTime::currentMSecsSinceEpoch()); } - } while (_config->nextHost(host, port)); + } while (_udpConfig->nextHost(host, port)); //-- Remove hosts that are no longer there foreach (const QString& ghost, goneHosts) { - _config->removeHost(ghost); + _udpConfig->removeHost(ghost); } } } @@ -194,7 +189,7 @@ void UDPLink::readBytes() // added to the list and will start receiving datagrams from here. Even a port scanner // would trigger this. // Add host to broadcast list if not yet present, or update its port - _config->addHost(sender.toString(), (int)senderPort); + _udpConfig->addHost(sender.toString(), (int)senderPort); } //-- Send whatever is left if(databuffer.size()) { @@ -248,7 +243,7 @@ bool UDPLink::_hardwareConnect() QHostAddress host = QHostAddress::AnyIPv4; _socket = new QUdpSocket(); _socket->setProxy(QNetworkProxy::NoProxy); - _connectState = _socket->bind(host, _config->localPort(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress); + _connectState = _socket->bind(host, _udpConfig->localPort(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress); if (_connectState) { //-- Make sure we have a large enough IO buffers #ifdef __mobile__ @@ -258,7 +253,7 @@ bool UDPLink::_hardwareConnect() _socket->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption, 256 * 1024); _socket->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 512 * 1024); #endif - _registerZeroconf(_config->localPort(), kZeroconfRegistration); + _registerZeroconf(_udpConfig->localPort(), kZeroconfRegistration); QObject::connect(_socket, &QUdpSocket::readyRead, this, &UDPLink::readBytes); emit connected(); } else { diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 1c1c977a9080f3e8a269d8e8b0f38cfbdef8ca8e..df0c58e7360c025946dc74a3de4690aa5ef1440e 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -177,10 +177,7 @@ public: bool connect(void); bool disconnect(void); - LinkConfiguration* getLinkConfiguration() { return _config; } - public slots: - /*! @brief Add a new host to broadcast messages to */ void addHost (const QString& host); /*! @brief Remove a host from broadcasting messages to */ @@ -189,23 +186,11 @@ 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 QByteArray data); -protected: - - QUdpSocket* _socket; - UDPConfiguration* _config; - bool _connectState; - private: // Links are only created/destroyed by LinkManager so constructor/destructor is not public - UDPLink(UDPConfiguration* config); + UDPLink(SharedLinkConfigurationPointer& config); ~UDPLink(); // From LinkInterface @@ -223,6 +208,9 @@ private: #endif bool _running; + QUdpSocket* _socket; + UDPConfiguration* _udpConfig; + bool _connectState; }; #endif // UDPLINK_H diff --git a/src/qgcunittest/LinkManagerTest.cc b/src/qgcunittest/LinkManagerTest.cc index bc5af5bb3a30eacdb83c1a03275c09760805e4cc..77f4e8b3c5da883e6dca21698ab9da41994ea04b 100644 --- a/src/qgcunittest/LinkManagerTest.cc +++ b/src/qgcunittest/LinkManagerTest.cc @@ -57,29 +57,29 @@ void LinkManagerTest::cleanup(void) void LinkManagerTest::_add_test(void) { Q_ASSERT(_linkMgr); - Q_ASSERT(_linkMgr->links()->count() == 0); + Q_ASSERT(_linkMgr->links().count() == 0); _connectMockLink(); - QCOMPARE(_linkMgr->links()->count(), 1); - QCOMPARE(_linkMgr->links()->value(0), _mockLink); + QCOMPARE(_linkMgr->links().count(), 1); + QCOMPARE(_linkMgr->links().at(0), _mockLink); } void LinkManagerTest::_delete_test(void) { Q_ASSERT(_linkMgr); - Q_ASSERT(_linkMgr->links()->count() == 0); + Q_ASSERT(_linkMgr->links().count() == 0); _connectMockLink(); _disconnectMockLink(); - QCOMPARE(_linkMgr->links()->count(), 0); + QCOMPARE(_linkMgr->links().count(), 0); } void LinkManagerTest::_addSignals_test(void) { Q_ASSERT(_linkMgr); - Q_ASSERT(_linkMgr->links()->count() == 0); + Q_ASSERT(_linkMgr->links().count() == 0); Q_ASSERT(_multiSpy->checkNoSignals() == true); _connectMockLink(); @@ -99,7 +99,7 @@ void LinkManagerTest::_addSignals_test(void) void LinkManagerTest::_deleteSignals_test(void) { Q_ASSERT(_linkMgr); - Q_ASSERT(_linkMgr->links()->count() == 0); + Q_ASSERT(_linkMgr->links().count() == 0); Q_ASSERT(_multiSpy->checkNoSignals() == true); _connectMockLink(); diff --git a/src/qgcunittest/TCPLinkTest.cc b/src/qgcunittest/TCPLinkTest.cc index f1a1835ceffbf63833f1aae05ec7e0224f44a9fc..996951eaf4896760f0839d562227c6e3ecaf7c19 100644 --- a/src/qgcunittest/TCPLinkTest.cc +++ b/src/qgcunittest/TCPLinkTest.cc @@ -17,8 +17,7 @@ #include "TCPLoopBackServer.h" TCPLinkTest::TCPLinkTest(void) - : _config(NULL) - , _link(NULL) + : _link(NULL) , _multiSpy(NULL) { @@ -31,13 +30,12 @@ void TCPLinkTest::init(void) Q_ASSERT(_link == nullptr); Q_ASSERT(_multiSpy == nullptr); - Q_ASSERT(_config == nullptr); - _config = new TCPConfiguration("MockTCP"); - _config->setAddress(QHostAddress::LocalHost); - _config->setPort(5760); - _link = new TCPLink(_config); - Q_ASSERT(_link != NULL); + TCPConfiguration* tcpConfig = new TCPConfiguration("MockTCP"); + tcpConfig->setAddress(QHostAddress::LocalHost); + tcpConfig->setPort(5760); + _sharedConfig = SharedLinkConfigurationPointer(tcpConfig); + _link = new TCPLink(_sharedConfig); _rgSignals[bytesReceivedSignalIndex] = SIGNAL(bytesReceived(LinkInterface*, QByteArray)); _rgSignals[connectedSignalIndex] = SIGNAL(connected(void)); @@ -55,7 +53,6 @@ void TCPLinkTest::cleanup(void) { Q_ASSERT(_multiSpy); Q_ASSERT(_link); - Q_ASSERT(_config); delete _multiSpy; _multiSpy = nullptr; @@ -63,15 +60,13 @@ void TCPLinkTest::cleanup(void) delete _link; _link = nullptr; - delete _config; - _config = nullptr; + _sharedConfig.clear(); UnitTest::cleanup(); } void TCPLinkTest::_connectFail_test(void) { - Q_ASSERT(_config); Q_ASSERT(_link); Q_ASSERT(_multiSpy); Q_ASSERT(_multiSpy->checkNoSignals() == true); @@ -110,7 +105,8 @@ void TCPLinkTest::_connectSucceed_test(void) Q_ASSERT(_multiSpy->checkNoSignals() == true); // Start the server side - TCPLoopBackServer* server = new TCPLoopBackServer(_config->address(), _config->port()); + TCPConfiguration* tcpConfig = qobject_cast(_sharedConfig.data()); + TCPLoopBackServer* server = new TCPLoopBackServer(tcpConfig->address(), tcpConfig->port()); Q_CHECK_PTR(server); // Connect to the server diff --git a/src/qgcunittest/TCPLinkTest.h b/src/qgcunittest/TCPLinkTest.h index 196da6ef02839d9f35630ee500281ab3125c94d3..e34d993da7fe78ff4de0b361eeb02233d4938e8d 100644 --- a/src/qgcunittest/TCPLinkTest.h +++ b/src/qgcunittest/TCPLinkTest.h @@ -60,11 +60,11 @@ private: //deleteLinkSignalMask = 1 << deleteLinkSignalIndex, }; - TCPConfiguration* _config; - TCPLink* _link; - MultiSignalSpy* _multiSpy; - static const size_t _cSignals = maxSignalIndex; - const char* _rgSignals[_cSignals]; + SharedLinkConfigurationPointer _sharedConfig; + TCPLink* _link; + MultiSignalSpy* _multiSpy; + static const size_t _cSignals = maxSignalIndex; + const char* _rgSignals[_cSignals]; }; #endif diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index 22267f376ba0cf3f90a91f90221acefdb55f55f4..55f26ec585b6f1823b3af25fdc28fffb94071c9a 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -85,7 +85,10 @@ void QGCMAVLinkLogPlayer::_selectLogFileForPlayback(void) linkConfig->setLogFilename(logFilename); linkConfig->setName(linkConfig->logFilenameShort()); _ui->logFileNameLabel->setText(linkConfig->logFilenameShort()); - _replayLink = (LogReplayLink*)qgcApp()->toolbox()->linkManager()->createConnectedLink(linkConfig); + + LinkManager* linkMgr = qgcApp()->toolbox()->linkManager(); + SharedLinkConfigurationPointer sharedConfig = linkMgr->addConfiguration(linkConfig); + _replayLink = (LogReplayLink*)qgcApp()->toolbox()->linkManager()->createConnectedLink(sharedConfig); connect(_replayLink, &LogReplayLink::logFileStats, this, &QGCMAVLinkLogPlayer::_logFileStats); connect(_replayLink, &LogReplayLink::playbackStarted, this, &QGCMAVLinkLogPlayer::_playbackStarted);