From 11244a49d9c4947f05e4e5ffbb05e669d0c34263 Mon Sep 17 00:00:00 2001 From: Thad House Date: Mon, 24 Oct 2022 20:04:16 -0700 Subject: [PATCH] [wpilib] Add IsConnected function to all gyros (#4465) --- wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp | 10 ++++++++++ wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 10 ++++++++++ wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp | 10 ++++++++++ wpilibc/src/main/native/include/frc/ADIS16448_IMU.h | 4 ++++ wpilibc/src/main/native/include/frc/ADIS16470_IMU.h | 4 ++++ wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h | 4 ++++ .../java/edu/wpi/first/wpilibj/ADIS16448_IMU.java | 12 ++++++++++++ .../java/edu/wpi/first/wpilibj/ADIS16470_IMU.java | 12 ++++++++++++ 8 files changed, 66 insertions(+) diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 05595a22a1..a3c7ee77a0 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -74,6 +74,8 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, m_spi_port(port), m_simDevice("Gyro:ADIS16448", port) { if (m_simDevice) { + m_connected = + m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true); m_simGyroAngleX = m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0); m_simGyroAngleY = @@ -150,6 +152,14 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0); wpi::SendableRegistry::AddLW(this, "ADIS16448", port); + m_connected = true; +} + +bool ADIS16448_IMU::IsConnected() const { + if (m_simConnected) { + return m_simConnected.Get(); + } + return m_connected; } /** diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 5e9117c788..bc2236d4bd 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -72,6 +72,8 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, m_calibration_time(static_cast(cal_time)), m_simDevice("Gyro:ADIS16470", port) { if (m_simDevice) { + m_connected = + m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true); m_simGyroAngleX = m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0); m_simGyroAngleY = @@ -147,6 +149,14 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0); wpi::SendableRegistry::AddLW(this, "ADIS16470", port); + m_connected = true; +} + +bool ADIS16470_IMU::IsConnected() const { + if (m_simConnected) { + return m_simConnected.Get(); + } + return m_connected; } /** diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp index b6b3452859..4d4b22dde3 100644 --- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp +++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp @@ -32,6 +32,8 @@ ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {} ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port), m_port(port), m_simDevice("Gyro:ADXRS450", port) { if (m_simDevice) { + m_simConnected = + m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true); m_simAngle = m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0); m_simRate = m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0); @@ -57,6 +59,14 @@ ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port + 1); wpi::SendableRegistry::AddLW(this, "ADXRS450_Gyro", port); + m_connected = true; +} + +bool ADXRS450_Gyro::IsConnected() const { + if (m_simConnected) { + return m_simConnected.Get(); + } + return m_connected; } static bool CalcParity(int v) { diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 01b0fecb3f..60d57a9f1a 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -205,6 +205,8 @@ class ADIS16448_IMU : public nt::NTSendable, int SetYawAxis(IMUAxis yaw_axis); + bool IsConnected() const; + int ConfigDecRate(uint16_t DecimationRate); /** @@ -361,10 +363,12 @@ class ADIS16448_IMU : public nt::NTSendable, CalibrationTime m_calibration_time{0}; SPI* m_spi = nullptr; DigitalInput* m_auto_interrupt = nullptr; + bool m_connected{false}; std::thread m_acquire_task; hal::SimDevice m_simDevice; + hal::SimBoolean m_simConnected; hal::SimDouble m_simGyroAngleX; hal::SimDouble m_simGyroAngleY; hal::SimDouble m_simGyroAngleZ; diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index ada01e2614..d7e71adb0e 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -159,6 +159,8 @@ class ADIS16470_IMU : public nt::NTSendable, int SetYawAxis(IMUAxis yaw_axis); + bool IsConnected() const; + // IMU yaw axis IMUAxis m_yaw_axis; @@ -382,10 +384,12 @@ class ADIS16470_IMU : public nt::NTSendable, SPI* m_spi = nullptr; DigitalInput* m_auto_interrupt = nullptr; double m_scaled_sample_rate = 2500.0; // Default sample rate setting + bool m_connected{false}; std::thread m_acquire_task; hal::SimDevice m_simDevice; + hal::SimBoolean m_simConnected; hal::SimDouble m_simGyroAngleX; hal::SimDouble m_simGyroAngleY; hal::SimDouble m_simGyroAngleZ; diff --git a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h index cb0dff8fc3..55414d464a 100644 --- a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h +++ b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h @@ -49,6 +49,8 @@ class ADXRS450_Gyro : public Gyro, ADXRS450_Gyro(ADXRS450_Gyro&&) = default; ADXRS450_Gyro& operator=(ADXRS450_Gyro&&) = default; + bool IsConnected() const; + /** * Return the actual angle in degrees that the robot is currently facing. * @@ -105,8 +107,10 @@ class ADXRS450_Gyro : public Gyro, private: SPI m_spi; SPI::Port m_port; + bool m_connected{false}; hal::SimDevice m_simDevice; + hal::SimBoolean m_simConnected; hal::SimDouble m_simAngle; hal::SimDouble m_simRate; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java index 20b34766bc..41cc1dc687 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java @@ -15,6 +15,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.networktables.NTSendable; @@ -183,8 +184,10 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { private DigitalInput m_reset_in; private DigitalOutput m_status_led; private Thread m_acquire_task; + private boolean m_connected; private SimDevice m_simDevice; + private SimBoolean m_simConnected; private SimDouble m_simGyroAngleX; private SimDouble m_simGyroAngleY; private SimDouble m_simGyroAngleZ; @@ -262,6 +265,7 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value); if (m_simDevice != null) { + m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); @@ -324,6 +328,14 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { // Report usage and post data to DS HAL.report(tResourceType.kResourceType_ADIS16448, 0); + m_connected = true; + } + + public boolean isConnected() { + if (m_simConnected != null) { + return m_simConnected.get(); + } + return m_connected; } /** */ 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 95b14f8166..ed7503ff37 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 @@ -14,6 +14,7 @@ package edu.wpi.first.wpilibj; // import java.lang.FdLibm.Pow; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.networktables.NTSendable; @@ -250,8 +251,10 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { private DigitalInput m_reset_in; private DigitalOutput m_status_led; private Thread m_acquire_task; + private boolean m_connected; private SimDevice m_simDevice; + private SimBoolean m_simConnected; private SimDouble m_simGyroAngleX; private SimDouble m_simGyroAngleY; private SimDouble m_simGyroAngleZ; @@ -293,6 +296,7 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); if (m_simDevice != null) { + m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); @@ -361,6 +365,14 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { // Report usage and post data to DS HAL.report(tResourceType.kResourceType_ADIS16470, 0); + m_connected = true; + } + + public boolean isConnected() { + if (m_simConnected != null) { + return m_simConnected.get(); + } + return m_connected; } /**