[xrp] Add GetRotation2d to Gyro (#7034)

xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.xrp;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 /**
  * Use a rate gyro to return the robots heading relative to a starting position.
@@ -154,6 +155,15 @@ public class XRPGyro {
     return getAngleZ();
   }
 
+  /**
+   * Gets the angle the robot is facing.
+   *
+   * @return A Rotation2d with the current heading.
+   */
+  public Rotation2d getRotation2d() {
+    return Rotation2d.fromDegrees(getAngle());
+  }
+
   /**
    * Return the rate of rotation of the gyro
    *

xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/xrp/XRPGyro.h"
 
+#include <units/angle.h>
+
 using namespace frc;
 
 XRPGyro::XRPGyro() : m_simDevice("Gyro:XRPGyro") {
@@ -28,6 +30,10 @@ double XRPGyro::GetAngle() const {
   return GetAngleZ();
 }
 
+frc::Rotation2d XRPGyro::GetRotation2d() const {
+  return frc::Rotation2d{units::degree_t{GetAngle()}};
+}
+
 double XRPGyro::GetRate() const {
   return GetRateZ();
 }

xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <frc/geometry/Rotation2d.h>
+
 #include <hal/SimDevice.h>
 
 namespace frc {
@@ -40,6 +42,13 @@ class XRPGyro {
    */
   double GetAngle() const;
 
+  /**
+   * Gets the angle the robot is facing.
+   *
+   * @return A Rotation2d with the current heading.
+   */
+  frc::Rotation2d GetRotation2d() const;
+
   /**
    * Return the rate of rotation of the gyro
    *