Commit 76677481 authored by Don Gagne's avatar Don Gagne

Reference counting for LinkInterface, LinkConfiguration

Prevent shutdown ordering crashes
parent 5040e3d9
......@@ -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 \
......
......@@ -155,8 +155,8 @@ void QGroundControlQmlGlobal::stopAllMockLinks(void)
#ifdef QT_DEBUG
LinkManager* linkManager = qgcApp()->toolbox()->linkManager();
for (int i=0; i<linkManager->links()->count(); i++) {
LinkInterface* link = linkManager->links()->value<LinkInterface*>(i);
for (int i=0; i<linkManager->links().count(); i++) {
LinkInterface* link = linkManager->links()[i];
MockLink* mockLink = qobject_cast<MockLink*>(link);
if (mockLink) {
......
......@@ -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; i<links->count(); i++) {
LinkInterface* link = links->value<LinkInterface*>(i);
LinkManager* linkMgr = _toolbox->linkManager();
for (int i=0; i<linkMgr->links().count(); i++) {
LinkInterface* link = linkMgr->links()[i];
if (link->isConnected()) {
mavlink_message_t message;
mavlink_msg_heartbeat_pack_chan(_mavlinkProtocol->getSystemId(),
......
......@@ -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<SerialLink*>(link);
if (pSerialLink) {
LinkConfiguration* pLinkConfig = pSerialLink->getLinkConfiguration();
if (pLinkConfig) {
SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(pLinkConfig);
LinkConfiguration* config = pSerialLink->getLinkConfiguration();
if (config) {
SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(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)
......
......@@ -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;
......
......@@ -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<LinkConfiguration> SharedLinkConfigurationPointer;
#endif // LINKCONFIGURATION_H
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* 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*>("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;
}
......@@ -19,9 +19,9 @@
#include <QDebug>
#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*");
}
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<LinkInterface> SharedLinkInterface;
typedef QSharedPointer<LinkInterface> SharedLinkInterfacePointer;
#endif // _LINKINTERFACE_H_
......@@ -7,15 +7,6 @@
*
****************************************************************************/
/**
* @file
* @brief Brief Description
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QList>
#include <QApplication>
#include <QDebug>
......@@ -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<SerialConfiguration*>(config);
SerialConfiguration* serialConfig = dynamic_cast<SerialConfiguration*>(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<UDPConfiguration*>(config));
pLink = new UDPLink(config);
break;
case LinkConfiguration::TypeTcp:
pLink = new TCPLink(dynamic_cast<TCPConfiguration*>(config));
pLink = new TCPLink(config);
break;
#ifdef QGC_ENABLE_BLUETOOTH
case LinkConfiguration::TypeBluetooth:
pLink = new BluetoothLink(dynamic_cast<BluetoothConfiguration*>(config));
pLink = new BluetoothLink(config);
break;
#endif
#ifndef __mobile__
case LinkConfiguration::TypeLogReplay:
pLink = new LogReplayLink(dynamic_cast<LogReplayLinkConfiguration*>(config));
pLink = new LogReplayLink(config);
break;
#endif
#ifdef QT_DEBUG
case LinkConfiguration::TypeMock:
pLink = new MockLink(dynamic_cast<MockConfiguration*>(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<LinkConfiguration*>(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<LinkInterface*>(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<LinkConfiguration*>(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<SerialConfiguration*>(i);
for (int i=0; i<_sharedAutoconnectConfigurations.count(); i++) {
SerialConfiguration* serialConfig = qobject_cast<SerialConfiguration*>(_sharedAutoconnectConfigurations[i].data());