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 =

frc971/wpilib/ADIS16448.h

diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 4fefd07..c8552bf 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -37,8 +37,6 @@
 
   void Quit() { run_ = false; }
 
-  bool gyros_are_zeroed() const { return gyros_are_zeroed_.load(); }
-
   double gyro_x_zeroed_offset() const { return gyro_x_zeroed_offset_; }
   double gyro_y_zeroed_offset() const { return gyro_y_zeroed_offset_; }
   double gyro_z_zeroed_offset() const { return gyro_z_zeroed_offset_; }
@@ -78,7 +76,7 @@
   ::std::atomic<bool> run_{true};
 
   // The averaged values of the gyro over 6 seconds after power up.
-  ::std::atomic<bool> gyros_are_zeroed_{false};
+  bool gyros_are_zeroed_ = false;
   double gyro_x_zeroed_offset_ = 0.0;
   double gyro_y_zeroed_offset_ = 0.0;
   double gyro_z_zeroed_offset_ = 0.0;

frc971/wpilib/BUILD

diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 5304207..c01f7cc 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -229,6 +229,7 @@
     ':imu_queue',
     '//aos/common/logging',
     '//aos/common/logging:queue_logging',
+    '//aos/common/messages:robot_state',
     '//aos/common:time',
     '//aos/linux_code:init',
     '//frc971/zeroing:averager',