commit 5f17a97a1001e0489f5e842b9cc3cc4c8d9338f5
author Brian Silverman <brians> Sun Feb 28 01:49:32 2016 -0500
committer Brian Silverman <brians> Sat Mar 05 13:43:41 2016 -0500
tree 2866eebf41f52385e0830418f2690f538c22a155
parent 5d7bb0c1c81ed52c9004f2ab3c0a06f2f05ad8d4

Add support for the IMU

Change-Id: Iaff4b088d9bec8edf439e00dda5b788cf87dff0c

frc971/wpilib/gyro_interface.cc

diff --git a/frc971/wpilib/gyro_interface.cc b/frc971/wpilib/gyro_interface.cc
index 4b440a9..6b3ab40 100644
--- a/frc971/wpilib/gyro_interface.cc
+++ b/frc971/wpilib/gyro_interface.cc
@@ -75,7 +75,11 @@
   static_assert(kBytes == sizeof(to_write),
                 "need the same number of bytes as sizeof(the data)");
 
-  if (__builtin_parity(to_write & ~1) == 0) to_write |= 1;
+  if (__builtin_parity(to_write & ~1) == 0) {
+    to_write |= 1;
+  } else {
+    to_write &= ~1;
+  }
 
   uint8_t to_send[kBytes], to_receive[kBytes];
   const uint32_t to_write_flipped = __builtin_bswap32(to_write);
@@ -124,7 +128,7 @@
 }
 
 double GyroInterface::ExtractAngle(uint32_t value) {
-  const int16_t reading = -(int16_t)(value >> 10 & 0xFFFF);
+  const int16_t reading = -static_cast<int16_t>(value >> 10 & 0xFFFF);
   return static_cast<double>(reading) * 2.0 * M_PI / 360.0 / 80.0;
 }