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

microRTPS: timesync: interpolate

This commit is contained in:
TSC21 2020-03-09 14:31:26 +00:00 committed by Nuno Marques
parent 2568d9ae20
commit 7de8bee29e
2 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
msg/templates/urtps/microRTPS_timesync.cpp.em
View File

@ -157,9 +157,15 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
return false;
}
double schedule = std::min((double)_num_samples / WINDOW_SIZE, 1.);
double alpha = ALPHA_INITIAL * (1. - schedule) + ALPHA_FINAL * schedule;
double beta = BETA_INTIIAL * (1. - schedule) + BETA_FINAL * schedule;
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;
}
int64_t offset_prev = _offset_ns.load();
updateOffset(static_cast<int64_t>((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + measurement_offset * alpha));

2
msg/templates/urtps/microRTPS_timesync.h.em
View File

@ -78,7 +78,7 @@ except AttributeError:
static constexpr double ALPHA_INITIAL = 0.05;
static constexpr double ALPHA_FINAL = 0.003;
static constexpr double BETA_INTIIAL = 0.05;
static constexpr double BETA_INITIAL = 0.05;
static constexpr double BETA_FINAL = 0.003;
static constexpr int WINDOW_SIZE = 500;
static constexpr int64_t UNKNOWN = 0;