From a6892b6cd51bd260f28dcadf20b9697ca4c122ce Mon Sep 17 00:00:00 2001 From: Jonah Bonner <47046556+jwbonner@users.noreply.github.com> Date: Fri, 18 Jul 2025 00:20:10 -0400 Subject: [PATCH] [hal, wpilib] Add support for onboard IMU mount orientations with Euler angles (#8061) --- .../main/java/edu/wpi/first/hal/IMUJNI.java | 20 +++++++- hal/src/main/native/cpp/jni/IMUJNI.cpp | 48 +++++++++++++++++-- hal/src/main/native/include/hal/IMU.h | 23 ++++++++- hal/src/main/native/sim/IMU.cpp | 9 +++- hal/src/main/native/systemcore/IMU.cpp | 44 +++++++++++++++-- wpilibc/src/main/native/cpp/OnboardIMU.cpp | 42 +++++++++++++--- .../edu/wpi/first/wpilibj/OnboardIMU.java | 25 +++++----- 7 files changed, 181 insertions(+), 30 deletions(-) diff --git a/hal/src/main/java/edu/wpi/first/hal/IMUJNI.java b/hal/src/main/java/edu/wpi/first/hal/IMUJNI.java index 31c7933f50..b4b554dec8 100644 --- a/hal/src/main/java/edu/wpi/first/hal/IMUJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/IMUJNI.java @@ -27,12 +27,28 @@ public class IMUJNI extends JNIWrapper { public static native void getIMUGyroRates(double[] rates); /** - * Get the angle about the axes of the IMU in radians. + * Get the angle, in radians, about the axes of the IMU in the "flat" orientation. * * @param angles array of size >= 3 to place the angle data in. The data will be in the form [x, * y, z]. */ - public static native void getIMUEulerAngles(double[] angles); + public static native void getIMUEulerAnglesFlat(double[] angles); + + /** + * Get the angle, in radians, about the axes of the IMU in the "landscape" orientation. + * + * @param angles array of size >= 3 to place the angle data in. The data will be in the form [x, + * y, z]. + */ + public static native void getIMUEulerAnglesLandscape(double[] angles); + + /** + * Get the angle, in radians, about the axes of the IMU in the "portrait" orientation. + * + * @param angles array of size >= 3 to place the angle data in. The data will be in the form [x, + * y, z]. + */ + public static native void getIMUEulerAnglesPortrait(double[] angles); /** * Get the orientation of the IMU as a quaternion. diff --git a/hal/src/main/native/cpp/jni/IMUJNI.cpp b/hal/src/main/native/cpp/jni/IMUJNI.cpp index 703e30ab61..36baafed88 100644 --- a/hal/src/main/native/cpp/jni/IMUJNI.cpp +++ b/hal/src/main/native/cpp/jni/IMUJNI.cpp @@ -72,11 +72,11 @@ Java_edu_wpi_first_hal_IMUJNI_getIMUGyroRates /* * Class: edu_wpi_first_hal_IMUJNI - * Method: getIMUEulerAngles + * Method: getIMUEulerAnglesFlat * Signature: ([D)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_IMUJNI_getIMUEulerAngles +Java_edu_wpi_first_hal_IMUJNI_getIMUEulerAnglesFlat (JNIEnv* env, jclass, jdoubleArray angles) { assertArraySize(env, angles, 3, "angles"); @@ -84,7 +84,49 @@ Java_edu_wpi_first_hal_IMUJNI_getIMUEulerAngles int32_t status = 0; HAL_EulerAngles3d data; - HAL_GetIMUEulerAngles(&data, &status); + HAL_GetIMUEulerAnglesFlat(&data, &status); + CheckStatus(env, status); + + double arr[]{data.x, data.y, data.z}; + env->SetDoubleArrayRegion(angles, 0, 3, arr); +} + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUEulerAnglesLandscape + * Signature: ([D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUEulerAnglesLandscape + (JNIEnv* env, jclass, jdoubleArray angles) +{ + assertArraySize(env, angles, 3, "angles"); + + int32_t status = 0; + HAL_EulerAngles3d data; + + HAL_GetIMUEulerAnglesLandscape(&data, &status); + CheckStatus(env, status); + + double arr[]{data.x, data.y, data.z}; + env->SetDoubleArrayRegion(angles, 0, 3, arr); +} + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUEulerAnglesPortrait + * Signature: ([D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUEulerAnglesPortrait + (JNIEnv* env, jclass, jdoubleArray angles) +{ + assertArraySize(env, angles, 3, "angles"); + + int32_t status = 0; + HAL_EulerAngles3d data; + + HAL_GetIMUEulerAnglesPortrait(&data, &status); CheckStatus(env, status); double arr[]{data.x, data.y, data.z}; diff --git a/hal/src/main/native/include/hal/IMU.h b/hal/src/main/native/include/hal/IMU.h index 77e48de410..5775645c68 100644 --- a/hal/src/main/native/include/hal/IMU.h +++ b/hal/src/main/native/include/hal/IMU.h @@ -35,12 +35,31 @@ void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status); void HAL_GetIMUGyroRates(HAL_GyroRate3d* rates, int32_t* status); /** - * Get the angle about the axes of the IMU in radians. + * Get the angle, in radians, about the axes of the IMU in the "flat" + * orientation. * * @param[out] angles the angle data * @param[out] status the error code, or 0 for success */ -void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status); +void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status); + +/** + * Get the angle, in radians, about the axes of the IMU in the "landscape" + * orientation. + * + * @param[out] angles the angle data + * @param[out] status the error code, or 0 for success + */ +void HAL_GetIMUEulerAnglesLandscape(HAL_EulerAngles3d* angles, int32_t* status); + +/** + * Get the angle, in radians, about the axes of the IMU in the "portrait" + * orientation. + * + * @param[out] angles the angle data + * @param[out] status the error code, or 0 for success + */ +void HAL_GetIMUEulerAnglesPortrait(HAL_EulerAngles3d* angles, int32_t* status); /** * Get the orientation of the IMU as a quaternion. diff --git a/hal/src/main/native/sim/IMU.cpp b/hal/src/main/native/sim/IMU.cpp index 48bd61013e..8149bdbe5e 100644 --- a/hal/src/main/native/sim/IMU.cpp +++ b/hal/src/main/native/sim/IMU.cpp @@ -12,7 +12,14 @@ void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) { void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) { *rate = {}; } -void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status) { +void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status) { + *angles = {}; +} +void HAL_GetIMUEulerAnglesLandscape(HAL_EulerAngles3d* angles, + int32_t* status) { + *angles = {}; +} +void HAL_GetIMUEulerAnglesPortrait(HAL_EulerAngles3d* angles, int32_t* status) { *angles = {}; } void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) { diff --git a/hal/src/main/native/systemcore/IMU.cpp b/hal/src/main/native/systemcore/IMU.cpp index 23e3c2e934..98dc8a3f60 100644 --- a/hal/src/main/native/systemcore/IMU.cpp +++ b/hal/src/main/native/systemcore/IMU.cpp @@ -21,7 +21,9 @@ namespace { constexpr const char* kRawAccelKey = IMU_PREFIX "rawaccel"; constexpr const char* kRawGyroKey = IMU_PREFIX "rawgyro"; constexpr const char* kQuaternionKey = IMU_PREFIX "quat"; -constexpr const char* kEulerAngleKey = IMU_PREFIX "euler"; +constexpr const char* kEulerAnglesFlatKey = IMU_PREFIX "euler_flat"; +constexpr const char* kEulerAnglesLandscapeKey = IMU_PREFIX "euler_landscape"; +constexpr const char* kEulerAnglesPortraitKey = IMU_PREFIX "euler_portrait"; constexpr const char* kYawFlatKey = IMU_PREFIX "yaw_flat"; constexpr const char* kYawLandscapeKey = IMU_PREFIX "yaw_landscape"; constexpr const char* kYawPortraitKey = IMU_PREFIX "yaw_portrait"; @@ -31,7 +33,12 @@ struct IMU { : rawAccelSub{inst.GetDoubleArrayTopic(kRawAccelKey).Subscribe({})}, rawGyroSub{inst.GetDoubleArrayTopic(kRawGyroKey).Subscribe({})}, quatSub{inst.GetDoubleArrayTopic(kQuaternionKey).Subscribe({})}, - eulerAngleSub{inst.GetDoubleArrayTopic(kEulerAngleKey).Subscribe({})}, + eulerFlatSub{ + inst.GetDoubleArrayTopic(kEulerAnglesFlatKey).Subscribe({})}, + eulerLandscapeSub{ + inst.GetDoubleArrayTopic(kEulerAnglesLandscapeKey).Subscribe({})}, + eulerPortraitSub{ + inst.GetDoubleArrayTopic(kEulerAnglesPortraitKey).Subscribe({})}, yawFlatSub{inst.GetDoubleTopic(kYawFlatKey).Subscribe(0)}, yawLandscapeSub{inst.GetDoubleTopic(kYawLandscapeKey).Subscribe(0)}, yawPortraitSub{inst.GetDoubleTopic(kYawPortraitKey).Subscribe(0)} {} @@ -39,7 +46,9 @@ struct IMU { nt::DoubleArraySubscriber rawAccelSub; nt::DoubleArraySubscriber rawGyroSub; nt::DoubleArraySubscriber quatSub; - nt::DoubleArraySubscriber eulerAngleSub; + nt::DoubleArraySubscriber eulerFlatSub; + nt::DoubleArraySubscriber eulerLandscapeSub; + nt::DoubleArraySubscriber eulerPortraitSub; nt::DoubleSubscriber yawFlatSub; nt::DoubleSubscriber yawLandscapeSub; nt::DoubleSubscriber yawPortraitSub; @@ -85,8 +94,33 @@ void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) { .z = update.value[2] * kDegreesToRadians}; } -void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status) { - auto update = imu->eulerAngleSub.GetAtomic(); +void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status) { + auto update = imu->eulerFlatSub.GetAtomic(); + if (update.value.size() != 3) { + *status = INCOMPATIBLE_STATE; + return; + } + *angles = HAL_EulerAngles3d{.timestamp = update.time, + .x = update.value[0] * kDegreesToRadians, + .y = update.value[1] * kDegreesToRadians, + .z = update.value[2] * kDegreesToRadians}; +} + +void HAL_GetIMUEulerAnglesLandscape(HAL_EulerAngles3d* angles, + int32_t* status) { + auto update = imu->eulerLandscapeSub.GetAtomic(); + if (update.value.size() != 3) { + *status = INCOMPATIBLE_STATE; + return; + } + *angles = HAL_EulerAngles3d{.timestamp = update.time, + .x = update.value[0] * kDegreesToRadians, + .y = update.value[1] * kDegreesToRadians, + .z = update.value[2] * kDegreesToRadians}; +} + +void HAL_GetIMUEulerAnglesPortrait(HAL_EulerAngles3d* angles, int32_t* status) { + auto update = imu->eulerPortraitSub.GetAtomic(); if (update.value.size() != 3) { *status = INCOMPATIBLE_STATE; return; diff --git a/wpilibc/src/main/native/cpp/OnboardIMU.cpp b/wpilibc/src/main/native/cpp/OnboardIMU.cpp index 43fb2a8906..37f77d3636 100644 --- a/wpilibc/src/main/native/cpp/OnboardIMU.cpp +++ b/wpilibc/src/main/native/cpp/OnboardIMU.cpp @@ -58,26 +58,56 @@ Quaternion OnboardIMU::GetQuaternion() { return Quaternion{val.w, val.x, val.y, val.z}; } -units::radian_t GetAngleX() { +units::radian_t OnboardIMU::GetAngleX() { HAL_EulerAngles3d val; int32_t status = 0; - HAL_GetIMUEulerAngles(&val, &status); + switch (m_mountOrientation) { + case kFlat: + HAL_GetIMUEulerAnglesFlat(&val, &status); + break; + case kLandscape: + HAL_GetIMUEulerAnglesLandscape(&val, &status); + break; + case kPortrait: + HAL_GetIMUEulerAnglesPortrait(&val, &status); + break; + } FRC_CheckErrorStatus(status, "Onboard IMU"); return units::radian_t{val.x}; } -units::radian_t GetAngleY() { +units::radian_t OnboardIMU::GetAngleY() { HAL_EulerAngles3d val; int32_t status = 0; - HAL_GetIMUEulerAngles(&val, &status); + switch (m_mountOrientation) { + case kFlat: + HAL_GetIMUEulerAnglesFlat(&val, &status); + break; + case kLandscape: + HAL_GetIMUEulerAnglesLandscape(&val, &status); + break; + case kPortrait: + HAL_GetIMUEulerAnglesPortrait(&val, &status); + break; + } FRC_CheckErrorStatus(status, "Onboard IMU"); return units::radian_t{val.y}; } -units::radian_t GetAngleZ() { +units::radian_t OnboardIMU::GetAngleZ() { HAL_EulerAngles3d val; int32_t status = 0; - HAL_GetIMUEulerAngles(&val, &status); + switch (m_mountOrientation) { + case kFlat: + HAL_GetIMUEulerAnglesFlat(&val, &status); + break; + case kLandscape: + HAL_GetIMUEulerAnglesLandscape(&val, &status); + break; + case kPortrait: + HAL_GetIMUEulerAnglesPortrait(&val, &status); + break; + } FRC_CheckErrorStatus(status, "Onboard IMU"); return units::radian_t{val.z}; } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java index ade58b7d99..a1f2bfc508 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java @@ -159,7 +159,14 @@ public class OnboardIMU { private double[] getRawEulerAngles() { double[] anglesRaw = new double[3]; - IMUJNI.getIMUEulerAngles(anglesRaw); + switch (m_mountOrientation) { + case kFlat -> IMUJNI.getIMUEulerAnglesFlat(anglesRaw); + case kLandscape -> IMUJNI.getIMUEulerAnglesLandscape(anglesRaw); + case kPortrait -> IMUJNI.getIMUEulerAnglesPortrait(anglesRaw); + default -> { + // NOP + } + } return anglesRaw; } @@ -176,16 +183,12 @@ public class OnboardIMU { } private double getYawNoOffset() { - switch (m_mountOrientation) { - case kFlat: - return IMUJNI.getIMUYawFlat(); - case kLandscape: - return IMUJNI.getIMUYawLandscape(); - case kPortrait: - return IMUJNI.getIMUYawPortrait(); - default: - return 0; - } + return switch (m_mountOrientation) { + case kFlat -> IMUJNI.getIMUYawFlat(); + case kLandscape -> IMUJNI.getIMUYawLandscape(); + case kPortrait -> IMUJNI.getIMUYawPortrait(); + default -> 0; + }; } private final MountOrientation m_mountOrientation;