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

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