forked from Archive/PX4-Autopilot
uxrce_dds_client: optimizations and instrumentation
- skip ping session if data flowing bidirectionally - add perf counters for loop time and interval - skip blocking poll if there's input data to read
This commit is contained in:
parent
b115d3cd44
commit
a1cce7e961
|
@ -56,7 +56,7 @@
|
||||||
|
|
||||||
using namespace time_literals;
|
using namespace time_literals;
|
||||||
|
|
||||||
void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp,
|
static void on_time(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp,
|
||||||
int64_t originate_timestamp, void *args)
|
int64_t originate_timestamp, void *args)
|
||||||
{
|
{
|
||||||
// latest round trip time (RTT)
|
// latest round trip time (RTT)
|
||||||
|
@ -76,21 +76,15 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void on_time_no_sync(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp,
|
static void on_time_no_sync(uxrSession *session, int64_t current_time, int64_t received_timestamp,
|
||||||
|
int64_t transmit_timestamp,
|
||||||
int64_t originate_timestamp, void *args)
|
int64_t originate_timestamp, void *args)
|
||||||
{
|
{
|
||||||
session->time_offset = 0;
|
session->time_offset = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void on_request(uxrSession *session, uxrObjectId object_id, uint16_t request_id, SampleIdentity *sample_id,
|
||||||
void on_request(
|
ucdrBuffer *ub, uint16_t length, void *args)
|
||||||
uxrSession *session,
|
|
||||||
uxrObjectId object_id,
|
|
||||||
uint16_t request_id,
|
|
||||||
SampleIdentity *sample_id,
|
|
||||||
ucdrBuffer *ub,
|
|
||||||
uint16_t length,
|
|
||||||
void *args)
|
|
||||||
{
|
{
|
||||||
(void) request_id;
|
(void) request_id;
|
||||||
(void) length;
|
(void) length;
|
||||||
|
@ -229,6 +223,9 @@ UxrceddsClient::~UxrceddsClient()
|
||||||
delete _transport_serial;
|
delete _transport_serial;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
perf_free(_loop_perf);
|
||||||
|
perf_free(_loop_interval_perf);
|
||||||
|
|
||||||
#if defined(UXRCE_DDS_CLIENT_UDP)
|
#if defined(UXRCE_DDS_CLIENT_UDP)
|
||||||
|
|
||||||
if (_transport_udp) {
|
if (_transport_udp) {
|
||||||
|
@ -474,9 +471,9 @@ void UxrceddsClient::run()
|
||||||
}
|
}
|
||||||
|
|
||||||
// create VehicleCommand replier
|
// create VehicleCommand replier
|
||||||
if (num_of_repliers < MAX_NUM_REPLIERS) {
|
if (_num_of_repliers < MAX_NUM_REPLIERS) {
|
||||||
if (add_replier(new VehicleCommandSrv(&session, reliable_out, reliable_in, participant_id, _client_namespace,
|
if (add_replier(new VehicleCommandSrv(&session, reliable_out, reliable_in, participant_id, _client_namespace,
|
||||||
num_of_repliers))) {
|
_num_of_repliers))) {
|
||||||
PX4_ERR("replier init failed");
|
PX4_ERR("replier init failed");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -522,45 +519,48 @@ void UxrceddsClient::run()
|
||||||
|
|
||||||
while (!should_exit() && _connected) {
|
while (!should_exit() && _connected) {
|
||||||
|
|
||||||
/* Wait for topic updates for max 1000 ms (1sec) */
|
perf_begin(_loop_perf);
|
||||||
int poll = px4_poll(&_subs->fds[0], (sizeof(_subs->fds) / sizeof(_subs->fds[0])), 1000);
|
perf_count(_loop_interval_perf);
|
||||||
|
|
||||||
|
int orb_poll_timeout_ms = 10;
|
||||||
|
|
||||||
|
int bytes_available = 0;
|
||||||
|
|
||||||
|
if (ioctl(_fd, FIONREAD, (unsigned long)&bytes_available) == OK) {
|
||||||
|
if (bytes_available > 10) {
|
||||||
|
orb_poll_timeout_ms = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Wait for topic updates for max 10 ms */
|
||||||
|
int poll = px4_poll(_subs->fds, (sizeof(_subs->fds) / sizeof(_subs->fds[0])), orb_poll_timeout_ms);
|
||||||
|
|
||||||
/* Handle the poll results */
|
/* Handle the poll results */
|
||||||
if (poll == 0) {
|
if (poll > 0) {
|
||||||
/* Timeout, no updates in selected uorbs */
|
_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
|
||||||
continue;
|
|
||||||
|
|
||||||
} else if (poll < 0) {
|
} else {
|
||||||
/* Error */
|
if (poll < 0) {
|
||||||
|
// poll error
|
||||||
if (poll_error_counter < 10 || poll_error_counter % 50 == 0) {
|
if (poll_error_counter < 10 || poll_error_counter % 50 == 0) {
|
||||||
/* Prevent flooding */
|
// prevent flooding
|
||||||
PX4_ERR("ERROR while polling uorbs: %d", poll);
|
PX4_ERR("ERROR while polling uorbs: %d", poll);
|
||||||
}
|
}
|
||||||
|
|
||||||
poll_error_counter++;
|
poll_error_counter++;
|
||||||
continue;
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
|
// run session with 0 timeout (non-blocking)
|
||||||
|
uxr_run_session_timeout(&session, 0);
|
||||||
|
|
||||||
// check if there are available replies
|
// check if there are available replies
|
||||||
process_replies();
|
process_replies();
|
||||||
|
|
||||||
// Run the session until we receive no more data or up to a maximum number of iterations.
|
|
||||||
// The maximum observed number of iterations was 6 (SITL). If we were to run only once, data starts to get
|
|
||||||
// delayed, causing registered flight modes to time out.
|
|
||||||
for (int i = 0; i < 10; ++i) {
|
|
||||||
const uint32_t prev_num_payload_received = _pubs->num_payload_received;
|
|
||||||
uxr_run_session_timeout(&session, 0);
|
|
||||||
|
|
||||||
if (_pubs->num_payload_received == prev_num_payload_received) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// time sync session
|
// time sync session
|
||||||
if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) {
|
if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) {
|
||||||
if (uxr_sync_session(&session, 100) && _timesync.sync_converged()) {
|
|
||||||
|
if (uxr_sync_session(&session, 10) && _timesync.sync_converged()) {
|
||||||
//PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset);
|
//PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset);
|
||||||
last_sync_session = hrt_absolute_time();
|
last_sync_session = hrt_absolute_time();
|
||||||
|
|
||||||
|
@ -568,6 +568,15 @@ void UxrceddsClient::run()
|
||||||
syncSystemClock(&session);
|
syncSystemClock(&session);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (!_timesync_converged && _timesync.sync_converged()) {
|
||||||
|
PX4_INFO("time sync converged");
|
||||||
|
|
||||||
|
} else if (_timesync_converged && !_timesync.sync_converged()) {
|
||||||
|
PX4_WARN("time sync no longer converged");
|
||||||
|
}
|
||||||
|
|
||||||
|
_timesync_converged = _timesync.sync_converged();
|
||||||
}
|
}
|
||||||
|
|
||||||
handleMessageFormatRequest();
|
handleMessageFormatRequest();
|
||||||
|
@ -589,8 +598,14 @@ void UxrceddsClient::run()
|
||||||
last_status_update = now;
|
last_status_update = now;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Handle ping
|
// Handle ping, unless we're actively sending & receiving payloads successfully
|
||||||
if (now - last_ping > 500_ms) {
|
if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) {
|
||||||
|
_connected = true;
|
||||||
|
num_pings_missed = 0;
|
||||||
|
last_ping = now;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (hrt_elapsed_time(&last_ping) > 1_s) {
|
||||||
last_ping = now;
|
last_ping = now;
|
||||||
|
|
||||||
if (had_ping_reply) {
|
if (had_ping_reply) {
|
||||||
|
@ -600,15 +615,20 @@ void UxrceddsClient::run()
|
||||||
++num_pings_missed;
|
++num_pings_missed;
|
||||||
}
|
}
|
||||||
|
|
||||||
uxr_ping_agent_session(&session, 0, 1);
|
int timeout_ms = 1'000; // 1 second
|
||||||
|
uint8_t attempts = 1;
|
||||||
|
uxr_ping_agent_session(&session, timeout_ms, attempts);
|
||||||
|
|
||||||
had_ping_reply = false;
|
had_ping_reply = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (num_pings_missed > 2) {
|
if (num_pings_missed >= 3) {
|
||||||
PX4_INFO("No ping response, disconnecting");
|
PX4_INFO("No ping response, disconnecting");
|
||||||
_connected = false;
|
_connected = false;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
perf_end(_loop_perf);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -761,10 +781,10 @@ bool UxrceddsClient::setBaudrate(int fd, unsigned baud)
|
||||||
|
|
||||||
bool UxrceddsClient::add_replier(SrvBase *replier)
|
bool UxrceddsClient::add_replier(SrvBase *replier)
|
||||||
{
|
{
|
||||||
if (num_of_repliers < MAX_NUM_REPLIERS) {
|
if (_num_of_repliers < MAX_NUM_REPLIERS) {
|
||||||
repliers_[num_of_repliers] = replier;
|
_repliers[_num_of_repliers] = replier;
|
||||||
|
|
||||||
num_of_repliers++;
|
_num_of_repliers++;
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
|
@ -773,11 +793,12 @@ bool UxrceddsClient::add_replier(SrvBase *replier)
|
||||||
void UxrceddsClient::process_requests(uxrObjectId object_id, SampleIdentity *sample_id, ucdrBuffer *ub,
|
void UxrceddsClient::process_requests(uxrObjectId object_id, SampleIdentity *sample_id, ucdrBuffer *ub,
|
||||||
const int64_t time_offset_us)
|
const int64_t time_offset_us)
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < num_of_repliers; i++) {
|
for (uint8_t i = 0; i < _num_of_repliers; i++) {
|
||||||
if (object_id.id == repliers_[i]->replier_id_.id
|
if (object_id.id == _repliers[i]->replier_id_.id
|
||||||
&& object_id.type == repliers_[i]->replier_id_.type) {
|
&& object_id.type == _repliers[i]->replier_id_.type) {
|
||||||
repliers_[i]->process_request(ub, time_offset_us);
|
|
||||||
memcpy(&(repliers_[i]->sample_id_), sample_id, sizeof(repliers_[i]->sample_id_));
|
_repliers[i]->process_request(ub, time_offset_us);
|
||||||
|
memcpy(&(_repliers[i]->sample_id_), sample_id, sizeof(_repliers[i]->sample_id_));
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -785,18 +806,19 @@ void UxrceddsClient::process_requests(uxrObjectId object_id, SampleIdentity *sam
|
||||||
|
|
||||||
void UxrceddsClient::process_replies()
|
void UxrceddsClient::process_replies()
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < num_of_repliers; i++) {
|
for (uint8_t i = 0; i < _num_of_repliers; i++) {
|
||||||
repliers_[i]->process_reply();
|
_repliers[i]->process_reply();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void UxrceddsClient::delete_repliers()
|
void UxrceddsClient::delete_repliers()
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < num_of_repliers; i++) {
|
for (uint8_t i = 0; i < _num_of_repliers; i++) {
|
||||||
delete (repliers_[i]);
|
delete (_repliers[i]);
|
||||||
|
_repliers[i] = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
num_of_repliers = 0;
|
_num_of_repliers = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int UxrceddsClient::custom_command(int argc, char *argv[])
|
int UxrceddsClient::custom_command(int argc, char *argv[])
|
||||||
|
@ -845,6 +867,11 @@ int UxrceddsClient::print_status()
|
||||||
PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
|
PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
PX4_INFO("timesync converged: %s", _timesync.sync_converged() ? "true" : "false");
|
||||||
|
|
||||||
|
perf_print_counter(_loop_perf);
|
||||||
|
perf_print_counter(_loop_interval_perf);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -44,6 +44,8 @@
|
||||||
|
|
||||||
#include <lib/timesync/Timesync.hpp>
|
#include <lib/timesync/Timesync.hpp>
|
||||||
|
|
||||||
|
#include <lib/perf/perf_counter.h>
|
||||||
|
|
||||||
#if defined(CONFIG_NET) || defined(__PX4_POSIX)
|
#if defined(CONFIG_NET) || defined(__PX4_POSIX)
|
||||||
# define UXRCE_DDS_CLIENT_UDP 1
|
# define UXRCE_DDS_CLIENT_UDP 1
|
||||||
#endif
|
#endif
|
||||||
|
@ -157,8 +159,8 @@ private:
|
||||||
SendTopicsSubs *_subs{nullptr};
|
SendTopicsSubs *_subs{nullptr};
|
||||||
RcvTopicsPubs *_pubs{nullptr};
|
RcvTopicsPubs *_pubs{nullptr};
|
||||||
|
|
||||||
SrvBase *repliers_[MAX_NUM_REPLIERS];
|
SrvBase *_repliers[MAX_NUM_REPLIERS];
|
||||||
uint8_t num_of_repliers{0};
|
uint8_t _num_of_repliers{0};
|
||||||
|
|
||||||
uxrCommunication *_comm{nullptr};
|
uxrCommunication *_comm{nullptr};
|
||||||
int _fd{-1};
|
int _fd{-1};
|
||||||
|
@ -167,8 +169,13 @@ private:
|
||||||
int _last_payload_rx_rate{}; ///< in B/s
|
int _last_payload_rx_rate{}; ///< in B/s
|
||||||
bool _connected{false};
|
bool _connected{false};
|
||||||
|
|
||||||
|
bool _timesync_converged{false};
|
||||||
|
|
||||||
Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS};
|
Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS};
|
||||||
|
|
||||||
|
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||||
|
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
|
||||||
|
|
||||||
DEFINE_PARAMETERS(
|
DEFINE_PARAMETERS(
|
||||||
(ParamInt<px4::params::UXRCE_DDS_DOM_ID>) _param_uxrce_dds_dom_id,
|
(ParamInt<px4::params::UXRCE_DDS_DOM_ID>) _param_uxrce_dds_dom_id,
|
||||||
(ParamInt<px4::params::UXRCE_DDS_KEY>) _param_uxrce_key,
|
(ParamInt<px4::params::UXRCE_DDS_KEY>) _param_uxrce_key,
|
||||||
|
|
Loading…
Reference in New Issue