commit 9ab4d9d5c60df8a4a70ef584b0c3a33cb8a23800
author Austin Schuh <austin.linux@gmail.com> Tue Feb 16 23:55:44 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Wed Feb 17 00:01:04 2016 -0800
tree 2cb8d4be20d0a87cd389fbeb3aab3448d70a05a3
parent 1338b27ed0932b271cf7c4a5c374ad7804b2534d

Fixed off by one cycle issue in the gyro kalman filter update.

Change-Id: I1693ae0f39765d06c5be6035a164cc75085858ec

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index ca395cb..50c5df7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -47,6 +47,14 @@
 
   kf_.set_controller_index(dt_openloop_.controller_index());
 
+  bool gyro_valid = false;
+  if (gyro_reading.FetchLatest()) {
+    LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+    last_gyro_heading_ = gyro_reading->angle;
+    last_gyro_rate_ = gyro_reading->velocity;
+    gyro_valid = true;
+  }
+
   {
     Eigen::Matrix<double, 3, 1> Y;
     Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
@@ -75,12 +83,9 @@
   if (!bad_pos) {
     const double left_encoder = position->left_encoder;
     const double right_encoder = position->right_encoder;
-    if (gyro_reading.FetchLatest()) {
-      LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
+    if (gyro_valid) {
       dt_closedloop_.SetPosition(left_encoder, right_encoder,
                                  gyro_reading->angle);
-      last_gyro_heading_ = gyro_reading->angle;
-      last_gyro_rate_ = gyro_reading->velocity;
     } else {
       dt_closedloop_.SetRawPosition(left_encoder, right_encoder);
     }