I2C Mux

config/template.json

@@ -39,6 +39,13 @@
 				"enabled": false,
 				"bus": 2,
 				"device_address": 56
+			},
+			"mux": {
+				"enabled": false,
+				"bus": 2,
+				"mux_address": 112,
+				"sensor_type": "accelerometer",
+				"channels": [0, 1, 2, 3]
 			}
 		},
 		"motors": {

lib/core/types.hpp

@@ -37,21 +37,21 @@ using KeyenceData          = std::array<std::uint32_t, kNumKeyence>;
 // data produced by the accelerometer sensor
 // values are in milli-g (standard gravity)
 struct RawAccelerationData {
-  RawAccelerationData(const std::int32_t x,
-                      const std::int32_t y,
-                      const std::int32_t z,
-                      const TimePoint measured_at)
+  RawAccelerationData(std::int32_t x        = 0,
+                      std::int32_t y        = 0,
+                      std::int32_t z        = 0,
+                      TimePoint measured_at = std::chrono::system_clock::now())
       : x(x),
         y(y),
         z(z),
         measured_at(measured_at)
   {
   }
 
-  const std::int32_t x;
-  const std::int32_t y;
-  const std::int32_t z;
-  const TimePoint measured_at;
+  std::int32_t x;
+  std::int32_t y;
+  std::int32_t z;
+  TimePoint measured_at;
 };
 
 }  // namespace hyped::core

lib/debug/repl.cpp

@@ -257,6 +257,57 @@ std::optional<std::unique_ptr<Repl>> Repl::fromFile(const std::string &path)
     const auto bus            = temperature["bus"].GetUint();
     repl->addTemperatureCommands(bus, device_address);
   }
+
+  // Add Mux commands
+  if (!sensors.HasMember("mux")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.mux' in configuration file");
+    return std::nullopt;
+  }
+  const auto mux = sensors["mux"].GetObject();
+  if (!mux.HasMember("enabled")) {
+    logger_.log(core::LogLevel::kFatal,
+                "Missing required field 'sensors.mux.enabled' in configuration file");
+    return std::nullopt;
+  }
+  if (mux["enabled"].GetBool()) {
+    if (!mux.HasMember("bus")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.mux.bus' in configuration file");
+      return std::nullopt;
+    }
+    const auto bus = mux["bus"].GetUint();
+    if (!mux.HasMember("mux_address")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.mux.device_address' in configuration file");
+      return std::nullopt;
+    }
+    const auto mux_address = mux["mux_address"].GetUint();
+    if (!mux.HasMember("sensor_type")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.mux.sensor_type' in configuration file");
+      return std::nullopt;
+    }
+    const auto sensor_type = mux["sensor_type"].GetString();
+    if (!mux.HasMember("sensor_address")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.mux.sensor_address' in configuration file");
+      return std::nullopt;
+    }
+    const auto sensor_address = mux["sensor_address"].GetUint();
+    if (!mux.HasMember("channels")) {
+      logger_.log(core::LogLevel::kFatal,
+                  "Missing required field 'sensors.mux.channels' in configuration file");
+      return std::nullopt;
+    }
+    const auto channels = mux["channels"].GetArray();
+    std::vector<std::uint8_t> channels_vector;
+    for (auto &channel : channels) {
+      channels_vector.push_back(channel.GetUint());
+    }
+    repl->addI2cMuxCommands(bus, mux_address, sensor_type, sensor_address, channels_vector);
+  }
+
   if (!debugger.HasMember("motors")) {
     logger_.log(core::LogLevel::kFatal,
                 "Missing required field 'debugger.motors' in configuration file");
@@ -644,6 +695,11 @@ void Repl::addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t d
   const auto optional_accelerometer
     = sensors::Accelerometer::create(logger_, i2c, bus, device_address);
   const auto accelerometer = std::make_shared<sensors::Accelerometer>(*optional_accelerometer);
+  const core::Result configuration_result = *accelerometer->configure();
+  if (configuration_result == core::Result::kFailure) {
+    logger_.log(core::LogLevel::kFatal, "Failed to configure the accelerometer sensor");
+    return;
+  }
   Command accelerometer_read_command;
   std::stringstream identifier;
   identifier << "accelerometer 0x" << std::hex << static_cast<int>(device_address) << " read";
@@ -679,6 +735,12 @@ void Repl::addTemperatureCommands(const std::uint8_t bus, const std::uint8_t dev
   const auto i2c                  = std::move(*optional_i2c);
   const auto optional_temperature = sensors::Temperature::create(logger_, i2c, bus, device_address);
   const auto temperature          = std::make_shared<sensors::Temperature>(*optional_temperature);
+  // Calibrate the temperature sensor
+  const core::Result configuration_result = *temperature->configure();
+  if (configuration_result == core::Result::kFailure) {
+    logger_.log(core::LogLevel::kFatal, "Failed to configure the temperature sensor");
+    return;
+  }
   Command temperature_read_command;
   std::stringstream identifier;
   identifier << "temperature 0x" << std::hex << static_cast<int>(device_address) << " read";
@@ -700,6 +762,60 @@ void Repl::addTemperatureCommands(const std::uint8_t bus, const std::uint8_t dev
   addCommand(temperature_read_command);
 }
 
+void Repl::addI2cMuxCommands(const std::uint8_t bus,
+                             const std::uint8_t mux_address,
+                             const std::string &sensor_type,
+                             const std::uint8_t sensor_address,
+                             const std::vector<std::uint8_t> &channels)
+{
+  const auto optional_i2c = getI2c(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);
+  if (sensor_type == "accelerometer") {
+    // TODOLater: Figure out how to not hardcode this
+    std::array<std::unique_ptr<sensors::II2cMuxSensor<core::RawAccelerationData>>, 4>
+      accelerometers;
+    for (int i = 0; i < channels.size(); i++) {
+      const auto optional_accelerometer
+        = sensors::Accelerometer::create(logger_, i2c, channels[i], sensor_address);
+      accelerometers[i]
+        = std::make_unique<sensors::Accelerometer>(std::move(*optional_accelerometer));
+    }
+    // sensors::I2cMux<core::RawAccelerationData, 4> mux(logger_, i2c, mux_address, accelerometers);
+    std::shared_ptr<sensors::I2cMux<core::RawAccelerationData, 4>> mux_ptr
+      = std::make_shared<sensors::I2cMux<core::RawAccelerationData, 4>>(
+        logger_, i2c, mux_address, accelerometers);
+    Command mux_read_command;
+    std::stringstream identifier;
+    identifier << "mux 0x" << std::hex << static_cast<int>(mux_address) << " read";
+    mux_read_command.name = identifier.str();
+    std::stringstream description;
+    description << "Read mux 0x" << std::hex << static_cast<int>(mux_address) << " on "
+                << "I2C bus " << static_cast<int>(bus);
+    mux_read_command.description = description.str();
+    mux_read_command.handler     = [this, mux_ptr, bus]() {
+      const auto value = mux_ptr->readAllChannels();
+      if (!value) {
+        logger_.log(core::LogLevel::kFatal, "Failed to read the mux from bus %d", bus);
+      } else {
+        const std::array<core::RawAccelerationData, 4> mux_result = *value;
+        for (int i = 0; i < mux_result.size(); i++) {
+          logger_.log(core::LogLevel::kInfo,
+                      "Accelerometer %d: \n x %d \n y %d \n z %d",
+                      i,
+                      mux_result[i].x,
+                      mux_result[i].y,
+                      mux_result[i].z);
+        }
+      }
+    };
+    addCommand(mux_read_command);
+  }
+}
+
 void Repl::addMotorControllerCommands(const std::string &bus)
 {
   const auto optional_can = getCan(bus);

lib/debug/repl.hpp

@@ -11,6 +11,7 @@
 #include <vector>
 
 #include <core/logger.hpp>
+#include <core/types.hpp>
 #include <io/can.hpp>
 #include <io/hardware_adc.hpp>
 #include <io/hardware_can.hpp>
@@ -24,6 +25,8 @@
 #include <motors/frequency_calculator.hpp>
 #include <motors/time_frequency_calculator.hpp>
 #include <sensors/accelerometer.hpp>
+#include <sensors/i2c_mux.hpp>
+#include <sensors/i2c_sensors.hpp>
 #include <sensors/temperature.hpp>
 
 namespace hyped::debug {
@@ -54,6 +57,11 @@ class Repl {
   void addSpiCommands(const std::uint8_t bus);
   void addAccelerometerCommands(const std::uint8_t bus, const std::uint8_t device_address);
   void addTemperatureCommands(const std::uint8_t bus, const std::uint8_t device_address);
+  void addI2cMuxCommands(const std::uint8_t bus,
+                         const std::uint8_t mux_address,
+                         const std::string &sensor_type,
+                         const std::uint8_t sensor_address,
+                         const std::vector<std::uint8_t> &channels);
   void addUartCommands(const std::uint8_t bus);
   void addMotorControllerCommands(const std::string &bus);
 

lib/sensors/accelerometer.cpp

@@ -8,26 +8,10 @@ std::optional<Accelerometer> Accelerometer::create(core::ILogger &logger,
                                                    const std::uint8_t device_address)
 {
   if (device_address != kDefaultAccelerometerAddress
-      || device_address != kAlternativeAccelerometerAddress) {
+      && device_address != kAlternativeAccelerometerAddress) {
     logger.log(core::LogLevel::kFatal, "Invalid device address for accelerometer");
     return std::nullopt;
   }
-  // check we are communicating with the correct sensor
-  const auto device_id = i2c->readByte(device_address, kDeviceIdAddress);
-  if (!device_id) {
-    logger.log(core::LogLevel::kFatal, "Failed to read the accelerometer device ID");
-    return std::nullopt;
-  }
-  if (*device_id != kExpectedDeviceIdValue) {
-    logger.log(core::LogLevel::kFatal, "Failure, mismatched device ID for accelerometer");
-    return std::nullopt;
-  }
-  const auto ctrl1_result = i2c->writeByteToRegister(device_address, kCtrl1Address, kCtrl1Value);
-  if (ctrl1_result == core::Result::kFailure) { return std::nullopt; };
-  const auto ctrl2_result = i2c->writeByteToRegister(device_address, kCtrl2Address, kCtrl2Value);
-  if (ctrl2_result == core::Result::kFailure) { return std::nullopt; };
-  const auto ctrl6_result = i2c->writeByteToRegister(device_address, kCtrl6Address, kCtrl6Value);
-  if (ctrl6_result == core::Result::kFailure) { return std::nullopt; };
   return Accelerometer(logger, i2c, channel, device_address);
 }
 
@@ -46,6 +30,28 @@ Accelerometer::~Accelerometer()
 {
 }
 
+std::optional<core::Result> Accelerometer::configure()
+{
+  // check we are communicating with the correct sensor
+  const auto device_id = i2c_->readByte(device_address_, kDeviceIdAddress);
+  if (!device_id) {
+    logger_.log(core::LogLevel::kFatal, "Failed to read the accelerometer device ID");
+    return std::nullopt;
+  }
+  if (*device_id != kExpectedDeviceIdValue) {
+    logger_.log(core::LogLevel::kFatal, "Failure, mismatched device ID for accelerometer");
+    return std::nullopt;
+  }
+  const auto ctrl1_result = i2c_->writeByteToRegister(device_address_, kCtrl1Address, kCtrl1Value);
+  if (ctrl1_result == core::Result::kFailure) { return std::nullopt; };
+  const auto ctrl2_result = i2c_->writeByteToRegister(device_address_, kCtrl2Address, kCtrl2Value);
+  if (ctrl2_result == core::Result::kFailure) { return std::nullopt; };
+  const auto ctrl6_result = i2c_->writeByteToRegister(device_address_, kCtrl6Address, kCtrl6Value);
+  if (ctrl6_result == core::Result::kFailure) { return std::nullopt; };
+  logger_.log(core::LogLevel::kDebug, "Successfully configured accelerometer");
+  return core::Result::kSuccess;
+}
+
 std::optional<core::Result> Accelerometer::isValueReady()
 {
   // check to see if the values are ready to be read

lib/sensors/accelerometer.hpp

@@ -29,6 +29,8 @@ class Accelerometer : public II2cMuxSensor<core::RawAccelerationData> {
                                              const std::uint8_t device_address);
   ~Accelerometer();
 
+  std::optional<core::Result> configure() override;
+
   /*
    * @brief Checks if the accelerometer is ready to be read
    * @return kSuccess if the sensor is ready to be read,
@@ -37,9 +39,9 @@ class Accelerometer : public II2cMuxSensor<core::RawAccelerationData> {
    */
   std::optional<core::Result> isValueReady();
 
-  std::optional<core::RawAccelerationData> read();
+  std::optional<core::RawAccelerationData> read() override;
 
-  std::uint8_t getChannel() const;
+  std::uint8_t getChannel() const override;
 
  private:
   Accelerometer(core::ILogger &logger,

lib/sensors/i2c_mux.hpp

@@ -57,6 +57,17 @@ I2cMux<T, N>::I2cMux(core::ILogger &logger,
       max_num_unusable_sensors_(static_cast<std::uint8_t>(kFailureThreshold * N))
 {
   static_assert(N <= 8, "The I2c mux can only have up to 8 channels");
+  for (std::uint8_t i = 0; i < N; ++i) {
+    const auto &sensor               = sensors_.at(i);
+    const auto channel_select_result = selectChannel(sensor->getChannel());
+    if (channel_select_result == core::Result::kFailure) {
+      logger_.log(core::LogLevel::kFatal, "Failed to select channel %d from the i2c mux", i);
+    }
+    const auto result = sensor->configure();
+    if (!result) {
+      logger_.log(core::LogLevel::kFatal, "Failed to configure sensor on channel %d", i);
+    }
+  }
 }
 
 template<typename T, std::uint8_t N>

lib/sensors/i2c_sensors.hpp

@@ -16,8 +16,9 @@ class II2cMuxSensor {
   /*
    * This function carries out the initilization steps for a particular sensor.
    */
-  virtual std::optional<T> read()         = 0;
-  virtual std::uint8_t getChannel() const = 0;
+  virtual std::optional<core::Result> configure() = 0;
+  virtual std::optional<T> read()                 = 0;
+  virtual std::uint8_t getChannel() const         = 0;
   virtual ~II2cMuxSensor() {}
 };
 

lib/sensors/temperature.cpp

@@ -12,14 +12,6 @@ std::optional<Temperature> Temperature::create(core::ILogger &logger,
     logger.log(core::LogLevel::kFatal, "Invalid device address for temperature sensor");
     return std::nullopt;
   }
-  const auto write_result = i2c->writeByteToRegister(device_address, kCtrl, kConfigurationSetting);
-  if (write_result == core::Result::kFailure) {
-    logger.log(
-      core::LogLevel::kFatal, "Failed to configure temperature sensor at channel %d", channel);
-    return std::nullopt;
-  }
-  logger.log(
-    core::LogLevel::kDebug, "Successful to configure temperature sensor at channel %d", channel);
   return Temperature(logger, i2c, channel, device_address);
 }
 
@@ -38,6 +30,20 @@ Temperature::~Temperature()
 {
 }
 
+std::optional<core::Result> Temperature::configure()
+{
+  const auto write_result
+    = i2c_->writeByteToRegister(device_address_, kCtrl, kConfigurationSetting);
+  if (write_result == core::Result::kFailure) {
+    logger_.log(
+      core::LogLevel::kFatal, "Failed to configure temperature sensor at channel %d", channel_);
+    return std::nullopt;
+  }
+  logger_.log(
+    core::LogLevel::kDebug, "Successful to configure temperature sensor at channel %d", channel_);
+  return core::Result::kSuccess;
+}
+
 std::optional<core::Result> Temperature::checkStatus()
 {
   const auto status_check_result = i2c_->readByte(device_address_, kStatus);

lib/sensors/temperature.hpp

@@ -24,6 +24,8 @@ class Temperature : public II2cMuxSensor<std::int16_t> {
 
   ~Temperature();
 
+  std::optional<core::Result> configure() override;
+
   /*
    * @brief Checks if the temperature sensor is ready to be read
    * @return kSuccess if the sensor is ready to be read,
@@ -35,9 +37,9 @@ class Temperature : public II2cMuxSensor<std::int16_t> {
   /**
    * @brief Reads the temperature from the sensor
    */
-  std::optional<std::int16_t> read();
+  std::optional<std::int16_t> read() override;
 
-  std::uint8_t getChannel() const;
+  std::uint8_t getChannel() const override;
 
  private:
   Temperature(core::ILogger &logger,

lib/utils/dummy_i2c_sensor.cpp

@@ -6,9 +6,9 @@ DummyI2cSensor::DummyI2cSensor()
 {
 }
 
-core::Result DummyI2cSensor::configure()
+std::optional<core::Result> DummyI2cSensor::configure()
 {
-  return core::Result::kSuccess;
+  return std::nullopt;
 }
 
 std::optional<std::uint8_t> DummyI2cSensor::read()