From fe28fa1ded4ce6bce59483795cd897d24bd7ea96 Mon Sep 17 00:00:00 2001 From: Thad House Date: Tue, 26 Nov 2024 01:23:32 +0000 Subject: [PATCH] [wpilibj] Fix ADIS16470 Gyro (#7434) --- .../src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java | 6 +++++- 1 file changed, 5 insertions(+), 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 index 4e38b713bf2..c2700b82c9b 100644 --- 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) {