Allow resetting localizer via queue
We can now dynamically reset the localizer to allow, e.g., creating a
button in joystick reader that resets the robot position.
Change-Id: I4a9882b59b7efa660f496086b920e57a3231009f
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 5d5b58d..af07089 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -40,6 +40,8 @@
::aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, double longitudinal_accelerometer) = 0;
+ // Reset the absolute position of the estimator.
+ virtual void ResetPosition(double x, double y, double theta) = 0;
// There are several subtly different norms floating around for state
// matrices. In order to avoid that mess, we jus tprovide direct accessors for
// the values that most people care about.
@@ -92,6 +94,13 @@
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
}
+ void ResetPosition(double x, double y, double theta) override {
+ ekf_.ResetInitialState(
+ ::aos::monotonic_clock::now(),
+ (Ekf::State() << x, y, theta, 0, 0, 0, 0, 0, 0, 0).finished(),
+ ekf_.P());
+ };
+
double x() const override { return ekf_.X_hat(StateIdx::kX); }
double y() const override { return ekf_.X_hat(StateIdx::kY); }
double theta() const override { return ekf_.X_hat(StateIdx::kTheta); }