Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] Add support for onboard IMU mount orientations with Euler angles (#8061)

Browse Source
This commit is contained in:
Jonah Bonner
2025-07-18 00:20:10 -04:00
committed by GitHub
parent fa65657746
commit a6892b6cd5
7 changed files with 181 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
hal/src/main/java/edu/wpi/first/hal/IMUJNI.java
View File

@@ -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.

48
hal/src/main/native/cpp/jni/IMUJNI.cpp
View File

@@ -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};

23
hal/src/main/native/include/hal/IMU.h
View File

@@ -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.

9
hal/src/main/native/sim/IMU.cpp
View File

@@ -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) {

44
hal/src/main/native/systemcore/IMU.cpp
View File

@@ -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;

42
wpilibc/src/main/native/cpp/OnboardIMU.cpp
View File

@@ -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};
}

25
wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java
View File

@@ -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;