Adjust incoming timestamp for unmodeled camera delays
Change-Id: Icca283ab317b9fa08ba1514c698b9f9fb5fd533f
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 17dabde..54c2ebc 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -365,9 +365,12 @@
const auto now = aos::monotonic_clock::now();
for (const auto &received : unpacked->frames) {
auto to_send = control_loops::drivetrain::camera_frames.MakeMessage();
+ // Add an extra 10ms delay to account for unmodeled delays that Austin
+ // thinks exists.
to_send->timestamp =
- std::chrono::nanoseconds((now - received.age).time_since_epoch())
- .count();
+ std::chrono::nanoseconds(
+ (now - received.age - ::std::chrono::milliseconds(10))
+ .time_since_epoch()).count();
to_send->num_targets = received.targets.size();
for (size_t i = 0; i < received.targets.size(); ++i) {
to_send->targets[i].distance = received.targets[i].distance;