fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 16:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink ulog streaming: add rate limiting

Browse Source 
This limits the maximum bandwidth of ulog streaming to 70% of the specified
mavlink datarate. If less is used, the leftover is assigned to the mavlink
streams, if more is used, it starts to drop.

mavlink status outputs the currently used rate, to check if a link is
saturated.
This commit is contained in:
Beat Küng
2016-10-19 13:13:47 +02:00
committed by Lorenz Meier
parent 8f5656f033
commit 318c970477
4 changed files with 54 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_ulog.cpp
+27 -5
View File
@@ -41,14 +41,20 @@
#include "mavlink_ulog.h"
#include <px4_log.h>
#include <errno.h>
#include <mathlib/mathlib.h>
bool MavlinkULog::_init = false;
MavlinkULog *MavlinkULog::_instance = nullptr;
sem_t MavlinkULog::_lock;
const float MavlinkULog::_rate_calculation_delta_t = 0.1f;
MavlinkULog::MavlinkULog(uint8_t target_system, uint8_t target_component)
: _target_system(target_system), _target_component(target_component)
MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
: _target_system(target_system), _target_component(target_component),
_max_rate_factor(max_rate_factor),
_max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t * _max_rate_factor * datarate /
(MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))),
_current_rate_factor(max_rate_factor)
{
_ulog_stream_sub = orb_subscribe(ORB_ID(ulog_stream));
@@ -57,6 +63,7 @@ MavlinkULog::MavlinkULog(uint8_t target_system, uint8_t target_component)
}
_waiting_for_initial_ack = true;
_last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization
_next_rate_check = _last_sent_time + _rate_calculation_delta_t * 1.e6f;
}
MavlinkULog::~MavlinkULog()
@@ -123,7 +130,7 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
bool updated = false;
int ret = orb_check(_ulog_stream_sub, &updated);
while (updated && !ret) {
while (updated && !ret && _current_num_msgs < _max_num_messages) {
orb_copy(ORB_ID(ulog_stream), _ulog_stream_sub, &_ulog_data);
if (_ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) {
_sent_tries = 1;
@@ -152,9 +159,24 @@ int MavlinkULog::handle_update(mavlink_channel_t channel)
memcpy(msg.data, _ulog_data.data, sizeof(msg.data));
mavlink_msg_logging_data_send_struct(channel, &msg);
}
++_current_num_msgs;
ret = orb_check(_ulog_stream_sub, &updated);
}
//need to update the rate?
hrt_abstime t = hrt_absolute_time();
if (t > _next_rate_check) {
if (_current_num_msgs < _max_num_messages) {
_current_rate_factor = _max_rate_factor * (float)_current_num_msgs / _max_num_messages;
} else {
_current_rate_factor = _max_rate_factor;
}
_current_num_msgs = 0;
_next_rate_check = t + _rate_calculation_delta_t * 1.e6f;
PX4_DEBUG("current rate=%.3f (max=%i msgs in %.3fs)", (double)_current_rate_factor, _max_num_messages,
(double)_rate_calculation_delta_t);
}
return 0;
}
@@ -167,13 +189,13 @@ void MavlinkULog::initialize()
_init = true;
}
MavlinkULog* MavlinkULog::try_start(uint8_t target_system, uint8_t target_component)
MavlinkULog* MavlinkULog::try_start(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
{
MavlinkULog *ret = nullptr;
bool failed = false;
lock();
if (!_instance) {
ret = _instance = new MavlinkULog(target_system, target_component);
ret = _instance = new MavlinkULog(datarate, max_rate_factor, target_system, target_component);
if (!_instance) {
failed = true;
}