GyroAxis.hpp

Go to the documentation of this file.

// Copyright (c) 2026 Yet Another Software Suite
// SPDX-License-Identifier: LGPL-3.0-or-later
 
#pragma once
 
#include <numbers>
#include <wpi/math/geometry/Quaternion.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/units/angle.hpp>
 
namespace yams::mechanisms::config {
 
enum class GyroAxis {
  ROLL,
  PITCH,
  YAW
};
 
inline wpi::math::Rotation3d ToRobotAttitude(GyroAxis axis,
                                             const wpi::math::Rotation3d& gyroAttitude) {
  if (axis == GyroAxis::YAW) {
    return gyroAttitude;
  }
  const wpi::units::radian_t zero{0};
  const wpi::units::radian_t quarterTurn{std::numbers::pi / 2};
  const wpi::math::Rotation3d gyroToRobot =
      axis == GyroAxis::ROLL ? wpi::math::Rotation3d{zero, -quarterTurn, zero}
                             : wpi::math::Rotation3d{quarterTurn, zero, zero};
  const auto q = gyroToRobot.GetQuaternion();
  return wpi::math::Rotation3d{q * gyroAttitude.GetQuaternion() * q.Inverse()};
}
 
}  // namespace yams::mechanisms::config