Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
76677481
Commit
76677481
authored
Dec 26, 2016
by
Don Gagne
Browse files
Reference counting for LinkInterface, LinkConfiguration
Prevent shutdown ordering crashes
parent
5040e3d9
Changes
25
Show whitespace changes
Inline
Side-by-side
qgroundcontrol.pro
View file @
76677481
...
@@ -641,6 +641,7 @@ SOURCES += \
...
@@ -641,6 +641,7 @@ SOURCES += \
src/VehicleSetup/JoystickConfigController.cc \
src/VehicleSetup/JoystickConfigController.cc \
src/audio/QGCAudioWorker.cpp \
src/audio/QGCAudioWorker.cpp \
src/comm/LinkConfiguration.cc \
src/comm/LinkConfiguration.cc \
src/comm/LinkInterface.cc \
src/comm/LinkManager.cc \
src/comm/LinkManager.cc \
src/comm/MAVLinkProtocol.cc \
src/comm/MAVLinkProtocol.cc \
src/comm/QGCMAVLink.cc \
src/comm/QGCMAVLink.cc \
...
...
src/QmlControls/QGroundControlQmlGlobal.cc
View file @
76677481
...
@@ -155,8 +155,8 @@ void QGroundControlQmlGlobal::stopAllMockLinks(void)
...
@@ -155,8 +155,8 @@ void QGroundControlQmlGlobal::stopAllMockLinks(void)
#ifdef QT_DEBUG
#ifdef QT_DEBUG
LinkManager
*
linkManager
=
qgcApp
()
->
toolbox
()
->
linkManager
();
LinkManager
*
linkManager
=
qgcApp
()
->
toolbox
()
->
linkManager
();
for
(
int
i
=
0
;
i
<
linkManager
->
links
()
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
linkManager
->
links
()
.
count
();
i
++
)
{
LinkInterface
*
link
=
linkManager
->
links
()
->
value
<
LinkInterface
*>
(
i
)
;
LinkInterface
*
link
=
linkManager
->
links
()
[
i
]
;
MockLink
*
mockLink
=
qobject_cast
<
MockLink
*>
(
link
);
MockLink
*
mockLink
=
qobject_cast
<
MockLink
*>
(
link
);
if
(
mockLink
)
{
if
(
mockLink
)
{
...
...
src/Vehicle/MultiVehicleManager.cc
View file @
76677481
...
@@ -306,9 +306,9 @@ void MultiVehicleManager::setGcsHeartbeatEnabled(bool gcsHeartBeatEnabled)
...
@@ -306,9 +306,9 @@ void MultiVehicleManager::setGcsHeartbeatEnabled(bool gcsHeartBeatEnabled)
void
MultiVehicleManager
::
_sendGCSHeartbeat
(
void
)
void
MultiVehicleManager
::
_sendGCSHeartbeat
(
void
)
{
{
// Send a heartbeat out on each link
// Send a heartbeat out on each link
QmlObjectListModel
*
link
s
=
_toolbox
->
linkManager
()
->
links
()
;
LinkManager
*
link
Mgr
=
_toolbox
->
linkManager
();
for
(
int
i
=
0
;
i
<
link
s
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
link
Mgr
->
links
().
count
();
i
++
)
{
LinkInterface
*
link
=
link
s
->
value
<
LinkInterface
*>
(
i
)
;
LinkInterface
*
link
=
link
Mgr
->
links
()[
i
]
;
if
(
link
->
isConnected
())
{
if
(
link
->
isConnected
())
{
mavlink_message_t
message
;
mavlink_message_t
message
;
mavlink_msg_heartbeat_pack_chan
(
_mavlinkProtocol
->
getSystemId
(),
mavlink_msg_heartbeat_pack_chan
(
_mavlinkProtocol
->
getSystemId
(),
...
...
src/Vehicle/Vehicle.cc
View file @
76677481
...
@@ -926,8 +926,9 @@ bool Vehicle::_containsLink(LinkInterface* link)
...
@@ -926,8 +926,9 @@ bool Vehicle::_containsLink(LinkInterface* link)
void
Vehicle
::
_addLink
(
LinkInterface
*
link
)
void
Vehicle
::
_addLink
(
LinkInterface
*
link
)
{
{
if
(
!
_containsLink
(
link
))
{
if
(
!
_containsLink
(
link
))
{
_links
+=
link
;
qCDebug
(
VehicleLog
)
<<
"_addLink:"
<<
QString
(
"%1"
).
arg
((
ulong
)
link
,
0
,
16
);
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
::
linkInactive
,
this
,
&
Vehicle
::
_linkInactiveOrDeleted
);
connect
(
qgcApp
()
->
toolbox
()
->
linkManager
(),
&
LinkManager
::
linkDeleted
,
this
,
&
Vehicle
::
_linkInactiveOrDeleted
);
connect
(
qgcApp
()
->
toolbox
()
->
linkManager
(),
&
LinkManager
::
linkDeleted
,
this
,
&
Vehicle
::
_linkInactiveOrDeleted
);
}
}
...
@@ -938,6 +939,7 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
...
@@ -938,6 +939,7 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
qCDebug
(
VehicleLog
)
<<
"_linkInactiveOrDeleted linkCount"
<<
_links
.
count
();
qCDebug
(
VehicleLog
)
<<
"_linkInactiveOrDeleted linkCount"
<<
_links
.
count
();
_links
.
removeOne
(
link
);
_links
.
removeOne
(
link
);
_updatePriorityLink
();
if
(
_links
.
count
()
==
0
&&
!
_allLinksInactiveSent
)
{
if
(
_links
.
count
()
==
0
&&
!
_allLinksInactiveSent
)
{
qCDebug
(
VehicleLog
)
<<
"All links inactive"
;
qCDebug
(
VehicleLog
)
<<
"All links inactive"
;
...
@@ -983,26 +985,42 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
...
@@ -983,26 +985,42 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
emit
messagesSentChanged
();
emit
messagesSentChanged
();
}
}
/// @return Direct usb connection link to board if one, NULL if none
void
Vehicle
::
_updatePriorityLink
(
void
)
LinkInterface
*
Vehicle
::
priorityLink
(
void
)
{
{
#ifndef NO_SERIAL_LINK
#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
())
{
if
(
link
->
isConnected
())
{
SerialLink
*
pSerialLink
=
qobject_cast
<
SerialLink
*>
(
link
);
SerialLink
*
pSerialLink
=
qobject_cast
<
SerialLink
*>
(
link
);
if
(
pSerialLink
)
{
if
(
pSerialLink
)
{
LinkConfiguration
*
pLinkC
onfig
=
pSerialLink
->
getLinkConfiguration
();
LinkConfiguration
*
c
onfig
=
pSerialLink
->
getLinkConfiguration
();
if
(
pLinkC
onfig
)
{
if
(
c
onfig
)
{
SerialConfiguration
*
pSerialConfig
=
qobject_cast
<
SerialConfiguration
*>
(
pLinkC
onfig
);
SerialConfiguration
*
pSerialConfig
=
qobject_cast
<
SerialConfiguration
*>
(
c
onfig
);
if
(
pSerialConfig
&&
pSerialConfig
->
usbDirect
())
{
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
#endif
return
_links
.
count
()
?
_links
[
0
]
:
NULL
;
}
}
void
Vehicle
::
setLatitude
(
double
latitude
)
void
Vehicle
::
setLatitude
(
double
latitude
)
...
...
src/Vehicle/Vehicle.h
View file @
76677481
...
@@ -416,8 +416,9 @@ public:
...
@@ -416,8 +416,9 @@ public:
MAV_TYPE
vehicleType
(
void
)
const
{
return
_vehicleType
;
}
MAV_TYPE
vehicleType
(
void
)
const
{
return
_vehicleType
;
}
Q_INVOKABLE
QString
vehicleTypeName
(
void
)
const
;
Q_INVOKABLE
QString
vehicleTypeName
(
void
)
const
;
/// Returns the highest quality link available to the Vehicle
/// Returns the highest quality link available to the Vehicle. If you need to hold a refernce to this link use
LinkInterface
*
priorityLink
(
void
);
/// LinkManager::sharedLinkInterfaceForGet to get QSharedPointer for link.
LinkInterface
*
priorityLink
(
void
)
{
return
_priorityLink
.
data
();
}
/// Sends a message to the specified link
/// Sends a message to the specified link
/// @return true: message sent, false: Link no longer connected
/// @return true: message sent, false: Link no longer connected
...
@@ -720,6 +721,7 @@ private:
...
@@ -720,6 +721,7 @@ private:
void
_handleMavlinkLoggingDataAcked
(
mavlink_message_t
&
message
);
void
_handleMavlinkLoggingDataAcked
(
mavlink_message_t
&
message
);
void
_ackMavlinkLogData
(
uint16_t
sequence
);
void
_ackMavlinkLogData
(
uint16_t
sequence
);
void
_sendNextQueuedMavCommand
(
void
);
void
_sendNextQueuedMavCommand
(
void
);
void
_updatePriorityLink
(
void
);
private:
private:
int
_id
;
///< Mavlink system id
int
_id
;
///< Mavlink system id
...
@@ -851,6 +853,8 @@ private:
...
@@ -851,6 +853,8 @@ private:
static
const
int
_lowBatteryAnnounceRepeatMSecs
;
// Amount of time in between each low battery announcement
static
const
int
_lowBatteryAnnounceRepeatMSecs
;
// Amount of time in between each low battery announcement
QElapsedTimer
_lowBatteryAnnounceTimer
;
QElapsedTimer
_lowBatteryAnnounceTimer
;
SharedLinkInterfacePointer
_priorityLink
;
// We always keep a reference to the priority link to manage shutdown ordering
// FactGroup facts
// FactGroup facts
Fact
_rollFact
;
Fact
_rollFact
;
...
...
src/comm/LinkConfiguration.h
View file @
76677481
...
@@ -7,7 +7,6 @@
...
@@ -7,7 +7,6 @@
*
*
****************************************************************************/
****************************************************************************/
#ifndef LINKCONFIGURATION_H
#ifndef LINKCONFIGURATION_H
#define LINKCONFIGURATION_H
#define LINKCONFIGURATION_H
...
@@ -37,7 +36,7 @@ public:
...
@@ -37,7 +36,7 @@ public:
// Property accessors
// Property accessors
const
QString
name
(
void
)
{
return
_name
;
}
QString
name
(
void
)
const
{
return
_name
;
}
LinkInterface
*
link
(
void
)
{
return
_link
;
}
LinkInterface
*
link
(
void
)
{
return
_link
;
}
void
setName
(
const
QString
name
);
void
setName
(
const
QString
name
);
...
@@ -190,4 +189,6 @@ private:
...
@@ -190,4 +189,6 @@ private:
bool
_autoConnect
;
///< This connection is started automatically at boot
bool
_autoConnect
;
///< This connection is started automatically at boot
};
};
typedef
QSharedPointer
<
LinkConfiguration
>
SharedLinkConfigurationPointer
;
#endif // LINKCONFIGURATION_H
#endif // LINKCONFIGURATION_H
src/comm/LinkInterface.cc
0 → 100644
View file @
76677481
/****************************************************************************
*
* (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.0
f
/
((
float
)
totalTime
/
1000.0
f
))
:
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
;
}
src/comm/LinkInterface.h
View file @
76677481
...
@@ -19,9 +19,9 @@
...
@@ -19,9 +19,9 @@
#include
<QDebug>
#include
<QDebug>
#include
"QGCMAVLink.h"
#include
"QGCMAVLink.h"
#include
"LinkConfiguration.h"
class
LinkManager
;
class
LinkManager
;
class
LinkConfiguration
;
/**
/**
* The link interface defines the interface for all links used to communicate
* The link interface defines the interface for all links used to communicate
...
@@ -36,17 +36,15 @@ class LinkInterface : public QThread
...
@@ -36,17 +36,15 @@ class LinkInterface : public QThread
friend
class
LinkManager
;
friend
class
LinkManager
;
public:
public:
~
LinkInterface
()
{
_config
->
setLink
(
NULL
);
}
Q_PROPERTY
(
bool
active
READ
active
WRITE
setActive
NOTIFY
activeChanged
)
Q_PROPERTY
(
bool
active
READ
active
WRITE
setActive
NOTIFY
activeChanged
)
// Property accessors
// Property accessors
bool
active
(
void
)
{
return
_active
;
}
bool
active
(
void
)
{
return
_active
;
}
void
setActive
(
bool
active
)
{
_active
=
active
;
emit
activeChanged
(
active
);
}
void
setActive
(
bool
active
)
{
_active
=
active
;
emit
activeChanged
(
active
);
}
/**
LinkConfiguration
*
getLinkConfiguration
(
void
)
{
return
_config
.
data
();
}
* @brief Get link configuration
* @return A pointer to the instance of LinkConfiguration
**/
virtual
LinkConfiguration
*
getLinkConfiguration
()
=
0
;
/* Connection management */
/* Connection management */
...
@@ -116,13 +114,7 @@ public:
...
@@ -116,13 +114,7 @@ public:
/// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only
/// 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
/// set into the link when it is added to LinkManager
uint8_t
mavlinkChannel
(
void
)
const
uint8_t
mavlinkChannel
(
void
)
const
;
{
if
(
!
_mavlinkChannelSet
)
{
qWarning
()
<<
"Call to LinkInterface::mavlinkChannel with _mavlinkChannelSet == false"
;
}
return
_mavlinkChannel
;
}
// These are left unimplemented in order to cause linker errors which indicate incorrect usage of
// 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.
// connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
...
@@ -192,43 +184,21 @@ signals:
...
@@ -192,43 +184,21 @@ signals:
protected:
protected:
// Links are only created by LinkManager so constructor is not public
// Links are only created by LinkManager so constructor is not public
LinkInterface
()
:
LinkInterface
(
SharedLinkConfigurationPointer
&
config
);
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*"
);
}
/// This function logs the send times and amounts of datas for input. Data is used for calculating
/// This function logs the send times and amounts of datas for input. Data is used for calculating
/// the transmission rate.
/// the transmission rate.
/// @param byteCount Number of bytes received
/// @param byteCount Number of bytes received
/// @param time Time in ms send occurred
/// @param time Time in ms send occurred
void
_logInputDataRate
(
quint64
byteCount
,
qint64
time
)
{
void
_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
/// This function logs the send times and amounts of datas for output. Data is used for calculating
/// the transmission rate.
/// the transmission rate.
/// @param byteCount Number of bytes sent
/// @param byteCount Number of bytes sent
/// @param time Time in ms receive occurred
/// @param time Time in ms receive occurred
void
_logOutputDataRate
(
quint64
byteCount
,
qint64
time
)
{
void
_logOutputDataRate
(
quint64
byteCount
,
qint64
time
);
if
(
_enableRateCollection
)
_logDataRateToBuffer
(
_outDataWriteAmounts
,
_outDataWriteTimes
,
&
_outDataIndex
,
byteCount
,
time
);
SharedLinkConfigurationPointer
_config
;
}
private:
private:
/**
/**
...
@@ -243,24 +213,7 @@ private:
...
@@ -243,24 +213,7 @@ private:
* @param bytes The amount of bytes transmit.
* @param bytes The amount of bytes transmit.
* @param time The time (in ms) this transmission occurred.
* @param time The time (in ms) this transmission occurred.
*/
*/
void
_logDataRateToBuffer
(
quint64
*
bytesBuffer
,
qint64
*
timeBuffer
,
int
*
writeIndex
,
quint64
bytes
,
qint64
time
)
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
;
}
/**
/**
* @brief getCurrentDataRate Get the current data rate given a data rate buffer.
* @brief getCurrentDataRate Get the current data rate given a data rate buffer.
...
@@ -275,48 +228,7 @@ private:
...
@@ -275,48 +228,7 @@ private:
* @param dataWriteAmounts The amount of data (in bits) that was transferred.
* @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.
* @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
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.0
f
/
((
float
)
totalTime
/
1000.0
f
))
:
0
;
// Finally return our calculated data rate.
return
dataRate
;
}
/**
/**
* @brief Connect this interface logically
* @brief Connect this interface logically
...
@@ -328,7 +240,7 @@ private:
...
@@ -328,7 +240,7 @@ private:
virtual
void
_disconnect
(
void
)
=
0
;
virtual
void
_disconnect
(
void
)
=
0
;
/// Sets the mavlink channel to use for this link
/// 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
bool
_mavlinkChannelSet
;
///< true: _mavlinkChannel has been set
uint8_t
_mavlinkChannel
;
///< mavlink channel to use for this link, as used by mavlink_parse_char
uint8_t
_mavlinkChannel
;
///< mavlink channel to use for this link, as used by mavlink_parse_char
...
@@ -355,6 +267,6 @@ private:
...
@@ -355,6 +267,6 @@ private:
bool
_enableRateCollection
;
bool
_enableRateCollection
;
};
};
typedef
QSharedPointer
<
LinkInterface
>
SharedLinkInterface
;
typedef
QSharedPointer
<
LinkInterface
>
SharedLinkInterface
Pointer
;
#endif // _LINKINTERFACE_H_
#endif // _LINKINTERFACE_H_
src/comm/LinkManager.cc
View file @
76677481
...
@@ -7,15 +7,6 @@
...
@@ -7,15 +7,6 @@
*
*
****************************************************************************/
****************************************************************************/
/**
* @file
* @brief Brief Description
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include
<QList>
#include
<QList>
#include
<QApplication>
#include
<QApplication>
#include
<QDebug>
#include
<QDebug>
...
@@ -108,17 +99,21 @@ void LinkManager::setToolbox(QGCToolbox *toolbox)
...
@@ -108,17 +99,21 @@ void LinkManager::setToolbox(QGCToolbox *toolbox)
}
}
LinkInterface
*
LinkManager
::
createConnectedLink
(
LinkConfiguration
*
config
)
LinkInterface
*
LinkManager
::
createConnectedLink
(
Shared
LinkConfiguration
Pointer
&
config
)
{
{
Q_ASSERT
(
config
);
if
(
!
config
)
{
qWarning
()
<<
"LinkManager::createConnectedLink called with NULL config"
;
return
NULL
;
}
LinkInterface
*
pLink
=
NULL
;
LinkInterface
*
pLink
=
NULL
;
switch
(
config
->
type
())
{
switch
(
config
->
type
())
{
#ifndef NO_SERIAL_LINK
#ifndef NO_SERIAL_LINK
case
LinkConfiguration
::
TypeSerial
:
case
LinkConfiguration
::
TypeSerial
:
{
{
SerialConfiguration
*
serialConfig
=
dynamic_cast
<
SerialConfiguration
*>
(
config
);
SerialConfiguration
*
serialConfig
=
dynamic_cast
<
SerialConfiguration
*>
(
config
.
data
()
);
if
(
serialConfig
)
{
if
(
serialConfig
)
{
pLink
=
new
SerialLink
(
serialC
onfig
);
pLink
=
new
SerialLink
(
c
onfig
);
if
(
serialConfig
->
usbDirect
())
{
if
(
serialConfig
->
usbDirect
())
{
_activeLinkCheckList
.
append
((
SerialLink
*
)
pLink
);
_activeLinkCheckList
.
append
((
SerialLink
*
)
pLink
);
if
(
!
_activeLinkCheckTimer
.
isActive
())
{
if
(
!
_activeLinkCheckTimer
.
isActive
())
{
...
@@ -130,43 +125,45 @@ LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config)
...
@@ -130,43 +125,45 @@ LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config)
break
;
break
;
#endif
#endif
case
LinkConfiguration
::
TypeUdp
:
case
LinkConfiguration
::
TypeUdp
:
pLink
=
new
UDPLink
(
dynamic_cast
<
UDPConfiguration
*>
(
config
)
)
;
pLink
=
new
UDPLink
(
config
);
break
;
break
;
case
LinkConfiguration
::
TypeTcp
:
case
LinkConfiguration
::
TypeTcp
:
pLink
=
new
TCPLink
(
dynamic_cast
<
TCPConfiguration
*>
(
config
)
)
;
pLink
=
new
TCPLink
(
config
);
break
;
break
;
#ifdef QGC_ENABLE_BLUETOOTH
#ifdef QGC_ENABLE_BLUETOOTH
case
LinkConfiguration
::
TypeBluetooth
:
case
LinkConfiguration
::
TypeBluetooth
:
pLink
=
new
BluetoothLink
(
dynamic_cast
<
BluetoothConfiguration
*>
(
config
)
)
;
pLink
=
new
BluetoothLink
(
config
);
break
;
break
;
#endif
#endif
#ifndef __mobile__
#ifndef __mobile__
case
LinkConfiguration
::
TypeLogReplay
:
case
LinkConfiguration
::
TypeLogReplay
:
pLink
=
new
LogReplayLink
(
dynamic_cast
<
LogReplayLinkConfiguration
*>
(
config
)
)
;
pLink
=
new
LogReplayLink
(
config
);
break
;
break
;
#endif
#endif
#ifdef QT_DEBUG
#ifdef QT_DEBUG
case
LinkConfiguration
::
TypeMock
:
case
LinkConfiguration
::
TypeMock
:
pLink
=
new
MockLink
(
dynamic_cast
<
MockConfiguration
*>
(
config
)
)
;
pLink
=
new
MockLink
(
config
);
break
;
break
;
#endif
#endif
case
LinkConfiguration
::
TypeLast
:
case
LinkConfiguration
::
TypeLast
:
default:
default:
break
;
break
;
}
}
if
(
pLink
)
{
if
(
pLink
)
{
_addLink
(
pLink
);
_addLink
(
pLink
);
connectLink
(
pLink
);
connectLink
(
pLink
);
}
}
return
pLink
;
return
pLink
;
}
}
LinkInterface
*
LinkManager
::
createConnectedLink
(
const
QString
&
name
)
LinkInterface
*
LinkManager
::
createConnectedLink
(
const
QString
&
name
)
{
{
Q_ASSERT
(
name
.
isEmpty
()
==
false
);
Q_ASSERT
(
name
.
isEmpty
()
==
false
);
for
(
int
i
=
0
;
i
<
_
link
Configurations
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
shared
Configurations
.
count
();
i
++
)
{
LinkConfiguration
*
conf
=
_
linkConfigurations
.
value
<
Link
Configuration
*>
(
i
)
;
Shared
LinkConfiguration
Pointer
&
conf
=
_
shared
Configuration
s
[
i
]
;
if
(
conf
&&
conf
->
name
()
==
name
)
if
(
conf
->
name
()
==
name
)
return
createConnectedLink
(
conf
);
return
createConnectedLink
(
conf
);
}
}
return
NULL
;
return
NULL
;
...
@@ -183,7 +180,7 @@ void LinkManager::_addLink(LinkInterface* link)
...
@@ -183,7 +180,7 @@ void LinkManager::_addLink(LinkInterface* link)
return
;
return
;
}
}
if
(
!
_links
.
contains
(
link
))
{
if
(
!
contains
Link
(
link
))
{
bool
channelSet
=
false
;
bool
channelSet
=
false
;
// Find a mavlink channel to use for this link, Channel 0 is reserved for internal use.
// 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)
...
@@ -205,7 +202,7 @@ void LinkManager::_addLink(LinkInterface* link)
qWarning
()
<<
"Ran out of mavlink channels"
;
qWarning
()
<<
"Ran out of mavlink channels"
;
}
}
_
l
inks
.
append
(
link
);
_
sharedL
inks
.
append
(
SharedLinkInterfacePointer
(
link
)
)
;
emit
newLink
(
link
);
emit
newLink
(
link
);
}
}
...
@@ -225,8 +222,8 @@ void LinkManager::_addLink(LinkInterface* link)
...
@@ -225,8 +222,8 @@ void LinkManager::_addLink(LinkInterface* link)
void
LinkManager
::
disconnectAll
(
void
)
void
LinkManager
::
disconnectAll
(
void
)
{
{
// Walk list in reverse order to preserve indices during delete
// Walk list in reverse order to preserve indices during delete
for
(
int
i
=
_
l
inks
.
count
()
-
1
;
i
>=
0
;
i
--
)
{
for
(
int
i
=
_
sharedL
inks
.
count
()
-
1
;
i
>=
0
;
i
--
)
{
disconnectLink
(
_
links
.
value
<
LinkInterface
*>
(
i
));
disconnectLink
(
_
sharedLinks
[
i
].
data
(
));
}
}
}
}
...
@@ -243,23 +240,22 @@ bool LinkManager::connectLink(LinkInterface* link)
...
@@ -243,23 +240,22 @@ bool LinkManager::connectLink(LinkInterface* link)
void
LinkManager
::
disconnectLink
(
LinkInterface
*
link
)
void
LinkManager
::
disconnectLink
(
LinkInterface
*
link
)
{
{
if
(
!
link
||
!
_links
.
contains
(
link
))
{
if
(
!
link
||
!
contains
Link
(
link
))
{
return
;
return
;
}
}
link
->
_disconnect
();
link
->
_disconnect
();
LinkConfiguration
*
config
=
link
->
getLinkConfiguration
();
LinkConfiguration
*
config
=
link
->
getLinkConfiguration
();
if
(
config
)
{
for
(
int
i
=
0
;
i
<
_sharedAutoconnectConfigurations
.
count
();
i
++
)
{
if
(
_autoconnectConfigurations
.
contains
(
config
))
{
if
(
_sharedAutoconnectConfigurations
[
i
].
data
()
==
config
)
{
config
->
setLink
(
NULL
);
qCDebug
(
LinkManagerLog
)
<<
"Removing disconnected autoconnect config"
<<
config
->
name
();
_sharedAutoconnectConfigurations
.
removeAt
(
i
);
break
;
}
}
}
}
_deleteLink
(
link
);
_deleteLink
(
link
);
if
(
_autoconnectConfigurations
.
contains
(
config
))
{
qCDebug
(
LinkManagerLog
)
<<
"Removing disconnected autoconnect config"
<<
config
->
name
();
_autoconnectConfigurations
.
removeOne
(
config
);
delete
config
;
}
}
}
void
LinkManager
::
_deleteLink
(
LinkInterface
*
link
)
void
LinkManager
::
_deleteLink
(
LinkInterface
*
link
)
...
@@ -276,13 +272,29 @@ void LinkManager::_deleteLink(LinkInterface* link)
...
@@ -276,13 +272,29 @@ void LinkManager::_deleteLink(LinkInterface* link)
// Free up the mavlink channel associated with this link
// Free up the mavlink channel associated with this link
_mavlinkChannelsUsedBitMask
&=
~
(
1
<<
link
->
mavlinkChannel
());
_mavlinkChannelsUsedBitMask
&=
~
(
1
<<
link
->
mavlinkChannel
());
_links
.
removeOne
(
link
);
for
(
int
i
=
0
;
i
<
_sharedLinks
.
count
();
i
++
)
{
delete
link
;
if
(
_sharedLinks
[
i
].
data
()
==
link
)
{
_sharedLinks
.
removeAt
(
i
);
break
;
}
}
// Emit removal of link
// Emit removal of link
emit
linkDeleted
(
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
/// @brief If all new connections should be suspended a message is displayed to the user and true
/// is returned;
/// is returned;
bool
LinkManager
::
_connectionsSuspendedMsg
(
void
)
bool
LinkManager
::
_connectionsSuspendedMsg
(
void
)
...
@@ -328,11 +340,10 @@ void LinkManager::saveLinkConfigurationList()
...
@@ -328,11 +340,10 @@ void LinkManager::saveLinkConfigurationList()
QSettings
settings
;
QSettings
settings
;
settings
.
remove
(
LinkConfiguration
::
settingsRoot
());
settings
.
remove
(
LinkConfiguration
::
settingsRoot
());
int
trueCount
=
0
;
int
trueCount
=
0
;
for
(
int
i
=
0
;
i
<
_
link
Configurations
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
shared
Configurations
.
count
();
i
++
)
{
LinkConfiguration
*
linkConfig
=
_
linkConfigurations
.
value
<
Link
Configuration
*>
(
i
)
;
Shared
LinkConfiguration
Pointer
linkConfig
=
_
shared
Configuration
s
[
i
]
;
if
(
linkConfig
)
{
if
(
linkConfig
)
{
if
(
!
linkConfig
->
isDynamic
())
if
(
!
linkConfig
->
isDynamic
())
{
{
QString
root
=
LinkConfiguration
::
settingsRoot
();
QString
root
=
LinkConfiguration
::
settingsRoot
();
root
+=
QString
(
"/Link%1"
).
arg
(
trueCount
++
);
root
+=
QString
(
"/Link%1"
).
arg
(
trueCount
++
);
settings
.
setValue
(
root
+
"/name"
,
linkConfig
->
name
());
settings
.
setValue
(
root
+
"/name"
,
linkConfig
->
name
());
...
@@ -404,7 +415,7 @@ void LinkManager::loadLinkConfigurationList()
...
@@ -404,7 +415,7 @@ void LinkManager::loadLinkConfigurationList()
//-- Have the instance load its own values
//-- Have the instance load its own values
pLink
->
setAutoConnect
(
autoConnect
);
pLink
->
setAutoConnect
(
autoConnect
);
pLink
->
loadSettings
(
settings
,
root
);
pLink
->
loadSettings
(
settings
,
root
);
_link
Configuration
s
.
append
(
pLink
);
add
Configuration
(
pLink
);
linksChanged
=
true
;
linksChanged
=
true
;
}
}
}
else
{
}
else
{
...
@@ -434,12 +445,12 @@ SerialConfiguration* LinkManager::_autoconnectConfigurationsContainsPort(const Q
...
@@ -434,12 +445,12 @@ SerialConfiguration* LinkManager::_autoconnectConfigurationsContainsPort(const Q
{
{
QString
searchPort
=
portName
.
trimmed
();
QString
searchPort
=
portName
.
trimmed
();
for
(
int
i
=
0
;
i
<
_
a
utoconnectConfigurations
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
sharedA
utoconnectConfigurations
.
count
();
i
++
)
{
SerialConfiguration
*
link
Config
=
_autoconnectConfigurations
.
value
<
Serial
Configuration
*>
(
i
);
SerialConfiguration
*
serial
Config
=
qobject_cast
<
SerialConfiguration
*>
(
_sharedAutoconnect
Configuration
s
[
i
].
data
()
);
if
(
link
Config
)
{
if
(
serial
Config
)
{
if
(
link
Config
->
portName
()
==
searchPort
)
{
if
(
serial
Config
->
portName
()
==
searchPort
)
{
return
link
Config
;
return
serial
Config
;
}
}
}
else
{
}
else
{
qWarning
()
<<
"Internal error"
;
qWarning
()
<<
"Internal error"
;
...
@@ -457,8 +468,8 @@ void LinkManager::_updateAutoConnectLinks(void)
...
@@ -457,8 +468,8 @@ void LinkManager::_updateAutoConnectLinks(void)
// Re-add UDP if we need to
// Re-add UDP if we need to
bool
foundUDP
=
false
;
bool
foundUDP
=
false
;
for
(
int
i
=
0
;
i
<
_
l
inks
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
sharedL
inks
.
count
();
i
++
)
{
LinkConfiguration
*
linkConfig
=
_
links
.
value
<
LinkInterface
*>
(
i
)
->
getLinkConfiguration
();
LinkConfiguration
*
linkConfig
=
_
sharedLinks
[
i
]
->
getLinkConfiguration
();
if
(
linkConfig
->
type
()
==
LinkConfiguration
::
TypeUdp
&&
linkConfig
->
name
()
==
_defaultUPDLinkName
)
{
if
(
linkConfig
->
type
()
==
LinkConfiguration
::
TypeUdp
&&
linkConfig
->
name
()
==
_defaultUPDLinkName
)
{
foundUDP
=
true
;
foundUDP
=
true
;
break
;
break
;
...
@@ -469,8 +480,8 @@ void LinkManager::_updateAutoConnectLinks(void)
...
@@ -469,8 +480,8 @@ void LinkManager::_updateAutoConnectLinks(void)
UDPConfiguration
*
udpConfig
=
new
UDPConfiguration
(
_defaultUPDLinkName
);
UDPConfiguration
*
udpConfig
=
new
UDPConfiguration
(
_defaultUPDLinkName
);
udpConfig
->
setLocalPort
(
QGC_UDP_LOCAL_PORT
);
udpConfig
->
setLocalPort
(
QGC_UDP_LOCAL_PORT
);
udpConfig
->
setDynamic
(
true
);
udpConfig
->
setDynamic
(
true
);
_l
inkConfiguration
s
.
append
(
udpConfig
);
SharedL
inkConfiguration
Pointer
config
=
addConfiguration
(
udpConfig
);
createConnectedLink
(
udpC
onfig
);
createConnectedLink
(
c
onfig
);
emit
linkConfigurationsChanged
();
emit
linkConfigurationsChanged
();
}
}
...
@@ -591,8 +602,8 @@ void LinkManager::_updateAutoConnectLinks(void)
...
@@ -591,8 +602,8 @@ void LinkManager::_updateAutoConnectLinks(void)
pSerialConfig
->
setBaud
(
boardType
==
QGCSerialPortInfo
::
BoardTypeSikRadio
?
57600
:
115200
);
pSerialConfig
->
setBaud
(
boardType
==
QGCSerialPortInfo
::
BoardTypeSikRadio
?
57600
:
115200
);
pSerialConfig
->
setDynamic
(
true
);
pSerialConfig
->
setDynamic
(
true
);
pSerialConfig
->
setPortName
(
portInfo
.
systemLocation
());
pSerialConfig
->
setPortName
(
portInfo
.
systemLocation
());
_
a
utoconnectConfigurations
.
append
(
pSerialConfig
);
_
sharedA
utoconnectConfigurations
.
append
(
SharedLinkConfigurationPointer
(
pSerialConfig
)
)
;
createConnectedLink
(
pSerialConfig
);
createConnectedLink
(
_sharedAutoconnectConfigurations
.
last
()
);
}
}
}
}
}
}
...
@@ -607,13 +618,13 @@ void LinkManager::_updateAutoConnectLinks(void)
...
@@ -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
// Now we go through the current configuration list and make sure any dynamic config has gone away
QList
<
LinkConfiguration
*>
_confToDelete
;
QList
<
LinkConfiguration
*>
_confToDelete
;
for
(
int
i
=
0
;
i
<
_
a
utoconnectConfigurations
.
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
_
sharedA
utoconnectConfigurations
.
count
();
i
++
)
{
SerialConfiguration
*
link
Config
=
_autoconnectConfigurations
.
value
<
Serial
Configuration
*>
(
i
);
SerialConfiguration
*
serial
Config
=
qobject_cast
<
SerialConfiguration
*>
(
_sharedAutoconnect
Configuration
s
[
i
].
data
()
);
if
(
link
Config
)
{
if
(
serial
Config
)
{
if
(
!
currentPorts
.
contains
(
link
Config
->
portName
()))
{
if
(
!
currentPorts
.
contains
(
serial
Config
->
portName
()))
{
if
(
link
Config
->
link
())
{
if
(
serial
Config
->
link
())
{
if
(
link
Config
->
link
()
->
isConnected
())
{
if
(
serial
Config
->
link
()
->
isConnected
())
{
if
(
link
Config
->
link
()
->
active
())
{
if
(
serial
Config
->
link
()
->
active
())
{
// We don't remove links which are still connected which have been active with a vehicle on them
// 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
// 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.
// Disconnect. Once the user disconnects, the link will be removed.
...
@@ -621,7 +632,7 @@ void LinkManager::_updateAutoConnectLinks(void)
...
@@ -621,7 +632,7 @@ void LinkManager::_updateAutoConnectLinks(void)
}
}
}
}
}
}
_confToDelete
.
append
(
link
Config
);
_confToDelete
.
append
(
serial
Config
);
}
}
}
else
{
}
else
{
qWarning
()
<<
"Internal error"
;
qWarning
()
<<
"Internal error"
;
...
@@ -631,7 +642,12 @@ void LinkManager::_updateAutoConnectLinks(void)
...
@@ -631,7 +642,12 @@ void LinkManager::_updateAutoConnectLinks(void)
// Now remove all configs that are gone
// Now remove all configs that are gone
foreach
(
LinkConfiguration
*
pDeleteConfig
,
_confToDelete
)
{
foreach
(
LinkConfiguration
*
pDeleteConfig
,
_confToDelete
)
{
qCDebug
(
LinkManagerLog
)
<<
"Removing unused autoconnect config"
<<
pDeleteConfig
->
name
();
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
())
{
if
(
pDeleteConfig
->
link
())
{
disconnectLink
(
pDeleteConfig
->
link
());
disconnectLink
(
pDeleteConfig
->
link
());
}
}
...
@@ -789,7 +805,7 @@ bool LinkManager::endCreateConfiguration(LinkConfiguration* config)
...
@@ -789,7 +805,7 @@ bool LinkManager::endCreateConfiguration(LinkConfiguration* config)
{
{
Q_ASSERT
(
config
!=
NULL
);
Q_ASSERT
(
config
!=
NULL
);
_fixUnnamed
(
config
);
_fixUnnamed
(
config
);
_link
Configuration
s
.
append
(
config
);
add
Configuration
(
config
);
saveLinkConfigurationList
();
saveLinkConfigurationList
();
return
true
;
return
true
;
}
}
...
@@ -883,16 +899,19 @@ void LinkManager::removeConfiguration(LinkConfiguration* config)
...
@@ -883,16 +899,19 @@ void LinkManager::removeConfiguration(LinkConfiguration* config)
if
(
iface
)
{
if
(
iface
)
{
disconnectLink
(
iface
);
disconnectLink
(
iface
);
}
}
// Remove configuration
_linkConfigurations
.
removeOne
(
config
);
_removeConfiguration
(
config
);
delete
config
;
// Save list
saveLinkConfigurationList
();
saveLinkConfigurationList
();
}
}
bool
LinkManager
::
isAutoconnectLink
(
LinkInterface
*
link
)
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
)
bool
LinkManager
::
isBluetoothAvailable
(
void
)
...
@@ -908,7 +927,7 @@ void LinkManager::_activeLinkCheck(void)
...
@@ -908,7 +927,7 @@ void LinkManager::_activeLinkCheck(void)
if
(
_activeLinkCheckList
.
count
()
!=
0
)
{
if
(
_activeLinkCheckList
.
count
()
!=
0
)
{
link
=
_activeLinkCheckList
.
takeFirst
();
link
=
_activeLinkCheckList
.
takeFirst
();
if
(
_links
.
contains
(
link
)
&&
link
->
isConnected
())
{
if
(
contains
Link
(
link
)
&&
link
->
isConnected
())
{
// Make sure there is a vehicle on the link
// Make sure there is a vehicle on the link
QmlObjectListModel
*
vehicles
=
_toolbox
->
multiVehicleManager
()
->
vehicles
();
QmlObjectListModel
*
vehicles
=
_toolbox
->
multiVehicleManager
()
->
vehicles
();
for
(
int
i
=
0
;
i
<
vehicles
->
count
();
i
++
)
{
for
(
int
i
=
0
;
i
<
vehicles
->
count
();
i
++
)
{
...
@@ -945,3 +964,45 @@ void LinkManager::_activeLinkCheck(void)
...
@@ -945,3 +964,45 @@ void LinkManager::_activeLinkCheck(void)
}
}
}
}
#endif
#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
<
LinkInterface
*>
LinkManager
::
links
(
void
)
{
QList
<
LinkInterface
*>
rawLinks
;
for
(
int
i
=
0
;
i
<
_sharedLinks
.
count
();
i
++
)
{
rawLinks
.
append
(
_sharedLinks
[
i
].
data
());
}
return
rawLinks
;
}
src/comm/LinkManager.h
View file @
76677481
...
@@ -69,17 +69,10 @@ public:
...
@@ -69,17 +69,10 @@ public:
Q_PROPERTY
(
bool
autoconnectLibrePilot
READ
autoconnectLibrePilot
WRITE
setAutoconnectLibrePilot
NOTIFY
autoconnectLibrePilotChanged
)
Q_PROPERTY
(
bool
autoconnectLibrePilot
READ
autoconnectLibrePilot
WRITE
setAutoconnectLibrePilot
NOTIFY
autoconnectLibrePilotChanged
)
Q_PROPERTY
(
bool
isBluetoothAvailable
READ
isBluetoothAvailable
CONSTANT
)
Q_PROPERTY
(
bool
isBluetoothAvailable
READ
isBluetoothAvailable
CONSTANT
)
/// LinkInterface Accessor
Q_PROPERTY
(
QmlObjectListModel
*
linkConfigurations
READ
_qmlLinkConfigurations
NOTIFY
linkConfigurationsChanged
)
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
)
Q_PROPERTY
(
QStringList
linkTypeStrings
READ
linkTypeStrings
CONSTANT
)
/// List of supported baud rates for serial links
Q_PROPERTY
(
QStringList
serialBaudRates
READ
serialBaudRates
CONSTANT
)
Q_PROPERTY
(
QStringList
serialBaudRates
READ
serialBaudRates
CONSTANT
)
/// List of comm ports display names
Q_PROPERTY
(
QStringList
serialPortStrings
READ
serialPortStrings
NOTIFY
commPortStringsChanged
)
Q_PROPERTY
(
QStringList
serialPortStrings
READ
serialPortStrings
NOTIFY
commPortStringsChanged
)
/// List of comm ports
Q_PROPERTY
(
QStringList
serialPorts
READ
serialPorts
NOTIFY
commPortsChanged
)
Q_PROPERTY
(
QStringList
serialPorts
READ
serialPorts
NOTIFY
commPortsChanged
)
// Create/Edit Link Configuration
// Create/Edit Link Configuration
...
@@ -100,8 +93,7 @@ public:
...
@@ -100,8 +93,7 @@ public:
bool
autoconnectLibrePilot
(
void
)
{
return
_autoconnectLibrePilot
;
}
bool
autoconnectLibrePilot
(
void
)
{
return
_autoconnectLibrePilot
;
}
bool
isBluetoothAvailable
(
void
);
bool
isBluetoothAvailable
(
void
);
QmlObjectListModel
*
links
(
void
)
{
return
&
_links
;
}
QList
<
LinkInterface
*>
links
(
void
);
QmlObjectListModel
*
linkConfigurations
(
void
)
{
return
&
_linkConfigurations
;
}
QStringList
linkTypeStrings
(
void
)
const
;
QStringList
linkTypeStrings
(
void
)
const
;
QStringList
serialBaudRates
(
void
);
QStringList
serialBaudRates
(
void
);
QStringList
serialPortStrings
(
void
);
QStringList
serialPortStrings
(
void
);
...
@@ -132,7 +124,7 @@ public:
...
@@ -132,7 +124,7 @@ public:
/// Creates, connects (and adds) a link based on the given configuration instance.
/// Creates, connects (and adds) a link based on the given configuration instance.
/// Link takes ownership of config.
/// Link takes ownership of config.
Q_INVOKABLE
LinkInterface
*
createConnectedLink
(
LinkConfiguration
*
config
);
Q_INVOKABLE
LinkInterface
*
createConnectedLink
(
Shared
LinkConfiguration
Pointer
&
config
);
/// Creates, connects (and adds) a link based on the given configuration name.
/// Creates, connects (and adds) a link based on the given configuration name.
LinkInterface
*
createConnectedLink
(
const
QString
&
name
);
LinkInterface
*
createConnectedLink
(
const
QString
&
name
);
...
@@ -165,6 +157,17 @@ public:
...
@@ -165,6 +157,17 @@ public:
// Override from QGCTool
// Override from QGCTool
virtual
void
setToolbox
(
QGCToolbox
*
toolbox
);
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:
signals:
void
autoconnectUDPChanged
(
bool
autoconnect
);
void
autoconnectUDPChanged
(
bool
autoconnect
);
void
autoconnectPixhawkChanged
(
bool
autoconnect
);
void
autoconnectPixhawkChanged
(
bool
autoconnect
);
...
@@ -204,11 +207,13 @@ private slots:
...
@@ -204,11 +207,13 @@ private slots:
#endif
#endif
private:
private:
QmlObjectListModel
*
_qmlLinkConfigurations
(
void
)
{
return
&
_qmlConfigurations
;
}
bool
_connectionsSuspendedMsg
(
void
);
bool
_connectionsSuspendedMsg
(
void
);
void
_updateAutoConnectLinks
(
void
);
void
_updateAutoConnectLinks
(
void
);
void
_updateSerialPorts
();
void
_updateSerialPorts
();
void
_fixUnnamed
(
LinkConfiguration
*
config
);
void
_fixUnnamed
(
LinkConfiguration
*
config
);
bool
_setAutoconnectWorker
(
bool
&
currentAutoconnect
,
bool
newAutoconnect
,
const
char
*
autoconnectKey
);
bool
_setAutoconnectWorker
(
bool
&
currentAutoconnect
,
bool
newAutoconnect
,
const
char
*
autoconnectKey
);
void
_removeConfiguration
(
LinkConfiguration
*
config
);
#ifndef NO_SERIAL_LINK
#ifndef NO_SERIAL_LINK
SerialConfiguration
*
_autoconnectConfigurationsContainsPort
(
const
QString
&
portName
);
SerialConfiguration
*
_autoconnectConfigurationsContainsPort
(
const
QString
&
portName
);
...
@@ -223,9 +228,11 @@ private:
...
@@ -223,9 +228,11 @@ private:
MAVLinkProtocol
*
_mavlinkProtocol
;
MAVLinkProtocol
*
_mavlinkProtocol
;
QmlObjectListModel
_links
;
QmlObjectListModel
_linkConfigurations
;
QList
<
SharedLinkInterfacePointer
>
_sharedLinks
;
QmlObjectListModel
_autoconnectConfigurations
;
QList
<
SharedLinkConfigurationPointer
>
_sharedConfigurations
;
QList
<
SharedLinkConfigurationPointer
>
_sharedAutoconnectConfigurations
;
QmlObjectListModel
_qmlConfigurations
;
QMap
<
QString
,
int
>
_autoconnectWaitList
;
///< key: QGCSerialPortInfo.systemLocation, value: wait count
QMap
<
QString
,
int
>
_autoconnectWaitList
;
///< key: QGCSerialPortInfo.systemLocation, value: wait count
QStringList
_commPortList
;
QStringList
_commPortList
;
...
...
src/comm/LogReplayLink.cc
View file @
76677481
...
@@ -64,12 +64,13 @@ QString LogReplayLinkConfiguration::logFilenameShort(void)
...
@@ -64,12 +64,13 @@ QString LogReplayLinkConfiguration::logFilenameShort(void)
return
fi
.
fileName
();
return
fi
.
fileName
();
}
}
LogReplayLink
::
LogReplayLink
(
LogReplayLinkConfiguration
*
config
)
:
LogReplayLink
::
LogReplayLink
(
SharedLinkConfigurationPointer
&
config
)
_connected
(
false
),
:
LinkInterface
(
config
)
_replayAccelerationFactor
(
1.0
f
)
,
_logReplayConfig
(
qobject_cast
<
LogReplayLinkConfiguration
*>
(
config
.
data
()))
,
_connected
(
false
)
,
_replayAccelerationFactor
(
1.0
f
)
{
{
Q_ASSERT
(
config
);
Q_ASSERT
(
_logReplayConfig
);
_config
=
config
;
_readTickTimer
.
moveToThread
(
this
);
_readTickTimer
.
moveToThread
(
this
);
...
@@ -184,7 +185,7 @@ quint64 LogReplayLink::_seekToNextMavlinkMessage(mavlink_message_t* nextMsg)
...
@@ -184,7 +185,7 @@ quint64 LogReplayLink::_seekToNextMavlinkMessage(mavlink_message_t* nextMsg)
bool
LogReplayLink
::
_loadLogFile
(
void
)
bool
LogReplayLink
::
_loadLogFile
(
void
)
{
{
QString
errorMsg
;
QString
errorMsg
;
QString
logFilename
=
_
c
onfig
->
logFilename
();
QString
logFilename
=
_
logReplayC
onfig
->
logFilename
();
QFileInfo
logFileInfo
;
QFileInfo
logFileInfo
;
int
logDurationSecondsTotal
;
int
logDurationSecondsTotal
;
...
...
src/comm/LogReplayLink.h
View file @
76677481
...
@@ -57,8 +57,6 @@ class LogReplayLink : public LinkInterface
...
@@ -57,8 +57,6 @@ class LogReplayLink : public LinkInterface
friend
class
LinkManager
;
friend
class
LinkManager
;
public:
public:
virtual
LinkConfiguration
*
getLinkConfiguration
()
{
return
_config
;
}
/// @return true: log is currently playing, false: log playback is paused
/// @return true: log is currently playing, false: log playback is paused
bool
isPlaying
(
void
)
{
return
_readTickTimer
.
isActive
();
}
bool
isPlaying
(
void
)
{
return
_readTickTimer
.
isActive
();
}
...
@@ -111,7 +109,7 @@ private slots:
...
@@ -111,7 +109,7 @@ private slots:
private:
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
LogReplayLink
(
LogReplay
LinkConfiguration
*
config
);
LogReplayLink
(
Shared
LinkConfiguration
Pointer
&
config
);
~
LogReplayLink
();
~
LogReplayLink
();
void
_replayError
(
const
QString
&
errorMsg
);
void
_replayError
(
const
QString
&
errorMsg
);
...
@@ -129,7 +127,7 @@ private:
...
@@ -129,7 +127,7 @@ private:
// Virtuals from QThread
// Virtuals from QThread
virtual
void
run
(
void
);
virtual
void
run
(
void
);
LogReplayLinkConfiguration
*
_
c
onfig
;
LogReplayLinkConfiguration
*
_
logReplayC
onfig
;
bool
_connected
;
bool
_connected
;
QTimer
_readTickTimer
;
///< Timer which signals a read of next log record
QTimer
_readTickTimer
;
///< Timer which signals a read of next log record
...
...
src/comm/MAVLinkProtocol.cc
View file @
76677481
...
@@ -159,7 +159,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
...
@@ -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
// 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
// that come through after the link is disconnected. For these we just drop the data
// since the link is closed.
// since the link is closed.
if
(
!
_linkMgr
->
links
()
->
contains
(
link
))
{
if
(
!
_linkMgr
->
contains
Link
(
link
))
{
return
;
return
;
}
}
...
...
src/comm/MockLink.cc
View file @
76677481
...
@@ -44,8 +44,9 @@ const char* MockConfiguration::_vehicleTypeKey = "VehicleType";
...
@@ -44,8 +44,9 @@ const char* MockConfiguration::_vehicleTypeKey = "VehicleType";
const
char
*
MockConfiguration
::
_sendStatusTextKey
=
"SendStatusText"
;
const
char
*
MockConfiguration
::
_sendStatusTextKey
=
"SendStatusText"
;
const
char
*
MockConfiguration
::
_failureModeKey
=
"FailureMode"
;
const
char
*
MockConfiguration
::
_failureModeKey
=
"FailureMode"
;
MockLink
::
MockLink
(
MockConfiguration
*
config
)
MockLink
::
MockLink
(
SharedLinkConfigurationPointer
&
config
)
:
_missionItemHandler
(
this
,
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
())
:
LinkInterface
(
config
)
,
_missionItemHandler
(
this
,
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
())
,
_name
(
"MockLink"
)
,
_name
(
"MockLink"
)
,
_connected
(
false
)
,
_connected
(
false
)
,
_vehicleSystemId
(
_nextVehicleSystemId
++
)
,
_vehicleSystemId
(
_nextVehicleSystemId
++
)
...
@@ -67,14 +68,11 @@ MockLink::MockLink(MockConfiguration* config)
...
@@ -67,14 +68,11 @@ MockLink::MockLink(MockConfiguration* config)
,
_logDownloadCurrentOffset
(
0
)
,
_logDownloadCurrentOffset
(
0
)
,
_logDownloadBytesRemaining
(
0
)
,
_logDownloadBytesRemaining
(
0
)
{
{
_config
=
config
;
MockConfiguration
*
mockConfig
=
qobject_cast
<
MockConfiguration
*>
(
_config
.
data
());
if
(
_config
)
{
_firmwareType
=
mockConfig
->
firmwareType
();
_firmwareType
=
config
->
firmwareType
();
_vehicleType
=
mockConfig
->
vehicleType
();
_vehicleType
=
config
->
vehicleType
();
_sendStatusText
=
mockConfig
->
sendStatusText
();
_sendStatusText
=
config
->
sendStatusText
();
_failureMode
=
mockConfig
->
failureMode
();
_failureMode
=
config
->
failureMode
();
_config
->
setLink
(
this
);
}
union
px4_custom_mode
px4_cm
;
union
px4_custom_mode
px4_cm
;
...
@@ -1041,12 +1039,12 @@ void MockConfiguration::updateSettings()
...
@@ -1041,12 +1039,12 @@ void MockConfiguration::updateSettings()
MockLink
*
MockLink
::
_startMockLink
(
MockConfiguration
*
mockConfig
)
MockLink
*
MockLink
::
_startMockLink
(
MockConfiguration
*
mockConfig
)
{
{
LinkManager
*
linkM
anage
r
=
qgcApp
()
->
toolbox
()
->
linkManager
();
LinkManager
*
linkM
g
r
=
qgcApp
()
->
toolbox
()
->
linkManager
();
mockConfig
->
setDynamic
(
true
);
mockConfig
->
setDynamic
(
true
);
linkManager
->
link
Configuration
s
()
->
append
(
mockConfig
);
SharedLinkConfigurationPointer
config
=
linkMgr
->
add
Configuration
(
mockConfig
);
return
qobject_cast
<
MockLink
*>
(
linkM
anage
r
->
createConnectedLink
(
mockC
onfig
));
return
qobject_cast
<
MockLink
*>
(
linkM
g
r
->
createConnectedLink
(
c
onfig
));
}
}
MockLink
*
MockLink
::
startPX4MockLink
(
bool
sendStatusText
,
MockConfiguration
::
FailureMode_t
failureMode
)
MockLink
*
MockLink
::
startPX4MockLink
(
bool
sendStatusText
,
MockConfiguration
::
FailureMode_t
failureMode
)
...
...
src/comm/MockLink.h
View file @
76677481
...
@@ -90,8 +90,7 @@ class MockLink : public LinkInterface
...
@@ -90,8 +90,7 @@ class MockLink : public LinkInterface
Q_OBJECT
Q_OBJECT
public:
public:
// LinkConfiguration is optional for MockLink
MockLink
(
SharedLinkConfigurationPointer
&
config
);
MockLink
(
MockConfiguration
*
config
=
NULL
);
~
MockLink
(
void
);
~
MockLink
(
void
);
// MockLink methods
// MockLink methods
...
@@ -126,8 +125,6 @@ public:
...
@@ -126,8 +125,6 @@ public:
bool
connect
(
void
);
bool
connect
(
void
);
bool
disconnect
(
void
);
bool
disconnect
(
void
);
LinkConfiguration
*
getLinkConfiguration
()
{
return
_config
;
}
/// Sets a failure mode for unit testing
/// Sets a failure mode for unit testing
/// @param failureMode Type of failure to simulate
/// @param failureMode Type of failure to simulate
void
setMissionItemFailureMode
(
MockLinkMissionItemHandler
::
FailureMode_t
failureMode
);
void
setMissionItemFailureMode
(
MockLinkMissionItemHandler
::
FailureMode_t
failureMode
);
...
@@ -216,7 +213,6 @@ private:
...
@@ -216,7 +213,6 @@ private:
uint32_t
_mavCustomMode
;
uint32_t
_mavCustomMode
;
uint8_t
_mavState
;
uint8_t
_mavState
;
MockConfiguration
*
_config
;
MAV_AUTOPILOT
_firmwareType
;
MAV_AUTOPILOT
_firmwareType
;
MAV_TYPE
_vehicleType
;
MAV_TYPE
_vehicleType
;
...
...
src/comm/SerialLink.cc
View file @
76677481
...
@@ -30,19 +30,19 @@ QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog")
...
@@ -30,19 +30,19 @@ QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog")
static
QStringList
kSupportedBaudRates
;
static
QStringList
kSupportedBaudRates
;
SerialLink
::
SerialLink
(
SerialConfiguration
*
config
)
SerialLink
::
SerialLink
(
SharedLinkConfigurationPointer
&
config
)
:
LinkInterface
(
config
)
,
_port
(
NULL
)
,
_bytesRead
(
0
)
,
_stopp
(
false
)
,
_reqReset
(
false
)
,
_serialConfig
(
qobject_cast
<
SerialConfiguration
*>
(
config
.
data
()))
{
{
_bytesRead
=
0
;
Q_ASSERT
(
_serialConfig
);
_port
=
Q_NULLPTR
;
_stopp
=
false
;
qCDebug
(
SerialLinkLog
)
<<
"Create SerialLink "
<<
_serialConfig
->
portName
()
<<
_serialConfig
->
baud
()
<<
_serialConfig
->
flowControl
()
_reqReset
=
false
;
<<
_serialConfig
->
parity
()
<<
_serialConfig
->
dataBits
()
<<
_serialConfig
->
stopBits
();
Q_ASSERT
(
config
!=
NULL
);
qCDebug
(
SerialLinkLog
)
<<
"portName: "
<<
_serialConfig
->
portName
();
_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
();
}
}
void
SerialLink
::
requestReset
()
void
SerialLink
::
requestReset
()
...
@@ -53,10 +53,10 @@ void SerialLink::requestReset()
...
@@ -53,10 +53,10 @@ void SerialLink::requestReset()
SerialLink
::~
SerialLink
()
SerialLink
::~
SerialLink
()
{
{
// Disconnect link from configuration
_config
->
setLink
(
NULL
);
_disconnect
();
_disconnect
();
if
(
_port
)
delete
_port
;
if
(
_port
)
{
delete
_port
;
}
_port
=
NULL
;
_port
=
NULL
;
}
}
...
@@ -70,7 +70,7 @@ bool SerialLink::_isBootloader()
...
@@ -70,7 +70,7 @@ bool SerialLink::_isBootloader()
{
{
qCDebug
(
SerialLinkLog
)
<<
"PortName : "
<<
info
.
portName
()
<<
"Description : "
<<
info
.
description
();
qCDebug
(
SerialLinkLog
)
<<
"PortName : "
<<
info
.
portName
()
<<
"Description : "
<<
info
.
description
();
qCDebug
(
SerialLinkLog
)
<<
"Manufacturer: "
<<
info
.
manufacturer
();
qCDebug
(
SerialLinkLog
)
<<
"Manufacturer: "
<<
info
.
manufacturer
();
if
(
info
.
portName
().
trimmed
()
==
_
c
onfig
->
portName
()
&&
if
(
info
.
portName
().
trimmed
()
==
_
serialC
onfig
->
portName
()
&&
(
info
.
description
().
toLower
().
contains
(
"bootloader"
)
||
(
info
.
description
().
toLower
().
contains
(
"bootloader"
)
||
info
.
description
().
toLower
().
contains
(
"px4 bl"
)
||
info
.
description
().
toLower
().
contains
(
"px4 bl"
)
||
info
.
description
().
toLower
().
contains
(
"px4 fmu v1.6"
)))
{
info
.
description
().
toLower
().
contains
(
"px4 fmu v1.6"
)))
{
...
@@ -159,7 +159,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
...
@@ -159,7 +159,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
_port
=
NULL
;
_port
=
NULL
;
}
}
qCDebug
(
SerialLinkLog
)
<<
"SerialLink: hardwareConnect to "
<<
_
c
onfig
->
portName
();
qCDebug
(
SerialLinkLog
)
<<
"SerialLink: hardwareConnect to "
<<
_
serialC
onfig
->
portName
();
// If we are in the Pixhawk bootloader code wait for it to timeout
// If we are in the Pixhawk bootloader code wait for it to timeout
if
(
_isBootloader
())
{
if
(
_isBootloader
())
{
...
@@ -181,7 +181,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
...
@@ -181,7 +181,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
}
}
}
}
_port
=
new
QSerialPort
(
_
c
onfig
->
portName
());
_port
=
new
QSerialPort
(
_
serialC
onfig
->
portName
());
QObject
::
connect
(
_port
,
static_cast
<
void
(
QSerialPort
::*
)(
QSerialPort
::
SerialPortError
)
>
(
&
QSerialPort
::
error
),
QObject
::
connect
(
_port
,
static_cast
<
void
(
QSerialPort
::*
)(
QSerialPort
::
SerialPortError
)
>
(
&
QSerialPort
::
error
),
this
,
&
SerialLink
::
linkError
);
this
,
&
SerialLink
::
linkError
);
...
@@ -219,17 +219,17 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
...
@@ -219,17 +219,17 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString&
_port
->
setDataTerminalReady
(
true
);
_port
->
setDataTerminalReady
(
true
);
qCDebug
(
SerialLinkLog
)
<<
"Configuring port"
;
qCDebug
(
SerialLinkLog
)
<<
"Configuring port"
;
_port
->
setBaudRate
(
_
c
onfig
->
baud
());
_port
->
setBaudRate
(
_
serialC
onfig
->
baud
());
_port
->
setDataBits
(
static_cast
<
QSerialPort
::
DataBits
>
(
_
c
onfig
->
dataBits
()));
_port
->
setDataBits
(
static_cast
<
QSerialPort
::
DataBits
>
(
_
serialC
onfig
->
dataBits
()));
_port
->
setFlowControl
(
static_cast
<
QSerialPort
::
FlowControl
>
(
_
c
onfig
->
flowControl
()));
_port
->
setFlowControl
(
static_cast
<
QSerialPort
::
FlowControl
>
(
_
serialC
onfig
->
flowControl
()));
_port
->
setStopBits
(
static_cast
<
QSerialPort
::
StopBits
>
(
_
c
onfig
->
stopBits
()));
_port
->
setStopBits
(
static_cast
<
QSerialPort
::
StopBits
>
(
_
serialC
onfig
->
stopBits
()));
_port
->
setParity
(
static_cast
<
QSerialPort
::
Parity
>
(
_
c
onfig
->
parity
()));
_port
->
setParity
(
static_cast
<
QSerialPort
::
Parity
>
(
_
serialC
onfig
->
parity
()));
emit
communicationUpdate
(
getName
(),
"Opened port!"
);
emit
communicationUpdate
(
getName
(),
"Opened port!"
);
emit
connected
();
emit
connected
();
qCDebug
(
SerialLinkLog
)
<<
"Connection SeriaLink: "
<<
"with settings"
<<
_
c
onfig
->
portName
()
qCDebug
(
SerialLinkLog
)
<<
"Connection SeriaLink: "
<<
"with settings"
<<
_
serialC
onfig
->
portName
()
<<
_
c
onfig
->
baud
()
<<
_
c
onfig
->
dataBits
()
<<
_
c
onfig
->
parity
()
<<
_
c
onfig
->
stopBits
();
<<
_
serialC
onfig
->
baud
()
<<
_
serialC
onfig
->
dataBits
()
<<
_
serialC
onfig
->
parity
()
<<
_
serialC
onfig
->
stopBits
();
return
true
;
// successful connection
return
true
;
// successful connection
}
}
...
@@ -281,7 +281,7 @@ bool SerialLink::isConnected() const
...
@@ -281,7 +281,7 @@ bool SerialLink::isConnected() const
QString
SerialLink
::
getName
()
const
QString
SerialLink
::
getName
()
const
{
{
return
_
c
onfig
->
portName
();
return
_
serialC
onfig
->
portName
();
}
}
/**
/**
...
@@ -294,7 +294,7 @@ qint64 SerialLink::getConnectionSpeed() const
...
@@ -294,7 +294,7 @@ qint64 SerialLink::getConnectionSpeed() const
if
(
_port
)
{
if
(
_port
)
{
baudRate
=
_port
->
baudRate
();
baudRate
=
_port
->
baudRate
();
}
else
{
}
else
{
baudRate
=
_
c
onfig
->
baud
();
baudRate
=
_
serialC
onfig
->
baud
();
}
}
qint64
dataRate
;
qint64
dataRate
;
switch
(
baudRate
)
switch
(
baudRate
)
...
@@ -334,11 +334,11 @@ qint64 SerialLink::getConnectionSpeed() const
...
@@ -334,11 +334,11 @@ qint64 SerialLink::getConnectionSpeed() const
void
SerialLink
::
_resetConfiguration
()
void
SerialLink
::
_resetConfiguration
()
{
{
if
(
_port
)
{
if
(
_port
)
{
_port
->
setBaudRate
(
_
c
onfig
->
baud
());
_port
->
setBaudRate
(
_
serialC
onfig
->
baud
());
_port
->
setDataBits
(
static_cast
<
QSerialPort
::
DataBits
>
(
_
c
onfig
->
dataBits
()));
_port
->
setDataBits
(
static_cast
<
QSerialPort
::
DataBits
>
(
_
serialC
onfig
->
dataBits
()));
_port
->
setFlowControl
(
static_cast
<
QSerialPort
::
FlowControl
>
(
_
c
onfig
->
flowControl
()));
_port
->
setFlowControl
(
static_cast
<
QSerialPort
::
FlowControl
>
(
_
serialC
onfig
->
flowControl
()));
_port
->
setStopBits
(
static_cast
<
QSerialPort
::
StopBits
>
(
_
c
onfig
->
stopBits
()));
_port
->
setStopBits
(
static_cast
<
QSerialPort
::
StopBits
>
(
_
serialC
onfig
->
stopBits
()));
_port
->
setParity
(
static_cast
<
QSerialPort
::
Parity
>
(
_
c
onfig
->
parity
()));
_port
->
setParity
(
static_cast
<
QSerialPort
::
Parity
>
(
_
serialC
onfig
->
parity
()));
}
}
}
}
...
@@ -349,11 +349,6 @@ void SerialLink::_emitLinkError(const QString& errorMsg)
...
@@ -349,11 +349,6 @@ void SerialLink::_emitLinkError(const QString& errorMsg)
emit
communicationError
(
tr
(
"Link Error"
),
msg
.
arg
(
getName
()).
arg
(
errorMsg
));
emit
communicationError
(
tr
(
"Link Error"
),
msg
.
arg
(
getName
()).
arg
(
errorMsg
));
}
}
LinkConfiguration
*
SerialLink
::
getLinkConfiguration
()
{
return
_config
;
}
//--------------------------------------------------------------------------
//--------------------------------------------------------------------------
//-- SerialConfiguration
//-- SerialConfiguration
...
...
src/comm/SerialLink.h
View file @
76677481
...
@@ -133,7 +133,6 @@ class SerialLink : public LinkInterface
...
@@ -133,7 +133,6 @@ class SerialLink : public LinkInterface
public:
public:
// LinkInterface
// LinkInterface
LinkConfiguration
*
getLinkConfiguration
();
QString
getName
()
const
;
QString
getName
()
const
;
void
requestReset
();
void
requestReset
();
bool
isConnected
()
const
;
bool
isConnected
()
const
;
...
@@ -168,7 +167,7 @@ private slots:
...
@@ -168,7 +167,7 @@ private slots:
private:
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
SerialLink
(
S
erial
Configuration
*
config
);
SerialLink
(
S
haredLink
Configuration
Pointer
&
config
);
~
SerialLink
();
~
SerialLink
();
// From LinkInterface
// From LinkInterface
...
@@ -186,7 +185,7 @@ private:
...
@@ -186,7 +185,7 @@ private:
volatile
bool
_reqReset
;
volatile
bool
_reqReset
;
QMutex
_stoppMutex
;
// Mutex for accessing _stopp
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.
QByteArray
_transmitBuffer
;
// An internal buffer for receiving data from member functions and actually transmitting them via the serial port.
SerialConfiguration
*
_
c
onfig
;
SerialConfiguration
*
_
serialC
onfig
;
signals:
signals:
void
aboutToCloseFlag
();
void
aboutToCloseFlag
();
...
...
src/comm/TCPLink.cc
View file @
76677481
...
@@ -24,16 +24,14 @@
...
@@ -24,16 +24,14 @@
///
///
/// @author Don Gagne <don@thegagnes.com>
/// @author Don Gagne <don@thegagnes.com>
TCPLink
::
TCPLink
(
TCPConfiguration
*
config
)
TCPLink
::
TCPLink
(
SharedLinkConfigurationPointer
&
config
)
:
_config
(
config
)
:
LinkInterface
(
config
)
,
_tcpConfig
(
qobject_cast
<
TCPConfiguration
*>
(
config
.
data
()))
,
_socket
(
NULL
)
,
_socket
(
NULL
)
,
_socketIsConnected
(
false
)
,
_socketIsConnected
(
false
)
{
{
Q_ASSERT
(
_config
!=
NULL
);
Q_ASSERT
(
_tcpConfig
);
// 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/
moveToThread
(
this
);
moveToThread
(
this
);
//qDebug() << "TCP Created " << _config->name();
}
}
TCPLink
::~
TCPLink
()
TCPLink
::~
TCPLink
()
...
@@ -69,7 +67,7 @@ void TCPLink::_writeDebugBytes(const QByteArray data)
...
@@ -69,7 +67,7 @@ void TCPLink::_writeDebugBytes(const QByteArray data)
ascii
.
append
(
219
);
ascii
.
append
(
219
);
}
}
}
}
qDebug
()
<<
"Sent"
<<
size
<<
"bytes to"
<<
_
c
onfig
->
address
().
toString
()
<<
":"
<<
_
c
onfig
->
port
()
<<
"data:"
;
qDebug
()
<<
"Sent"
<<
size
<<
"bytes to"
<<
_
tcpC
onfig
->
address
().
toString
()
<<
":"
<<
_
tcpC
onfig
->
port
()
<<
"data:"
;
qDebug
()
<<
bytes
;
qDebug
()
<<
bytes
;
qDebug
()
<<
"ASCII:"
<<
ascii
;
qDebug
()
<<
"ASCII:"
<<
ascii
;
}
}
...
@@ -148,7 +146,7 @@ bool TCPLink::_hardwareConnect()
...
@@ -148,7 +146,7 @@ bool TCPLink::_hardwareConnect()
_socket
=
new
QTcpSocket
();
_socket
=
new
QTcpSocket
();
QSignalSpy
errorSpy
(
_socket
,
static_cast
<
void
(
QTcpSocket
::*
)(
QAbstractSocket
::
SocketError
)
>
(
&
QTcpSocket
::
error
));
QSignalSpy
errorSpy
(
_socket
,
static_cast
<
void
(
QTcpSocket
::*
)(
QAbstractSocket
::
SocketError
)
>
(
&
QTcpSocket
::
error
));
_socket
->
connectToHost
(
_
c
onfig
->
address
(),
_
c
onfig
->
port
());
_socket
->
connectToHost
(
_
tcpC
onfig
->
address
(),
_
tcpC
onfig
->
port
());
QObject
::
connect
(
_socket
,
&
QTcpSocket
::
readyRead
,
this
,
&
TCPLink
::
readBytes
);
QObject
::
connect
(
_socket
,
&
QTcpSocket
::
readyRead
,
this
,
&
TCPLink
::
readBytes
);
QObject
::
connect
(
_socket
,
static_cast
<
void
(
QTcpSocket
::*
)(
QAbstractSocket
::
SocketError
)
>
(
&
QTcpSocket
::
error
),
QObject
::
connect
(
_socket
,
static_cast
<
void
(
QTcpSocket
::*
)(
QAbstractSocket
::
SocketError
)
>
(
&
QTcpSocket
::
error
),
...
@@ -189,7 +187,7 @@ bool TCPLink::isConnected() const
...
@@ -189,7 +187,7 @@ bool TCPLink::isConnected() const
QString
TCPLink
::
getName
()
const
QString
TCPLink
::
getName
()
const
{
{
return
_
c
onfig
->
name
();
return
_
tcpC
onfig
->
name
();
}
}
qint64
TCPLink
::
getConnectionSpeed
()
const
qint64
TCPLink
::
getConnectionSpeed
()
const
...
...
src/comm/TCPLink.h
View file @
76677481
...
@@ -121,7 +121,6 @@ class TCPLink : public LinkInterface
...
@@ -121,7 +121,6 @@ class TCPLink : public LinkInterface
public:
public:
QTcpSocket
*
getSocket
(
void
)
{
return
_socket
;
}
QTcpSocket
*
getSocket
(
void
)
{
return
_socket
;
}
virtual
LinkConfiguration
*
getLinkConfiguration
()
{
return
_config
;
}
void
signalBytesWritten
(
void
);
void
signalBytesWritten
(
void
);
...
@@ -160,7 +159,7 @@ protected:
...
@@ -160,7 +159,7 @@ protected:
private:
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
TCPLink
(
TCP
Configuration
*
config
);
TCPLink
(
SharedLink
Configuration
Pointer
&
config
);
~
TCPLink
();
~
TCPLink
();
// From LinkInterface
// From LinkInterface
...
@@ -174,7 +173,7 @@ private:
...
@@ -174,7 +173,7 @@ private:
void
_writeDebugBytes
(
const
QByteArray
data
);
void
_writeDebugBytes
(
const
QByteArray
data
);
#endif
#endif
TCPConfiguration
*
_
c
onfig
;
TCPConfiguration
*
_
tcpC
onfig
;
QTcpSocket
*
_socket
;
QTcpSocket
*
_socket
;
bool
_socketIsConnected
;
bool
_socketIsConnected
;
...
...
src/comm/UDPLink.cc
View file @
76677481
...
@@ -65,27 +65,22 @@ static QString get_ip_address(const QString& address)
...
@@ -65,27 +65,22 @@ static QString get_ip_address(const QString& address)
return
QString
(
""
);
return
QString
(
""
);
}
}
UDPLink
::
UDPLink
(
UDPConfiguration
*
config
)
UDPLink
::
UDPLink
(
SharedLinkConfigurationPointer
&
config
)
:
_socket
(
NULL
)
:
LinkInterface
(
config
)
,
_connectState
(
false
)
#if defined(QGC_ZEROCONF_ENABLED)
#if defined(QGC_ZEROCONF_ENABLED)
,
_dnssServiceRef
(
NULL
)
,
_dnssServiceRef
(
NULL
)
#endif
#endif
,
_running
(
false
)
,
_running
(
false
)
,
_socket
(
NULL
)
,
_udpConfig
(
qobject_cast
<
UDPConfiguration
*>
(
config
.
data
()))
,
_connectState
(
false
)
{
{
Q_ASSERT
(
config
!=
NULL
);
Q_ASSERT
(
_udpConfig
);
_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/
moveToThread
(
this
);
moveToThread
(
this
);
}
}
UDPLink
::~
UDPLink
()
UDPLink
::~
UDPLink
()
{
{
// Disconnect link from configuration
_config
->
setLink
(
NULL
);
_disconnect
();
_disconnect
();
// Tell the thread to exit
// Tell the thread to exit
_running
=
false
;
_running
=
false
;
...
@@ -121,17 +116,17 @@ void UDPLink::_restartConnection()
...
@@ -121,17 +116,17 @@ void UDPLink::_restartConnection()
QString
UDPLink
::
getName
()
const
QString
UDPLink
::
getName
()
const
{
{
return
_
c
onfig
->
name
();
return
_
udpC
onfig
->
name
();
}
}
void
UDPLink
::
addHost
(
const
QString
&
host
)
void
UDPLink
::
addHost
(
const
QString
&
host
)
{
{
_
c
onfig
->
addHost
(
host
);
_
udpC
onfig
->
addHost
(
host
);
}
}
void
UDPLink
::
removeHost
(
const
QString
&
host
)
void
UDPLink
::
removeHost
(
const
QString
&
host
)
{
{
_
c
onfig
->
removeHost
(
host
);
_
udpC
onfig
->
removeHost
(
host
);
}
}
void
UDPLink
::
_writeBytes
(
const
QByteArray
data
)
void
UDPLink
::
_writeBytes
(
const
QByteArray
data
)
...
@@ -143,7 +138,7 @@ void UDPLink::_writeBytes(const QByteArray data)
...
@@ -143,7 +138,7 @@ void UDPLink::_writeBytes(const QByteArray data)
// Send to all connected systems
// Send to all connected systems
QString
host
;
QString
host
;
int
port
;
int
port
;
if
(
_
c
onfig
->
firstHost
(
host
,
port
))
{
if
(
_
udpC
onfig
->
firstHost
(
host
,
port
))
{
do
{
do
{
QHostAddress
currentHost
(
host
);
QHostAddress
currentHost
(
host
);
if
(
_socket
->
writeDatagram
(
data
,
currentHost
,
(
quint16
)
port
)
<
0
)
{
if
(
_socket
->
writeDatagram
(
data
,
currentHost
,
(
quint16
)
port
)
<
0
)
{
...
@@ -162,10 +157,10 @@ void UDPLink::_writeBytes(const QByteArray data)
...
@@ -162,10 +157,10 @@ void UDPLink::_writeBytes(const QByteArray data)
// unit sent by UDP.
// unit sent by UDP.
_logOutputDataRate
(
data
.
size
(),
QDateTime
::
currentMSecsSinceEpoch
());
_logOutputDataRate
(
data
.
size
(),
QDateTime
::
currentMSecsSinceEpoch
());
}
}
}
while
(
_
c
onfig
->
nextHost
(
host
,
port
));
}
while
(
_
udpC
onfig
->
nextHost
(
host
,
port
));
//-- Remove hosts that are no longer there
//-- Remove hosts that are no longer there
foreach
(
const
QString
&
ghost
,
goneHosts
)
{
foreach
(
const
QString
&
ghost
,
goneHosts
)
{
_
c
onfig
->
removeHost
(
ghost
);
_
udpC
onfig
->
removeHost
(
ghost
);
}
}
}
}
}
}
...
@@ -194,7 +189,7 @@ void UDPLink::readBytes()
...
@@ -194,7 +189,7 @@ void UDPLink::readBytes()
// added to the list and will start receiving datagrams from here. Even a port scanner
// added to the list and will start receiving datagrams from here. Even a port scanner
// would trigger this.
// would trigger this.
// Add host to broadcast list if not yet present, or update its port
// Add host to broadcast list if not yet present, or update its port
_
c
onfig
->
addHost
(
sender
.
toString
(),
(
int
)
senderPort
);
_
udpC
onfig
->
addHost
(
sender
.
toString
(),
(
int
)
senderPort
);
}
}
//-- Send whatever is left
//-- Send whatever is left
if
(
databuffer
.
size
())
{
if
(
databuffer
.
size
())
{
...
@@ -248,7 +243,7 @@ bool UDPLink::_hardwareConnect()
...
@@ -248,7 +243,7 @@ bool UDPLink::_hardwareConnect()
QHostAddress
host
=
QHostAddress
::
AnyIPv4
;
QHostAddress
host
=
QHostAddress
::
AnyIPv4
;
_socket
=
new
QUdpSocket
();
_socket
=
new
QUdpSocket
();
_socket
->
setProxy
(
QNetworkProxy
::
NoProxy
);
_socket
->
setProxy
(
QNetworkProxy
::
NoProxy
);
_connectState
=
_socket
->
bind
(
host
,
_
c
onfig
->
localPort
(),
QAbstractSocket
::
ReuseAddressHint
|
QUdpSocket
::
ShareAddress
);
_connectState
=
_socket
->
bind
(
host
,
_
udpC
onfig
->
localPort
(),
QAbstractSocket
::
ReuseAddressHint
|
QUdpSocket
::
ShareAddress
);
if
(
_connectState
)
{
if
(
_connectState
)
{
//-- Make sure we have a large enough IO buffers
//-- Make sure we have a large enough IO buffers
#ifdef __mobile__
#ifdef __mobile__
...
@@ -258,7 +253,7 @@ bool UDPLink::_hardwareConnect()
...
@@ -258,7 +253,7 @@ bool UDPLink::_hardwareConnect()
_socket
->
setSocketOption
(
QAbstractSocket
::
SendBufferSizeSocketOption
,
256
*
1024
);
_socket
->
setSocketOption
(
QAbstractSocket
::
SendBufferSizeSocketOption
,
256
*
1024
);
_socket
->
setSocketOption
(
QAbstractSocket
::
ReceiveBufferSizeSocketOption
,
512
*
1024
);
_socket
->
setSocketOption
(
QAbstractSocket
::
ReceiveBufferSizeSocketOption
,
512
*
1024
);
#endif
#endif
_registerZeroconf
(
_
c
onfig
->
localPort
(),
kZeroconfRegistration
);
_registerZeroconf
(
_
udpC
onfig
->
localPort
(),
kZeroconfRegistration
);
QObject
::
connect
(
_socket
,
&
QUdpSocket
::
readyRead
,
this
,
&
UDPLink
::
readBytes
);
QObject
::
connect
(
_socket
,
&
QUdpSocket
::
readyRead
,
this
,
&
UDPLink
::
readBytes
);
emit
connected
();
emit
connected
();
}
else
{
}
else
{
...
...
Prev
1
2
Next
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment