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), f_clk_(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] = std::make_optional<TMC51x0<CommType>>(comm_, f_clk_, 0, 0);
    }
 
    // Initialize extra device slots as empty
    for (size_t i = num_onboard_devices_; i < MaxDevices; ++i) {
      drivers_[i] = std::nullopt;
    }
  }
 
  [[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 Result<void>(ErrorCode::INVALID_STATE);
    }
    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
      }
 
      // Update the driver's node address to the programmed address
      drivers_[i]->SetUartNodeAddress(target_address);
 
      // After programming, the chip's NAO automatically goes LOW (per
      // datasheet) This makes the next chip accessible at address 0 Small delay
      // to ensure NAO settles
      comm_.DelayMs(10);
 
      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);
    }
 
    drivers_[index]->SetUartNodeAddress(target_address);
    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] =
        std::make_optional<TMC51x0<CommType>>(comm_, f_clk_, 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] = std::nullopt;
    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 {
    bool all_success = true;
    for (size_t i = 0; i < MaxDevices; ++i) {
      if (drivers_[i].has_value()) {
        if (!drivers_[i]->Initialize(config)) {
          all_success = false;
        }
      }
    }
    return all_success;
  }
 
private:
  CommType &comm_;              
  uint8_t num_onboard_devices_; 
  uint8_t
      num_active_devices_; 
  uint32_t f_clk_;         
  std::array<std::optional<TMC51x0<CommType>>, MaxDevices>
      drivers_; 
};
 
} // namespace tmc51x0