Publish down estimate and reset bias when enabled the first time.
Change-Id: Ibe9f276c9882083ae30dbc3b5d4bc938619a3f35
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2280648..70d47a6 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -80,6 +80,11 @@
const ::frc971::control_loops::DrivetrainQueue::Position *position,
::frc971::control_loops::DrivetrainQueue::Output *output,
::frc971::control_loops::DrivetrainQueue::Status *status) {
+ if (!has_been_enabled_ && output) {
+ has_been_enabled_ = true;
+ down_estimator_.mutable_X_hat(1, 0) = 0.0;
+ }
+
// TODO(austin): Put gear detection logic here.
switch (dt_config_.shifter_type) {
case ShifterType::SIMPLE_SHIFTER:
@@ -220,6 +225,7 @@
status->right_voltage_error = kf_.X_hat(5, 0);
status->estimated_angular_velocity_error = kf_.X_hat(6, 0);
status->estimated_heading = integrated_kf_heading_;
+ status->ground_angle = down_estimator_.X_hat(0, 0);
dt_openloop_.PopulateStatus(status);
dt_closedloop_.PopulateStatus(status);