diff --git a/config/template.json b/config/template.json index 08a2ccf1..5653f8b0 100644 --- a/config/template.json +++ b/config/template.json @@ -29,6 +29,11 @@ "enabled": true, "bus": 2, "device_address": 25 + }, + "gyroscope": { + "enabled": true, + "bus": 2, + "device_address": 105 } } } diff --git a/lib/core/types.hpp b/lib/core/types.hpp index ccb3ccc7..0cdba4a3 100644 --- a/lib/core/types.hpp +++ b/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 \ No newline at end of file diff --git a/lib/debug/repl.cpp b/lib/debug/repl.cpp index 4928e4cb..332f65ca 100644 --- a/lib/debug/repl.cpp +++ b/lib/debug/repl.cpp @@ -196,6 +196,28 @@ std::optional> 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(*optional_gyroscope); + Command gyroscope_read_command; + std::stringstream identifier; + identifier << "gyroscope 0x" << std::hex << static_cast(device_address) << " read"; + gyroscope_read_command.name = identifier.str(); + std::stringstream description; + description << "Read gyroscope sensor 0x" << std::hex << static_cast(device_address) + << " on " + << "I2C bus " << static_cast(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 value; + + while (true) { + std::optional value = gyroscope->read(core::Axis::kX); + const auto result = value.value(); + + std::chrono::duration 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(bus); diff --git a/lib/debug/repl.hpp b/lib/debug/repl.hpp index 49e1bb25..d00a36d9 100644 --- a/lib/debug/repl.hpp +++ b/lib/debug/repl.hpp @@ -14,6 +14,9 @@ #include #include #include +#include +#include +#include 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_; diff --git a/lib/sensors/gyroscope.cpp b/lib/sensors/gyroscope.cpp new file mode 100644 index 00000000..a45d3989 --- /dev/null +++ b/lib/sensors/gyroscope.cpp @@ -0,0 +1,146 @@ +#include "gyroscope.hpp" + +namespace hyped::sensors { + +Gyroscope::Gyroscope(core::ILogger &logger, + std::shared_ptr 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 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 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 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 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::create(core::ILogger &logger, + std::shared_ptr 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 \ No newline at end of file diff --git a/lib/sensors/gyroscope.hpp b/lib/sensors/gyroscope.hpp new file mode 100644 index 00000000..714dc39b --- /dev/null +++ b/lib/sensors/gyroscope.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include "i2c_sensors.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include + +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 create(core::ILogger &logger, + std::shared_ptr i2c, + const std::uint8_t channel, + const std::uint8_t device_address + = kDefaultGyroscopeAddress); + ~Gyroscope(); + + std::optional read(core::Axis axis); + std::uint8_t getChannel() const; + + private: + Gyroscope(core::ILogger &logger, + std::shared_ptr i2c, + const std::uint8_t channel, + const std::uint8_t device_address); + + core::ILogger &logger_; + std::shared_ptr i2c_; + const std::uint8_t channel_; + const std::uint8_t device_address_; +}; + +} // namespace hyped::sensors \ No newline at end of file