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