From 2e2ba1bddf8b105dcbd984567e9d95a14b9eb90f Mon Sep 17 00:00:00 2001 From: licornes-fluos <54073764+licornes-fluos@users.noreply.github.com> Date: Tue, 2 Jul 2024 00:42:08 +0100 Subject: [PATCH] SNS - Accelerometer REPL Commands (#98) Co-authored-by: H-Allen Co-authored-by: Tom Lonergan Co-authored-by: David Beechey --- config/debug.toml | 3 +- lib/debug/commands/accelerometer_commands.cpp | 82 +++++++++++++++++++ lib/debug/commands/accelerometer_commands.hpp | 19 +++++ lib/debug/repl.cpp | 13 ++- lib/sensors/CMakeLists.txt | 1 + lib/sensors/accelerometer.cpp | 28 ++++--- lib/sensors/accelerometer.hpp | 20 ++--- lib/sensors/accelerometer_node.cpp | 82 +++++++++++++++++++ lib/sensors/accelerometer_node.hpp | 29 +++++++ 9 files changed, 251 insertions(+), 26 deletions(-) create mode 100644 lib/debug/commands/accelerometer_commands.cpp create mode 100644 lib/debug/commands/accelerometer_commands.hpp create mode 100644 lib/sensors/accelerometer_node.cpp create mode 100644 lib/sensors/accelerometer_node.hpp diff --git a/config/debug.toml b/config/debug.toml index 80388389..634021cc 100644 --- a/config/debug.toml +++ b/config/debug.toml @@ -24,7 +24,8 @@ enabled = false [sensors.accelerometer] enabled = false bus = 2 -address = 25 +address = 29 +mux_channel = 0 [sensors.temperature] enabled = false diff --git a/lib/debug/commands/accelerometer_commands.cpp b/lib/debug/commands/accelerometer_commands.cpp new file mode 100644 index 00000000..2bcc8654 --- /dev/null +++ b/lib/debug/commands/accelerometer_commands.cpp @@ -0,0 +1,82 @@ +#include "accelerometer_commands.hpp" + +#include + +#include "sensors/i2c_mux.hpp" + +namespace hyped::debug { +core::Result AccelerometerCommands::addCommands(core::ILogger &logger, + const std::shared_ptr &repl, + toml::v3::node_view config) +{ + // get bus + const auto optional_bus = config["bus"].value(); + if (!optional_bus) { + logger.log(core::LogLevel::kFatal, "No I2C bus specified"); + return core::Result::kFailure; + }; + const auto bus = *optional_bus; + + // get I2C instance + auto optional_i2c = repl->getI2c(bus); + if (!optional_i2c) { + logger.log(core::LogLevel::kFatal, "Error creating I2C bus"); + return core::Result::kFailure; + }; + const auto i2c = std::move(*optional_i2c); + + const auto optional_mux_channel = config["mux_channel"].value(); + if (!optional_mux_channel) { + logger.log(core::LogLevel::kFatal, "No mux channel specified"); + return core::Result::kFailure; + } + const auto mux_channel = *optional_mux_channel; + + auto optional_i2c_mux + = sensors::I2cMux::create(logger, i2c, sensors::kDefaultMuxAddress, mux_channel); + if (!optional_i2c_mux) { + logger.log(core::LogLevel::kFatal, "Failed to create I2C mux"); + return core::Result::kFailure; + } + const auto i2c_mux = std::move(*optional_i2c_mux); + + // get address + const auto optional_address = config["address"].value(); + if (!optional_address) { + logger.log(core::LogLevel::kFatal, "Invalid address"); + return core::Result::kFailure; + } + const auto address = *optional_address; + + // create sensor + const auto optional_accelerometer_sensor = sensors::Accelerometer::create( + logger, i2c_mux, static_cast(address)); + if (!optional_accelerometer_sensor) { + logger.log(core::LogLevel::kFatal, "Failed to create accelerometer sensor"); + return core::Result::kFailure; + } + const auto &accelerometer_sensor = *optional_accelerometer_sensor; + const auto *const read_command_name = "accelerometer read"; + const auto *const read_command_description = "Read from the accelerometer"; + const auto *const read_command_usage = "accelerometer read"; + const auto read_command_handler + = [&logger, accelerometer_sensor](const std::vector &) { + const auto value_ready = accelerometer_sensor->isValueReady(); + if (!value_ready) { + logger.log(core::LogLevel::kFatal, "Value is not ready"); + return; + } + const auto acceleration = accelerometer_sensor->read(); + if (!acceleration) { + logger.log(core::LogLevel::kFatal, "Failed to read acceleration"); + return; + } + logger.log(core::LogLevel::kDebug, "Acceleration: %s m/s", acceleration); + }; + auto read_command = std::make_unique( + read_command_name, read_command_description, read_command_usage, read_command_handler); + repl->addCommand(std::move(read_command)); + return core::Result::kSuccess; +} + +} // namespace hyped::debug diff --git a/lib/debug/commands/accelerometer_commands.hpp b/lib/debug/commands/accelerometer_commands.hpp new file mode 100644 index 00000000..4cc2fa64 --- /dev/null +++ b/lib/debug/commands/accelerometer_commands.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include + +#include +#include +#include +#include + +namespace hyped::debug { + +class AccelerometerCommands { + public: + static core::Result addCommands(core::ILogger &logger, + const std::shared_ptr &repl, + toml::v3::node_view config); +}; + +} // namespace hyped::debug diff --git a/lib/debug/repl.cpp b/lib/debug/repl.cpp index 46512b5a..ba80241b 100644 --- a/lib/debug/repl.cpp +++ b/lib/debug/repl.cpp @@ -2,6 +2,7 @@ #include +#include "commands/accelerometer_commands.hpp" #include "commands/adc_commands.hpp" #include "commands/can_commands.hpp" #include "commands/gpio_commands.hpp" @@ -32,7 +33,15 @@ std::optional> Repl::create(core::ILogger &logger, logger.log(core::LogLevel::kFatal, "Error parsing TOML file: %s", e.description()); return std::nullopt; } - if (config["io"]["adc"]["enabled"].value_or(false)) { + if (config["sensors"]["accelerometer"]["enabled"].value_or(false)) { + const auto result + = AccelerometerCommands::addCommands(logger, repl, config["sensors"]["accelerometer"]); + if (result == core::Result::kFailure) { + logger.log(core::LogLevel::kFatal, "Error adding Accelerometer commands"); + return std::nullopt; + } + } + if (config["io"]["adc"]) { const auto result = AdcCommands::addCommands(logger, repl); if (result == core::Result::kFailure) { logger.log(core::LogLevel::kFatal, "Error adding ADC commands"); @@ -147,7 +156,7 @@ void Repl::run() input = history_[history_.size() - 1 - i]; } } else if (key == debug::KeyPress::kBackspace) { - if (input.empty()) { input.pop_back(); } + if (!input.empty()) { input.pop_back(); } } else if (key == debug::KeyPress::kTab) { const auto result = findMatch(input); if (result == std::nullopt) { continue; } diff --git a/lib/sensors/CMakeLists.txt b/lib/sensors/CMakeLists.txt index 365e7980..ac30562d 100644 --- a/lib/sensors/CMakeLists.txt +++ b/lib/sensors/CMakeLists.txt @@ -8,3 +8,4 @@ include_directories( "${CMAKE_SOURCE_DIR}/lib" "${CMAKE_CURRENT_SOURCE_DIR}" ) +target_link_libraries(${target} PUBLIC tomlplusplus::tomlplusplus) diff --git a/lib/sensors/accelerometer.cpp b/lib/sensors/accelerometer.cpp index 0c8e37ff..0fc754d1 100644 --- a/lib/sensors/accelerometer.cpp +++ b/lib/sensors/accelerometer.cpp @@ -4,17 +4,15 @@ namespace hyped::sensors { -std::optional Accelerometer::create(core::ILogger &logger, - const std::shared_ptr &i2c, - const std::uint8_t device_address) +std::optional> Accelerometer::create( + core::ILogger &logger, + const std::shared_ptr &i2c, + const accelerometerAddress device_address) { - if (device_address != kDefaultAccelerometerAddress - && device_address != kAlternativeAccelerometerAddress) { - logger.log(core::LogLevel::kFatal, "Invalid device address for accelerometer"); - return std::nullopt; - } + const auto device_address_int = static_cast(device_address); + // check we are communicating with the correct sensor - const auto device_id = i2c->readByte(device_address, kDeviceIdAddress); + const auto device_id = i2c->readByte(device_address_int, kDeviceIdAddress); if (!device_id) { logger.log(core::LogLevel::kFatal, "Failed to read the accelerometer device ID"); return std::nullopt; @@ -23,13 +21,16 @@ std::optional Accelerometer::create(core::ILogger &logger, logger.log(core::LogLevel::kFatal, "Failure, mismatched device ID for accelerometer"); return std::nullopt; } - const auto ctrl1_result = i2c->writeByteToRegister(device_address, kCtrl1Address, kCtrl1Value); + const auto ctrl1_result + = i2c->writeByteToRegister(device_address_int, kCtrl1Address, kCtrl1Value); if (ctrl1_result == core::Result::kFailure) { return std::nullopt; }; - const auto ctrl2_result = i2c->writeByteToRegister(device_address, kCtrl2Address, kCtrl2Value); + const auto ctrl2_result + = i2c->writeByteToRegister(device_address_int, kCtrl2Address, kCtrl2Value); if (ctrl2_result == core::Result::kFailure) { return std::nullopt; }; - const auto ctrl6_result = i2c->writeByteToRegister(device_address, kCtrl6Address, kCtrl6Value); + const auto ctrl6_result + = i2c->writeByteToRegister(device_address_int, kCtrl6Address, kCtrl6Value); if (ctrl6_result == core::Result::kFailure) { return std::nullopt; }; - return Accelerometer(logger, i2c, device_address); + return std::make_shared(logger, i2c, device_address_int); } Accelerometer::Accelerometer(core::ILogger &logger, @@ -40,6 +41,7 @@ Accelerometer::Accelerometer(core::ILogger &logger, device_address_(device_address), low_byte_address_(kXOutLow), high_byte_address_(kXOutHigh) + { } diff --git a/lib/sensors/accelerometer.hpp b/lib/sensors/accelerometer.hpp index 6cf632a0..d31d97f8 100644 --- a/lib/sensors/accelerometer.hpp +++ b/lib/sensors/accelerometer.hpp @@ -15,15 +15,18 @@ namespace hyped::sensors { constexpr std::array kAxisLabels = {"x-axis", "y-axis", "z-axis"}; -// possible addresses for the accelerometer (from the datasheet) -constexpr std::uint8_t kDefaultAccelerometerAddress = 0x19; -constexpr std::uint8_t kAlternativeAccelerometerAddress = 0x18; +enum class accelerometerAddress { k1D = 0x1D, k1E = 0x1E }; class Accelerometer { public: - static std::optional create(core::ILogger &logger, - const std::shared_ptr &i2c, - const std::uint8_t device_address); + static std::optional> create( + core::ILogger &logger, + const std::shared_ptr &i2c, + const accelerometerAddress device_address); + + Accelerometer(core::ILogger &logger, + const std::shared_ptr &i2c, + const std::uint8_t device_address); /* * @brief Checks if the accelerometer is ready to be read @@ -36,9 +39,6 @@ class Accelerometer { std::optional read(); private: - Accelerometer(core::ILogger &logger, - const std::shared_ptr &i2c, - const std::uint8_t device_address); std::optional getRawAcceleration(const core::Axis axis); static std::int32_t getAccelerationFromRawValue(const std::int16_t rawAcceleration); void setRegisterAddressFromAxis(const core::Axis axis); @@ -74,7 +74,7 @@ class Accelerometer { static constexpr std::uint8_t kDataReady = 0x27; static constexpr std::uint8_t kDeviceIdAddress = 0x0F; - static constexpr std::uint8_t kExpectedDeviceIdValue = 0x44; + static constexpr std::uint8_t kExpectedDeviceIdValue = 0x43; }; } // namespace hyped::sensors diff --git a/lib/sensors/accelerometer_node.cpp b/lib/sensors/accelerometer_node.cpp new file mode 100644 index 00000000..d4aa7e4c --- /dev/null +++ b/lib/sensors/accelerometer_node.cpp @@ -0,0 +1,82 @@ +#include "accelerometer.hpp" +#include "accelerometer_node.hpp" + +#include + +#include "core/logger.hpp" +#include "core/mqtt.hpp" +#include "core/wall_clock.hpp" +#include "io/hardware_i2c.hpp" + +namespace hyped::sensors { + +core::Result AccelerometerNode::startNode(toml::v3::node_view config, + const std::string &mqtt_ip, + const std::uint32_t mqtt_port) +{ + auto time = core::WallClock(); + auto logger = core::Logger("Accelerometer", core::LogLevel::kDebug, time); + auto optional_mqtt = core::Mqtt::create(logger, "Accelerometer", mqtt_ip, mqtt_port); + if (!optional_mqtt) { + logger.log(core::LogLevel::kFatal, "Failed to create MQTT client"); + return core::Result::kFailure; + } + auto mqtt = *optional_mqtt; + auto optional_i2c = io::HardwareI2c::create(logger, 2); + if (!optional_i2c) { + logger.log(core::LogLevel::kFatal, "Failed to create I2C bus"); + return core::Result::kFailure; + } + auto i2c = *optional_i2c; + auto optional_accelerometer = Accelerometer::create(logger, i2c, accelerometerAddress::k1D); + if (!optional_accelerometer) { + logger.log(core::LogLevel::kFatal, "Failed to create accelerometer"); + return core::Result::kFailure; + } + auto accelerometer = *optional_accelerometer; + auto node = AccelerometerNode(logger, time, mqtt, accelerometer); + node.run(); + return core::Result::kSuccess; +} + +AccelerometerNode::AccelerometerNode(core::Logger &logger, + core::ITimeSource &time, + std::shared_ptr mqtt, + std::shared_ptr accelerometer) + : logger_(logger), + time_(time), + mqtt_(std::move(mqtt)), + accelerometer_(std::move(accelerometer)) +{ +} + +void AccelerometerNode::run() +{ + while (true) { + auto result = accelerometer_->isValueReady(); + if (!result) { continue; } + if (*result == core::Result::kFailure) { continue; } + auto optional_acceration = accelerometer_->read(); + if (!optional_acceration) { + logger_.log(core::LogLevel::kWarn, "Failed to read accelerometer data"); + continue; + } + const auto acceleration = *optional_acceration; + const auto topic = core::MqttTopic::kAccelerometer; + auto message_payload = std::make_shared(); + message_payload->SetObject(); + rapidjson::Value acceleration_x(acceleration.x); + rapidjson::Value acceleration_y(acceleration.y); + rapidjson::Value acceleration_z(acceleration.z); + message_payload->AddMember("x", acceleration_x, message_payload->GetAllocator()); + message_payload->AddMember("y", acceleration_y, message_payload->GetAllocator()); + message_payload->AddMember("z", acceleration_z, message_payload->GetAllocator()); + const core::MqttMessage::Header header{ + .timestamp = static_cast(time_.now().time_since_epoch().count()), + .priority = core::MqttMessagePriority::kNormal}; + const core::MqttMessage message{topic, header, message_payload}; + mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce); + } +} + +} // namespace hyped::sensors diff --git a/lib/sensors/accelerometer_node.hpp b/lib/sensors/accelerometer_node.hpp new file mode 100644 index 00000000..8135a8c9 --- /dev/null +++ b/lib/sensors/accelerometer_node.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include "accelerometer.hpp" + +#include "core/mqtt.hpp" +#include "core/types.hpp" +#include + +namespace hyped::sensors { + +class AccelerometerNode { + public: + static core::Result startNode(toml::v3::node_view config, + const std::string &mqtt_ip, + const std::uint32_t mqtt_port); + AccelerometerNode(core::Logger &logger, + core::ITimeSource &time, + std::shared_ptr mqtt, + std::shared_ptr accelerometer); + void run(); + + private: + core::Logger logger_; + core::ITimeSource &time_; + std::shared_ptr mqtt_; + std::shared_ptr accelerometer_; +}; + +} // namespace hyped::sensors