[hal, wpilib] Add systemcore IMU (#8016)

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 <jni.h>
+
+#include <string_view>
+
+#include <fmt/format.h>
+
+#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(&timestamp);
+}
+
+/*
+ * 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(&timestamp);
+}
+
+/*
+ * 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(&timestamp);
+}
+
+}  // extern "C"

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 <stdint.h>
+
+#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
+/** @} */

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 <stdint.h>
+
+#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

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"

hal/src/main/native/systemcore/HAL.cpp

@@ -59,6 +59,7 @@ void InitializeHAL() {
   InitializeEncoder();
   InitializeFRCDriverStation();
   InitializeI2C();
+  InitializeIMU();
   InitializeMain();
   InitializeNotifier();
   InitializeCTREPDP();

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();

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 <numbers>
+
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTableInstance.h>
+
+#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"