Handle having timestamps in only one direction
We occasionally see log files with timestamps going only in one
direction. These are parsable with some modifications.
1) Don't require timestamps in both directions when solving
2) Fix some rounding errors in the timestamp solution.
3) Add a small offset so the solver tries to produce a 1-2 ns minimum
delay if there is a constraint in only one direction.
Change-Id: I4b490716052688ade6586d3c5768b20cff6be7a5
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 272bee4..3da0733 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -402,6 +402,21 @@
)
aos_config(
+ name = "multinode_pingpong_split4_config",
+ src = "multinode_pingpong_split4.json",
+ flatbuffers = [
+ "//aos/events:ping_fbs",
+ "//aos/events:pong_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:remote_message_fbs",
+ "//aos/network:timestamp_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//aos/events:aos_config"],
+)
+
+aos_config(
name = "multinode_pingpong_combined_config",
src = "multinode_pingpong_combined.json",
flatbuffers = [
@@ -427,6 +442,7 @@
data = [
":multinode_pingpong_combined_config",
":multinode_pingpong_split3_config",
+ ":multinode_pingpong_split4_config",
":multinode_pingpong_split_config",
"//aos/events:pingpong_config",
],
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index a506850..fd3f5cb 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -1504,7 +1504,8 @@
while (true) {
auto it = node_state.second.constraints.find(current_boot);
if (it == node_state.second.constraints.end()) {
- LOG(WARNING) << "Unconnected boot in set > 1";
+ LOG(WARNING) << "Unconnected boot " << current_boot
+ << " in set > 1 for node " << node_state.first;
break;
}
@@ -1536,7 +1537,8 @@
while (true) {
auto it = node_state.second.constraints.find(current_boot);
if (it == node_state.second.constraints.end()) {
- LOG(WARNING) << "Unconnected boot in set > 1";
+ LOG(WARNING) << "Unconnected boot " << current_boot
+ << " in set > 1 for node " << node_state.first;
break;
}
@@ -1570,7 +1572,7 @@
}
CHECK_EQ(sorted_boots.size(), node_state.second.boots.size())
- << ": Graph failed to reach all the nodes.";
+ << ": Graph failed to reach all the boots on node " << node_state.first;
VLOG(1) << "Node " << node_state.first;
size_t boot_count = 0;
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 3b0b672..ec7ed10 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -3994,6 +3994,79 @@
::testing::ElementsAre(realtime_clock::epoch() + chrono::seconds(8)));
}
+// Tests that we properly handle one direction being down.
+TEST(MissingDirectionTest, OneDirection) {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(ArtifactPath(
+ "aos/events/logging/multinode_pingpong_split4_config.json"));
+ message_bridge::TestingTimeConverter time_converter(
+ configuration::NodesCount(&config.message()));
+ SimulatedEventLoopFactory event_loop_factory(&config.message());
+ event_loop_factory.SetTimeConverter(&time_converter);
+
+ NodeEventLoopFactory *const pi1 =
+ event_loop_factory.GetNodeEventLoopFactory("pi1");
+ const size_t pi1_index = configuration::GetNodeIndex(
+ event_loop_factory.configuration(), pi1->node());
+ NodeEventLoopFactory *const pi2 =
+ event_loop_factory.GetNodeEventLoopFactory("pi2");
+ const size_t pi2_index = configuration::GetNodeIndex(
+ event_loop_factory.configuration(), pi2->node());
+ std::vector<std::string> filenames;
+
+ {
+ CHECK_EQ(pi1_index, 0u);
+ CHECK_EQ(pi2_index, 1u);
+
+ time_converter.AddNextTimestamp(
+ distributed_clock::epoch(),
+ {BootTimestamp::epoch(), BootTimestamp::epoch()});
+
+ const chrono::nanoseconds reboot_time = chrono::milliseconds(5000);
+ time_converter.AddNextTimestamp(
+ distributed_clock::epoch() + reboot_time,
+ {BootTimestamp{.boot = 1,
+ .time = monotonic_clock::epoch()},
+ BootTimestamp::epoch() + reboot_time});
+ }
+
+ const std::string kLogfile2_1 =
+ aos::testing::TestTmpDir() + "/multi_logfile2.1/";
+ const std::string kLogfile1_1 =
+ aos::testing::TestTmpDir() + "/multi_logfile1.1/";
+ util::UnlinkRecursive(kLogfile2_1);
+ util::UnlinkRecursive(kLogfile1_1);
+
+ pi2->Disconnect(pi1->node());
+
+ pi1->AlwaysStart<Ping>("ping");
+ pi2->AlwaysStart<Pong>("pong");
+
+ {
+ LoggerState pi2_logger = LoggerState::MakeLogger(
+ pi2, &event_loop_factory, SupportedCompressionAlgorithms()[0]);
+
+ event_loop_factory.RunFor(chrono::milliseconds(95));
+
+ pi2_logger.StartLogger(kLogfile2_1);
+
+ event_loop_factory.RunFor(chrono::milliseconds(6000));
+
+ pi2->Connect(pi1->node());
+
+ LoggerState pi1_logger = LoggerState::MakeLogger(
+ pi1, &event_loop_factory, SupportedCompressionAlgorithms()[0]);
+ pi1_logger.StartLogger(kLogfile1_1);
+
+ event_loop_factory.RunFor(chrono::milliseconds(5000));
+ pi1_logger.AppendAllFilenames(&filenames);
+ pi2_logger.AppendAllFilenames(&filenames);
+ }
+
+ const std::vector<LogFile> sorted_parts = SortParts(filenames);
+ ConfirmReadable(filenames);
+}
+
} // namespace testing
} // namespace logger
} // namespace aos
diff --git a/aos/events/logging/multinode_pingpong_split4.json b/aos/events/logging/multinode_pingpong_split4.json
new file mode 100644
index 0000000..eed4880
--- /dev/null
+++ b/aos/events/logging/multinode_pingpong_split4.json
@@ -0,0 +1,185 @@
+{
+ "channels": [
+ {
+ "name": "/pi1/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi1",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi2",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ /* Logged on pi1 locally */
+ {
+ "name": "/pi1/aos",
+ "type": "aos.timing.Report",
+ "source_node": "pi1",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.timing.Report",
+ "source_node": "pi2",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/pi1/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "logger": "LOCAL_LOGGER",
+ "source_node": "pi1"
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "logger": "LOCAL_LOGGER",
+ "source_node": "pi2"
+ },
+ {
+ "name": "/pi1/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "logger": "LOCAL_LOGGER",
+ "source_node": "pi1"
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "logger": "LOCAL_LOGGER",
+ "source_node": "pi2"
+ },
+ {
+ "name": "/pi1/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["pi2"],
+ "source_node": "pi1",
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi1"],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["pi1"],
+ "source_node": "pi2",
+ "destination_nodes": [
+ {
+ "name": "pi1",
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi1/aos/remote_timestamps/pi2/pi1/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "logger": "NOT_LOGGED",
+ "num_senders": 2,
+ "source_node": "pi1"
+ },
+ {
+ "name": "/pi1/aos/remote_timestamps/pi2/test/aos-examples-Ping",
+ "type": "aos.message_bridge.RemoteMessage",
+ "logger": "NOT_LOGGED",
+ "num_senders": 2,
+ "source_node": "pi1",
+ "frequency": 150
+ },
+ {
+ "name": "/pi2/aos/remote_timestamps/pi1/test/aos-examples-Pong",
+ "type": "aos.message_bridge.RemoteMessage",
+ "logger": "NOT_LOGGED",
+ "num_senders": 2,
+ "source_node": "pi2",
+ "frequency": 150
+ },
+ /* Forwarded to pi2 */
+ {
+ "name": "/test",
+ "type": "aos.examples.Ping",
+ "source_node": "pi1",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["pi2"],
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "pi1"
+ ],
+ "time_to_live": 5000000
+ }
+ ],
+ "frequency": 150
+ },
+ /* Forwarded back to pi1.
+ * The message is logged both on the sending node and the receiving node
+ * (to make it easier to look at the results for now).
+ *
+ * The timestamps are logged on the receiving node.
+ */
+ {
+ "name": "/test",
+ "type": "aos.examples.Pong",
+ "source_node": "pi2",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": ["pi1"],
+ "destination_nodes": [
+ {
+ "name": "pi1",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ],
+ "frequency": 150
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "pi1"
+ },
+ "rename": {
+ "name": "/pi1/aos"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "pi2"
+ },
+ "rename": {
+ "name": "/pi2/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "pi1",
+ "hostname": "raspberrypi",
+ "port": 9971
+ },
+ {
+ "name": "pi2",
+ "hostname": "raspberrypi2",
+ "port": 9971
+ }
+ ]
+}
diff --git a/aos/network/BUILD b/aos/network/BUILD
index bc68c90..ac9d96e 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -507,6 +507,7 @@
"//aos:configuration",
"//aos/events/logging:boot_timestamp",
"//aos/time",
+ "@com_google_absl//absl/numeric:int128",
"@com_google_absl//absl/strings",
],
)
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index d6419a0..0619b5e 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -39,9 +39,6 @@
node_mapping_.resize(count, 0);
}
-// TODO(austin): Add linear inequality constraints too. Currently we just
-// enforce them.
-//
// TODO(austin): Add a rate of change constraint from the last sample. 1
// ms/s. Figure out how to define it. Do this last. This lets us handle
// constraints going away, and constraints close in time.
@@ -50,8 +47,42 @@
bool success = true;
for (size_t i = 0u; i < clock_offset_filter_for_node_.size(); ++i) {
for (const struct FilterPair &filter : clock_offset_filter_for_node_[i]) {
- success = success && filter.filter->ValidateSolution(
- solution[i], solution[filter.b_index]);
+ // There's nothing in this direction, so there will be nothing to
+ // validate.
+ if (filter.filter->timestamps_size(
+ base_clock_[i].boot, base_clock_[filter.b_index].boot) == 0u) {
+ // For a boot to exist, we need to have some observations between it and
+ // another boot. We wouldn't bother to build a problem to solve for
+ // this node otherwise. Confirm that is true so we at least get
+ // notified if that assumption falls apart.
+ bool valid = false;
+ for (const struct FilterPair &other_filter :
+ clock_offset_filter_for_node_[filter.b_index]) {
+ if (other_filter.b_index == i) {
+ // Found our match. Confirm it has timestamps.
+ if (other_filter.filter->timestamps_size(
+ base_clock_[filter.b_index].boot, base_clock_[i].boot) !=
+ 0u) {
+ valid = true;
+ }
+ break;
+ }
+ }
+ if (!valid) {
+ Debug();
+ LOG(FATAL) << "Found no timestamps in either direction between nodes "
+ << i << " and " << filter.b_index;
+ }
+ continue;
+ }
+ const bool iteration = filter.filter->ValidateSolution(
+ solution[i], solution[filter.b_index]);
+ if (!iteration) {
+ filter.filter->ValidateSolution(solution[i], 0.0,
+ solution[filter.b_index], 0.0);
+ }
+
+ success = success && iteration;
}
}
return success;
@@ -62,6 +93,14 @@
Eigen::VectorXd grad = Eigen::VectorXd::Zero(live_nodes_);
for (size_t i = 0; i < clock_offset_filter_for_node_.size(); ++i) {
for (const struct FilterPair &filter : clock_offset_filter_for_node_[i]) {
+ // Especially when reboots are involved, it isn't guarenteed that there
+ // will be timestamps going both ways. In this case, we want to avoid the
+ // cost.
+ if (!filter.filter->timestamps_size(base_clock_[i].boot,
+ base_clock_[filter.b_index].boot)) {
+ continue;
+ }
+
// Reminder, our cost function has the following form.
// ((tb - (1 + ma) ta - ba)^2
// We are ignoring the slope when taking the derivative and applying the
@@ -70,11 +109,28 @@
//
const size_t a_solution_index = NodeToFullSolutionIndex(i);
const size_t b_solution_index = NodeToFullSolutionIndex(filter.b_index);
+
+ // Most of the time, we properly converge to the right answer when only
+ // one of the two constraints exists. But, with rounding involved, we
+ // will end up generating 2 timelines with 2 problems:
+ // 1) If the TX and RX times are identical (zero network delay), then it
+ // is very hard to order the two messages without adding something on
+ // top of time.
+ // 2) In the presence of rounding, we can violate our constraints by 1
+ // ns, failing validation.
+ //
+ // Rather than teach the solver about these constraints, we can instead
+ // have it try to solve for an offset which is a bit bigger than it is
+ // supposed to be. Since both directions, when they exist, will have this
+ // extra factor, the solution will be the same (or close enough).
+ constexpr double kMinNetworkDelay = 2.0;
+
const double error =
- 2.0 * filter.filter->OffsetError(base_clock_[i],
- time_offsets(a_solution_index),
- base_clock_[filter.b_index],
- time_offsets(b_solution_index));
+ 2.0 * (filter.filter->OffsetError(base_clock_[i],
+ time_offsets(a_solution_index),
+ base_clock_[filter.b_index],
+ time_offsets(b_solution_index)) -
+ kMinNetworkDelay);
grad(a_solution_index) += -error;
grad(b_solution_index) += error;
@@ -302,7 +358,7 @@
for (size_t i = 0; i < points.size(); ++i) {
if (points[i] != logger::BootTimestamp::max_time()) {
- VLOG(2) << "Solving for node " << i << " of " << base_clock(i) << " in "
+ VLOG(2) << "Solving for node " << i << " of " << points[i] << " in "
<< solution_number << " cycles";
}
}
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index ddb945d..87133d1 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -24,6 +24,18 @@
return ss.str();
}
+std::string TimeString(const aos::monotonic_clock::time_point t_base, double t,
+ std::chrono::nanoseconds o_base, double o) {
+ std::stringstream ss;
+ ss << "O(" << t_base << ", " << t
+ << ") = " << o_base.count() + static_cast<int64_t>(std::round(o)) << "ns, "
+ << o - std::round(o) << ", remote "
+ << t_base + o_base +
+ chrono::nanoseconds(static_cast<int64_t>(std::round(t + o)))
+ << "." << t + o - static_cast<int64_t>(std::round(t + o));
+ return ss.str();
+}
+
std::string TimeString(
const std::tuple<aos::monotonic_clock::time_point, std::chrono::nanoseconds>
t) {
@@ -533,7 +545,10 @@
// ta
// Since dt < 0, we shift by dt * slope to get that value
return std::get<1>(p0) +
- chrono::duration_cast<chrono::nanoseconds>(dt * kMaxVelocity());
+ chrono::nanoseconds(static_cast<int64_t>(
+ (absl::int128(dt.count() - MaxVelocityRatio::den / 2) *
+ absl::int128(MaxVelocityRatio::num)) /
+ absl::int128(MaxVelocityRatio::den)));
} else {
// Extrapolate forwards, using the (negative) MaxVelocity slope
// Same concept, except going foward past our last (most recent) sample:
@@ -543,7 +558,10 @@
// ta
// Since dt > 0, we shift by - dt * slope to get that value
return std::get<1>(p0) -
- chrono::duration_cast<chrono::nanoseconds>(dt * kMaxVelocity());
+ chrono::nanoseconds(static_cast<int64_t>(
+ (absl::int128(dt.count() + MaxVelocityRatio::den / 2) *
+ absl::int128(MaxVelocityRatio::num)) /
+ absl::int128(MaxVelocityRatio::den)));
}
}
@@ -616,6 +634,7 @@
chrono::nanoseconds NoncausalTimestampFilter::ExtrapolateOffset(
std::tuple<monotonic_clock::time_point, std::chrono::nanoseconds> p0,
monotonic_clock::time_point /*ta_base*/, double /*ta*/) {
+ // TODO(austin): 128 bit math again? ...
// For this version, use the base offset from p0 as the base for the offset
return std::get<1>(p0);
}
@@ -697,7 +716,7 @@
std::pair<chrono::nanoseconds, double>
NoncausalTimestampFilter::SingleFilter::Offset(
monotonic_clock::time_point ta_base, double ta) const {
- CHECK_GT(timestamps_size(), 0u);
+ CHECK_GT(timestamps_size(), 0u) << node_names_;
if (IsOutsideSamples(ta_base, ta)) {
// Special case size = 1 or ta_base before first timestamp or
// after last timesteamp, so we need to extrapolate out
@@ -741,7 +760,10 @@
NormalizeTimestamps(&ta_base, &ta);
NormalizeTimestamps(&tb_base, &tb);
- const SingleFilter *f = filter(ta_base.boot, tb_base.boot);
+ const SingleFilter *f = maybe_filter(ta_base.boot, tb_base.boot);
+ if (f == nullptr || f->timestamps_size() == 0u) {
+ return "0";
+ }
if (f->IsOutsideSamples(ta_base.time, ta)) {
auto reference_timestamp = f->GetReferenceTimestamp(ta_base.time, ta);
@@ -794,6 +816,66 @@
}
bool NoncausalTimestampFilter::SingleFilter::ValidateSolution(
+ aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base, double tb) const {
+ NormalizeTimestamps(&ta_base, &ta);
+ NormalizeTimestamps(&tb_base, &tb);
+ CHECK_GT(timestamps_size(), 0u);
+ if (ta_base < std::get<0>(timestamp(0)) && has_popped_) {
+ LOG(ERROR) << node_names_ << " O(" << ta_base << ", " << ta
+ << ") is before the start and we have forgotten the answer.";
+ return false;
+ }
+ if (IsOutsideSamples(ta_base, ta)) {
+ // Special case size = 1 or ta_base before first timestamp or
+ // after last timestamp, so we need to extrapolate out
+ auto reference_timestamp = GetReferenceTimestamp(ta_base, ta);
+
+ // Special case size = 1 or ta before first timestamp, so we extrapolate
+ const chrono::nanoseconds offset_base =
+ NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp,
+ ta_base, ta);
+ const double offset_remainder =
+ NoncausalTimestampFilter::ExtrapolateOffsetRemainder(
+ reference_timestamp, ta_base, ta);
+
+ // We want to do offset + ta > tb, but we need to do it with minimal
+ // numerical precision problems.
+ if (static_cast<double>((offset_base + ta_base - tb_base).count()) >=
+ tb - ta - offset_remainder) {
+ LOG(ERROR) << node_names_ << " "
+ << TimeString(ta_base, ta, offset_base, offset_remainder)
+ << " > solution time "
+ << tb_base + chrono::nanoseconds(
+ static_cast<int64_t>(std::round(tb)))
+ << ", " << tb - std::round(tb) << " foo";
+ LOG(INFO) << "Remainder " << offset_remainder;
+ return false;
+ }
+ return true;
+ }
+
+ std::pair<std::tuple<monotonic_clock::time_point, chrono::nanoseconds>,
+ std::tuple<monotonic_clock::time_point, chrono::nanoseconds>>
+ points = FindTimestamps(ta_base, ta);
+ const chrono::nanoseconds offset_base =
+ NoncausalTimestampFilter::InterpolateOffset(points.first, points.second,
+ ta_base, ta);
+ const double offset = NoncausalTimestampFilter::InterpolateOffsetRemainder(
+ points.first, points.second, ta_base, ta);
+ if (static_cast<double>((offset_base + ta_base - tb_base).count()) >=
+ tb - offset - ta) {
+ LOG(ERROR) << node_names_ << " "
+ << TimeString(ta_base, ta, offset_base, offset)
+ << " > solution time " << tb_base << ", " << tb;
+ LOG(ERROR) << "Bracketing times are " << TimeString(points.first) << " and "
+ << TimeString(points.second);
+ return false;
+ }
+ return true;
+}
+
+bool NoncausalTimestampFilter::SingleFilter::ValidateSolution(
aos::monotonic_clock::time_point ta,
aos::monotonic_clock::time_point tb) const {
CHECK_GT(timestamps_size(), 0u);
@@ -810,7 +892,7 @@
// Special case size = 1 or ta before first timestamp, so we extrapolate
const chrono::nanoseconds offset =
NoncausalTimestampFilter::ExtrapolateOffset(reference_timestamp, ta);
- if (offset + ta > tb) {
+ if (offset + ta >= tb) {
LOG(ERROR) << node_names_ << " " << TimeString(ta, offset)
<< " > solution time " << tb;
return false;
@@ -824,7 +906,7 @@
const chrono::nanoseconds offset =
NoncausalTimestampFilter::InterpolateOffset(points.first, points.second,
ta);
- if (offset + ta > tb) {
+ if (offset + ta >= tb) {
LOG(ERROR) << node_names_ << " " << TimeString(ta, offset)
<< " > solution time " << tb;
LOG(ERROR) << "Bracketing times are " << TimeString(points.first) << " and "
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index c395554..6486a1b 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -7,6 +7,7 @@
#include <cstdio>
#include <deque>
+#include "absl/numeric/int128.h"
#include "aos/configuration.h"
#include "aos/events/logging/boot_timestamp.h"
#include "aos/time/time.h"
@@ -19,7 +20,11 @@
// integer and divide by the value (e.g., / 1000)
// Max velocity to clamp the filter to in seconds/second.
-inline constexpr double kMaxVelocity() { return 0.001; }
+typedef std::ratio<1, 1000> MaxVelocityRatio;
+inline constexpr double kMaxVelocity() {
+ return static_cast<double>(MaxVelocityRatio::num) /
+ static_cast<double>(MaxVelocityRatio::den);
+}
// This class handles filtering differences between clocks across a network.
//
@@ -280,6 +285,11 @@
logger::BootTimestamp tb) const {
return filter(ta.boot, tb.boot)->ValidateSolution(ta.time, tb.time);
}
+ bool ValidateSolution(logger::BootTimestamp ta_base, double ta,
+ logger::BootTimestamp tb_base, double tb) const {
+ return filter(ta_base.boot, tb_base.boot)
+ ->ValidateSolution(ta_base.time, ta, tb_base.time, tb);
+ }
// Adds a new sample to our filtered timestamp list.
void Sample(logger::BootTimestamp monotonic_now,
@@ -297,6 +307,17 @@
return result;
}
+ // Returns the number of timestamps for a specific boot. This is useful to
+ // determine if there are observations in this direction or not.
+ size_t timestamps_size(const size_t boota, const size_t bootb) const {
+ const SingleFilter *f = maybe_filter(boota, bootb);
+ if (f == nullptr) {
+ return 0u;
+ } else {
+ return f->timestamps_size();
+ }
+ }
+
// For testing only:
void Debug() const {
for (const BootFilter &filter : filters_) {
@@ -545,12 +566,17 @@
//
// We are doing this here so as points get added in any order, we don't
// confuse ourselves about what really happened.
- if (doffset > dt * kMaxVelocity()) {
+ if (absl::int128(doffset.count()) *
+ absl::int128(MaxVelocityRatio::den) >
+ absl::int128(dt.count()) * absl::int128(MaxVelocityRatio::num)) {
+ DCHECK_GE(dt.count(), 0);
const aos::monotonic_clock::duration adjusted_initial_time =
std::get<1>(timestamps_[1]) -
aos::monotonic_clock::duration(
static_cast<aos::monotonic_clock::duration::rep>(
- dt.count() * kMaxVelocity()));
+ absl::int128(dt.count() + MaxVelocityRatio::den / 2) *
+ absl::int128(MaxVelocityRatio::num) /
+ absl::int128(MaxVelocityRatio::den)));
return std::make_tuple(std::get<0>(timestamps_[0]),
adjusted_initial_time);
@@ -564,6 +590,9 @@
// success.
bool ValidateSolution(aos::monotonic_clock::time_point ta,
aos::monotonic_clock::time_point tb) const;
+ bool ValidateSolution(aos::monotonic_clock::time_point ta_base, double ta,
+ aos::monotonic_clock::time_point tb_base,
+ double tb) const;
void Sample(monotonic_clock::time_point monotonic_now,
std::chrono::nanoseconds sample_ns);
@@ -663,14 +692,23 @@
return result;
}
- const SingleFilter *filter(int boota, int bootb) const {
+ const SingleFilter *maybe_filter(int boota, int bootb) const {
auto it =
std::lower_bound(filters_.begin(), filters_.end(),
std::make_pair(boota, bootb), FilterLessThanLower);
CHECK(it != filters_.end());
- CHECK(it->boot == std::make_pair(boota, bootb))
+ if (it->boot == std::make_pair(boota, bootb)) {
+ return &it->filter;
+ } else {
+ return nullptr;
+ }
+ }
+
+ const SingleFilter *filter(int boota, int bootb) const {
+ const SingleFilter *result = maybe_filter(boota, bootb);
+ CHECK(result != nullptr)
<< NodeNames() << " Failed to find " << boota << ", " << bootb;
- return &it->filter;
+ return result;
}
private:
diff --git a/aos/network/timestamp_filter_test.cc b/aos/network/timestamp_filter_test.cc
index 37d02ac..a5a4b98 100644
--- a/aos/network/timestamp_filter_test.cc
+++ b/aos/network/timestamp_filter_test.cc
@@ -854,22 +854,28 @@
NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1), t1),
o1);
- // With small offset (<1/kMaxVelocity() sec), low precision can't
- // tell the difference since the delta is a fraction of a nanosecond
+ // Test that we round correctly when extrapolating.
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t1, o1), t1 + chrono::nanoseconds(-400)),
+ o1);
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
std::make_tuple(t1, o1), t1 + chrono::nanoseconds(-800)),
+ o1 - chrono::nanoseconds(1));
+
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t1, o1), t1 + chrono::nanoseconds(400)),
o1);
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
- std::make_tuple(t1, o1), e + chrono::nanoseconds(1000)),
- o1 - chrono::nanoseconds(
- int64_t((t1 - (e + chrono::nanoseconds(1000))).count() *
- kMaxVelocity())));
+ std::make_tuple(t1, o1), t1 + chrono::nanoseconds(800)),
+ o1 - chrono::nanoseconds(1));
+ EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
+ std::make_tuple(t1, o1), t1 + chrono::nanoseconds(1000)),
+ o1 - chrono::nanoseconds(1));
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(
std::make_tuple(t1, o1), t1 + chrono::nanoseconds(-9000)),
- o1 + chrono::nanoseconds(int64_t(
- chrono::nanoseconds(-9000).count() * kMaxVelocity())));
+ o1 - chrono::nanoseconds(9));
// Test base + double version
EXPECT_EQ(NoncausalTimestampFilter::ExtrapolateOffset(std::make_tuple(t1, o1),