Add quaternion orientation to LocalizerOutput
Change-Id: I45477182ee14978754d4c3a3f9fdc45a64cd5e3e
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 5cbee51..56c2769 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -257,6 +257,7 @@
const Eigen::AngleAxis<double> orientation(
Eigen::AngleAxis<double>(xytheta()(kTheta), Eigen::Vector3d::UnitZ()) *
down_estimator_.X_hat());
+ last_orientation_ = orientation;
const Eigen::Vector3d absolute_accel =
orientation * dt_config_.imu_transform * kG * accel;
@@ -611,6 +612,13 @@
output_builder.add_x(model_based_.xytheta()(0));
output_builder.add_y(model_based_.xytheta()(1));
output_builder.add_theta(model_based_.xytheta()(2));
+ const Eigen::Quaterniond &orientation = model_based_.orientation();
+ Quaternion quaternion;
+ quaternion.mutate_x(orientation.x());
+ quaternion.mutate_y(orientation.y());
+ quaternion.mutate_z(orientation.z());
+ quaternion.mutate_w(orientation.w());
+ output_builder.add_orientation(&quaternion);
builder.CheckOk(builder.Send(output_builder.Finish()));
last_output_send_ = event_loop_->monotonic_now();
}