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

microRTPS: timesync: cleanup and style fix

Browse Source
This commit is contained in:
TSC21
2020-03-10 11:32:16 +00:00
committed by Nuno Marques
parent 512c405834
commit 04ec2041e3
2 changed files with 82 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/microRTPS_timesync.cpp.em
+25 -68
View File
@@ -73,34 +73,18 @@ TimeSync::TimeSync()
_request_reset_counter(0),
_last_msg_seq(0),
_last_remote_msg_seq(0)
{}
{ }
TimeSync::~TimeSync() { stop(); }
@[if ros2_distro]@
void TimeSync::start(const Timesync_Publisher* pub) {
@[else]@
void TimeSync::start(const timesync_Publisher* pub) {
@[end if]@
void TimeSync::start(const TimesyncPublisher* pub) {
stop();
_timesync_pub = (*pub);
auto run = [this]() {
while (!_request_stop) {
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
@(package)::msg::dds_::Timesync_ msg = newTimesyncMsg();
@[ else]@
timesync_ msg = newTimesyncMsg();
@[ end if]@
@[else]@
@[ if ros2_distro]@
@(package)::msg::Timesync msg = newTimesyncMsg();
@[ else]@
timesync msg = newTimesyncMsg();
@[ end if]@
@[end if]@
timesync_msg_t msg = newTimesyncMsg();
_timesync_pub.publish(&msg);
@@ -135,6 +119,11 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
std::cout << std::endl << "Timesync clock changed, resetting" << std::endl;
}
if (_num_samples == 0) {
updateOffset(measurement_offset);
_skew_ns_per_sync = 0;
}
if (_num_samples >= WINDOW_SIZE) {
if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) {
_request_reset_counter++;
@@ -145,11 +134,6 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
}
}
if (_num_samples == 0) {
updateOffset(measurement_offset);
_skew_ns_per_sync = 0;
}
// ignore if rtti > 10ms
if (rtti > 15ll * 1000ll * 1000ll) {
std::cout << std::endl
@@ -157,49 +141,37 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
return false;
}
double alpha = ALPHA_FINAL;
double beta = BETA_FINAL;
double alpha = ALPHA_FINAL;
double beta = BETA_FINAL;
if (_num_samples < WINDOW_SIZE) {
double schedule = (double)_num_samples / WINDOW_SIZE;
double s = 1. - exp(.5 * (1. - 1. / (1. - schedule)));
alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL;
beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL;
}
if (_num_samples < WINDOW_SIZE) {
double schedule = (double)_num_samples / WINDOW_SIZE;
double s = 1. - exp(.5 * (1. - 1. / (1. - schedule)));
alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL;
beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL;
}
int64_t 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);
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);
_num_samples++;
return true;
}
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
void TimeSync::processTimesyncMsg(@(package)::msg::dds_::Timesync_* msg) {
@[ else]@
void TimeSync::processTimesyncMsg(timesync_* msg) {
@[ end if]@
@[else]@
@[ if ros2_distro]@
void TimeSync::processTimesyncMsg(@(package)::msg::Timesync* msg) {
@[ else]@
void TimeSync::processTimesyncMsg(timesync* msg) {
@[ end if]@
@[end if]@
void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
if (msg->sys_id() == 1 && msg->seq() != _last_remote_msg_seq) {
if (msg->tc1() > 0) {
_last_remote_msg_seq = msg->seq();
_last_remote_msg_seq = msg->seq();
if (msg->tc1() > 0) {
if (!addMeasurement(msg->ts1(), msg->tc1(), getMonoRawTimeNSec())) {
std::cerr << "Offset not updated" << std::endl;
}
} else if (msg->tc1() == 0) {
_last_remote_msg_seq = msg->seq();
msg->timestamp() = getMonoTimeUSec();
msg->sys_id() = 0;
msg->seq()++;
@@ -210,23 +182,8 @@ void TimeSync::processTimesyncMsg(timesync* msg) {
}
}
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
@(package)::msg::dds_::Timesync_ TimeSync::newTimesyncMsg() {
@(package)::msg::dds_::Timesync_ msg{};
@[ else]@
timesync_ TimeSync::newTimesyncMsg() {
timesync_ msg{};
@[ end if]@
@[else]@
@[ if ros2_distro]@
@(package)::msg::Timesync TimeSync::newTimesyncMsg() {
@(package)::msg::Timesync msg{};
@[ else]@
timesync TimeSync::newTimesyncMsg() {
timesync msg{};
@[ end if]@
@[end if]@
timesync_msg_t TimeSync::newTimesyncMsg() {
timesync_msg_t msg{};
msg.timestamp() = getMonoTimeUSec();
msg.sys_id() = 0;
msg/templates/urtps/microRTPS_timesync.h.em
+57 -30
View File
@@ -85,65 +85,92 @@ static constexpr int64_t UNKNOWN = 0;
static constexpr int64_t TRIGGER_RESET_THRESHOLD_NS = 100ll * 1000ll * 1000ll;
static constexpr int REQUEST_RESET_COUNTER_THRESHOLD = 5;
@# Sets the timesync DDS type according to the FastRTPS and ROS2 version
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
using timesync_msg_t = @(package)::msg::dds_::Timesync_;
@[ else]@
using timesync_msg_t = timesync_;
@[ end if]@
@[else]@
@[ if ros2_distro]@
using timesync_msg_t = @(package)::msg::Timesync;
@[ else]@
using timesync_msg_t = timesync;
@[ end if]@
@[end if]@
@# Sets the timesync publisher entity depending on using ROS2 or not
@[if ros2_distro]@
using TimesyncPublisher = Timesync_Publisher;
@[else]@
using TimesyncPublisher = timesync_Publisher;
@[end if]@
class TimeSync {
public:
TimeSync();
virtual ~TimeSync();
@[if ros2_distro]@
void start(const Timesync_Publisher* pub);
@[else]@
void start(const timesync_Publisher* pub);
@[end if]@
/**
* @@brief Starts the timesync publishing thread
* @@param[in] pub The timesync publisher entity to use
*/
void start(const TimesyncPublisher* pub);
/**
* @@brief Resets the filter
*/
void reset();
/**
* @@brief Stops the timesync publishing thread
*/
void stop();
/**
* Get clock monotonic time (raw) in nanoseconds
* @@brief Get clock monotonic time (raw) in nanoseconds
* @@return System CLOCK_MONOTONIC_RAW time in nanoseconds
*/
inline int64_t getMonoRawTimeNSec() {
timespec t;
clock_gettime(CLOCK_MONOTONIC_RAW, &t);
return (int64_t)t.tv_sec * 1000000000LL + t.tv_nsec;
return static_cast<int64_t>(t.tv_sec * 1000000000LL + t.tv_nsec);
}
/**
* Get system monotonic time in microseconds
* @@brief Get system monotonic time in microseconds
* @@return System CLOCK_MONOTONIC time in microseconds
*/
inline int64_t getMonoTimeUSec() {
timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return (int64_t)(t.tv_sec * 1000000000LL + t.tv_nsec) / 1000LL;
return static_cast<int64_t>(t.tv_sec * 1000000000LL + t.tv_nsec) / 1000LL;
}
/**
* @@brief Adds a time offset measurement to be filtered
* @@param[in] local_t1_ns The agent CLOCK_MONOTONIC_RAW time in nanoseconds when the message was sent
* @@param[in] remote_t2_ns The (client) remote CLOCK_MONOTONIC time in nanoseconds
* @@param[in] local_t3_ns The agent current CLOCK_MONOTONIC time in nanoseconds
* @@return true or false depending if the time offset was updated
*/
bool addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns);
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
void processTimesyncMsg(@(package)::msg::dds_::Timesync_* msg);
/**
* @@brief Processes DDS timesync message
* @@param[in,out] msg The timestamp msg to be processed
*/
void processTimesyncMsg(timesync_msg_t* msg);
@(package)::msg::dds_::Timesync_ newTimesyncMsg();
@[ else]@
void processTimesyncMsg(timesync_* msg);
timesync_ newTimesyncMsg();
@[ end if]@
@[else]@
@[ if ros2_distro]@
void processTimesyncMsg(@(package)::msg::Timesync* msg);
@(package)::msg::Timesync newTimesyncMsg();
@[ else]@
void processTimesyncMsg(timesync* msg);
timesync newTimesyncMsg();
@[ end if]@
@[end if]@
/**
* @@brief Creates a new timesync DDS message to be sent from the agent to the client
* @@return A new timesync message with the origin in the agent and with the agent timestamp
*/
timesync_msg_t newTimesyncMsg();
/**
* @@brief Get the time sync offset in nanoseconds
* @@return The offset in nanosecods
* @@return The offset in nanoseconds
*/
inline int64_t getOffset() { return _offset_ns.load(); }