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
dd747599
Commit
dd747599
authored
Sep 04, 2018
by
Gus Grubba
Browse files
Just compute total sent on the fly rather then keeping a running count.
parent
2a3619ce
Changes
2
Hide whitespace changes
Inline
Side-by-side
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
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