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
d4124c20
Commit
d4124c20
authored
Jun 26, 2018
by
acfloria
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Use any mavlink message instead of only the heartbeat to determine if the link is active
parent
348091b3
Changes
7
Hide whitespace changes
Inline
Side-by-side
Showing
7 changed files
with
52 additions
and
56 deletions
+52
-56
qgroundcontrol.pro
qgroundcontrol.pro
+2
-2
LinkInterface.cc
src/comm/LinkInterface.cc
+15
-15
LinkInterface.h
src/comm/LinkInterface.h
+9
-9
LinkManager.cc
src/comm/LinkManager.cc
+3
-7
LinkManager.h
src/comm/LinkManager.h
+1
-1
MavlinkMessagesTimer.cc
src/comm/MavlinkMessagesTimer.cc
+10
-10
MavlinkMessagesTimer.h
src/comm/MavlinkMessagesTimer.h
+12
-12
No files found.
qgroundcontrol.pro
View file @
d4124c20
...
...
@@ -393,14 +393,14 @@ HEADERS += \
src
/
api
/
QGCOptions
.
h
\
src
/
api
/
QGCSettings
.
h
\
src
/
api
/
QmlComponentInfo
.
h
\
src
/
comm
/
Heartbeat
Timer
.
h
src
/
comm
/
MavlinkMessages
Timer
.
h
SOURCES
+=
\
src
/
api
/
QGCCorePlugin
.
cc
\
src
/
api
/
QGCOptions
.
cc
\
src
/
api
/
QGCSettings
.
cc
\
src
/
api
/
QmlComponentInfo
.
cc
\
src
/
comm
/
Heartbeat
Timer
.
cc
src
/
comm
/
MavlinkMessages
Timer
.
cc
#
#
Unit
Test
specific
configuration
goes
here
(
requires
full
debug
build
with
all
plugins
)
...
...
src/comm/LinkInterface.cc
View file @
d4124c20
...
...
@@ -12,7 +12,7 @@
bool
LinkInterface
::
active
()
const
{
QMapIterator
<
int
/* vehicle id */
,
HeartbeatTimer
*>
iter
(
_heartbeat
Timers
);
QMapIterator
<
int
/* vehicle id */
,
MavlinkMessagesTimer
*>
iter
(
_mavlinkMessages
Timers
);
while
(
iter
.
hasNext
())
{
iter
.
next
();
if
(
iter
.
value
()
->
getActive
())
{
...
...
@@ -25,8 +25,8 @@ bool LinkInterface::active() const
bool
LinkInterface
::
link_active
(
int
vehicle_id
)
const
{
if
(
_
heartbeat
Timers
.
contains
(
vehicle_id
))
{
return
_
heartbeat
Timers
.
value
(
vehicle_id
)
->
getActive
();
if
(
_
mavlinkMessages
Timers
.
contains
(
vehicle_id
))
{
return
_
mavlinkMessages
Timers
.
value
(
vehicle_id
)
->
getActive
();
}
else
{
return
false
;
}
...
...
@@ -189,24 +189,24 @@ void LinkInterface::_activeChanged(bool active, int vehicle_id)
emit
activeChanged
(
this
,
active
,
vehicle_id
);
}
void
LinkInterface
::
start
Heartbeat
Timer
(
int
vehicle_id
)
{
if
(
_
heartbeat
Timers
.
contains
(
vehicle_id
))
{
_
heartbeat
Timers
.
value
(
vehicle_id
)
->
restartTimer
();
void
LinkInterface
::
start
MavlinkMessages
Timer
(
int
vehicle_id
)
{
if
(
_
mavlinkMessages
Timers
.
contains
(
vehicle_id
))
{
_
mavlinkMessages
Timers
.
value
(
vehicle_id
)
->
restartTimer
();
}
else
{
_
heartbeatTimers
.
insert
(
vehicle_id
,
new
Heartbeat
Timer
(
vehicle_id
,
_highLatency
));
QObject
::
connect
(
_
heartbeatTimers
.
value
(
vehicle_id
),
&
Heartbeat
Timer
::
activeChanged
,
this
,
&
LinkInterface
::
_activeChanged
);
_
heartbeat
Timers
.
value
(
vehicle_id
)
->
init
();
_
mavlinkMessagesTimers
.
insert
(
vehicle_id
,
new
MavlinkMessages
Timer
(
vehicle_id
,
_highLatency
));
QObject
::
connect
(
_
mavlinkMessagesTimers
.
value
(
vehicle_id
),
&
MavlinkMessages
Timer
::
activeChanged
,
this
,
&
LinkInterface
::
_activeChanged
);
_
mavlinkMessages
Timers
.
value
(
vehicle_id
)
->
init
();
}
}
void
LinkInterface
::
stop
Heartbeat
Timer
()
{
QMapIterator
<
int
/* vehicle id */
,
HeartbeatTimer
*>
iter
(
_heartbeat
Timers
);
void
LinkInterface
::
stop
MavlinkMessages
Timer
()
{
QMapIterator
<
int
/* vehicle id */
,
MavlinkMessagesTimer
*>
iter
(
_mavlinkMessages
Timers
);
while
(
iter
.
hasNext
())
{
iter
.
next
();
QObject
::
disconnect
(
iter
.
value
(),
&
Heartbeat
Timer
::
activeChanged
,
this
,
&
LinkInterface
::
_activeChanged
);
_
heartbeat
Timers
[
iter
.
key
()]
->
deleteLater
();
_
heartbeat
Timers
[
iter
.
key
()]
=
nullptr
;
QObject
::
disconnect
(
iter
.
value
(),
&
MavlinkMessages
Timer
::
activeChanged
,
this
,
&
LinkInterface
::
_activeChanged
);
_
mavlinkMessages
Timers
[
iter
.
key
()]
->
deleteLater
();
_
mavlinkMessages
Timers
[
iter
.
key
()]
=
nullptr
;
}
_
heartbeat
Timers
.
clear
();
_
mavlinkMessages
Timers
.
clear
();
}
src/comm/LinkInterface.h
View file @
d4124c20
...
...
@@ -21,7 +21,7 @@
#include "QGCMAVLink.h"
#include "LinkConfiguration.h"
#include "
Heartbeat
Timer.h"
#include "
MavlinkMessages
Timer.h"
class
LinkManager
;
...
...
@@ -39,7 +39,7 @@ class LinkInterface : public QThread
public:
virtual
~
LinkInterface
()
{
stop
Heartbeat
Timer
();
stop
MavlinkMessages
Timer
();
_config
->
setLink
(
NULL
);
}
...
...
@@ -264,19 +264,19 @@ private:
void
_setMavlinkChannel
(
uint8_t
channel
);
/**
* @brief start
Heartbeat
Timer
* @brief start
MavlinkMessages
Timer
*
* Start/restart the
heartbeat
timer for the specific vehicle.
* Start/restart the
mavlink messages
timer for the specific vehicle.
* If no timer exists an instance is allocated.
*/
void
start
Heartbeat
Timer
(
int
vehicle_id
);
void
start
MavlinkMessages
Timer
(
int
vehicle_id
);
/**
* @brief stop
Heartbeat
Timer
* @brief stop
MavlinkMessages
Timer
*
* Stop and deallocate the
heartbeat
timers for all vehicles if any exists.
* Stop and deallocate the
mavlink messages
timers for all vehicles if any exists.
*/
void
stop
Heartbeat
Timer
();
void
stop
MavlinkMessages
Timer
();
bool
_mavlinkChannelSet
;
///< true: _mavlinkChannel has been set
uint8_t
_mavlinkChannel
;
///< mavlink channel to use for this link, as used by mavlink_parse_char
...
...
@@ -303,7 +303,7 @@ private:
bool
_decodedFirstMavlinkPacket
;
///< true: link has correctly decoded it's first mavlink packet
bool
_isPX4Flow
;
QMap
<
int
/* vehicle id */
,
HeartbeatTimer
*>
_heartbeat
Timers
;
QMap
<
int
/* vehicle id */
,
MavlinkMessagesTimer
*>
_mavlinkMessages
Timers
;
};
typedef
QSharedPointer
<
LinkInterface
>
SharedLinkInterfacePointer
;
...
...
src/comm/LinkManager.cc
View file @
d4124c20
...
...
@@ -80,7 +80,7 @@ void LinkManager::setToolbox(QGCToolbox *toolbox)
_autoConnectSettings
=
toolbox
->
settingsManager
()
->
autoConnectSettings
();
_mavlinkProtocol
=
_toolbox
->
mavlinkProtocol
();
connect
(
_mavlinkProtocol
,
&
MAVLinkProtocol
::
vehicleHeartbeatInfo
,
this
,
&
LinkManager
::
_heartbeat
Received
);
connect
(
_mavlinkProtocol
,
&
MAVLinkProtocol
::
messageReceived
,
this
,
&
LinkManager
::
_mavlinkMessage
Received
);
connect
(
&
_portListTimer
,
&
QTimer
::
timeout
,
this
,
&
LinkManager
::
_updateAutoConnectLinks
);
_portListTimer
.
start
(
_autoconnectUpdateTimerMSecs
);
// timeout must be long enough to get past bootloader on second pass
...
...
@@ -1008,10 +1008,6 @@ void LinkManager::_freeMavlinkChannel(int channel)
_mavlinkChannelsUsedBitMask
&=
~
(
1
<<
channel
);
}
void
LinkManager
::
_heartbeatReceived
(
LinkInterface
*
link
,
int
vehicleId
,
int
componentId
,
int
vehicleFirmwareType
,
int
vehicleType
)
{
Q_UNUSED
(
componentId
);
Q_UNUSED
(
vehicleFirmwareType
);
Q_UNUSED
(
vehicleType
);
link
->
startHeartbeatTimer
(
vehicleId
);
void
LinkManager
::
_mavlinkMessageReceived
(
LinkInterface
*
link
,
mavlink_message_t
message
)
{
link
->
startMavlinkMessagesTimer
(
message
.
sysid
);
}
src/comm/LinkManager.h
View file @
d4124c20
...
...
@@ -204,7 +204,7 @@ private:
SerialConfiguration
*
_autoconnectConfigurationsContainsPort
(
const
QString
&
portName
);
#endif
void
_
heartbeatReceived
(
LinkInterface
*
link
,
int
vehicleId
,
int
componentId
,
int
vehicleFirmwareType
,
int
vehicleTyp
e
);
void
_
mavlinkMessageReceived
(
LinkInterface
*
link
,
mavlink_message_t
messag
e
);
bool
_configUpdateSuspended
;
///< true: stop updating configuration list
bool
_configurationsLoaded
;
///< true: Link configurations have been loaded
...
...
src/comm/
Heartbeat
Timer.cc
→
src/comm/
MavlinkMessages
Timer.cc
View file @
d4124c20
...
...
@@ -7,11 +7,11 @@
*
****************************************************************************/
#include "
Heartbeat
Timer.h"
#include "
MavlinkMessages
Timer.h"
#include <QDebug>
HeartbeatTimer
::
Heartbeat
Timer
(
int
vehicle_id
,
bool
high_latency
)
:
MavlinkMessagesTimer
::
MavlinkMessages
Timer
(
int
vehicle_id
,
bool
high_latency
)
:
_active
(
true
),
_timer
(
new
QTimer
),
_vehicleID
(
vehicle_id
),
...
...
@@ -19,22 +19,22 @@ HeartbeatTimer::HeartbeatTimer(int vehicle_id, bool high_latency) :
{
}
void
Heartbeat
Timer
::
init
()
void
MavlinkMessages
Timer
::
init
()
{
if
(
!
_high_latency
)
{
_timer
->
setInterval
(
_
heartbeat
ReceivedTimeoutMSecs
);
_timer
->
setSingleShot
(
tru
e
);
_timer
->
setInterval
(
_
message
ReceivedTimeoutMSecs
);
_timer
->
setSingleShot
(
fals
e
);
_timer
->
start
();
}
emit
activeChanged
(
true
,
_vehicleID
);
QObject
::
connect
(
_timer
,
&
QTimer
::
timeout
,
this
,
&
Heartbeat
Timer
::
timerTimeout
);
QObject
::
connect
(
_timer
,
&
QTimer
::
timeout
,
this
,
&
MavlinkMessages
Timer
::
timerTimeout
);
}
HeartbeatTimer
::~
Heartbeat
Timer
()
MavlinkMessagesTimer
::~
MavlinkMessages
Timer
()
{
if
(
_timer
)
{
QObject
::
disconnect
(
_timer
,
&
QTimer
::
timeout
,
this
,
&
Heartbeat
Timer
::
timerTimeout
);
QObject
::
disconnect
(
_timer
,
&
QTimer
::
timeout
,
this
,
&
MavlinkMessages
Timer
::
timerTimeout
);
_timer
->
stop
();
delete
_timer
;
_timer
=
nullptr
;
...
...
@@ -43,7 +43,7 @@ HeartbeatTimer::~HeartbeatTimer()
emit
activeChanged
(
false
,
_vehicleID
);
}
void
Heartbeat
Timer
::
restartTimer
()
void
MavlinkMessages
Timer
::
restartTimer
()
{
if
(
!
_active
)
{
_active
=
true
;
...
...
@@ -53,7 +53,7 @@ void HeartbeatTimer::restartTimer()
_timer
->
start
();
}
void
Heartbeat
Timer
::
timerTimeout
()
void
MavlinkMessages
Timer
::
timerTimeout
()
{
if
(
!
_high_latency
)
{
if
(
_active
)
{
...
...
src/comm/
Heartbeat
Timer.h
→
src/comm/
MavlinkMessages
Timer.h
View file @
d4124c20
...
...
@@ -7,34 +7,34 @@
*
****************************************************************************/
#ifndef _
HEARTBEAT
TIMER_H_
#define _
HEARTBEAT
TIMER_H_
#ifndef _
MAVLINKMESSAGES
TIMER_H_
#define _
MAVLINKMESSAGES
TIMER_H_
#include <QTimer>
#include <QObject>
/**
* @brief The
Heartbeat
Timer class
* @brief The
MavlinkMessages
Timer class
*
* Track the
heartbeat
for a single vehicle on one link.
* As long as regular
heartbeat
s are received the status is active. On the timer timeout
* Track the
mavlink messages
for a single vehicle on one link.
* As long as regular
message
s are received the status is active. On the timer timeout
* status is set to inactive. On any status change the activeChanged signal is emitted.
* If high_latency is true then active is always true.
*/
class
Heartbeat
Timer
:
public
QObject
class
MavlinkMessages
Timer
:
public
QObject
{
Q_OBJECT
public:
/**
* @brief
Heartbeat
Timer
* @brief
MavlinkMessages
Timer
*
* Constructor
*
* @param vehicle_id: The vehicle ID for which the heartbeat will be tracked.
* @param high_latency: Indicates if the link is a high latency one.
*/
Heartbeat
Timer
(
int
vehicle_id
,
bool
high_latency
);
MavlinkMessages
Timer
(
int
vehicle_id
,
bool
high_latency
);
/**
* @brief init
...
...
@@ -43,7 +43,7 @@ public:
*/
void
init
();
~
Heartbeat
Timer
();
~
MavlinkMessages
Timer
();
/**
* @brief getActive
...
...
@@ -68,7 +68,7 @@ signals:
/**
* @brief heartbeatTimeout
*
* Emitted if no
heartbeat
is received over the specified time.
* Emitted if no
mavlink message
is received over the specified time.
*
* @param vehicle_id: The vehicle ID for which the heartbeat timed out.
*/
...
...
@@ -100,7 +100,7 @@ private:
int
_vehicleID
=
-
1
;
// Vehicle ID for which the heartbeat is tracked.
bool
_high_latency
=
false
;
// Indicates if the link is a high latency link or not.
static
const
int
_
heartbeat
ReceivedTimeoutMSecs
=
3500
;
// Signal connection lost after 3.5 seconds of no messages
static
const
int
_
message
ReceivedTimeoutMSecs
=
3500
;
// Signal connection lost after 3.5 seconds of no messages
};
#endif // _
HEARTBEAT
TIMER_H_
#endif // _
MAVLINKMESSAGES
TIMER_H_
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a 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