SNS-Gyroscope

config/template.json

@@ -29,6 +29,11 @@
 				"enabled": true,
 				"bus": 2,
 				"device_address": 25
+			},
+			"gyroscope": {
+				"enabled": true,
+				"bus": 2,
+				"device_address": 105
 			}
 		}
 	}

lib/core/types.hpp

@@ -62,6 +62,18 @@ struct RawAccelerationData {
   const TimePoint measured_at;
 };
 
+struct GyroscopeData {
+  GyroscopeData(const core::Float x,
+                      const TimePoint measured_at)
+      : x(x),
+        measured_at(measured_at)
+  {
+  }
+
+  const core::Float x;
+  const TimePoint measured_at;
+};
+
 enum class Axis { kX = 0, kY, kZ };
 
-}  // namespace hyped::core
+}  // namespace hyped::core

lib/debug/repl.cpp

@@ -196,6 +196,28 @@ std::optional<std::unique_ptr<Repl>> Repl::fromFile(const std::string &path)
     const auto bus            = accelerometer["bus"].GetUint();
     repl->addAccelerometerCommands(bus, device_address);
   }
+
+  if (!sensors.HasMember("gyroscope")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.gyroscope' in configuration file");
+    return std::nullopt;
+  }
+  const auto gyroscope = sensors["gyroscope"].GetObject();
+  if (!gyroscope.HasMember("enabled")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.gyroscope.enabled' in configuration file");
+    return std::nullopt;
+  }
+  if (gyroscope["enabled"].GetBool()) {
+    if (!gyroscope.HasMember("bus")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.gyroscope.bus' in configuration file");
+      return std::nullopt;
+    }
+    const auto device_address = gyroscope["device_address"].GetUint();
+    const auto bus            = gyroscope["bus"].GetUint();
+    repl->addGyroscopeCommands(bus, device_address);
+  }
   return repl;
 }
 
@@ -493,6 +515,53 @@ void Repl::addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t d
   addCommand(accelerometer_read_command);
 }
 
+void Repl::addGyroscopeCommands(const std::uint8_t bus, const std::uint8_t device_address)
+{
+  const auto optional_i2c = io::HardwareI2c::create(logger_, bus);
+  if (!optional_i2c) {
+    logger_.log(core::LogLevel::kFatal, "Failed to create I2C instance on bus %d", bus);
+    return;
+  }
+  const auto i2c                = std::move(*optional_i2c);
+  const auto optional_gyroscope = sensors::Gyroscope::create(logger_, i2c, bus, device_address);
+  const auto gyroscope          = std::make_shared<sensors::Gyroscope>(*optional_gyroscope);
+  Command gyroscope_read_command;
+  std::stringstream identifier;
+  identifier << "gyroscope 0x" << std::hex << static_cast<int>(device_address) << " read";
+  gyroscope_read_command.name = identifier.str();
+  std::stringstream description;
+  description << "Read gyroscope sensor 0x" << std::hex << static_cast<int>(device_address)
+              << " on "
+              << "I2C bus " << static_cast<int>(bus);
+  gyroscope_read_command.description = description.str();
+  gyroscope_read_command.handler     = [this, gyroscope, bus]() {
+    core::Float Angle = 0;
+    core::Float Old_data = 0;
+    auto Old_time = std::chrono::system_clock::now();
+    std::optional<core::GyroscopeData> value;
+
+    while (true) {
+      std::optional<core::GyroscopeData> value = gyroscope->read(core::Axis::kX);
+      const auto result = value.value();
+
+      std::chrono::duration<double> elapsed_seconds = result.measured_at - Old_time;
+
+      auto time = (core::Float) elapsed_seconds.count();
+
+      Angle = 0.5*(Old_data+result.x)*time + Angle;
+
+      Old_time = result.measured_at;
+      Old_data = result.x;
+
+      logger_.log(core::LogLevel::kInfo, "Gyroscope is : %f", Angle);
+
+    };
+
+  };
+  addCommand(gyroscope_read_command);
+}
+
+
 void Repl::addUartCommands(const std::uint8_t bus)
 {
   const UartBus uart_bus   = static_cast<UartBus>(bus);

lib/debug/repl.hpp

@@ -14,6 +14,9 @@
 #include <io/hardware_uart.hpp>
 #include <io/pwm.hpp>
 #include <sensors/accelerometer.hpp>
+#include <sensors/gyroscope.hpp>
+#include <unistd.h>
+#include <chrono>
 
 namespace hyped::debug {
 
@@ -41,6 +44,7 @@ class Repl {
   void addPwmCommands(const std::uint8_t module);
   void addSpiCommands(const std::uint8_t bus);
   void addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t device_address);
+  void addGyroscopeCommands(const std::uint8_t bus, const std::uint8_t device_address);
   void addUartCommands(const std::uint8_t bus);
 
   core::ILogger &logger_;

lib/sensors/gyroscope.cpp

@@ -0,0 +1,146 @@
+#include "gyroscope.hpp"
+
+namespace hyped::sensors {
+
+Gyroscope::Gyroscope(core::ILogger &logger,
+                     std::shared_ptr<io::II2c> i2c,
+                     const std::uint8_t channel,
+                     const std::uint8_t device_address)
+    : logger_(logger),
+      i2c_(i2c),
+      channel_(channel),
+      device_address_(device_address)
+{
+}
+
+Gyroscope::~Gyroscope()
+{
+}
+
+std::optional<core::GyroscopeData> Gyroscope::read(core::Axis axis)
+{
+  switch (axis) {
+    case core::Axis::kX: {
+      const auto gyroscope_x_high_byte = i2c_->readByte(device_address_, 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(device_address_, 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 << 8) | *gyroscope_x_low_byte);
+      logger_.log(
+        core::LogLevel::kDebug, "Successfully read x-axis from gyroscope at channel %d", channel_);
+
+      core::Float X_Value = (core::Float)x_axis;
+      X_Value *= 0.00875;
+      const std::optional<core::GyroscopeData> X_values(std::in_place,X_Value,std::chrono::system_clock::now());
+
+        return X_values;
+    }
+    case core::Axis::kY: {
+      const auto gyroscope_y_high_byte = i2c_->readByte(device_address_, 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(device_address_, 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 << 8) | *gyroscope_y_low_byte);
+      logger_.log(
+        core::LogLevel::kDebug, "Successfully read y-axis from gyroscope at channel %d", channel_);
+
+      core::Float Y_Value = (core::Float)y_axis;
+      Y_Value *= 0.00875;
+      const std::optional<core::GyroscopeData> Y_values(std::in_place,Y_Value,std::chrono::system_clock::now());
+
+        return Y_values;
+    }
+    case core::Axis::kZ: {
+      const auto gyroscope_z_high_byte = i2c_->readByte(device_address_, 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(device_address_, 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 << 8) | *gyroscope_z_low_byte);
+      logger_.log(
+        core::LogLevel::kDebug, "Successfully read z-axis from gyroscope at channel %d", channel_);
+
+      core::Float Z_Value = (core::Float)z_axis;
+      Z_Value *= 0.00875;
+      const std::optional<core::GyroscopeData> Z_values(std::in_place,Z_Value,std::chrono::system_clock::now());
+
+        return Z_values;
+    }
+    default: {
+      logger_.log(core::LogLevel::kFatal, "Gave an invalid axis");
+      return std::nullopt;
+    }
+  }
+}
+
+std::optional<Gyroscope> Gyroscope::create(core::ILogger &logger,
+                                           std::shared_ptr<io::II2c> i2c,
+                                           const std::uint8_t channel,
+                                           const std::uint8_t device_address)
+{
+  const core::Result write_result_from_ctrl1
+    = i2c->writeByteToRegister(device_address, kCtrl1, kConfigurationSetting1);
+  if (write_result_from_ctrl1 == core::Result::kFailure) {
+    logger.log(
+      core::LogLevel::kFatal,
+      "Failed to configure the power mode setting in first control gyroscope at channel %d",
+      channel);
+    return std::nullopt;
+  }
+  const core::Result write_result_from_ctrl2
+    = i2c->writeByteToRegister(device_address, kCtrl2, kConfigurationSetting2);
+  if (write_result_from_ctrl2 == core::Result::kFailure) {
+    logger.log(core::LogLevel::kFatal,
+               "Failed to configure the High pass filter in second control gyroscope at channel %d",
+               channel);
+    return std::nullopt;
+  }
+  const core::Result write_result_from_ctrl3
+    = i2c->writeByteToRegister(device_address, kCtrl3, kConfigurationSetting3);
+  if (write_result_from_ctrl3 == core::Result::kFailure) {
+    logger.log(
+      core::LogLevel::kFatal,
+      "Failed to configure the Boot and Interrupts in third control gyroscope at channel %d",
+      channel);
+    return std::nullopt;
+  }
+  const core::Result write_result_from_ctrl5
+    = i2c->writeByteToRegister(device_address, kCtrl5, kConfigurationSetting5);
+  if (write_result_from_ctrl5 == core::Result::kFailure) {
+    logger.log(core::LogLevel::kFatal,
+               "Failed to enable FIFO in fifth control gyroscope at channel %d",
+               channel);
+    return std::nullopt;
+  }
+  return Gyroscope(logger, i2c, channel, device_address);
+}
+
+std::uint8_t Gyroscope::getChannel() const
+{
+  return channel_;
+}
+
+}  // namespace hyped::sensors

lib/sensors/gyroscope.hpp

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