commit 9993fb33c207645f68b63bc3ed4dffb52cca0604
author Austin Schuh <austin.linux@gmail.com> Wed Mar 15 20:17:46 2017 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 22 21:00:54 2017 -0700
tree 230a9760ababc7ba009c2ac6e69e6092be03ce82
parent 69f8a4f249a0a1e22aa02b166cf3ca10cbbd9e5f

Drivetrain now resets the saved gyro value if it is stale.

Change-Id: I624f4c035babe8c22a633fd16dccc9b960d055c7

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index aa6cd60..7a44b64 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -21,6 +21,8 @@
 
 using frc971::sensors::gyro_reading;
 using frc971::imu_values;
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
 
 namespace frc971 {
 namespace control_loops {
@@ -81,6 +83,8 @@
     const ::frc971::control_loops::DrivetrainQueue::Position *position,
     ::frc971::control_loops::DrivetrainQueue::Output *output,
     ::frc971::control_loops::DrivetrainQueue::Status *status) {
+  monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+
   if (!has_been_enabled_ && output) {
     has_been_enabled_ = true;
     down_estimator_.mutable_X_hat(1, 0) = 0.0;
@@ -160,30 +164,35 @@
       if (is_latest_imu_values) {
         LOG_STRUCT(DEBUG, "using", *imu_values.get());
         last_gyro_rate_ = imu_values->gyro_x;
+        last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::IMU_Y_GYRO:
       if (is_latest_imu_values) {
         LOG_STRUCT(DEBUG, "using", *imu_values.get());
         last_gyro_rate_ = imu_values->gyro_y;
+        last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::IMU_Z_GYRO:
       if (is_latest_imu_values) {
         LOG_STRUCT(DEBUG, "using", *imu_values.get());
         last_gyro_rate_ = imu_values->gyro_z;
+        last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::SPARTAN_GYRO:
       if (gyro_reading.FetchLatest()) {
         LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
         last_gyro_rate_ = gyro_reading->velocity;
+        last_gyro_time_ = monotonic_now;
       }
       break;
     case GyroType::FLIPPED_SPARTAN_GYRO:
       if (gyro_reading.FetchLatest()) {
         LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
         last_gyro_rate_ = -gyro_reading->velocity;
+        last_gyro_time_ = monotonic_now;
       }
       break;
     default:
@@ -191,6 +200,10 @@
       break;
   }
 
+  if (monotonic_now > last_gyro_time_ + chrono::milliseconds(20)) {
+    last_gyro_rate_ = 0.0;
+  }
+
   {
     Eigen::Matrix<double, 3, 1> Y;
     Y << position->left_encoder, position->right_encoder, last_gyro_rate_;