[wpilibj] Fix ADIS16470 Gyro (#7434)

wpilibsuite · Nov 26, 2024 · fe28fa1 · fe28fa1
1 parent b7eb9fb
commit fe28fa1
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
@@ -426,6 +426,10 @@ public boolean isConnected() {
     return m_connected;
   }
 
+  private static int toUShort(int upper, int lower) {
+    return ((upper & 0xFF) << 8) + (lower & 0xFF);
+  }
+
   private static int toShort(int upper, int lower) {
     return (short) (((upper & 0xFF) << 8) + (lower & 0xFF));
   }
@@ -609,7 +613,7 @@ private int readRegister(int reg) {
     m_spi.write(buf, 2);
     m_spi.read(false, buf, 2);
 
-    return (buf[0] << 8) & buf[1];
+    return toUShort(buf[0], buf[1]);
   }
 
   private void writeRegister(int reg, int val) {