Process LocalizerOutput messages in 2022 drivetrain
Make it so that the roborio will actually pay attention to the pi's
localizer.
Change-Id: I8db97cbba65beef8c2ec863a792e57960ead996a
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
index d6afd56..dd2e67d 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -558,6 +558,10 @@
auto builder = output_sender_.MakeBuilder();
LocalizerOutput::Builder output_builder =
builder.MakeBuilder<LocalizerOutput>();
+ // TODO(james): Should we bother to try to estimate time offsets for
+ // the pico?
+ output_builder.add_monotonic_timestamp_ns(
+ value->monotonic_timestamp_ns());
output_builder.add_x(model_based_.xytheta()(0));
output_builder.add_y(model_based_.xytheta()(1));
output_builder.add_theta(model_based_.xytheta()(2));