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

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 <hal/IMU.h>
+
+#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(&timestamp);
+      break;
+    case kLandscape:
+      val = HAL_GetIMUYawLandscape(&timestamp);
+      break;
+    case kPortrait:
+      val = HAL_GetIMUYawPortrait(&timestamp);
+      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};
+}

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 <frc/geometry/Quaternion.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Rotation3d.h>
+
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+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