commit 943fcbdd3f6a79a81ab885e4404e9a980963ccfa
author Austin Schuh <austin.linux@gmail.com> Sun Apr 03 21:35:41 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Apr 13 22:21:45 2016 -0700
tree e5a368c73fd56ba32b01ae5e3667f3cc2cda636f
parent 5900d1420439b1339969a7f8ab5554f536b9744b

Zero gyros when enabled.

Change-Id: Ifc300f1b65b0c7ab7a0b3b6d3c4c48f100c53103

frc971/wpilib/ADIS16448.cc

diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index c5e083e..cf1a6f8 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -7,6 +7,7 @@
 #include <math.h>
 
 #include "aos/common/logging/queue_logging.h"
+#include "aos/common/messages/robot_state.q.h"
 #include "aos/common/time.h"
 #include "aos/linux_code/init.h"
 
@@ -198,22 +199,30 @@
 
     // The first few seconds of samples are averaged and subtracted from
     // subsequent samples for zeroing purposes.
-    if (!gyros_are_zeroed()) {
+    if (!gyros_are_zeroed_) {
       average_gyro_x.AddData(message->gyro_x);
       average_gyro_y.AddData(message->gyro_y);
       average_gyro_z.AddData(message->gyro_z);
 
       if (average_gyro_x.full() && average_gyro_y.full() &&
           average_gyro_z.full()) {
-        gyro_x_zeroed_offset_ = average_gyro_x.GetAverage();
-        gyro_y_zeroed_offset_ = average_gyro_y.GetAverage();
-        gyro_z_zeroed_offset_ = average_gyro_z.GetAverage();
-        LOG(DEBUG, "total gyro zero offset X:%f, Y:%f, Z:%f\n",
-            gyro_x_zeroed_offset_, gyro_y_zeroed_offset_, gyro_z_zeroed_offset_);
-        gyros_are_zeroed_.store(true);
+        ::aos::joystick_state.FetchLatest();
+        if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
+          gyro_x_zeroed_offset_ = -average_gyro_x.GetAverage();
+          gyro_y_zeroed_offset_ = -average_gyro_y.GetAverage();
+          gyro_z_zeroed_offset_ = -average_gyro_z.GetAverage();
+          LOG(INFO, "total gyro zero offset X:%f, Y:%f, Z:%f\n",
+              gyro_x_zeroed_offset_, gyro_y_zeroed_offset_,
+              gyro_z_zeroed_offset_);
+          gyros_are_zeroed_ = true;
+        }
       }
     }
 
+    message->gyro_x += gyro_x_zeroed_offset_;
+    message->gyro_y += gyro_y_zeroed_offset_;
+    message->gyro_z += gyro_z_zeroed_offset_;
+
     message->accelerometer_x =
         ConvertValue(&to_receive[10], kAccelerometerLsbG);
     message->accelerometer_y =