Fix log sorting for good
Unfortunately, this is really hard to split up, so a bunch of stuff
happens at once.
When a message is sent from node A -> node B, add support for sending
that timestamp back from node B to node A for logging.
{
"name": "/pi1/aos",
"type": "aos.message_bridge.Timestamp",
"source_node": "pi1",
"frequency": 10,
"max_size": 200,
"destination_nodes": [
{
"name": "pi2",
"priority": 1,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
"timestamp_logger_nodes": ["pi1"]
}
]
},
This gives us a way to log enough information on node A such that
everything is self contained. We know all the messages we sent to B,
and when they got there, so we can recreate the time offset and replay
the node.
This data is then published over
{ "name": "/aos/remote_timestamps/pi2", "type": ".aos.logger.MessageHeader"}
The logger then treats that channel specially and log the contents
directly as though the message contents were received on the remote
node.
This (among other things) exposes log sorting problems. Use our fancy
new infinite precision noncausal filter to estimate time precise enough
to actually order events. This gets us down to 2-3 ns of error due to
integer precision.
Change-Id: Ia843c5176a2c4efc227e669c07d7bb4c7cbe7c91
diff --git a/aos/network/BUILD b/aos/network/BUILD
index f9fe7ea..69482e1 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -268,6 +268,7 @@
name = "message_bridge_test_common_config",
src = "message_bridge_test_common.json",
flatbuffers = [
+ "//aos/events/logging:logger_fbs",
"//aos/events:ping_fbs",
"//aos/events:pong_fbs",
"//aos/network:message_bridge_client_fbs",
diff --git a/aos/network/message_bridge_client_lib.cc b/aos/network/message_bridge_client_lib.cc
index b371181..00040e1 100644
--- a/aos/network/message_bridge_client_lib.cc
+++ b/aos/network/message_bridge_client_lib.cc
@@ -78,6 +78,8 @@
logger::MessageHeader::Builder message_header_builder(fbb);
message_header_builder.add_channel_index(0);
message_header_builder.add_monotonic_sent_time(0);
+ message_header_builder.add_realtime_sent_time(0);
+ message_header_builder.add_queue_index(0);
message_header_builder.add_monotonic_remote_time(0);
message_header_builder.add_realtime_remote_time(0);
message_header_builder.add_remote_queue_index(0);
@@ -250,6 +252,10 @@
message_header->channel_index());
message_reception_reply_.mutable_message()->mutate_monotonic_sent_time(
message_header->monotonic_sent_time());
+ message_reception_reply_.mutable_message()->mutate_realtime_sent_time(
+ message_header->realtime_sent_time());
+ message_reception_reply_.mutable_message()->mutate_queue_index(
+ message_header->queue_index());
// And capture the relevant data needed to generate the forwarding
// MessageHeader.
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index 2832339..de836ae 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -1,5 +1,6 @@
#include "aos/network/message_bridge_server_lib.h"
+#include "absl/strings/str_cat.h"
#include "absl/types/span.h"
#include "aos/events/logging/logger.h"
#include "aos/events/logging/logger_generated.h"
@@ -85,14 +86,55 @@
// and flushes. Whee.
}
-void ChannelState::HandleDelivery(sctp_assoc_t /*rcv_assoc_id*/,
- uint16_t /*ssn*/,
+void ChannelState::HandleDelivery(sctp_assoc_t rcv_assoc_id, uint16_t /*ssn*/,
absl::Span<const uint8_t> data) {
const logger::MessageHeader *message_header =
flatbuffers::GetRoot<logger::MessageHeader>(data.data());
+ for (Peer &peer : peers_) {
+ if (peer.sac_assoc_id == rcv_assoc_id) {
+ if (peer.timestamp_logger != nullptr) {
+ // TODO(austin): Need to implement reliable sending of the delivery
+ // timestamps. Track what made it, and retry what didn't.
+ //
+ // This needs to be munged and cleaned up to match the timestamp
+ // standard.
+
+ aos::Sender<logger::MessageHeader>::Builder builder =
+ peer.timestamp_logger->MakeBuilder();
+
+ logger::MessageHeader::Builder message_header_builder =
+ builder.MakeBuilder<logger::MessageHeader>();
+
+ message_header_builder.add_channel_index(
+ message_header->channel_index());
+
+ message_header_builder.add_queue_index(
+ message_header->remote_queue_index());
+ message_header_builder.add_monotonic_sent_time(
+ message_header->monotonic_remote_time());
+ message_header_builder.add_realtime_sent_time(
+ message_header->realtime_remote_time());
+
+ // Swap the remote and sent metrics. They are from the sender's
+ // perspective, not the receiver's perspective.
+ message_header_builder.add_monotonic_remote_time(
+ message_header->monotonic_sent_time());
+ message_header_builder.add_realtime_remote_time(
+ message_header->realtime_sent_time());
+ message_header_builder.add_remote_queue_index(
+ message_header->queue_index());
+
+ builder.Send(message_header_builder.Finish());
+ }
+ break;
+ }
+ }
+
while (sent_messages_.size() > 0u) {
if (sent_messages_.begin()->message().monotonic_sent_time() ==
- message_header->monotonic_sent_time()) {
+ message_header->monotonic_sent_time() &&
+ sent_messages_.begin()->message().queue_index() ==
+ message_header->queue_index()) {
sent_messages_.pop_front();
continue;
}
@@ -124,11 +166,12 @@
// time out eventually. Need to sort that out.
}
-void ChannelState::AddPeer(const Connection *connection, int node_index,
- ServerConnection *server_connection_statistics,
- bool logged_remotely) {
+void ChannelState::AddPeer(
+ const Connection *connection, int node_index,
+ ServerConnection *server_connection_statistics, bool logged_remotely,
+ aos::Sender<logger::MessageHeader> *timestamp_logger) {
peers_.emplace_back(connection, node_index, server_connection_statistics,
- logged_remotely);
+ logged_remotely, timestamp_logger);
}
int ChannelState::NodeDisconnected(sctp_assoc_t assoc_id) {
@@ -168,6 +211,7 @@
CHECK(event_loop_->node() != nullptr) << ": No nodes configured.";
int32_t max_size = 0;
+ timestamp_loggers_.resize(event_loop->configuration()->nodes()->size());
// Seed up all the per-node connection state.
// We are making the assumption here that every connection is bidirectional
@@ -217,13 +261,30 @@
for (const Connection *connection : *channel->destination_nodes()) {
const Node *other_node = configuration::GetNode(
event_loop_->configuration(), connection->name()->string_view());
+ const size_t other_node_index = configuration::GetNodeIndex(
+ event_loop_->configuration(), other_node);
+
+ const bool delivery_time_is_logged =
+ configuration::ConnectionDeliveryTimeIsLoggedOnNode(
+ connection, event_loop_->node());
+
+ // Conditionally create the timestamp logger if we are supposed to log
+ // timestamps from it.
+ if (delivery_time_is_logged && !timestamp_loggers_[other_node_index]) {
+ timestamp_loggers_[other_node_index] =
+ event_loop_->MakeSender<logger::MessageHeader>(
+ absl::StrCat("/aos/remote_timestamps/",
+ connection->name()->string_view()));
+ }
state->AddPeer(
connection,
configuration::GetNodeIndex(event_loop_->configuration(),
connection->name()->string_view()),
server_status_.FindServerConnection(
connection->name()->string_view()),
- configuration::ChannelMessageIsLoggedOnNode(channel, other_node));
+ configuration::ChannelMessageIsLoggedOnNode(channel, other_node),
+ delivery_time_is_logged ? ×tamp_loggers_[other_node_index]
+ : nullptr);
}
// Don't subscribe to timestamps on the timestamp channel. Those get
diff --git a/aos/network/message_bridge_server_lib.h b/aos/network/message_bridge_server_lib.h
index b6a4f05..ad384f4 100644
--- a/aos/network/message_bridge_server_lib.h
+++ b/aos/network/message_bridge_server_lib.h
@@ -32,10 +32,12 @@
struct Peer {
Peer(const Connection *new_connection, int new_node_index,
ServerConnection *new_server_connection_statistics,
- bool new_logged_remotely)
+ bool new_logged_remotely,
+ aos::Sender<logger::MessageHeader> *new_timestamp_logger)
: connection(new_connection),
node_index(new_node_index),
server_connection_statistics(new_server_connection_statistics),
+ timestamp_logger(new_timestamp_logger),
logged_remotely(new_logged_remotely) {}
// Valid if != 0.
@@ -45,6 +47,7 @@
const aos::Connection *connection;
const int node_index;
ServerConnection *server_connection_statistics;
+ aos::Sender<logger::MessageHeader> *timestamp_logger = nullptr;
// If true, this message will be logged on a receiving node. We need to
// keep it around to log it locally if that fails.
@@ -60,7 +63,8 @@
// Adds a new peer.
void AddPeer(const Connection *connection, int node_index,
ServerConnection *server_connection_statistics,
- bool logged_remotely);
+ bool logged_remotely,
+ aos::Sender<logger::MessageHeader> *timestamp_logger);
// Returns true if this channel has the same name and type as the other
// channel.
@@ -111,6 +115,7 @@
// Event loop to schedule everything on.
aos::ShmEventLoop *event_loop_;
+ std::vector<aos::Sender<logger::MessageHeader>> timestamp_loggers_;
SctpServer server_;
MessageBridgeServerStatus server_status_;
diff --git a/aos/network/message_bridge_test.cc b/aos/network/message_bridge_test.cc
index 8d52292..aa0a034 100644
--- a/aos/network/message_bridge_test.cc
+++ b/aos/network/message_bridge_test.cc
@@ -3,6 +3,7 @@
#include <chrono>
#include <thread>
+#include "absl/strings/str_cat.h"
#include "aos/events/ping_generated.h"
#include "aos/events/pong_generated.h"
#include "aos/network/message_bridge_client_lib.h"
@@ -10,11 +11,22 @@
#include "aos/network/team_number.h"
namespace aos {
+void SetShmBase(const std::string_view base);
+
namespace message_bridge {
namespace testing {
namespace chrono = std::chrono;
+void DoSetShmBase(const std::string_view node) {
+ const char *tmpdir_c_str = getenv("TEST_TMPDIR");
+ if (tmpdir_c_str != nullptr) {
+ aos::SetShmBase(absl::StrCat(tmpdir_c_str, "/", node));
+ } else {
+ aos::SetShmBase(absl::StrCat("/dev/shm/", node));
+ }
+}
+
// Test that we can send a ping message over sctp and receive it.
TEST(MessageBridgeTest, PingPong) {
// This is rather annoying to set up. We need to start up a client and
@@ -44,9 +56,11 @@
aos::configuration::ReadConfig(
"aos/network/message_bridge_test_client_config.json");
+ DoSetShmBase("pi1");
FLAGS_application_name = "pi1_message_bridge_server";
// Force ourselves to be "raspberrypi" and allocate everything.
FLAGS_override_hostname = "raspberrypi";
+
aos::ShmEventLoop pi1_server_event_loop(&server_config.message());
MessageBridgeServer pi1_message_bridge_server(&pi1_server_event_loop);
@@ -60,9 +74,22 @@
aos::Sender<examples::Ping> ping_sender =
ping_event_loop.MakeSender<examples::Ping>("/test");
+ aos::ShmEventLoop pi1_test_event_loop(&server_config.message());
+ aos::Fetcher<logger::MessageHeader> message_header_fetcher1 =
+ pi1_test_event_loop.MakeFetcher<logger::MessageHeader>(
+ "/pi1/aos/remote_timestamps/pi2");
+
+ // Fetchers for confirming the remote timestamps made it.
+ aos::Fetcher<examples::Ping> ping_on_pi1_fetcher =
+ ping_event_loop.MakeFetcher<examples::Ping>("/test");
+ aos::Fetcher<Timestamp> pi1_on_pi1_timestamp_fetcher =
+ ping_event_loop.MakeFetcher<Timestamp>("/aos");
+
// Now do it for "raspberrypi2", the client.
FLAGS_application_name = "pi2_message_bridge_client";
FLAGS_override_hostname = "raspberrypi2";
+ DoSetShmBase("pi2");
+
aos::ShmEventLoop pi2_client_event_loop(&pi2_config.message());
MessageBridgeClient pi2_message_bridge_client(&pi2_client_event_loop);
@@ -80,11 +107,24 @@
aos::Fetcher<ClientStatistics> client_statistics_fetcher =
test_event_loop.MakeFetcher<ClientStatistics>("/aos");
+ aos::Fetcher<logger::MessageHeader> message_header_fetcher2 =
+ test_event_loop.MakeFetcher<logger::MessageHeader>(
+ "/pi2/aos/remote_timestamps/pi1");
+
+ // Event loop for fetching data delivered to pi2 from pi1 to match up
+ // messages.
+ aos::ShmEventLoop delivered_messages_event_loop(&pi2_config.message());
+ aos::Fetcher<Timestamp> pi1_on_pi2_timestamp_fetcher =
+ delivered_messages_event_loop.MakeFetcher<Timestamp>("/pi1/aos");
+ aos::Fetcher<examples::Ping> ping_on_pi2_fetcher =
+ delivered_messages_event_loop.MakeFetcher<examples::Ping>("/test");
+ EXPECT_FALSE(ping_on_pi2_fetcher.Fetch());
+ EXPECT_FALSE(pi1_on_pi2_timestamp_fetcher.Fetch());
// Count the pongs.
int pong_count = 0;
pong_event_loop.MakeWatcher(
- "/test2", [&pong_count](const examples::Ping &ping) {
+ "/test", [&pong_count](const examples::Ping &ping) {
++pong_count;
LOG(INFO) << "Got ping back " << FlatbufferToJson(&ping);
});
@@ -191,11 +231,11 @@
ping_event_loop.MakeWatcher("/pi1/aos", [](const Timestamp ×tamp) {
EXPECT_TRUE(timestamp.has_offsets());
- LOG(INFO) << FlatbufferToJson(×tamp);
+ LOG(INFO) << "/pi1/aos Timestamp " << FlatbufferToJson(×tamp);
});
pong_event_loop.MakeWatcher("/pi2/aos", [](const Timestamp ×tamp) {
EXPECT_TRUE(timestamp.has_offsets());
- LOG(INFO) << FlatbufferToJson(×tamp);
+ LOG(INFO) << "/pi2/aos Timestamp " << FlatbufferToJson(×tamp);
});
// Run for 5 seconds to make sure we have time to estimate the offset.
@@ -206,6 +246,96 @@
quit->Setup(ping_event_loop.monotonic_now() + chrono::milliseconds(5050));
});
+ // Find the channel index for both the /pi1/aos Timestamp channel and Ping
+ // channel.
+ const size_t pi1_timestamp_channel = configuration::ChannelIndex(
+ pong_event_loop.configuration(), pi1_on_pi2_timestamp_fetcher.channel());
+ const size_t ping_timestamp_channel =
+ configuration::ChannelIndex(delivered_messages_event_loop.configuration(),
+ ping_on_pi2_fetcher.channel());
+
+ for (const Channel *channel : *ping_event_loop.configuration()->channels()) {
+ VLOG(1) << "Channel "
+ << configuration::ChannelIndex(ping_event_loop.configuration(),
+ channel)
+ << " " << configuration::CleanedChannelToString(channel);
+ }
+
+ // For each remote timestamp we get back, confirm that it is either a ping
+ // message, or a timestamp we sent out. Also confirm that the timestamps are
+ // correct.
+ ping_event_loop.MakeWatcher(
+ "/pi1/aos/remote_timestamps/pi2",
+ [pi1_timestamp_channel, ping_timestamp_channel, &ping_on_pi2_fetcher,
+ &ping_on_pi1_fetcher, &pi1_on_pi2_timestamp_fetcher,
+ &pi1_on_pi1_timestamp_fetcher](const logger::MessageHeader &header) {
+ VLOG(1) << aos::FlatbufferToJson(&header);
+
+ const aos::monotonic_clock::time_point header_monotonic_sent_time(
+ chrono::nanoseconds(header.monotonic_sent_time()));
+ const aos::realtime_clock::time_point header_realtime_sent_time(
+ chrono::nanoseconds(header.realtime_sent_time()));
+ const aos::monotonic_clock::time_point header_monotonic_remote_time(
+ chrono::nanoseconds(header.monotonic_remote_time()));
+ const aos::realtime_clock::time_point header_realtime_remote_time(
+ chrono::nanoseconds(header.realtime_remote_time()));
+
+ const Context *pi1_context = nullptr;
+ const Context *pi2_context = nullptr;
+
+ if (header.channel_index() == pi1_timestamp_channel) {
+ // Find the forwarded message.
+ while (pi1_on_pi2_timestamp_fetcher.context().monotonic_event_time <
+ header_monotonic_sent_time) {
+ ASSERT_TRUE(pi1_on_pi2_timestamp_fetcher.FetchNext());
+ }
+
+ // And the source message.
+ while (pi1_on_pi1_timestamp_fetcher.context().monotonic_event_time <
+ header_monotonic_remote_time) {
+ ASSERT_TRUE(pi1_on_pi1_timestamp_fetcher.FetchNext());
+ }
+
+ pi1_context = &pi1_on_pi1_timestamp_fetcher.context();
+ pi2_context = &pi1_on_pi2_timestamp_fetcher.context();
+ } else if (header.channel_index() == ping_timestamp_channel) {
+ // Find the forwarded message.
+ while (ping_on_pi2_fetcher.context().monotonic_event_time <
+ header_monotonic_sent_time) {
+ ASSERT_TRUE(ping_on_pi2_fetcher.FetchNext());
+ }
+
+ // And the source message.
+ while (ping_on_pi1_fetcher.context().monotonic_event_time <
+ header_monotonic_remote_time) {
+ ASSERT_TRUE(ping_on_pi1_fetcher.FetchNext());
+ }
+
+ pi1_context = &ping_on_pi1_fetcher.context();
+ pi2_context = &ping_on_pi2_fetcher.context();
+ } else {
+ LOG(FATAL) << "Unknown channel";
+ }
+
+ // Confirm the forwarded message has matching timestamps to the
+ // timestamps we got back.
+ EXPECT_EQ(pi2_context->queue_index, header.queue_index());
+ EXPECT_EQ(pi2_context->monotonic_event_time,
+ header_monotonic_sent_time);
+ EXPECT_EQ(pi2_context->realtime_event_time, header_realtime_sent_time);
+ EXPECT_EQ(pi2_context->realtime_remote_time,
+ header_realtime_remote_time);
+ EXPECT_EQ(pi2_context->monotonic_remote_time,
+ header_monotonic_remote_time);
+
+ // Confirm the forwarded message also matches the source message.
+ EXPECT_EQ(pi1_context->queue_index, header.queue_index());
+ EXPECT_EQ(pi1_context->monotonic_event_time,
+ header_monotonic_remote_time);
+ EXPECT_EQ(pi1_context->realtime_event_time,
+ header_realtime_remote_time);
+ });
+
// Start everything up. Pong is the only thing we don't know how to wait on,
// so start it first.
std::thread pong_thread([&pong_event_loop]() { pong_event_loop.Run(); });
@@ -261,6 +391,10 @@
EXPECT_GE(pi2_server_statistics_count, 2);
EXPECT_GE(pi1_client_statistics_count, 2);
EXPECT_GE(pi2_client_statistics_count, 2);
+
+ // Confirm we got timestamps back!
+ EXPECT_TRUE(message_header_fetcher1.Fetch());
+ EXPECT_TRUE(message_header_fetcher2.Fetch());
}
// Test that the client disconnecting triggers the server offsets on both sides
@@ -290,6 +424,7 @@
FLAGS_application_name = "pi1_message_bridge_server";
// Force ourselves to be "raspberrypi" and allocate everything.
FLAGS_override_hostname = "raspberrypi";
+ DoSetShmBase("pi1");
aos::ShmEventLoop pi1_server_event_loop(&server_config.message());
MessageBridgeServer pi1_message_bridge_server(&pi1_server_event_loop);
@@ -305,6 +440,7 @@
// Now do it for "raspberrypi2", the client.
FLAGS_override_hostname = "raspberrypi2";
+ DoSetShmBase("pi2");
FLAGS_application_name = "pi2_message_bridge_server";
aos::ShmEventLoop pi2_server_event_loop(&pi2_config.message());
MessageBridgeServer pi2_message_bridge_server(&pi2_server_event_loop);
@@ -496,6 +632,7 @@
FLAGS_application_name = "pi1_message_bridge_server";
// Force ourselves to be "raspberrypi" and allocate everything.
FLAGS_override_hostname = "raspberrypi";
+ DoSetShmBase("pi1");
aos::ShmEventLoop pi1_server_event_loop(&server_config.message());
MessageBridgeServer pi1_message_bridge_server(&pi1_server_event_loop);
@@ -513,6 +650,7 @@
// Now do it for "raspberrypi2", the client.
FLAGS_override_hostname = "raspberrypi2";
+ DoSetShmBase("pi2");
FLAGS_application_name = "pi2_message_bridge_client";
aos::ShmEventLoop pi2_client_event_loop(&pi2_config.message());
MessageBridgeClient pi2_message_bridge_client(&pi2_client_event_loop);
diff --git a/aos/network/message_bridge_test_common.json b/aos/network/message_bridge_test_common.json
index a8475b1..3ac2796 100644
--- a/aos/network/message_bridge_test_common.json
+++ b/aos/network/message_bridge_test_common.json
@@ -25,7 +25,9 @@
"destination_nodes": [
{
"name": "pi2",
- "priority": 1
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi1"]
}
]
},
@@ -38,25 +40,13 @@
"destination_nodes": [
{
"name": "pi1",
- "priority": 1
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi2"]
}
]
},
{
- "name": "/pi1_forwarded/aos",
- "type": "aos.message_bridge.Timestamp",
- "source_node": "pi2",
- "frequency": 10,
- "max_size": 200
- },
- {
- "name": "/pi2_forwarded/aos",
- "type": "aos.message_bridge.Timestamp",
- "source_node": "pi1",
- "frequency": 10,
- "max_size": 200
- },
- {
"name": "/pi1/aos",
"type": "aos.message_bridge.ServerStatistics",
"source_node": "pi1",
@@ -81,6 +71,18 @@
"frequency": 2
},
{
+ "name": "/pi1/aos/remote_timestamps/pi2",
+ "type": "aos.logger.MessageHeader",
+ "source_node": "pi1",
+ "frequency": 10
+ },
+ {
+ "name": "/pi2/aos/remote_timestamps/pi1",
+ "type": "aos.logger.MessageHeader",
+ "source_node": "pi2",
+ "frequency": 10
+ },
+ {
"name": "/pi1/aos",
"type": "aos.timing.Report",
"source_node": "pi1",
@@ -110,11 +112,6 @@
]
},
{
- "name": "/test2",
- "type": "aos.examples.Ping",
- "source_node": "pi2"
- },
- {
"name": "/test",
"type": "aos.examples.Pong",
"source_node": "pi2",
@@ -148,34 +145,6 @@
"rename": {
"name": "/pi2/aos"
}
- },
- {
- "match": {
- "name": "/test",
- "type": "aos.examples.Ping",
- "source_node": "pi2"
- },
- "rename": {
- "name": "/test2"
- }
- },
- {
- "match": {
- "name": "/pi1/aos*",
- "source_node": "pi2"
- },
- "rename": {
- "name": "/pi1_forwarded/aos"
- }
- },
- {
- "match": {
- "name": "/pi2/aos*",
- "source_node": "pi1"
- },
- "rename": {
- "name": "/pi2_forwarded/aos"
- }
}
]
}
diff --git a/aos/network/timestamp_filter.cc b/aos/network/timestamp_filter.cc
index ba8f9bd..116607c 100644
--- a/aos/network/timestamp_filter.cc
+++ b/aos/network/timestamp_filter.cc
@@ -49,7 +49,7 @@
VLOG(1) << " " << this << " Sample at " << monotonic_now << " is "
<< sample_ns.count() << "ns, Base is " << base_offset_.count();
CHECK_GE(monotonic_now, last_time_)
- << ": Being asked to filter backwards in time!";
+ << ": " << this << " Being asked to filter backwards in time!";
// Compute the sample offset as a double (seconds), taking into account the
// base offset.
const double sample =
@@ -380,7 +380,7 @@
const double hard_max = fwd_.offset();
const double hard_min = -rev_.offset();
const double average = (hard_max + hard_min) / 2.0;
- VLOG(1) << " Max(fwd) " << hard_max << " min(rev) " << hard_min;
+ VLOG(1) << this << " Max(fwd) " << hard_max << " min(rev) " << hard_min;
// We don't want to clip the offset to the hard min/max. We really want to
// keep it within a band around the middle. ratio of 0.3 means stay within
// +- 0.15 of the middle of the hard min and max.
@@ -390,7 +390,7 @@
// Update regardless for the first sample from both the min and max.
if (*last_time == aos::monotonic_clock::min_time) {
- VLOG(1) << " No last time " << average;
+ VLOG(1) << this << " No last time " << average;
offset_ = average;
offset_velocity_ = 0.0;
} else {
@@ -415,7 +415,7 @@
(offset_velocity_ -
(fwd_.filtered_velocity() - rev_.filtered_velocity()) / 2.0);
- VLOG(1) << " last time " << offset_;
+ VLOG(1) << this << " last time " << offset_;
}
*last_time = monotonic_now;
@@ -424,14 +424,14 @@
// reverse samples.
if (!MissingSamples()) {
*sample_pointer_ = offset_;
- VLOG(1) << "Updating sample to " << offset_;
+ VLOG(1) << this << " Updating sample to " << offset_;
} else {
- VLOG(1) << "Don't have both samples.";
+ VLOG(1) << this << " Don't have both samples.";
if (last_fwd_time_ == aos::monotonic_clock::min_time) {
- VLOG(1) << " Missing forward";
+ VLOG(1) << this << " Missing forward";
}
if (last_rev_time_ == aos::monotonic_clock::min_time) {
- VLOG(1) << " Missing reverse";
+ VLOG(1) << this << " Missing reverse";
}
}
}
@@ -657,6 +657,8 @@
void NoncausalOffsetEstimator::Sample(
const Node *node, aos::monotonic_clock::time_point node_delivered_time,
aos::monotonic_clock::time_point other_node_sent_time) {
+ VLOG(1) << "Sample delivered " << node_delivered_time << " sent "
+ << other_node_sent_time << " to " << node->name()->string_view();
if (node == node_a_) {
if (a_.Sample(node_delivered_time,
other_node_sent_time - node_delivered_time)) {
@@ -740,11 +742,16 @@
}
fit_ = AverageFits(a_.FitLine(), b_.FitLine());
if (offset_pointer_) {
+ VLOG(1) << "Setting offset to " << fit_.mpq_offset();
*offset_pointer_ = fit_.mpq_offset();
}
if (slope_pointer_) {
+ VLOG(1) << "Setting slope to " << fit_.mpq_slope();
*slope_pointer_ = -fit_.mpq_slope();
}
+ if (valid_pointer_) {
+ *valid_pointer_ = true;
+ }
if (VLOG_IS_ON(1)) {
LogFit("Refitting to");
diff --git a/aos/network/timestamp_filter.h b/aos/network/timestamp_filter.h
index 6994a8a..10f436e 100644
--- a/aos/network/timestamp_filter.h
+++ b/aos/network/timestamp_filter.h
@@ -408,6 +408,9 @@
void set_slope_pointer(mpq_class *slope_pointer) {
slope_pointer_ = slope_pointer;
}
+ void set_valid_pointer(bool *valid_pointer) {
+ valid_pointer_ = valid_pointer;
+ }
// Returns the data points from each filter.
const std::deque<
@@ -441,6 +444,7 @@
mpq_class *offset_pointer_ = nullptr;
mpq_class *slope_pointer_ = nullptr;
+ bool *valid_pointer_ = nullptr;
Line fit_{std::chrono::nanoseconds(0), 0.0};