SNS - Accelerometer REPL Commands (#98)

Co-authored-by: H-Allen <harveyjallen1@gmail.com>
Co-authored-by: Tom Lonergan <tomlonergan91@gmail.com>
Co-authored-by: David Beechey <david@dtbeechey.dev>

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

lib/debug/commands/accelerometer_commands.cpp

@@ -0,0 +1,82 @@
+#include "accelerometer_commands.hpp"
+
+#include <cstdint>
+
+#include "sensors/i2c_mux.hpp"
+
+namespace hyped::debug {
+core::Result AccelerometerCommands::addCommands(core::ILogger &logger,
+                                                const std::shared_ptr<Repl> &repl,
+                                                toml::v3::node_view<toml::v3::node> config)
+{
+  // get bus
+  const auto optional_bus = config["bus"].value<std::uint8_t>();
+  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<std::uint8_t>();
+  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<std::uint8_t>();
+  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<sensors::accelerometerAddress>(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<std::string> &) {
+        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<Command>(
+    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

lib/debug/commands/accelerometer_commands.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <memory>
+
+#include <core/logger.hpp>
+#include <core/types.hpp>
+#include <debug/repl.hpp>
+#include <sensors/accelerometer.hpp>
+
+namespace hyped::debug {
+
+class AccelerometerCommands {
+ public:
+  static core::Result addCommands(core::ILogger &logger,
+                                  const std::shared_ptr<Repl> &repl,
+                                  toml::v3::node_view<toml::v3::node> config);
+};
+
+}  // namespace hyped::debug

lib/debug/repl.cpp

@@ -2,6 +2,7 @@
 
 #include <optional>
 
+#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<std::shared_ptr<Repl>> 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; }

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)

lib/sensors/accelerometer.cpp

@@ -4,17 +4,15 @@
 
 namespace hyped::sensors {
 
-std::optional<Accelerometer> Accelerometer::create(core::ILogger &logger,
-                                                   const std::shared_ptr<io::II2c> &i2c,
-                                                   const std::uint8_t device_address)
+std::optional<std::shared_ptr<Accelerometer>> Accelerometer::create(
+  core::ILogger &logger,
+  const std::shared_ptr<io::II2c> &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<std::uint8_t>(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> 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<Accelerometer>(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)
+
 {
 }
 

lib/sensors/accelerometer.hpp

@@ -15,15 +15,18 @@ namespace hyped::sensors {
 
 constexpr std::array<std::string_view, 3> 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<Accelerometer> create(core::ILogger &logger,
-                                             const std::shared_ptr<io::II2c> &i2c,
-                                             const std::uint8_t device_address);
+  static std::optional<std::shared_ptr<Accelerometer>> create(
+    core::ILogger &logger,
+    const std::shared_ptr<io::II2c> &i2c,
+    const accelerometerAddress device_address);
+
+  Accelerometer(core::ILogger &logger,
+                const std::shared_ptr<io::II2c> &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<core::RawAccelerationData> read();
 
  private:
-  Accelerometer(core::ILogger &logger,
-                const std::shared_ptr<io::II2c> &i2c,
-                const std::uint8_t device_address);
   std::optional<std::int16_t> 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

lib/sensors/accelerometer_node.cpp

@@ -0,0 +1,82 @@
+#include "accelerometer.hpp"
+#include "accelerometer_node.hpp"
+
+#include <utility>
+
+#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<const toml::v3::node> 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<core::Mqtt> mqtt,
+                                     std::shared_ptr<Accelerometer> 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<rapidjson::Document>();
+    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<std::uint64_t>(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

lib/sensors/accelerometer_node.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "accelerometer.hpp"
+
+#include "core/mqtt.hpp"
+#include "core/types.hpp"
+#include <toml++/toml.hpp>
+
+namespace hyped::sensors {
+
+class AccelerometerNode {
+ public:
+  static core::Result startNode(toml::v3::node_view<const toml::v3::node> config,
+                                const std::string &mqtt_ip,
+                                const std::uint32_t mqtt_port);
+  AccelerometerNode(core::Logger &logger,
+                    core::ITimeSource &time,
+                    std::shared_ptr<core::Mqtt> mqtt,
+                    std::shared_ptr<Accelerometer> accelerometer);
+  void run();
+
+ private:
+  core::Logger logger_;
+  core::ITimeSource &time_;
+  std::shared_ptr<core::Mqtt> mqtt_;
+  std::shared_ptr<Accelerometer> accelerometer_;
+};
+
+}  // namespace hyped::sensors