dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

[uxrce_dds_client] wait for Timesync to converge 

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
This commit is contained in:
Beniamino Pozzan 2023-11-26 01:27:48 +00:00 committed by Daniel Agar
parent f01400de81
commit 2363c03bfe
2 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/timesync/Timesync.hpp
View File

@ -106,13 +106,6 @@ public:
int64_t offset() const { return (int64_t)_time_offset; }
private:
/**
* Online exponential filter to smooth time offset
*/
void add_sample(int64_t offset_us);
/**
* Return true if the timesync algorithm converged to a good estimate,
* return false otherwise
@ -124,6 +117,13 @@ private:
*/
void reset_filter();
private:
/**
* Online exponential filter to smooth time offset
*/
void add_sample(int64_t offset_us);
uORB::PublicationMulti<timesync_status_s> _timesync_status_pub{ORB_ID(timesync_status)};
uint32_t _sequence{0};

5
src/modules/uxrce_dds_client/uxrce_dds_client.cpp
View File

@ -441,7 +441,7 @@ void UxrceddsClient::run()
// Spin until sync with the Agent
while (_synchronize_timestamps) {
if (uxr_sync_session(&session, 1000)) {
if (uxr_sync_session(&session, 1000) && _timesync.sync_converged()) {
PX4_INFO("synchronized with time offset %-5" PRId64 "us", session.time_offset / 1000);
if (_param_uxrce_dds_syncc.get() > 0) {
@ -505,7 +505,7 @@ void UxrceddsClient::run()
// time sync session
if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) {
if (uxr_sync_session(&session, 100)) {
if (uxr_sync_session(&session, 100) && _timesync.sync_converged()) {
//PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset);
last_sync_session = hrt_absolute_time();
@ -563,6 +563,7 @@ void UxrceddsClient::run()
_last_payload_tx_rate = 0;
_last_payload_tx_rate = 0;
_subs->reset();
_timesync.reset_filter();
}
}