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

microRTPS: agent: publish timesync status

Browse Source
This commit is contained in:
TSC21
2021-08-12 08:44:53 +02:00
committed by Nuno Marques
parent a324e5465a
commit d31b7feb31
7 changed files with 104 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/microRTPS_timesync.cpp.em
+45 -10
View File
@@ -107,6 +107,21 @@ void TimeSync::start(TimesyncPublisher *pub)
_send_timesync_thread.reset(new std::thread(run));
}
void TimeSync::init_status_pub(TimesyncStatusPublisher *status_pub)
{
auto run = [this, status_pub]() {
while (!_request_stop) {
timesync_status_msg_t status_msg = newTimesyncStatusMsg();
status_pub->publish(&status_msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
};
_request_stop = false;
_send_timesync_status_thread.reset(new std::thread(run));
}
void TimeSync::stop()
{
_request_stop = true;
@@ -115,6 +130,7 @@ void TimeSync::stop()
if (_timesync_node_thread && _timesync_node_thread->joinable()) { _timesync_node_thread->join(); }
@[end if]@
if (_send_timesync_thread && _send_timesync_thread->joinable()) { _send_timesync_thread->join(); }
if (_send_timesync_status_thread && _send_timesync_status_thread->joinable()) { _send_timesync_status_thread->join(); }
}
void TimeSync::reset()
@@ -124,23 +140,23 @@ void TimeSync::reset()
}
@[if ros2_distro]@
int64_t TimeSync::getROSTimeNSec()
uint64_t TimeSync::getROSTimeNSec() const
{
return _timesync_node->now().nanoseconds();
}
int64_t TimeSync::getROSTimeUSec()
uint64_t TimeSync::getROSTimeUSec() const
{
return RCL_NS_TO_US(getROSTimeNSec());
}
@[else]@
int64_t TimeSync::getSteadyTimeNSec()
uint64_t TimeSync::getSteadyTimeNSec() const
{
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::nanoseconds>(time).time_since_epoch().count();
}
int64_t TimeSync::getSteadyTimeUSec()
uint64_t TimeSync::getSteadyTimeUSec() const
{
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::microseconds>(time).time_since_epoch().count();
@@ -149,10 +165,11 @@ int64_t TimeSync::getSteadyTimeUSec()
bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns)
{
int64_t rtti = local_t3_ns - local_t1_ns;
_rtti = local_t3_ns - local_t1_ns;
_remote_time_stamp = remote_t2_ns;
// assume rtti is evenly split both directions
int64_t remote_t3_ns = remote_t2_ns + rtti / 2ll;
int64_t remote_t3_ns = remote_t2_ns + _rtti.load() / 2ll;
int64_t measurement_offset = remote_t3_ns - local_t3_ns;
@@ -181,8 +198,8 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
}
// ignore if rtti > 50ms
if (rtti > 50ll * 1000ll * 1000ll) {
if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl; }
if (_rtti.load() > 50ll * 1000ll * 1000ll) {
if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << _rtti.load() / (1000ll * 1000ll) << "ms\033[0m" << std::endl; }
return false;
}
@@ -197,11 +214,11 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL;
}
int64_t offset_prev = _offset_ns.load();
_offset_prev = _offset_ns.load();
updateOffset(static_cast<int64_t>((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) +
measurement_offset * alpha));
_skew_ns_per_sync =
static_cast<int64_t>(beta * (_offset_ns.load() - offset_prev) + (1. - beta) * _skew_ns_per_sync);
static_cast<int64_t>(beta * (_offset_ns.load() - _offset_prev.load()) + (1. - beta) * _skew_ns_per_sync);
_num_samples++;
@@ -263,3 +280,21 @@ timesync_msg_t TimeSync::newTimesyncMsg()
return msg;
}
timesync_status_msg_t TimeSync::newTimesyncStatusMsg()
{
timesync_status_msg_t msg{};
@[if ros2_distro]@
setMsgTimestamp(&msg, getROSTimeUSec());
@[else]@
setMsgTimestamp(&msg, getSteadyTimeUSec());
@[end if]@
setMsgSourceProtocol(&msg, 1); // SOURCE_PROTOCOL_RTPS
setMsgRemoteTimeStamp(&msg, _remote_time_stamp.load() / 1000ULL);
setMsgObservedOffset(&msg, _offset_prev.load());
setMsgEstimatedOffset(&msg, _offset_ns.load());
setMsgRoundTripTime(&msg, _rtti.load() / 1000ll);
return msg;
}