SNS-Gyroscope

lib/core/types.hpp

@@ -9,6 +9,7 @@ static constexpr float kEpsilon = 0.0001;
 
 enum class DigitalSignal { kLow = 0, kHigh };
 enum class Result { kSuccess = 0, kFailure };
+enum class GyroscopeAxis { kX, kY, kZ };
 
 using Float = float;
 
@@ -40,4 +41,4 @@ using ImuData     = std::array<Float, kNumImus>;
 using EncoderData = std::array<std::uint32_t, kNumEncoders>;
 using KeyenceData = std::array<std::uint32_t, kNumKeyence>;
 
-}  // namespace hyped::core
+}  // namespace hyped::core

lib/sensors/gyroscope.cpp

@@ -0,0 +1,117 @@
+#include "gyroscope.hpp"
+
+namespace hyped::sensors {
+
+Gyroscope::Gyroscope(core::ILogger &logger, io::II2c &i2c, const std::uint8_t channel)
+    : logger_(logger),
+      i2c_(i2c),
+      channel_(channel)
+{
+}
+
+Gyroscope::~Gyroscope()
+{
+}
+
+std::optional<std::int16_t> Gyroscope::read(core::GyroscopeAxis axis)
+{
+  if (axis == core::GyroscopeAxis::kX) {
+    const auto gyroscope_x_high_byte = i2c_.readByte(kDefaultGyroscopeAddress, kDataXHigh);
+    if (!gyroscope_x_high_byte) {
+      logger_.log(
+        core::LogLevel::kFatal, "Failed to, read gyroscope X-axis high at channel %d", channel_);
+      return std::nullopt;
+    }
+    const auto gyroscope_x_low_byte = i2c_.readByte(kDefaultGyroscopeAddress, kDataXLow);
+    if (!gyroscope_x_low_byte) {
+      logger_.log(
+        core::LogLevel::kFatal, "Failed to, read gyroscope X-axis low at channel %d", channel_);
+      return std::nullopt;
+    }
+    const auto x_axis = ((gyroscope_x_high_byte.value() << 8) | gyroscope_x_low_byte.value());
+    logger_.log(
+      core::LogLevel::kDebug, "Successfully read x-axis from gyroscope at channel %d", channel_);
+    return x_axis;
+  } else if (axis == core::GyroscopeAxis::kY) {
+    const auto gyroscope_y_high_byte = i2c_.readByte(kDefaultGyroscopeAddress, kDataYHigh);
+    if (!gyroscope_y_high_byte) {
+      logger_.log(
+        core::LogLevel::kFatal, "Failed to, read gyroscope Y-axis high at channel %d", channel_);
+      return std::nullopt;
+    }
+    const auto gyroscope_y_low_byte = i2c_.readByte(kDefaultGyroscopeAddress, kDataYLow);
+    if (!gyroscope_y_low_byte) {
+      logger_.log(
+        core::LogLevel::kFatal, "Failed to, read gyroscope Y-axis low at channel %d", channel_);
+      return std::nullopt;
+    }
+    const auto y_axis = ((gyroscope_y_high_byte.value() << 8) | gyroscope_y_low_byte.value());
+    logger_.log(
+      core::LogLevel::kDebug, "Successfully read y-axis from gyroscope at channel %d", channel_);
+    return y_axis;
+  } else if (axis == core::GyroscopeAxis::kZ) {
+    const auto gyroscope_z_high_byte = i2c_.readByte(kDefaultGyroscopeAddress, kDataZHigh);
+    if (!gyroscope_z_high_byte) {
+      logger_.log(
+        core::LogLevel::kFatal, "Failed to, read gyroscope Z-axis high at channel %d", channel_);
+      return std::nullopt;
+    }
+    const auto gyroscope_z_low_byte = i2c_.readByte(kDefaultGyroscopeAddress, kDataZLow);
+    if (!gyroscope_z_low_byte) {
+      logger_.log(
+        core::LogLevel::kFatal, "Failed to, read gyroscope Z-axis low at channel %d", channel_);
+      return std::nullopt;
+    }
+    const auto z_axis = ((gyroscope_z_high_byte.value() << 8) | gyroscope_z_low_byte.value());
+    logger_.log(
+      core::LogLevel::kDebug, "Successfully read z-axis from gyroscope at channel %d", channel_);
+    return z_axis;
+  } else {
+    logger_.log(core::LogLevel::kFatal,
+                "Failed to, read gyroscope at channel %d, axis parameters are invalid types",
+                channel_);
+    return std::nullopt;
+  }
+}
+
+std::optional<Gyroscope> Gyroscope::create(core::ILogger &logger,
+                                           io::II2c &i2c,
+                                           const std::uint8_t channel)
+{
+  const core::Result write_result1
+    = i2c.writeByteToRegister(kDefaultGyroscopeAddress, kCtrl1, kConfigurationSetting1);
+  if (write_result1 == core::Result::kFailure) {
+    logger.log(
+      core::LogLevel::kFatal, "Failed to, configure gyroscope control 1 at channel %d", channel);
+    return std::nullopt;
+  }
+  const core::Result write_result2
+    = i2c.writeByteToRegister(kDefaultGyroscopeAddress, kCtrl2, kConfigurationSetting2);
+  if (write_result2 == core::Result::kFailure) {
+    logger.log(
+      core::LogLevel::kFatal, "Failed to, configure gyroscope control 2 at channel %d", channel);
+    return std::nullopt;
+  }
+  const core::Result write_result3
+    = i2c.writeByteToRegister(kDefaultGyroscopeAddress, kCtrl3, kConfigurationSetting3);
+  if (write_result3 == core::Result::kFailure) {
+    logger.log(
+      core::LogLevel::kFatal, "Failed to, configure gyroscope control 3 at channel %d", channel);
+    return std::nullopt;
+  }
+  const core::Result write_result5
+    = i2c.writeByteToRegister(kDefaultGyroscopeAddress, kCtrl5, kConfigurationSetting5);
+  if (write_result5 == core::Result::kFailure) {
+    logger.log(
+      core::LogLevel::kFatal, "Failed to, configure gyroscope control 5 at channel %d", channel);
+    return std::nullopt;
+  }
+  return Gyroscope(logger, i2c, channel);
+}
+
+std::uint8_t Gyroscope::getChannel()
+{
+  return channel_;
+}
+
+}  // namespace hyped::sensors

lib/sensors/gyroscope.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include "i2c_sensors.hpp"
+
+#include <cstdint>
+#include <optional>
+
+#include <core/logger.hpp>
+#include <io/i2c.hpp>
+
+namespace hyped::sensors {
+
+static constexpr std::uint8_t kDefaultGyroscopeAddress = 0x69;
+static constexpr std::uint8_t kDataXHigh               = 0x29;
+static constexpr std::uint8_t kDataXLow                = 0x28;
+static constexpr std::uint8_t kDataYHigh               = 0x2B;
+static constexpr std::uint8_t kDataYLow                = 0x2A;
+static constexpr std::uint8_t kDataZHigh               = 0x2D;
+static constexpr std::uint8_t kDataZLow                = 0x2C;
+static constexpr std::uint8_t kCtrl1                   = 0x20;
+static constexpr std::uint8_t kCtrl2                   = 0x21;
+static constexpr std::uint8_t kCtrl3                   = 0x22;
+static constexpr std::uint8_t kCtrl5                   = 0x24;
+static constexpr std::uint8_t kConfigurationSetting1   = 0xff;
+static constexpr std::uint8_t kConfigurationSetting2   = 0x20;
+static constexpr std::uint8_t kConfigurationSetting3   = 0xff;
+static constexpr std::uint8_t kConfigurationSetting5   = 0x40;
+
+class Gyroscope {
+ public:
+  static std::optional<Gyroscope> create(core::ILogger &logger,
+                                         io::II2c &i2c,
+                                         const std::uint8_t channel);
+  ~Gyroscope();
+
+  std::optional<std::int16_t> read(core::GyroscopeAxis axis);
+  std::uint8_t getChannel();
+
+ private:
+  Gyroscope(core::ILogger &logger, io::II2c &i2c, const std::uint8_t channel);
+
+  core::ILogger &logger_;
+  io::II2c &i2c_;
+  const std::uint8_t channel_;
+};
+
+}  // namespace hyped::sensors