Add a worst case bound to NoncausalTimestampFilter
With a bounded slew rate, we can define a function which is the worst
that time could change. Since the worst time could change is
represented by the extrapolation function, this is the max of the
extrapolation function for both bounding points.
This lets us start to enforce constraints on the worst case, rather
than on the average. Given what we've seen when we have outages, I
think that will close up a hole in our current logic. Usage will follow
soon.
Now that we are good at testing 128 bit math, just do it with 128 bit
math from the beginning.
Change-Id: I1dcb1522c39468dd742b8f97d77753fa57737c70
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index 7d25dee..bdfe67f 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -838,7 +838,42 @@
(std::get<1>(p1) - std::get<1>(p0)).count();
}
-std::pair<chrono::nanoseconds, double> NoncausalTimestampFilter::ExtrapolateOffset(
+chrono::nanoseconds NoncausalTimestampFilter::BoundOffset(
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta) {
+ // We are trying to solve for worst case offset given the two known points.
+ // This is on the two worst case lines from the two points, and we switch
+ // lines at the interstection. This is equivilent to the lowest of the two
+ // lines.
+ return std::max(NoncausalTimestampFilter::ExtrapolateOffset(p0, ta),
+ NoncausalTimestampFilter::ExtrapolateOffset(p1, ta));
+}
+
+std::pair<chrono::nanoseconds, double> NoncausalTimestampFilter::BoundOffset(
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta_base, double ta) {
+ DCHECK_GE(ta, 0.0);
+ DCHECK_LT(ta, 1.0);
+
+ const std::pair<chrono::nanoseconds, double> o0 =
+ NoncausalTimestampFilter::ExtrapolateOffset(p0, ta_base, ta);
+ const std::pair<chrono::nanoseconds, double> o1 =
+ NoncausalTimestampFilter::ExtrapolateOffset(p1, ta_base, ta);
+
+ // Want to calculate max(o0 + o0r, o1 + o1r) without precision problems.
+ if (static_cast<double>((o0.first - o1.first).count()) >
+ o1.second - o0.second) {
+ // Ok, o0 is now > o1. We want the max, so return o0.
+ return o0;
+ } else {
+ return o1;
+ }
+}
+
+std::pair<chrono::nanoseconds, double>
+NoncausalTimestampFilter::ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point ta_base, double ta) {
DCHECK_GE(ta, 0.0);
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 79e3c5b..a056fce 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -16,9 +16,6 @@
namespace aos {
namespace message_bridge {
-// TODO<jim>: Should do something to help with precision, like make it an
-// integer and divide by the value (e.g., / 1000)
-
// Max velocity to clamp the filter to in seconds/second.
typedef std::ratio<1, 1000> MaxVelocityRatio;
inline constexpr double kMaxVelocity() {
@@ -577,6 +574,11 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
monotonic_clock::time_point ta);
+ static std::chrono::nanoseconds BoundOffset(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta);
+
static std::chrono::nanoseconds ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point ta);
@@ -591,6 +593,11 @@
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> /*p1*/,
monotonic_clock::time_point ta_base, double ta);
+ static std::pair<std::chrono::nanoseconds, double> BoundOffset(
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
+ std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p1,
+ monotonic_clock::time_point ta_base, double ta);
+
static std::pair<std::chrono::nanoseconds, double> ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point ta_base, double ta);
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index d87e63d..67afc6e 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -980,6 +980,83 @@
}
}
+// Tests that all variants of BoundOffset do reasonable things.
+TEST_F(NoncausalTimestampFilterTest, BoundOffset) {
+ const monotonic_clock::time_point e = monotonic_clock::epoch();
+
+ const monotonic_clock::time_point t1 = e + chrono::nanoseconds(10000);
+ const chrono::nanoseconds o1 = chrono::nanoseconds(100);
+ //const double o1d = static_cast<double>(o1.count());
+
+ const monotonic_clock::time_point t2 = t1 + chrono::nanoseconds(100000);
+ const chrono::nanoseconds o2 = chrono::nanoseconds(150);
+ //const double o2d = static_cast<double>(o2.count());
+
+ EXPECT_EQ(NoncausalTimestampFilter::BoundOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1),
+ o1);
+ EXPECT_EQ(NoncausalTimestampFilter::BoundOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t1, 0.0),
+ std::pair(o1, 0.0));
+
+ EXPECT_EQ(NoncausalTimestampFilter::BoundOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t2),
+ o2);
+ EXPECT_EQ(NoncausalTimestampFilter::BoundOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), t2, 0.0),
+ std::pair(o2, 0.0));
+
+ // Iterate from before t1 to after t2 and confirm that the solution is right.
+ // We must always be >= than interpolation, and must also be equal to the max
+ // of extrapolating both. Since the numbers are small enough (by
+ // construction!), the double calculation will be close enough that we can
+ // trust it.
+
+ for (int i = -MaxVelocityRatio::den * MaxVelocityRatio::num * 6;
+ i < MaxVelocityRatio::den * MaxVelocityRatio::num * 6 + (t2 - t1).count(); ++i) {
+ monotonic_clock::time_point ta_base = t1;
+ const double ta_orig = static_cast<double>(i) / 3.0;
+ double ta = ta_orig;
+
+ NormalizeTimestamps(&ta_base, &ta);
+ CHECK_GE(ta, 0.0);
+ CHECK_LT(ta, 1.0);
+
+ const chrono::nanoseconds expected_offset_1 =
+ NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),
+ ta_base);
+ const chrono::nanoseconds expected_offset_2 =
+ NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t2, o2),
+ ta_base);
+
+ // Each of the extrapolation functions have their max at the points. They
+ // slope up before and down after. So, we want the max.
+ //
+ //
+ // p0 p1 |
+ // / \/ \ |
+ // / \ |
+
+ const std::pair<chrono::nanoseconds, double> offset =
+ NoncausalTimestampFilter::BoundOffset(
+ std::make_tuple(t1, o1), std::make_tuple(t2, o2), ta_base, ta);
+
+ EXPECT_EQ(std::max(expected_offset_1, expected_offset_2), offset.first);
+
+ const double expected_double_offset = std::max(
+ static_cast<double>(o1.count()) - std::abs(ta_orig) * kMaxVelocity(),
+ static_cast<double>(o2.count()) -
+ std::abs(ta_orig - (t2 - t1).count()) * kMaxVelocity());
+
+ EXPECT_NEAR(static_cast<double>(offset.first.count()) + offset.second,
+ expected_double_offset, 1e-9)
+ << ": i " << i << " t " << ta_base << " " << ta << " t1 " << t1
+ << " o1 " << o1.count() << "ns t2 " << t2 << " o2 " << o2.count()
+ << "ns Non-rounded: "
+ << std::max(expected_offset_1, expected_offset_2).count() << "ns";
+ }
+}
+
// Tests that FindTimestamps finds timestamps in a sequence.
TEST_F(NoncausalTimestampFilterTest, FindTimestamps) {
const BootTimestamp e{0, monotonic_clock::epoch()};