Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
76677481
Commit
76677481
authored
Dec 26, 2016
by
Don Gagne
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Reference counting for LinkInterface, LinkConfiguration
Prevent shutdown ordering crashes
parent
5040e3d9
Changes
25
Expand all
Show whitespace changes
Inline
Side-by-side
Showing
25 changed files
with
487 additions
and
360 deletions
+487
-360
qgroundcontrol.pro
qgroundcontrol.pro
+1
-0
QGroundControlQmlGlobal.cc
src/QmlControls/QGroundControlQmlGlobal.cc
+2
-2
MultiVehicleManager.cc
src/Vehicle/MultiVehicleManager.cc
+3
-3
Vehicle.cc
src/Vehicle/Vehicle.cc
+27
-9
Vehicle.h
src/Vehicle/Vehicle.h
+6
-2
LinkConfiguration.h
src/comm/LinkConfiguration.h
+3
-2
LinkInterface.cc
src/comm/LinkInterface.cc
+157
-0
LinkInterface.h
src/comm/LinkInterface.h
+17
-105
LinkManager.cc
src/comm/LinkManager.cc
+131
-70
LinkManager.h
src/comm/LinkManager.h
+29
-22
LogReplayLink.cc
src/comm/LogReplayLink.cc
+7
-6
LogReplayLink.h
src/comm/LogReplayLink.h
+2
-4
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+1
-1
MockLink.cc
src/comm/MockLink.cc
+11
-13
MockLink.h
src/comm/MockLink.h
+1
-5
SerialLink.cc
src/comm/SerialLink.cc
+32
-37
SerialLink.h
src/comm/SerialLink.h
+2
-3
TCPLink.cc
src/comm/TCPLink.cc
+7
-9
TCPLink.h
src/comm/TCPLink.h
+2
-3
UDPLink.cc
src/comm/UDPLink.cc
+17
-22
UDPLink.h
src/comm/UDPLink.h
+4
-16
LinkManagerTest.cc
src/qgcunittest/LinkManagerTest.cc
+7
-7
TCPLinkTest.cc
src/qgcunittest/TCPLinkTest.cc
+9
-13
TCPLinkTest.h
src/qgcunittest/TCPLinkTest.h
+5
-5
QGCMAVLinkLogPlayer.cc
src/ui/QGCMAVLinkLogPlayer.cc
+4
-1
No files found.
qgroundcontrol.pro
View file @
76677481
...
...
@@ -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 \
...
...
src/QmlControls/QGroundControlQmlGlobal.cc
View file @
76677481
...
...
@@ -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
)
{
...
...
src/Vehicle/MultiVehicleManager.cc
View file @
76677481
...
...
@@ -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
<
link
s
->
count
();
i
++
)
{
LinkInterface
*
link
=
link
s
->
value
<
LinkInterface
*>
(
i
)
;
LinkManager
*
linkMgr
=
_toolbox
->
linkManager
();
for
(
int
i
=
0
;
i
<
link
Mgr
->
links
().
count
();
i
++
)
{
LinkInterface
*
link
=
link
Mgr
->
links
()[
i
]
;
if
(
link
->
isConnected
())
{
mavlink_message_t
message
;
mavlink_msg_heartbeat_pack_chan
(
_mavlinkProtocol
->
getSystemId
(),
...
...
src/Vehicle/Vehicle.cc
View file @
76677481
...
...
@@ -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
*
pLinkC
onfig
=
pSerialLink
->
getLinkConfiguration
();
if
(
pLinkC
onfig
)
{
SerialConfiguration
*
pSerialConfig
=
qobject_cast
<
SerialConfiguration
*>
(
pLinkC
onfig
);
LinkConfiguration
*
c
onfig
=
pSerialLink
->
getLinkConfiguration
();
if
(
c
onfig
)
{
SerialConfiguration
*
pSerialConfig
=
qobject_cast
<
SerialConfiguration
*>
(
c
onfig
);
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
)
...
...
src/Vehicle/Vehicle.h
View file @
76677481
...
...
@@ -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
;
...
...
src/comm/LinkConfiguration.h
View file @
76677481
...
...
@@ -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
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 @@
#include <QDebug>
#include "QGCMAVLink.h"
#include "LinkConfiguration.h"
class
LinkManager
;
class
LinkConfiguration
;
/**
* The link interface defines the interface for all links used to communicate
...
...
@@ -36,17 +36,15 @@ class LinkInterface : public QThread
friend
class
LinkManager
;
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
);
}
/**
* @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
.
0
f
/
((
float
)
totalTime
/
1000
.
0
f
))
:
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
>
SharedLinkInterface
Pointer
;
#endif // _LINKINTERFACE_H_
src/comm/LinkManager.cc
View file @
76677481
This diff is collapsed.
Click to expand it.
src/comm/LinkManager.h
View file @
76677481
...
...
@@ -69,17 +69,10 @@ public:
Q_PROPERTY
(
bool
autoconnectLibrePilot
READ
autoconnectLibrePilot
WRITE
setAutoconnectLibrePilot
NOTIFY
autoconnectLibrePilotChanged
)
Q_PROPERTY
(
bool
isBluetoothAvailable
READ
isBluetoothAvailable
CONSTANT
)
/// LinkInterface Accessor
Q_PROPERTY
(
QmlObjectListModel
*
links
READ
links
CONSTANT
)
/// LinkConfiguration Accessor
Q_PROPERTY
(
QmlObjectListModel
*
linkConfigurations
READ
linkConfigurations
NOTIFY
linkConfigurationsChanged
)
/// List of comm type strings
Q_PROPERTY
(
QmlObjectListModel
*
linkConfigurations
READ
_qmlLinkConfigurations
NOTIFY
linkConfigurationsChanged
)
Q_PROPERTY
(
QStringList
linkTypeStrings
READ
linkTypeStrings
CONSTANT
)
/// List of supported baud rates for serial links
Q_PROPERTY
(
QStringList
serialBaudRates
READ
serialBaudRates
CONSTANT
)
/// List of comm ports display names
Q_PROPERTY
(
QStringList
serialPortStrings
READ
serialPortStrings
NOTIFY
commPortStringsChanged
)
/// List of comm ports
Q_PROPERTY
(
QStringList
serialPorts
READ
serialPorts
NOTIFY
commPortsChanged
)
// Create/Edit Link Configuration
...
...
@@ -100,8 +93,7 @@ public:
bool
autoconnectLibrePilot
(
void
)
{
return
_autoconnectLibrePilot
;
}
bool
isBluetoothAvailable
(
void
);
QmlObjectListModel
*
links
(
void
)
{
return
&
_links
;
}
QmlObjectListModel
*
linkConfigurations
(
void
)
{
return
&
_linkConfigurations
;
}
QList
<
LinkInterface
*>
links
(
void
);
QStringList
linkTypeStrings
(
void
)
const
;
QStringList
serialBaudRates
(
void
);
QStringList
serialPortStrings
(
void
);
...
...
@@ -132,7 +124,7 @@ public:
/// Creates, connects (and adds) a link based on the given configuration instance.
/// Link takes ownership of config.
Q_INVOKABLE
LinkInterface
*
createConnectedLink
(
LinkConfiguration
*
config
);
Q_INVOKABLE
LinkInterface
*
createConnectedLink
(
SharedLinkConfigurationPointer
&
config
);
/// Creates, connects (and adds) a link based on the given configuration name.
LinkInterface
*
createConnectedLink
(
const
QString
&
name
);
...
...
@@ -165,6 +157,17 @@ public:
// Override from QGCTool
virtual
void
setToolbox
(
QGCToolbox
*
toolbox
);
/// @return This mavlink channel is never assigned to a vehicle.
uint8_t
reservedMavlinkChannel
(
void
)
{
return
0
;
}
/// If you are going to hold a reference to a LinkInterface* in your object you must reference count it
/// by using this method to get access to the shared pointer.
SharedLinkInterfacePointer
sharedLinkInterfacePointerForLink
(
LinkInterface
*
link
);
bool
containsLink
(
LinkInterface
*
link
);
SharedLinkConfigurationPointer
addConfiguration
(
LinkConfiguration
*
config
);
signals:
void
autoconnectUDPChanged
(
bool
autoconnect
);
void
autoconnectPixhawkChanged
(
bool
autoconnect
);
...
...
@@ -204,11 +207,13 @@ private slots:
#endif
private:
QmlObjectListModel
*
_qmlLinkConfigurations
(
void
)
{
return
&
_qmlConfigurations
;
}
bool
_connectionsSuspendedMsg
(
void
);
void
_updateAutoConnectLinks
(
void
);
void
_updateSerialPorts
();
void
_fixUnnamed
(
LinkConfiguration
*
config
);
bool
_setAutoconnectWorker
(
bool
&
currentAutoconnect
,
bool
newAutoconnect
,
const
char
*
autoconnectKey
);
void
_removeConfiguration
(
LinkConfiguration
*
config
);
#ifndef NO_SERIAL_LINK
SerialConfiguration
*
_autoconnectConfigurationsContainsPort
(
const
QString
&
portName
);
...
...
@@ -223,9 +228,11 @@ private:
MAVLinkProtocol
*
_mavlinkProtocol
;
QmlObjectListModel
_links
;
QmlObjectListModel
_linkConfigurations
;
QmlObjectListModel
_autoconnectConfigurations
;
QList
<
SharedLinkInterfacePointer
>
_sharedLinks
;
QList
<
SharedLinkConfigurationPointer
>
_sharedConfigurations
;
QList
<
SharedLinkConfigurationPointer
>
_sharedAutoconnectConfigurations
;
QmlObjectListModel
_qmlConfigurations
;
QMap
<
QString
,
int
>
_autoconnectWaitList
;
///< key: QGCSerialPortInfo.systemLocation, value: wait count
QStringList
_commPortList
;
...
...
src/comm/LogReplayLink.cc
View file @
76677481
...
...
@@ -64,12 +64,13 @@ QString LogReplayLinkConfiguration::logFilenameShort(void)
return
fi
.
fileName
();
}
LogReplayLink
::
LogReplayLink
(
LogReplayLinkConfiguration
*
config
)
:
_connected
(
false
),
_replayAccelerationFactor
(
1.0
f
)
LogReplayLink
::
LogReplayLink
(
SharedLinkConfigurationPointer
&
config
)
:
LinkInterface
(
config
)
,
_logReplayConfig
(
qobject_cast
<
LogReplayLinkConfiguration
*>
(
config
.
data
()))
,
_connected
(
false
)
,
_replayAccelerationFactor
(
1.0
f
)
{
Q_ASSERT
(
config
);
_config
=
config
;
Q_ASSERT
(
_logReplayConfig
);
_readTickTimer
.
moveToThread
(
this
);
...
...
@@ -184,7 +185,7 @@ quint64 LogReplayLink::_seekToNextMavlinkMessage(mavlink_message_t* nextMsg)
bool
LogReplayLink
::
_loadLogFile
(
void
)
{
QString
errorMsg
;
QString
logFilename
=
_
c
onfig
->
logFilename
();
QString
logFilename
=
_
logReplayC
onfig
->
logFilename
();
QFileInfo
logFileInfo
;
int
logDurationSecondsTotal
;
...
...
src/comm/LogReplayLink.h
View file @
76677481
...
...
@@ -57,8 +57,6 @@ class LogReplayLink : public LinkInterface
friend
class
LinkManager
;
public:
virtual
LinkConfiguration
*
getLinkConfiguration
()
{
return
_config
;
}
/// @return true: log is currently playing, false: log playback is paused
bool
isPlaying
(
void
)
{
return
_readTickTimer
.
isActive
();
}
...
...
@@ -111,7 +109,7 @@ private slots:
private:
// Links are only created/destroyed by LinkManager so constructor/destructor is not public
LogReplayLink
(
LogReplayLinkConfiguration
*
config
);
LogReplayLink
(
SharedLinkConfigurationPointer
&
config
);
~
LogReplayLink
();
void
_replayError
(
const
QString
&
errorMsg
);
...
...
@@ -129,7 +127,7 @@ private:
// Virtuals from QThread
virtual
void
run
(
void
);
LogReplayLinkConfiguration
*
_
c
onfig
;
LogReplayLinkConfiguration
*
_
logReplayC
onfig
;
bool
_connected
;
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)
// Since receiveBytes signals cross threads we can end up with signals in the queue
// that come through after the link is disconnected. For these we just drop the data
// since the link is closed.
if
(
!
_linkMgr
->
links
()
->
contains
(
link
))
{
if
(
!
_linkMgr
->
containsLink
(
link
))
{
return
;
}
...
...
src/comm/MockLink.cc
View file @
76677481
...
...
@@ -44,8 +44,9 @@ const char* MockConfiguration::_vehicleTypeKey = "VehicleType";
const
char
*
MockConfiguration
::
_sendStatusTextKey
=
"SendStatusText"
;
const
char
*
MockConfiguration
::
_failureModeKey
=
"FailureMode"
;
MockLink
::
MockLink
(
MockConfiguration
*
config
)
:
_missionItemHandler
(
this
,
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
())
MockLink
::
MockLink
(
SharedLinkConfigurationPointer
&
config
)
:
LinkInterface
(
config
)
,
_missionItemHandler
(
this
,
qgcApp
()
->
toolbox
()
->
mavlinkProtocol
())
,
_name
(
"MockLink"
)
,
_connected
(
false
)
,
_vehicleSystemId
(
_nextVehicleSystemId
++
)
...
...
@@ -67,14 +68,11 @@ MockLink::MockLink(MockConfiguration* config)
,
_logDownloadCurrentOffset
(
0
)
,
_logDownloadBytesRemaining
(
0
)
{
_config
=
config
;
if
(
_config
)
{
_firmwareType
=
config
->
firmwareType
();
_vehicleType
=
config
->
vehicleType
();
_sendStatusText
=
config
->
sendStatusText
();
_failureMode
=
config
->
failureMode
();
_config
->
setLink
(
this
);
}
MockConfiguration
*
mockConfig
=
qobject_cast
<
MockConfiguration
*>
(
_config
.
data
());
_firmwareType
=
mockConfig
->
firmwareType
();
_vehicleType
=
mockConfig
->
vehicleType
();
_sendStatusText
=
mockConfig
->
sendStatusText
();
_failureMode
=
mockConfig
->
failureMode
();
union
px4_custom_mode
px4_cm
;
...
...
@@ -1041,12 +1039,12 @@ void MockConfiguration::updateSettings()
MockLink
*
MockLink
::
_startMockLink
(
MockConfiguration
*
mockConfig
)
{
LinkManager
*
linkM
anage
r
=
qgcApp
()
->
toolbox
()
->
linkManager
();
LinkManager
*
linkM
g
r
=
qgcApp
()
->
toolbox
()
->
linkManager
();
mockConfig
->
setDynamic
(
true
);
linkManager
->
linkConfigurations
()
->
append
(
mockConfig
);
SharedLinkConfigurationPointer
config
=
linkMgr
->
addConfiguration
(
mockConfig
);
return
qobject_cast
<
MockLink
*>
(
linkM
anager
->
createConnectedLink
(
mockC
onfig
));
return
qobject_cast
<
MockLink
*>
(
linkM
gr
->
createConnectedLink
(
c
onfig
));
}
MockLink
*
MockLink
::
startPX4MockLink
(
bool
sendStatusText
,
MockConfiguration
::
FailureMode_t
failureMode
)
...
...
src/comm/MockLink.h
View file @
76677481
...
...
@@ -90,8 +90,7 @@ class MockLink : public LinkInterface
Q_OBJECT
public:
// LinkConfiguration is optional for MockLink
MockLink
(
MockConfiguration
*
config
=
NULL
);
MockLink
(
SharedLinkConfigurationPointer
&
config
);
~
MockLink
(
void
);
// MockLink methods
...
...
@@ -126,8 +125,6 @@ public:
bool
connect
(
void
);
bool
disconnect
(
void
);
LinkConfiguration
*
getLinkConfiguration
()
{
return
_config
;
}
/// Sets a failure mode for unit testing
/// @param failureMode Type of failure to simulate
void
setMissionItemFailureMode
(
MockLinkMissionItemHandler
::
FailureMode_t
failureMode
);
...
...
@@ -216,7 +213,6 @@ private:
uint32_t
_mavCustomMode
;
uint8_t
_mavState
;
MockConfiguration
*
_config
;
MAV_AUTOPILOT
_firmwareType
;
MAV_TYPE
_vehicleType
;
...
...
src/comm/SerialLink.cc
View file @
76677481
...
...
@@ -30,19 +30,19 @@ QGC_LOGGING_CATEGORY(SerialLinkLog, "SerialLinkLog")
static
QStringList
kSupportedBaudRates
;