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);
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 69e9794..1330708 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -62,6 +62,8 @@
 
   bool left_high_requested_;
   bool right_high_requested_;
+
+  bool has_been_enabled_ = false;
 };
 
 }  // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 4723aef..da4fadc 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -141,6 +141,9 @@
 
     // True if the output voltage was capped last cycle.
     bool output_was_capped;
+
+    // The angle of the robot relative to the ground.
+    double ground_angle;
   };
 
   queue Goal goal;