From 142926b5fc67c4d679806591a919b920d3c1ee7f Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Tue, 9 Jan 2024 13:08:15 +0800 Subject: [PATCH 1/5] [wpilib] add ADIS16470 methods with no params Resolves #6180 --- wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 20 ++++++++++++++++ .../main/native/include/frc/ADIS16470_IMU.h | 14 +++++++++++ .../edu/wpi/first/wpilibj/ADIS16470_IMU.java | 24 +++++++++++++++++++ 3 files changed, 58 insertions(+) diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 1c5c7a16f5f..962b805a96b 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -809,6 +809,16 @@ units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const { return units::degree_t{0.0}; } +units::degree_t ADIS16470_IMU::GetAngle() const { + if (m_simGyroAngleZ) { + return units::degree_t{m_simGyroAngleZ.Get()}; + } + { + std::scoped_lock sync(m_mutex); + return units::degree_t{m_integ_angle_z}; + } +} + units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const { switch (axis) { case kYaw: @@ -856,6 +866,16 @@ units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const { return 0_deg_per_s; } +units::degrees_per_second_t ADIS16470_IMU::GetRate() const { + if (m_simGyroRateZ) { + return units::degrees_per_second_t{m_simGyroRateZ.Get()}; + } + { + std::scoped_lock sync(m_mutex); + return units::degrees_per_second_t{m_gyro_rate_z}; + } +} + units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const { if (m_simAccelX) { return units::meters_per_second_squared_t{m_simAccelX.Get()}; diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index f594c2bcd04..393d15b6234 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -223,6 +223,13 @@ class ADIS16470_IMU : public wpi::Sendable, */ units::degree_t GetAngle(IMUAxis axis) const; + /** + * Returns the Z axis angle (CCW positive). + * + * @return The Z axis angle (CCW positive). + */ + units::degree_t GetAngle() const; + /** * Returns the axis angular rate (CCW positive). * @@ -231,6 +238,13 @@ class ADIS16470_IMU : public wpi::Sendable, */ units::degrees_per_second_t GetRate(IMUAxis axis) const; + /** + * Returns the Z axis angular rate (CCW positive). + * + * @return Z axis angular rate (CCW positive). + */ + units::degrees_per_second_t GetRate() const; + /** * Returns the acceleration in the X axis. */ 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 b7236a57eea..abbe5f445c5 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 @@ -1136,6 +1136,18 @@ public synchronized double getAngle(IMUAxis axis) { return 0.0; } + /** + * Returns the Z axis angle in degrees (CCW positive). + * + * @return The Z axis angle in degrees (CCW positive). + */ + public synchronized double getAngle() { + if (m_simGyroAngleZ != null) { + return m_simGyroAngleZ.get(); + } + return m_integ_angle_z; + } + /** * Returns the axis angular rate in degrees per second (CCW positive). * @@ -1177,6 +1189,18 @@ public synchronized double getRate(IMUAxis axis) { return 0.0; } + /** + * Returns the Z axis angular rate in degrees per second (CCW positive). + * + * @return Z axis angular rate in degrees per second (CCW positive). + */ + public synchronized double getRate() { + if (m_simGyroRateZ != null) { + return m_simGyroRateZ.get(); + } + return m_gyro_rate_z; + } + /** * Returns which axis, kX, kY, or kZ, is set to the yaw axis. * From 5802a109f756c564e06bac54c629c99984c614f3 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Tue, 9 Jan 2024 13:39:02 +0800 Subject: [PATCH 2/5] apply review comments --- wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 20 --------- .../main/native/include/frc/ADIS16470_IMU.h | 15 ++----- .../edu/wpi/first/wpilibj/ADIS16470_IMU.java | 43 ++++++++++++++++--- 3 files changed, 41 insertions(+), 37 deletions(-) diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 962b805a96b..1c5c7a16f5f 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -809,16 +809,6 @@ units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const { return units::degree_t{0.0}; } -units::degree_t ADIS16470_IMU::GetAngle() const { - if (m_simGyroAngleZ) { - return units::degree_t{m_simGyroAngleZ.Get()}; - } - { - std::scoped_lock sync(m_mutex); - return units::degree_t{m_integ_angle_z}; - } -} - units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const { switch (axis) { case kYaw: @@ -866,16 +856,6 @@ units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const { return 0_deg_per_s; } -units::degrees_per_second_t ADIS16470_IMU::GetRate() const { - if (m_simGyroRateZ) { - return units::degrees_per_second_t{m_simGyroRateZ.Get()}; - } - { - std::scoped_lock sync(m_mutex); - return units::degrees_per_second_t{m_gyro_rate_z}; - } -} - units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const { if (m_simAccelX) { return units::meters_per_second_squared_t{m_simAccelX.Get()}; diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 393d15b6234..5bfd62c0577 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -218,25 +218,18 @@ class ADIS16470_IMU : public wpi::Sendable, /** * Returns the axis angle (CCW positive). * - * @param axis The IMUAxis whose angle to return. + * @param axis The IMUAxis whose angle to return. Defaults to user configured Yaw. * @return The axis angle (CCW positive). */ - units::degree_t GetAngle(IMUAxis axis) const; - - /** - * Returns the Z axis angle (CCW positive). - * - * @return The Z axis angle (CCW positive). - */ - units::degree_t GetAngle() const; + units::degree_t GetAngle(IMUAxis axis = IMUAxis::kYaw) const; /** * Returns the axis angular rate (CCW positive). * - * @param axis The IMUAxis whose rate to return. + * @param axis The IMUAxis whose rate to return. Defaults to user configured Yaw. * @return Axis angular rate (CCW positive). */ - units::degrees_per_second_t GetRate(IMUAxis axis) const; + units::degrees_per_second_t GetRate(IMUAxis axis = IMUAxis::kYaw) const; /** * Returns the Z axis angular rate (CCW positive). 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 abbe5f445c5..fbfb29c81f6 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 @@ -1142,10 +1142,25 @@ public synchronized double getAngle(IMUAxis axis) { * @return The Z axis angle in degrees (CCW positive). */ public synchronized double getAngle() { - if (m_simGyroAngleZ != null) { - return m_simGyroAngleZ.get(); + switch (m_yaw_axis) { + case kX: + if (m_simGyroAngleX != null) { + return m_simGyroAngleX.get(); + } + return m_integ_angle_x; + case kY: + if (m_simGyroAngleY != null) { + return m_simGyroAngleY.get(); + } + return m_integ_angle_y; + case kZ: + if (m_simGyroAngleZ != null) { + return m_simGyroAngleZ.get(); + } + return m_integ_angle_z; + default: } - return m_integ_angle_z; + return 0.0; } /** @@ -1195,10 +1210,26 @@ public synchronized double getRate(IMUAxis axis) { * @return Z axis angular rate in degrees per second (CCW positive). */ public synchronized double getRate() { - if (m_simGyroRateZ != null) { - return m_simGyroRateZ.get(); + switch (m_yaw_axis) { + case kX: + if (m_simGyroRateX != null) { + return m_simGyroRateX.get(); + } + return m_gyro_rate_x; + case kY: + if (m_simGyroRateY != null) { + return m_simGyroRateY.get(); + } + return m_gyro_rate_y; + case kZ: + if (m_simGyroRateZ != null) { + return m_simGyroRateZ.get(); + } + return m_gyro_rate_z; + default: } - return m_gyro_rate_z; + return 0.0; + } /** From afb7537f2f0875eee84a2d7250afa3ed8a202723 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Tue, 9 Jan 2024 13:40:41 +0800 Subject: [PATCH 3/5] fix docs --- .../main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 fbfb29c81f6..456522ecdbc 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 @@ -1137,9 +1137,9 @@ public synchronized double getAngle(IMUAxis axis) { } /** - * Returns the Z axis angle in degrees (CCW positive). + * Returns the Yaw axis angle in degrees (CCW positive). * - * @return The Z axis angle in degrees (CCW positive). + * @return The Yaw axis angle in degrees (CCW positive). */ public synchronized double getAngle() { switch (m_yaw_axis) { @@ -1205,9 +1205,9 @@ public synchronized double getRate(IMUAxis axis) { } /** - * Returns the Z axis angular rate in degrees per second (CCW positive). + * Returns the Yaw axis angular rate in degrees per second (CCW positive). * - * @return Z axis angular rate in degrees per second (CCW positive). + * @return Yaw axis angular rate in degrees per second (CCW positive). */ public synchronized double getRate() { switch (m_yaw_axis) { From 5ec23914d4ade786d3c0c31ef7f9e70ff5030794 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Tue, 9 Jan 2024 13:43:53 +0800 Subject: [PATCH 4/5] formatted --- wpilibc/src/main/native/include/frc/ADIS16470_IMU.h | 6 ++++-- .../src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java | 1 - 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 5bfd62c0577..3085e38e3b4 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -218,7 +218,8 @@ class ADIS16470_IMU : public wpi::Sendable, /** * Returns the axis angle (CCW positive). * - * @param axis The IMUAxis whose angle to return. Defaults to user configured Yaw. + * @param axis The IMUAxis whose angle to return. Defaults to user configured + * Yaw. * @return The axis angle (CCW positive). */ units::degree_t GetAngle(IMUAxis axis = IMUAxis::kYaw) const; @@ -226,7 +227,8 @@ class ADIS16470_IMU : public wpi::Sendable, /** * Returns the axis angular rate (CCW positive). * - * @param axis The IMUAxis whose rate to return. Defaults to user configured Yaw. + * @param axis The IMUAxis whose rate to return. Defaults to user configured + * Yaw. * @return Axis angular rate (CCW positive). */ units::degrees_per_second_t GetRate(IMUAxis axis = IMUAxis::kYaw) const; 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 456522ecdbc..67a5beca86a 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 @@ -1229,7 +1229,6 @@ public synchronized double getRate() { default: } return 0.0; - } /** From c45f5327c01d6042351b59bc586fba0ab59942da Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Wed, 10 Jan 2024 10:32:18 +0800 Subject: [PATCH 5/5] remove unnecessary function --- wpilibc/src/main/native/include/frc/ADIS16470_IMU.h | 7 ------- 1 file changed, 7 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 3085e38e3b4..66f5bae3354 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -233,13 +233,6 @@ class ADIS16470_IMU : public wpi::Sendable, */ units::degrees_per_second_t GetRate(IMUAxis axis = IMUAxis::kYaw) const; - /** - * Returns the Z axis angular rate (CCW positive). - * - * @return Z axis angular rate (CCW positive). - */ - units::degrees_per_second_t GetRate() const; - /** * Returns the acceleration in the X axis. */