fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 06:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink: Improve message handling / tracking

Browse Source 
The message handling was not obeying action focused messages and high-rate messages properly before. With this change update rates track the desired rates closely. Critical high-rate messages such as ADS-B are queued additionally to guarantee that all received packets are being correctly forwarded.
This commit is contained in:
Lorenz Meier
2017-07-29 16:12:41 +02:00
parent 556eb9e45a
commit 07ced9895c
8 changed files with 342 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_stream.cpp
+17 -10
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,6 +36,7 @@
* Mavlink messages stream implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <stdlib.h>
@@ -59,7 +60,7 @@ MavlinkStream::~MavlinkStream()
* Set messages interval in ms
*/
void
MavlinkStream::set_interval(const unsigned int interval)
MavlinkStream::set_interval(const int interval)
{
_interval = interval;
}
@@ -78,7 +79,7 @@ MavlinkStream::update(const hrt_abstime t)
// on the link scheduling
_last_sent = hrt_absolute_time();
#ifndef __PX4_QURT
send(t);
(void)send(t);
#endif
return 0;
}
@@ -90,13 +91,13 @@ MavlinkStream::update(const hrt_abstime t)
}
int64_t dt = t - _last_sent;
int interval = _interval;
int interval = (_interval > 0) ? _interval : 0;
if (!const_rate()) {
interval /= _mavlink->get_rate_mult();
}
// send the message if it is due or
// Send the message if it is due or
// if it will overrun the next scheduled send interval
// by 40% of the interval time. This helps to avoid
// sending a scheduled message on average slower than
@@ -106,17 +107,23 @@ MavlinkStream::update(const hrt_abstime t)
// This method is not theoretically optimal but a suitable
// stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz)
if (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4)) {
if (interval == 0 || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 4))) {
// interval expired, send message
bool sent = true;
#ifndef __PX4_QURT
send(t);
sent = send(t);
#endif
// if the interval is non-zero do not use the actual time but
// If the interval is non-zero do not use the actual time but
// increment at a fixed rate, so that processing delays do not
// distort the average rate
_last_sent = (interval > 0) ? _last_sent + interval : t;
if (sent) {
_last_sent = (interval > 0) ? _last_sent + interval : t;
return 0;
return 0;
} else {
return -1;
}
}
return -1;