tmc51x0_multi_node.hpp

Go to the documentation of this file.

 
#pragma once
#include <array>
#include <cstdint>
#include <optional>
 
#include "../tmc51x0.hpp"
#include "../tmc51x0_comm_interface.hpp"
 
namespace tmc51x0 {
 
template <typename CommType, size_t MaxDevices = 8> class TMC51x0MultiNode {
public:
  explicit TMC51x0MultiNode(CommType &comm, uint8_t num_onboard_devices,
                            uint32_t f_clk = ClockFreq::DEFAULT_F_CLK) noexcept
      : comm_(comm), num_onboard_devices_(num_onboard_devices),
        num_active_devices_(num_onboard_devices) {
    (void)f_clk;
    // Validate num_onboard_devices
    if (num_onboard_devices == 0 || num_onboard_devices > MaxDevices) {
      num_onboard_devices_ = 1;
      num_active_devices_ = 1;
    }
 
    // Create onboard TMC5160 instances, one per device
    // Initial node address is 0 (will be programmed via ProgramSequentially())
    // The actual programmed addresses will be 254, 253, 252, ... per datasheet
    for (uint8_t i = 0; i < num_onboard_devices_; ++i) {
      drivers_[i].emplace(comm_, 0, 0); // position 0, node address 0 until programmed
    }
 
    // Initialize extra device slots as empty
    for (size_t i = num_onboard_devices_; i < MaxDevices; ++i) {
      drivers_[i].reset();
    }
  }
 
  [[nodiscard]] uint8_t GetNumOnboardDevices() const noexcept {
    return num_onboard_devices_;
  }
 
  [[nodiscard]] uint8_t GetNumActiveDevices() const noexcept {
    return num_active_devices_;
  }
 
  [[nodiscard]] constexpr size_t GetMaxCapacity() const noexcept {
    return MaxDevices;
  }
 
  [[nodiscard]] bool IsDeviceActive(uint8_t index) const noexcept {
    if (index >= MaxDevices) {
      return false;
    }
    return drivers_[index].has_value();
  }
 
  Result<void> ProgramSequentially(uint8_t send_delay = 2) noexcept {
    // Count active devices to determine starting address
    uint8_t num_devices = 0;
    for (size_t i = 0; i < MaxDevices; ++i) {
      if (drivers_[i].has_value()) {
        num_devices++;
      }
    }
 
    if (num_devices == 0) {
      return Result<void>(ErrorCode::INVALID_STATE); // No devices to program
    }
 
    // Program devices in forward order (index 0, 1, 2, ...)
    // but assign addresses in reverse order (254, 253, 252, ...)
    // First chip: accessible at address 0 (NAI=GND)
    // After each programming, next chip becomes accessible at address 0
 
    uint8_t device_index = 0;
    for (size_t i = 0; i < MaxDevices; ++i) {
      if (!drivers_[i].has_value()) {
        continue; // Skip empty slots
      }
 
      // Calculate target address: start from 254 and count down
      // Device at index 0 gets address 254, index 1 gets 253, etc.
      uint8_t target_address = 254 - device_index;
 
      // Program device to target address
      // The device is currently accessible at address 0 (after previous chip's
      // NAO went LOW) or address 0 for the first chip (NAI=GND)
      if (!drivers_[i]->uartConfig.ConfigureUartNodeAddress(target_address,
                                                            send_delay)) {
        return Result<void>(
            ErrorCode::INVALID_STATE); // Failed to program device
      }
 
      // ConfigureUartNodeAddress() already moved the driver to target_address.
      // NAO does not change by itself: drive it low (OUTPUT bit 0 = 0) so the
      // next chip's NAI goes low and that chip answers at address 0
      // (TMC5160A datasheet section 5.4, figure 5.1; OUTPUT register 0x04).
      if (!drivers_[i]->io.SetSdoCfg0Polarity(false)) {
        return Result<void>(ErrorCode::COMM_ERROR);
      }
      comm_.DelayMs(1);
 
      device_index++;
    }
 
    return Result<void>();
  }
 
  Result<void> ProgramDevice(uint8_t index, uint8_t send_delay = 2) noexcept {
    if (index >= MaxDevices || !drivers_[index].has_value()) {
      return Result<void>(ErrorCode::INVALID_STATE);
    }
 
    // Calculate target address: 254 - index (per datasheet)
    uint8_t target_address = 254 - index;
 
    if (!drivers_[index]->uartConfig.ConfigureUartNodeAddress(target_address,
                                                              send_delay)) {
      return Result<void>(ErrorCode::INVALID_STATE);
    }
 
    // Drive NAO low so the next chip answers at address 0 (datasheet figure 5.1).
    if (!drivers_[index]->io.SetSdoCfg0Polarity(false)) {
      return Result<void>(ErrorCode::COMM_ERROR);
    }
    return Result<void>();
  }
 
  Result<void> AddDevice(uint8_t index) noexcept {
    // Validate index
    if (index < num_onboard_devices_ || index >= MaxDevices) {
      return Result<void>(ErrorCode::INVALID_STATE);
    }
 
    // Check if slot is already occupied
    if (drivers_[index].has_value()) {
      return Result<void>(ErrorCode::INVALID_STATE);
    }
 
    // Enforce sequential indexing: cannot skip indices
    // All indices before this one (starting from num_onboard_devices) must be
    // filled
    for (uint8_t i = num_onboard_devices_; i < index; ++i) {
      if (!drivers_[i].has_value()) {
        return Result<void>(ErrorCode::INVALID_STATE); // Cannot skip indices
      }
    }
 
    // Create device instance with initial address 0
    // The actual address will be programmed via ProgramSequentially() or
    // ProgramDevice()
    drivers_[index].emplace(comm_, 0, 0);
    num_active_devices_++;
 
    return Result<void>();
  }
 
  Result<void> RemoveDevice(uint8_t index) noexcept {
    // Cannot remove onboard devices
    if (index < num_onboard_devices_) {
      return Result<void>(ErrorCode::INVALID_STATE);
    }
 
    // Validate index
    if (index >= MaxDevices) {
      return Result<void>(ErrorCode::INVALID_STATE);
    }
 
    // Check if slot is actually active
    if (!drivers_[index].has_value()) {
      return Result<void>(ErrorCode::INVALID_STATE);
    }
 
    // Enforce sequential removal: can only remove from the end
    // Check if there are any devices after this index
    for (size_t i = index + 1; i < MaxDevices; ++i) {
      if (drivers_[i].has_value()) {
        return Result<void>(
            ErrorCode::INVALID_STATE); // Cannot remove device in the middle of
                                       // the chain
      }
    }
 
    // Remove device
    drivers_[index].reset();
    num_active_devices_--;
 
    return Result<void>();
  }
 
  [[nodiscard]] TMC51x0<CommType> &operator[](uint8_t index) noexcept {
    return drivers_[index].value();
  }
 
  [[nodiscard]] const TMC51x0<CommType> &
  operator[](uint8_t index) const noexcept {
    return drivers_[index].value();
  }
 
  Result<void>
  InitializeAll(const DriverConfig &config = DriverConfig()) noexcept {
    Result<void> first_error{};
    for (size_t i = 0; i < MaxDevices; ++i) {
      if (drivers_[i].has_value()) {
        auto result = drivers_[i]->Initialize(config);
        if (!result && first_error) {
          first_error = result; // keep initialising the others; report the first error
        }
      }
    }
    return first_error;
  }
 
private:
  CommType &comm_;              
  uint8_t num_onboard_devices_; 
  uint8_t
      num_active_devices_; 
  std::array<std::optional<TMC51x0<CommType>>, MaxDevices>
      drivers_; 
};
 
} // namespace tmc51x0