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.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;