commit 10d3fd44243304572daaec14417d62cc02274d7d
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Feb 25 21:57:36 2022 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Feb 26 16:17:15 2022 -0800
tree 8b178f39408f62e7a2a258accdca5976261913ed
parent 5ed29dd110f4a50815799bd4a94d053bd57f7671

Add quaternion orientation to LocalizerOutput

Change-Id: I45477182ee14978754d4c3a3f9fdc45a64cd5e3e
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

y2022/control_loops/localizer/localizer.cc

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();
           }

y2022/control_loops/localizer/localizer.h

diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
index 2c4d1a4..d1eaf5c 100644
--- a/y2022/control_loops/localizer/localizer.h
+++ b/y2022/control_loops/localizer/localizer.h
@@ -122,6 +122,8 @@
     }
   }
 
+  Eigen::Quaterniond orientation() const { return last_orientation_; }
+
   AccelState accel_state() const { return current_state_.accel_state; };
 
   void set_longitudinal_offset(double offset) { long_offset_ = offset; }
@@ -209,6 +211,7 @@
   double accel_residual_ = 0.0;
   double theta_rate_residual_ = 0.0;
   int hysteresis_count_ = 0;
+  Eigen::Quaterniond last_orientation_ = Eigen::Quaterniond::Identity();
 
   int clock_resets_ = 0;
 

y2022/control_loops/localizer/localizer_output.fbs

diff --git a/y2022/control_loops/localizer/localizer_output.fbs b/y2022/control_loops/localizer/localizer_output.fbs
index 078d723..03abf19 100644
--- a/y2022/control_loops/localizer/localizer_output.fbs
+++ b/y2022/control_loops/localizer/localizer_output.fbs
@@ -3,6 +3,13 @@
 // This provides a minimal output from the localizer that can be forwarded to
 // the roborio and used for corrections to its (simpler) localizer.
 
+struct Quaternion {
+  w:double (id: 0);
+  x:double (id: 1);
+  y:double (id: 2);
+  z:double (id: 3);
+}
+
 table LocalizerOutput {
   // Timestamp (on the source node) that this sample corresponds with. This
   // may be older than the sent time to account for delays in sensor readings.
@@ -12,6 +19,8 @@
   y:double (id: 2);
   // Current heading, in radians.
   theta:double (id: 3);
+  // Current estimate of the robot's 3-D rotation.
+  orientation:Quaternion (id: 4);
 }
 
 root_type LocalizerOutput;