Account for socketcan providing realtime timestamps
We are getting realtime timestamps back, but were treating them as
monotonic times. Fix that.
Change-Id: Ib92ced05870dcfa7a476ce2509b28eb307bda5ac
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/can_logger/asc_logger.cc b/frc971/can_logger/asc_logger.cc
index 6848938..5282d13 100644
--- a/frc971/can_logger/asc_logger.cc
+++ b/frc971/can_logger/asc_logger.cc
@@ -12,11 +12,11 @@
}
void AscLogger::HandleFrame(const CanFrame &frame) {
- if (!first_frame_monotonic_) {
- aos::monotonic_clock::time_point time(
- std::chrono::nanoseconds(frame.monotonic_timestamp_ns()));
+ if (!first_frame_realtime_) {
+ aos::realtime_clock::time_point time(
+ std::chrono::nanoseconds(frame.realtime_timestamp_ns()));
- first_frame_monotonic_ = time;
+ first_frame_realtime_ = time;
WriteHeader(output_, event_loop_->realtime_now());
}
@@ -60,11 +60,11 @@
} // namespace
void AscLogger::WriteFrame(std::ostream &file, const CanFrame &frame) {
- aos::monotonic_clock::time_point frame_timestamp(
- std::chrono::nanoseconds(frame.monotonic_timestamp_ns()));
+ aos::realtime_clock::time_point frame_timestamp(
+ std::chrono::nanoseconds(frame.realtime_timestamp_ns()));
std::chrono::duration<double> time(frame_timestamp -
- first_frame_monotonic_.value());
+ first_frame_realtime_.value());
// TODO: maybe this should not be hardcoded
const int device_id = 1;
diff --git a/frc971/can_logger/asc_logger.h b/frc971/can_logger/asc_logger.h
index 2be2eb2..e7d4e74 100644
--- a/frc971/can_logger/asc_logger.h
+++ b/frc971/can_logger/asc_logger.h
@@ -25,7 +25,7 @@
static void WriteHeader(std::ostream &file,
aos::realtime_clock::time_point start_time);
- std::optional<aos::monotonic_clock::time_point> first_frame_monotonic_;
+ std::optional<aos::realtime_clock::time_point> first_frame_realtime_;
std::ofstream output_;
diff --git a/frc971/can_logger/can_logger.cc b/frc971/can_logger/can_logger.cc
index cfc3dd8..7d33464 100644
--- a/frc971/can_logger/can_logger.cc
+++ b/frc971/can_logger/can_logger.cc
@@ -75,8 +75,8 @@
can_frame_builder.add_can_id(frame.can_id);
can_frame_builder.add_flags(frame.flags);
can_frame_builder.add_data(frame_data);
- can_frame_builder.add_monotonic_timestamp_ns(
- static_cast<std::chrono::nanoseconds>(
+ can_frame_builder.add_realtime_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::seconds(tv.tv_sec) +
std::chrono::microseconds(tv.tv_usec))
.count());
diff --git a/frc971/can_logger/can_logging.fbs b/frc971/can_logger/can_logging.fbs
index ba60241..c7a82f2 100644
--- a/frc971/can_logger/can_logging.fbs
+++ b/frc971/can_logger/can_logging.fbs
@@ -7,7 +7,7 @@
// The body of the CAN frame up to 64 bytes long.
data:[ubyte] (id: 1);
// The hardware timestamp of when the frame came in
- monotonic_timestamp_ns:uint64 (id: 2);
+ realtime_timestamp_ns:uint64 (id: 2);
// Additional flags for CAN FD
flags: ubyte (id: 3);
}
diff --git a/frc971/imu_fdcan/can_translator_lib.cc b/frc971/imu_fdcan/can_translator_lib.cc
index b5e5576..e4a927e 100644
--- a/frc971/imu_fdcan/can_translator_lib.cc
+++ b/frc971/imu_fdcan/can_translator_lib.cc
@@ -7,7 +7,8 @@
CANTranslator::CANTranslator(aos::EventLoop *event_loop,
std::string_view canframe_channel)
- : dual_imu_sender_(
+ : event_loop_(event_loop),
+ dual_imu_sender_(
event_loop->MakeSender<frc971::imu::DualImuStatic>("/imu")),
can_translator_status_sender_(
event_loop->MakeSender<frc971::imu::CanTranslatorStatusStatic>(
@@ -119,7 +120,19 @@
dual_imu_builder->set_max_packet_counter(
std::numeric_limits<uint16_t>::max());
- dual_imu_builder->set_kernel_timestamp(can_frame->monotonic_timestamp_ns());
+ // The timestamp coming from the CanFrame is a realtime timestamp. Calculate
+ // the offset into a monotonic time based on the clock samples from when the
+ // CanFrame was sent (this may not be ultra principled, as there are no strict
+ // bounds on how much time can pass between the clock samples we are using; in
+ // practice, the differences should be negligible).
+ const int64_t realtime_offset =
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ event_loop_->context().monotonic_event_time.time_since_epoch() -
+ event_loop_->context().realtime_event_time.time_since_epoch())
+ .count();
+
+ dual_imu_builder->set_kernel_timestamp(can_frame->realtime_timestamp_ns() +
+ realtime_offset);
dual_imu_builder.CheckOk(dual_imu_builder.Send());
}
diff --git a/frc971/imu_fdcan/can_translator_lib.h b/frc971/imu_fdcan/can_translator_lib.h
index 47a3638..12acb0a 100644
--- a/frc971/imu_fdcan/can_translator_lib.h
+++ b/frc971/imu_fdcan/can_translator_lib.h
@@ -17,6 +17,7 @@
private:
void HandleFrame(const can_logger::CanFrame *can_frame);
+ aos::EventLoop *event_loop_;
aos::Sender<imu::DualImuStatic> dual_imu_sender_;
aos::Sender<imu::CanTranslatorStatusStatic> can_translator_status_sender_;
diff --git a/frc971/imu_fdcan/can_translator_lib_test.cc b/frc971/imu_fdcan/can_translator_lib_test.cc
index 2fb2fe4..f8e6d85 100644
--- a/frc971/imu_fdcan/can_translator_lib_test.cc
+++ b/frc971/imu_fdcan/can_translator_lib_test.cc
@@ -44,12 +44,16 @@
};
TEST_F(CANTranslatorTest, CheckValidFrame) {
+ event_loop_factory_.GetNodeEventLoopFactory(can_frame_event_loop_->node())
+ ->SetRealtimeOffset(
+ aos::monotonic_clock::epoch() + std::chrono::seconds(0),
+ aos::realtime_clock::epoch() + std::chrono::seconds(100));
can_frame_event_loop_->OnRun([this] {
aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
can_frame_builder = can_frame_sender_.MakeStaticBuilder();
can_frame_builder->set_can_id(1);
- can_frame_builder->set_monotonic_timestamp_ns(100);
+ can_frame_builder->set_realtime_timestamp_ns(100e9 + 971);
auto can_data = can_frame_builder->add_data();
CHECK(can_data->reserve(sizeof(uint8_t) * 64));
@@ -164,6 +168,8 @@
EXPECT_EQ(dual_imu_fetcher_->tdk()->chip_states()->Get(0)->counter(), 31178);
EXPECT_EQ(dual_imu_fetcher_->tdk()->chip_states()->Get(0)->temperature(), 29);
+
+ EXPECT_EQ(dual_imu_fetcher_->kernel_timestamp(), 971);
}
TEST_F(CANTranslatorTest, CheckInvalidFrame) {
@@ -172,7 +178,7 @@
can_frame_builder = can_frame_sender_.MakeStaticBuilder();
can_frame_builder->set_can_id(2);
- can_frame_builder->set_monotonic_timestamp_ns(100);
+ can_frame_builder->set_realtime_timestamp_ns(100);
auto can_data = can_frame_builder->add_data();
CHECK(can_data->reserve(sizeof(uint8_t) * 1));
diff --git a/frc971/imu_fdcan/dual_imu.fbs b/frc971/imu_fdcan/dual_imu.fbs
index 88f02df..61cc976 100644
--- a/frc971/imu_fdcan/dual_imu.fbs
+++ b/frc971/imu_fdcan/dual_imu.fbs
@@ -37,7 +37,7 @@
// Timestamp from the board corresponding to these samples in nanoseconds.
// Future changes may lead us to add per-chip timestamps, but for now
// we treat them as being sampled at the same time.
- kernel_timestamp:uint64 (id: 0);
+ kernel_timestamp:int64 (id: 0);
// Packet counter that should increment by 1 for every incoming packet.
packet_counter:uint64 (id: 1);
// Value at which the packet counter wraps, such that