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
dd747599
Commit
dd747599
authored
Sep 04, 2018
by
Gus Grubba
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Just compute total sent on the fly rather then keeping a running count.
parent
2a3619ce
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
3 additions
and
9 deletions
+3
-9
MAVLinkProtocol.cc
src/comm/MAVLinkProtocol.cc
+3
-8
MAVLinkProtocol.h
src/comm/MAVLinkProtocol.h
+0
-1
No files found.
src/comm/MAVLinkProtocol.cc
View file @
dd747599
...
...
@@ -65,7 +65,6 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox)
,
_multiVehicleManager
(
nullptr
)
{
memset
(
totalReceiveCounter
,
0
,
sizeof
(
totalReceiveCounter
));
memset
(
totalSentCounter
,
0
,
sizeof
(
totalSentCounter
));
memset
(
totalLossCounter
,
0
,
sizeof
(
totalLossCounter
));
memset
(
runningLossPercent
,
0
,
sizeof
(
runningLossPercent
));
memset
(
firstMessage
,
1
,
sizeof
(
firstMessage
));
...
...
@@ -149,7 +148,6 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
int
channel
=
link
->
mavlinkChannel
();
totalReceiveCounter
[
channel
]
=
0
;
totalLossCounter
[
channel
]
=
0
;
totalSentCounter
[
channel
]
=
0
;
runningLossPercent
[
channel
]
=
0.0
f
;
for
(
int
i
=
0
;
i
<
256
;
i
++
)
{
firstMessage
[
channel
][
i
]
=
1
;
...
...
@@ -221,16 +219,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
}
// Log how many were lost
totalLossCounter
[
mavlinkChannel
]
+=
static_cast
<
uint64_t
>
(
lostMessages
);
// Compute actual count of sent messages
totalSentCounter
[
mavlinkChannel
]
+=
static_cast
<
uint64_t
>
(
lostMessages
);
}
else
{
totalSentCounter
[
mavlinkChannel
]
++
;
}
// And update the last sequence number for this system/component pair
lastIndex
[
_message
.
sysid
][
_message
.
compid
]
=
_message
.
seq
;;
// Calculate new loss ratio
float
receiveLossPercent
=
static_cast
<
float
>
(
totalLossCounter
[
mavlinkChannel
])
/
static_cast
<
float
>
(
totalSentCounter
[
mavlinkChannel
]);
uint64_t
totalSent
=
totalReceiveCounter
[
mavlinkChannel
]
+
totalLossCounter
[
mavlinkChannel
];
float
receiveLossPercent
=
static_cast
<
float
>
(
static_cast
<
double
>
(
totalLossCounter
[
mavlinkChannel
])
/
static_cast
<
double
>
(
totalSent
));
receiveLossPercent
*=
100.0
f
;
receiveLossPercent
=
(
receiveLossPercent
*
0.5
f
)
+
(
runningLossPercent
[
mavlinkChannel
]
*
0.5
f
);
runningLossPercent
[
mavlinkChannel
]
=
receiveLossPercent
;
...
...
@@ -310,7 +305,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
// Update MAVLink status on every 32th packet
if
((
totalReceiveCounter
[
mavlinkChannel
]
&
0x1F
)
==
0
)
{
emit
mavlinkMessageStatus
(
_message
.
sysid
,
totalSent
Counter
[
mavlinkChannel
]
,
totalReceiveCounter
[
mavlinkChannel
],
totalLossCounter
[
mavlinkChannel
],
receiveLossPercent
);
emit
mavlinkMessageStatus
(
_message
.
sysid
,
totalSent
,
totalReceiveCounter
[
mavlinkChannel
],
totalLossCounter
[
mavlinkChannel
],
receiveLossPercent
);
}
// The packet is emitted as a whole, as it is only 255 - 261 bytes short
...
...
src/comm/MAVLinkProtocol.h
View file @
dd747599
...
...
@@ -105,7 +105,6 @@ protected:
uint8_t
lastIndex
[
256
][
256
];
///< Store the last received sequence ID for each system/componenet pair
uint8_t
firstMessage
[
256
][
256
];
///< First message flag
uint64_t
totalReceiveCounter
[
MAVLINK_COMM_NUM_BUFFERS
];
///< The total number of successfully received messages
uint64_t
totalSentCounter
[
MAVLINK_COMM_NUM_BUFFERS
];
///< The calculated total number of messages sent to us
uint64_t
totalLossCounter
[
MAVLINK_COMM_NUM_BUFFERS
];
///< Total messages lost during transmission.
float
runningLossPercent
[
MAVLINK_COMM_NUM_BUFFERS
];
///< Loss rate
...
...
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