From 5dfc664b932ffb3d2827ad01c1b4509cc6c0affd Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Wed, 11 Jun 2025 00:57:42 -0400 Subject: [PATCH] [hal, wpilib] Add systemcore IMU (#8016) --- .../main/java/edu/wpi/first/hal/IMUJNI.java | 65 ++++++ hal/src/main/native/cpp/jni/IMUJNI.cpp | 154 ++++++++++++++ hal/src/main/native/include/hal/IMU.h | 79 +++++++ hal/src/main/native/include/hal/IMUTypes.h | 48 +++++ hal/src/main/native/sim/IMU.cpp | 30 +++ hal/src/main/native/systemcore/HAL.cpp | 1 + .../main/native/systemcore/HALInitializer.h | 1 + hal/src/main/native/systemcore/IMU.cpp | 130 ++++++++++++ wpilibc/src/main/native/cpp/OnboardIMU.cpp | 131 ++++++++++++ .../src/main/native/include/frc/OnboardIMU.h | 129 ++++++++++++ .../edu/wpi/first/wpilibj/OnboardIMU.java | 193 ++++++++++++++++++ 11 files changed, 961 insertions(+) create mode 100644 hal/src/main/java/edu/wpi/first/hal/IMUJNI.java create mode 100644 hal/src/main/native/cpp/jni/IMUJNI.cpp create mode 100644 hal/src/main/native/include/hal/IMU.h create mode 100644 hal/src/main/native/include/hal/IMUTypes.h create mode 100644 hal/src/main/native/sim/IMU.cpp create mode 100644 hal/src/main/native/systemcore/IMU.cpp create mode 100644 wpilibc/src/main/native/cpp/OnboardIMU.cpp create mode 100644 wpilibc/src/main/native/include/frc/OnboardIMU.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java diff --git a/hal/src/main/java/edu/wpi/first/hal/IMUJNI.java b/hal/src/main/java/edu/wpi/first/hal/IMUJNI.java new file mode 100644 index 0000000000..31c7933f50 --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/hal/IMUJNI.java @@ -0,0 +1,65 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.hal; + +/** + * IMU Functions. + * + * @see "hal/IMU.h" + */ +public class IMUJNI extends JNIWrapper { + /** + * Get the acceleration along the axes of the IMU in meters per second squared. + * + * @param accel array of size >= 3 to place the acceleration data in. The data will be in the form + * [x, y, z]. + */ + public static native void getIMUAcceleration(double[] accel); + + /** + * Get the angular rate about the axes of the IMU in radians per second. + * + * @param rates array of size >= 3 to place the angular rate data in. The data will be in the form + * [x, y, z]. + */ + public static native void getIMUGyroRates(double[] rates); + + /** + * Get the angle about the axes of the IMU in radians. + * + * @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); + + /** + * Get the orientation of the IMU as a quaternion. + * + * @param quat array of size >= 4 to place the quaternion data in. The data will be in the form + * [w, x, y, z]. + */ + public static native void getIMUQuaternion(double[] quat); + + /** + * Get the yaw value, in radians, of the IMU in the "flat" orientation. + * + * @return flat orientation yaw + */ + public static native double getIMUYawFlat(); + + /** + * Get the yaw value, in radians, of the IMU in the "landscape" orientation. + * + * @return landscape orientation yaw + */ + public static native double getIMUYawLandscape(); + + /** + * Get the yaw value, in radians, of the IMU in the "portrait" orientation. + * + * @return portrait orientation yaw + */ + public static native double getIMUYawPortrait(); +} diff --git a/hal/src/main/native/cpp/jni/IMUJNI.cpp b/hal/src/main/native/cpp/jni/IMUJNI.cpp new file mode 100644 index 0000000000..703e30ab61 --- /dev/null +++ b/hal/src/main/native/cpp/jni/IMUJNI.cpp @@ -0,0 +1,154 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include + +#include "HALUtil.h" +#include "edu_wpi_first_hal_IMUJNI.h" +#include "hal/IMU.h" + +using namespace hal; + +namespace { +void assertArraySize(JNIEnv* env, jarray array, int minimumSize, + std::string_view arrayName) { + jsize actualSize = env->GetArrayLength(array); + if (actualSize < minimumSize) { + ThrowIllegalArgumentException( + env, fmt::format("{} array too small: expected at least {}, got {}", + arrayName, minimumSize, actualSize)); + } +} +} // namespace + +extern "C" { + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUAcceleration + * Signature: ([D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUAcceleration + (JNIEnv* env, jclass, jdoubleArray accel) +{ + assertArraySize(env, accel, 3, "accel"); + + int32_t status = 0; + HAL_Acceleration3d data; + + HAL_GetIMUAcceleration(&data, &status); + CheckStatus(env, status); + + double arr[]{data.x, data.y, data.z}; + env->SetDoubleArrayRegion(accel, 0, 3, arr); +} + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUGyroRates + * Signature: ([D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUGyroRates + (JNIEnv* env, jclass, jdoubleArray rates) +{ + assertArraySize(env, rates, 3, "rates"); + + int32_t status = 0; + HAL_GyroRate3d data; + + HAL_GetIMUGyroRates(&data, &status); + CheckStatus(env, status); + + double arr[]{data.x, data.y, data.z}; + env->SetDoubleArrayRegion(rates, 0, 3, arr); +} + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUEulerAngles + * Signature: ([D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUEulerAngles + (JNIEnv* env, jclass, jdoubleArray angles) +{ + assertArraySize(env, angles, 3, "angles"); + + int32_t status = 0; + HAL_EulerAngles3d data; + + HAL_GetIMUEulerAngles(&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: getIMUQuaternion + * Signature: ([D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUQuaternion + (JNIEnv* env, jclass, jdoubleArray quat) +{ + assertArraySize(env, quat, 4, "quat"); + + int32_t status = 0; + HAL_Quaternion data; + + HAL_GetIMUQuaternion(&data, &status); + CheckStatus(env, status); + + double arr[]{data.w, data.x, data.y, data.z}; + env->SetDoubleArrayRegion(quat, 0, 4, arr); +} + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUYawFlat + * Signature: ()D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUYawFlat + (JNIEnv* env, jclass) +{ + int64_t timestamp; + return HAL_GetIMUYawFlat(×tamp); +} + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUYawLandscape + * Signature: ()D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUYawLandscape + (JNIEnv* env, jclass) +{ + int64_t timestamp; + return HAL_GetIMUYawLandscape(×tamp); +} + +/* + * Class: edu_wpi_first_hal_IMUJNI + * Method: getIMUYawPortrait + * Signature: ()D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_IMUJNI_getIMUYawPortrait + (JNIEnv* env, jclass) +{ + int64_t timestamp; + return HAL_GetIMUYawPortrait(×tamp); +} + +} // extern "C" diff --git a/hal/src/main/native/include/hal/IMU.h b/hal/src/main/native/include/hal/IMU.h new file mode 100644 index 0000000000..77e48de410 --- /dev/null +++ b/hal/src/main/native/include/hal/IMU.h @@ -0,0 +1,79 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "hal/IMUTypes.h" + +/** + * @defgroup hal_imu IMU Functions + * @ingroup hal_capi + * @{ + */ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Get the acceleration along the axes of the IMU in meters per second squared. + * + * @param[out] accel the acceleration data + * @param[out] status the error code, or 0 for success + */ +void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status); + +/** + * Get the angular rate about the axes of the IMU in radians per second. + * + * @param[out] rates the angular rate data + * @param[out] status the error code, or 0 for success + */ +void HAL_GetIMUGyroRates(HAL_GyroRate3d* rates, int32_t* status); + +/** + * Get the angle about the axes of the IMU in radians. + * + * @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); + +/** + * Get the orientation of the IMU as a quaternion. + * + * @param[out] quat the quaternion + * @param[out] status the error code, or 0 for success + */ +void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status); + +/** + * Get the yaw value, in radians, of the IMU in the "flat" orientation. + * + * @param[out] timestamp the timestamp of the sample + * @return flat orientation yaw + */ +double HAL_GetIMUYawFlat(int64_t* timestamp); + +/** + * Get the yaw value, in radians, of the IMU in the "landscape" orientation. + * + * @param[out] timestamp the timestamp of the sample + * @return landscape orientation yaw + */ +double HAL_GetIMUYawLandscape(int64_t* timestamp); + +/** + * Get the yaw value, in radians, of the IMU in the "portrait" orientation. + * + * @param[out] timestamp the timestamp of the sample + * @return portrait orientation yaw + */ +double HAL_GetIMUYawPortrait(int64_t* timestamp); +#ifdef __cplusplus +} // extern "C" +#endif +/** @} */ diff --git a/hal/src/main/native/include/hal/IMUTypes.h b/hal/src/main/native/include/hal/IMUTypes.h new file mode 100644 index 0000000000..8af9c37ce3 --- /dev/null +++ b/hal/src/main/native/include/hal/IMUTypes.h @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +struct HAL_Quaternion { + int64_t timestamp; + double w; + double x; + double y; + double z; +}; +typedef struct HAL_Quaternion HAL_Quaternion; + +struct HAL_Acceleration3d { + int64_t timestamp; + double x; + double y; + double z; +}; +typedef struct HAL_Acceleration3d HAL_Acceleration3d; + +struct HAL_GyroRate3d { + int64_t timestamp; + double x; + double y; + double z; +}; +typedef struct HAL_GyroRate3d HAL_GyroRate3d; + +struct HAL_EulerAngles3d { + int64_t timestamp; + double x; + double y; + double z; +}; +typedef struct HAL_EulerAngles3d HAL_EulerAngles3d; + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/hal/src/main/native/sim/IMU.cpp b/hal/src/main/native/sim/IMU.cpp new file mode 100644 index 0000000000..48bd61013e --- /dev/null +++ b/hal/src/main/native/sim/IMU.cpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/IMU.h" + +extern "C" { +// TODO(Ryan) implement sim +void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) { + *accel = {}; +} +void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) { + *rate = {}; +} +void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status) { + *angles = {}; +} +void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) { + *quat = {}; +} +double HAL_GetIMUYawFlat(int64_t* timestamp) { + return 0; +} +double HAL_GetIMUYawLandscape(int64_t* timestamp) { + return 0; +} +double HAL_GetIMUYawPortrait(int64_t* timestamp) { + return 0; +} +} // extern "C" diff --git a/hal/src/main/native/systemcore/HAL.cpp b/hal/src/main/native/systemcore/HAL.cpp index b3fd01c6c4..5e74560f7e 100644 --- a/hal/src/main/native/systemcore/HAL.cpp +++ b/hal/src/main/native/systemcore/HAL.cpp @@ -59,6 +59,7 @@ void InitializeHAL() { InitializeEncoder(); InitializeFRCDriverStation(); InitializeI2C(); + InitializeIMU(); InitializeMain(); InitializeNotifier(); InitializeCTREPDP(); diff --git a/hal/src/main/native/systemcore/HALInitializer.h b/hal/src/main/native/systemcore/HALInitializer.h index 8199ea6f3f..558f574207 100644 --- a/hal/src/main/native/systemcore/HALInitializer.h +++ b/hal/src/main/native/systemcore/HALInitializer.h @@ -32,6 +32,7 @@ extern void InitializeEncoder(); extern void InitializeFRCDriverStation(); extern void InitializeHAL(); extern void InitializeI2C(); +extern void InitializeIMU(); extern void InitializeMain(); extern void InitializeNotifier(); extern void InitializeCTREPDP(); diff --git a/hal/src/main/native/systemcore/IMU.cpp b/hal/src/main/native/systemcore/IMU.cpp new file mode 100644 index 0000000000..4782a3a5cc --- /dev/null +++ b/hal/src/main/native/systemcore/IMU.cpp @@ -0,0 +1,130 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "hal/IMU.h" + +#include + +#include +#include +#include + +#include "HALInitializer.h" +#include "SystemServerInternal.h" +#include "hal/Errors.h" + +#define IMU_PREFIX "/imu/" + +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* kYawFlatKey = IMU_PREFIX "yaw_flat"; +constexpr const char* kYawLandscapeKey = IMU_PREFIX "yaw_landscape"; +constexpr const char* kYawPortraitKey = IMU_PREFIX "yaw_portrait"; + +struct IMU { + explicit IMU(nt::NetworkTableInstance inst) + : rawAccelSub{inst.GetDoubleArrayTopic(kRawAccelKey).Subscribe({})}, + rawGyroSub{inst.GetDoubleArrayTopic(kRawGyroKey).Subscribe({})}, + quatSub{inst.GetDoubleArrayTopic(kQuaternionKey).Subscribe({})}, + eulerAngleSub{inst.GetDoubleArrayTopic(kEulerAngleKey).Subscribe({})}, + yawFlatSub{inst.GetDoubleTopic(kYawFlatKey).Subscribe(0)}, + yawLandscapeSub{inst.GetDoubleTopic(kYawLandscapeKey).Subscribe(0)}, + yawPortraitSub{inst.GetDoubleTopic(kYawPortraitKey).Subscribe(0)} {} + + nt::DoubleArraySubscriber rawAccelSub; + nt::DoubleArraySubscriber rawGyroSub; + nt::DoubleArraySubscriber quatSub; + nt::DoubleArraySubscriber eulerAngleSub; + nt::DoubleSubscriber yawFlatSub; + nt::DoubleSubscriber yawLandscapeSub; + nt::DoubleSubscriber yawPortraitSub; +}; + +static IMU* imu; + +constexpr double kDegreesToRadians = std::numbers::pi / 180.0; +constexpr double kGToMetersPerSecondSquared = 9.80665; +} // namespace + +namespace hal::init { +void InitializeIMU() { + static IMU imu_static{hal::GetSystemServer()}; + imu = &imu_static; +} +} // namespace hal::init + +extern "C" { +void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) { + auto update = imu->rawAccelSub.GetAtomic(); + if (update.value.size() != 3) { + *status = INCOMPATIBLE_STATE; + return; + } + *accel = + HAL_Acceleration3d{.timestamp = update.time, + .x = update.value[0] * kGToMetersPerSecondSquared, + .y = update.value[1] * kGToMetersPerSecondSquared, + .z = update.value[2] * kGToMetersPerSecondSquared}; +} + +void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) { + auto update = imu->rawGyroSub.GetAtomic(); + if (update.value.size() != 3) { + *status = INCOMPATIBLE_STATE; + return; + } + + *rate = HAL_GyroRate3d{.timestamp = update.time, + .x = update.value[0] * kDegreesToRadians, + .y = update.value[1] * kDegreesToRadians, + .z = update.value[2] * kDegreesToRadians}; +} + +void HAL_GetIMUEulerAngles(HAL_EulerAngles3d* angles, int32_t* status) { + auto update = imu->eulerAngleSub.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_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) { + auto update = imu->quatSub.GetAtomic(); + if (update.value.size() != 4) { + *status = INCOMPATIBLE_STATE; + return; + } + *quat = HAL_Quaternion{.timestamp = update.time, + .w = update.value[0], + .x = update.value[1], + .y = update.value[2], + .z = update.value[3]}; +} + +double HAL_GetIMUYawFlat(int64_t* timestamp) { + auto update = imu->yawFlatSub.GetAtomic(); + *timestamp = update.time; + return update.value * kDegreesToRadians; +} + +double HAL_GetIMUYawLandscape(int64_t* timestamp) { + auto update = imu->yawLandscapeSub.GetAtomic(); + *timestamp = update.time; + return update.value * kDegreesToRadians; +} + +double HAL_GetIMUYawPortrait(int64_t* timestamp) { + auto update = imu->yawFlatSub.GetAtomic(); + *timestamp = update.time; + return update.value * kDegreesToRadians; +} +} // extern "C" diff --git a/wpilibc/src/main/native/cpp/OnboardIMU.cpp b/wpilibc/src/main/native/cpp/OnboardIMU.cpp new file mode 100644 index 0000000000..43fb2a8906 --- /dev/null +++ b/wpilibc/src/main/native/cpp/OnboardIMU.cpp @@ -0,0 +1,131 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/OnboardIMU.h" + +#include + +#include "frc/Errors.h" + +using namespace frc; + +OnboardIMU::OnboardIMU(MountOrientation mountOrientation) + : m_mountOrientation{mountOrientation} { + // TODO: usage reporting +} + +units::radian_t OnboardIMU::GetYawNoOffset() { + int64_t timestamp; + double val; + switch (m_mountOrientation) { + case kFlat: + val = HAL_GetIMUYawFlat(×tamp); + break; + case kLandscape: + val = HAL_GetIMUYawLandscape(×tamp); + break; + case kPortrait: + val = HAL_GetIMUYawPortrait(×tamp); + break; + default: + val = 0; + } + return units::radian_t{val}; +} + +units::radian_t OnboardIMU::GetYaw() { + return GetYawNoOffset() - m_yawOffset; +} + +void OnboardIMU::ResetYaw() { + m_yawOffset = GetYawNoOffset(); +} + +Rotation2d OnboardIMU::GetRotation2d() { + return Rotation2d{GetYaw()}; +} + +Rotation3d OnboardIMU::GetRotation3d() { + return Rotation3d{GetQuaternion()}; +} + +Quaternion OnboardIMU::GetQuaternion() { + HAL_Quaternion val; + int32_t status = 0; + HAL_GetIMUQuaternion(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return Quaternion{val.w, val.x, val.y, val.z}; +} + +units::radian_t GetAngleX() { + HAL_EulerAngles3d val; + int32_t status = 0; + HAL_GetIMUEulerAngles(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::radian_t{val.x}; +} + +units::radian_t GetAngleY() { + HAL_EulerAngles3d val; + int32_t status = 0; + HAL_GetIMUEulerAngles(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::radian_t{val.y}; +} + +units::radian_t GetAngleZ() { + HAL_EulerAngles3d val; + int32_t status = 0; + HAL_GetIMUEulerAngles(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::radian_t{val.z}; +} + +units::radians_per_second_t OnboardIMU::GetGyroRateX() { + HAL_GyroRate3d val; + int32_t status = 0; + HAL_GetIMUGyroRates(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::radians_per_second_t{val.x}; +} + +units::radians_per_second_t OnboardIMU::GetGyroRateY() { + HAL_GyroRate3d val; + int32_t status = 0; + HAL_GetIMUGyroRates(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::radians_per_second_t{val.y}; +} + +units::radians_per_second_t OnboardIMU::GetGyroRateZ() { + HAL_GyroRate3d val; + int32_t status = 0; + HAL_GetIMUGyroRates(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::radians_per_second_t{val.z}; +} + +units::meters_per_second_squared_t OnboardIMU::GetAccelX() { + HAL_Acceleration3d val; + int32_t status = 0; + HAL_GetIMUAcceleration(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::meters_per_second_squared_t{val.x}; +} + +units::meters_per_second_squared_t OnboardIMU::GetAccelY() { + HAL_Acceleration3d val; + int32_t status = 0; + HAL_GetIMUAcceleration(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::meters_per_second_squared_t{val.x}; +} + +units::meters_per_second_squared_t OnboardIMU::GetAccelZ() { + HAL_Acceleration3d val; + int32_t status = 0; + HAL_GetIMUAcceleration(&val, &status); + FRC_CheckErrorStatus(status, "Onboard IMU"); + return units::meters_per_second_squared_t{val.x}; +} diff --git a/wpilibc/src/main/native/include/frc/OnboardIMU.h b/wpilibc/src/main/native/include/frc/OnboardIMU.h new file mode 100644 index 0000000000..f61c5b921a --- /dev/null +++ b/wpilibc/src/main/native/include/frc/OnboardIMU.h @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once +#include +#include +#include + +#include +#include +#include + +namespace frc { + +/** + * SystemCore onboard IMU + */ +class OnboardIMU { + public: + /** + * A mount orientation of SystemCore + */ + enum MountOrientation { + /** Flat */ + kFlat, + /** Landscape */ + kLandscape, + /** Portrait */ + kPortrait + }; + + /** + * Constructs a handle to the SystemCore onboard IMU. + * @param mountOrientation the mount orientation of SystemCore to determine + * yaw. + */ + explicit OnboardIMU(MountOrientation mountOrientation); + + /** + * Get the yaw value + * @return yaw value + */ + units::radian_t GetYaw(); + + /** + * Reset the current yaw value to 0. Future reads of the yaw value will be + * relative to the current orientation. + */ + void ResetYaw(); + + /** + * Get the yaw as a Rotation2d. + * @return yaw + */ + Rotation2d GetRotation2d(); + + /** + * Get the 3D orientation as a Rotation3d. + * @return 3D orientation + */ + Rotation3d GetRotation3d(); + + /** + * Get the 3D orientation as a Quaternion. + * @return 3D orientation + */ + Quaternion GetQuaternion(); + + /** + * Get the angle about the X axis of the IMU. + * @return angle about the X axis + */ + units::radian_t GetAngleX(); + + /** + * Get the angle about the Y axis of the IMU. + * @return angle about the Y axis + */ + units::radian_t GetAngleY(); + + /** + * Get the angle about the Z axis of the IMU. + * @return angle about the Z axis + */ + units::radian_t GetAngleZ(); + + /** + * Get the angular rate about the X axis of the IMU. + * @return angular rate about the X axis + */ + units::radians_per_second_t GetGyroRateX(); + + /** + * Get the angular rate about the Y axis of the IMU. + * @return angular rate about the Y axis + */ + units::radians_per_second_t GetGyroRateY(); + + /** + * Get the angular rate about the Z axis of the IMU. + * @return angular rate about the Z axis + */ + units::radians_per_second_t GetGyroRateZ(); + + /** + * Get the acceleration along the X axis of the IMU. + * @return acceleration along the X axis + */ + units::meters_per_second_squared_t GetAccelX(); + + /** + * Get the acceleration along the Z axis of the IMU. + * @return acceleration along the Z axis + */ + units::meters_per_second_squared_t GetAccelY(); + + /** + * Get the acceleration along the Z axis of the IMU. + * @return acceleration along the Z axis + */ + units::meters_per_second_squared_t GetAccelZ(); + + private: + units::radian_t GetYawNoOffset(); + MountOrientation m_mountOrientation; + units::radian_t m_yawOffset{0}; +}; +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java new file mode 100644 index 0000000000..8e36ac877e --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/OnboardIMU.java @@ -0,0 +1,193 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.IMUJNI; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; + +/** SystemCore onboard IMU. */ +public class OnboardIMU { + /** A mount orientation of SystemCore. */ + public enum MountOrientation { + /** Flat. */ + kFlat, + /** Landscape. */ + kLandscape, + /** Portrait. */ + kPortrait + } + + /** + * Constructs a handle to the SystemCore onboard IMU. + * + * @param mountOrientation the mount orientation of SystemCore to determine yaw. + */ + public OnboardIMU(MountOrientation mountOrientation) { + m_mountOrientation = mountOrientation; + } + + /** + * Get the yaw value in radians. + * + * @return yaw value in radians + */ + public double getYawRadians() { + return getYawNoOffset() - m_yawOffset; + } + + /** + * Reset the current yaw value to 0. Future reads of the yaw value will be relative to the current + * orientation. + */ + public void resetYaw() { + m_yawOffset = getYawNoOffset(); + } + + /** + * Get the yaw as a Rotation2d. + * + * @return yaw + */ + public Rotation2d getRotation2d() { + return new Rotation2d(getYawRadians()); + } + + /** + * Get the 3D orientation as a Rotation3d. + * + * @return 3D orientation + */ + public Rotation3d getRotation3d() { + return new Rotation3d(getQuaternion()); + } + + /** + * Get the 3D orientation as a Quaternion. + * + * @return 3D orientation + */ + public Quaternion getQuaternion() { + double[] quatRaw = new double[4]; + IMUJNI.getIMUQuaternion(quatRaw); + return new Quaternion(quatRaw[0], quatRaw[1], quatRaw[2], quatRaw[3]); + } + + /** + * Get the angle about the X axis of the IMU in radians. + * + * @return angle about the X axis in radians + */ + public double getAngleX() { + return getRawEulerAngles()[0]; + } + + /** + * Get the angle about the Y axis of the IMU in radians. + * + * @return angle about the Y axis in radians + */ + public double getAngleY() { + return getRawEulerAngles()[1]; + } + + /** + * Get the angle about the Z axis of the IMU in radians. + * + * @return angle about the Z axis in radians + */ + public double getAngleZ() { + return getRawEulerAngles()[2]; + } + + /** + * Get the angular rate about the X axis of the IMU in radians per second. + * + * @return angular rate about the X axis in radians per second + */ + public double getGyroRateX() { + return getRawGyroRates()[0]; + } + + /** + * Get the angular rate about the Y axis of the IMU in radians per second. + * + * @return angular rate about the Y axis in radians per second + */ + public double getGyroRateY() { + return getRawGyroRates()[1]; + } + + /** + * Get the angular rate about the Z axis of the IMU in radians per second. + * + * @return angular rate about the Z axis in radians per second + */ + public double getGyroRateZ() { + return getRawGyroRates()[2]; + } + + /** + * Get the acceleration along the X axis of the IMU in meters per second squared. + * + * @return acceleration along the X axis in meters per second squared + */ + public double getAccelX() { + return getRawAccels()[0]; + } + + /** + * Get the acceleration along the X axis of the IMU in meters per second squared. + * + * @return acceleration along the X axis in meters per second squared + */ + public double getAccelY() { + return getRawAccels()[1]; + } + + /** + * Get the acceleration along the X axis of the IMU in meters per second squared. + * + * @return acceleration along the X axis in meters per second squared + */ + public double getAccelZ() { + return getRawAccels()[2]; + } + + private double[] getRawEulerAngles() { + double[] anglesRaw = new double[3]; + IMUJNI.getIMUEulerAngles(anglesRaw); + return anglesRaw; + } + + private double[] getRawGyroRates() { + double[] ratesRaw = new double[3]; + IMUJNI.getIMUGyroRates(ratesRaw); + return ratesRaw; + } + + private double[] getRawAccels() { + double[] accelsRaw = new double[3]; + IMUJNI.getIMUAcceleration(accelsRaw); + return accelsRaw; + } + + private double getYawNoOffset() { + switch (m_mountOrientation) { + case kFlat: + return IMUJNI.getIMUYawFlat(); + case kLandscape: + return IMUJNI.getIMUYawLandscape(); + case kPortrait: + return IMUJNI.getIMUYawPortrait(); + default: + return 0; + } + } + + private final MountOrientation m_mountOrientation; + private double m_yawOffset; +}