tmc51x0_motor_calc.hpp

Go to the documentation of this file.

 
#pragma once
#include "../tmc51x0_types.hpp"
#include <algorithm>
#include <array>
#include <cmath>
 
namespace tmc51x0 {
 
// Datasheet constants (shared across calculation functions)
namespace MotorCalcConstants {
constexpr float VFS_MV = 325.0F;        // Typical full-scale voltage (mV) = 0.325V
constexpr float SQRT2 = 1.41421356237F; // √2
constexpr float TMC5130_VFS_MV = 320.0F;
constexpr float TMC5130_RSENSE_OFFSET_MOHM = 20.0F;
} // namespace MotorCalcConstants
 
inline bool CalculateMotorCurrent(const MotorSpec& motor_spec, uint32_t sense_resistor_mohm, uint32_t supply_voltage_mv,
                                  uint16_t run_current_ma, uint16_t hold_current_ma, uint8_t& irun, uint8_t& ihold,
                                  uint16_t& global_scaler, uint8_t chip_version = ChipVersion::TMC5160) noexcept {
  // Validate inputs
  if (sense_resistor_mohm == 0 || supply_voltage_mv == 0) {
    return false;
  }
 
  if (chip_version == ChipVersion::TMC5130) {
    if (run_current_ma == 0) {
      run_current_ma = motor_spec.rated_current_ma;
    }
    if (run_current_ma == 0) {
      return false;
    }
    if (hold_current_ma == 0) {
      hold_current_ma = static_cast<uint16_t>(static_cast<float>(run_current_ma) * 0.3F);
    }
    // I_RMS_max (CS = 31) in mA: VFS / (RSENSE + 20 mOhm) / sqrt(2)
    const float i_max_ma = (MotorCalcConstants::TMC5130_VFS_MV /
                            (static_cast<float>(sense_resistor_mohm) + MotorCalcConstants::TMC5130_RSENSE_OFFSET_MOHM)) /
                           MotorCalcConstants::SQRT2 * 1000.0F;
    if (static_cast<float>(run_current_ma) > i_max_ma * 1.1F) {
      return false;
    }
    // CS = I * 32 / I_max - 1, rounded down so the coil current never exceeds the request.
    auto cs_for = [i_max_ma](uint16_t ma) -> uint8_t {
      const float cs = (static_cast<float>(ma) * 32.0F / i_max_ma) - 1.0F;
      if (cs <= 0.0F) {
        return 0;
      }
      return static_cast<uint8_t>(std::min(31.0F, std::floor(cs + 1e-3F)));
    };
    irun = cs_for(run_current_ma);
    ihold = cs_for(hold_current_ma);
    if (ihold >= irun) {
      ihold = (irun > 0) ? static_cast<uint8_t>(irun - 1) : 0;
    }
    global_scaler = 256;
    return true;
  }
 
  // Use motor rated current if run_current_ma is 0
  if (run_current_ma == 0) {
    run_current_ma = motor_spec.rated_current_ma;
  }
  if (run_current_ma == 0) {
    return false; // No current specified
  }
 
  // Default hold current to 30% of run current if not specified
  if (hold_current_ma == 0) {
    float hold_current_float = run_current_ma * 0.3F;
    hold_current_ma = static_cast<uint16_t>(hold_current_float);
  }
 
  // Use shared constants
  constexpr float VFS_MV = MotorCalcConstants::VFS_MV;
  constexpr float SQRT2 = MotorCalcConstants::SQRT2;
 
  // Calculate maximum possible RMS current at full scale (GLOBAL_SCALER=256, CS=31)
  // I_RMS_max (A) = (256/256) * ((31+1)/32) * (VFS_V/RSENSE_Ω) * (1/√2)
  //               = 1.0 * 1.0 * (VFS_V/RSENSE_Ω) * (1/√2)
  // Converting from mV/mΩ to A: (VFS_mV/1000) / (RSENSE_mΩ/1000) = VFS_mV/RSENSE_mΩ (already in A)
  // Then convert to mA: multiply by 1000
  float i_rms_max_ma = ((VFS_MV / static_cast<float>(sense_resistor_mohm)) / SQRT2) * 1000.0F;
 
  // Check if desired current exceeds maximum possible
  if (static_cast<float>(run_current_ma) > i_rms_max_ma * 1.1F) { // Allow 10% tolerance
    return false;                                                  // Desired current too high for sense resistor
  }
 
  // Strategy: Use IRUN in optimal range (16-31) and fine-tune with GLOBAL_SCALER
  // Start with IRUN=31 (maximum CS) and calculate required GLOBAL_SCALER
  // Then adjust IRUN down if GLOBAL_SCALER would be too low
 
  // Calculate required GLOBAL_SCALER for IRUN=31.
  //
  // Datasheet equation (expressed in mA):
  //   I_RMS_ma = (GLOBAL_SCALER/256) * ((CS+1)/32) * I_RMS_MAX_ma
  // where:
  //   I_RMS_MAX_ma = (VFS/RSENSE) * (1/√2) * 1000
  //
  // For CS=31: ((CS+1)/32)=1, so:
  //   GLOBAL_SCALER = I_RMS_ma * 256 / I_RMS_MAX_ma
  //
  // NOTE: We intentionally use i_rms_max_ma here to keep units consistent (mA).
  float global_scaler_float =
      (static_cast<float>(run_current_ma) * 256.0F) / std::max(i_rms_max_ma, 1.0F);
 
  // Constrain GLOBAL_SCALER to valid range (32-256, where 0 = 256)
  auto calculated_scaler = static_cast<uint16_t>(std::round(global_scaler_float));
  calculated_scaler = std::max<uint16_t>(calculated_scaler, 32);
  calculated_scaler = std::min<uint16_t>(calculated_scaler, 256);
 
  // If GLOBAL_SCALER is at maximum (256), we can reduce IRUN for better precision
  // Try to find optimal IRUN in range 16-31
  uint8_t optimal_irun = 31;
  uint16_t optimal_scaler = calculated_scaler;
 
  if (calculated_scaler >= 200) {
    // Try reducing IRUN to get GLOBAL_SCALER in better range (128-200)
    for (uint8_t test_irun = 30; test_irun >= 16; --test_irun) {
      // GLOBAL_SCALER = I_RMS_ma * 256 * 32 / ((CS+1) * I_RMS_MAX_ma)
      float test_scaler_float =
          (static_cast<float>(run_current_ma) * 256.0F * 32.0F) /
          (static_cast<float>(test_irun + 1) * std::max(i_rms_max_ma, 1.0F));
      auto test_scaler = static_cast<uint16_t>(std::round(test_scaler_float));
 
      if (test_scaler >= 32 && test_scaler <= 200) {
        optimal_irun = test_irun;
        optimal_scaler = test_scaler;
        break;
      }
    }
  }
 
  // Ensure IRUN meets minimum for automatic tuning (IRUN ≥ 8)
  if (optimal_irun < 8) {
    optimal_irun = 8;
    // Recalculate scaler for IRUN=8
    float scaler_float =
        (static_cast<float>(run_current_ma) * 256.0F * 32.0F) / (9.0F * std::max(i_rms_max_ma, 1.0F));
    optimal_scaler = static_cast<uint16_t>(std::round(scaler_float));
    optimal_scaler = std::max<uint16_t>(optimal_scaler, 32);
    optimal_scaler = std::min<uint16_t>(optimal_scaler, 256);
  }
 
  // Calculate IHOLD using same method
  // Calculate required IHOLD (use same scaler as IRUN)
  // I_RMS_ma = (GLOBAL_SCALER/256) * ((IHOLD+1)/32) * I_RMS_MAX_ma
  // Rearranged:
  //   IHOLD = (I_RMS_ma * 256 * 32) / (GLOBAL_SCALER * I_RMS_MAX_ma) - 1
  float ihold_float =
      ((static_cast<float>(hold_current_ma) * 256.0F * 32.0F) / 
       (static_cast<float>(optimal_scaler) * std::max(i_rms_max_ma, 1.0F))) -
      1.0F;
 
  auto calculated_ihold = static_cast<uint8_t>(std::round(ihold_float));
 
  // Constrain IHOLD to valid range (0-31) and ensure it's less than IRUN
  calculated_ihold = std::min<uint8_t>(calculated_ihold, 31);
  if (calculated_ihold >= optimal_irun) {
    calculated_ihold = (optimal_irun > 0) ? (optimal_irun - 1) : 0;
  }
 
  irun = optimal_irun;
  ihold = calculated_ihold;
  global_scaler = optimal_scaler;
 
  return true;
}
 
inline uint16_t CalculateStealthChopLowerLimit(const MotorSpec& motor_spec, uint32_t supply_voltage_mv, uint8_t tbl,
                                               uint8_t pwm_freq, uint32_t f_clk = 12000000U) noexcept {
  if (motor_spec.winding_resistance_mohm == 0) {
    return 0; // Cannot calculate without resistance
  }
 
  // Blank time in clock cycles based on TBL setting
  constexpr std::array<uint8_t, 4> blank_times = {16, 24, 36, 54};
  const uint8_t tbl_index = std::min(tbl, static_cast<uint8_t>(3));
  uint8_t t_blank = blank_times[tbl_index];
 
  // PWM frequency divider based on PWM_FREQ setting
  constexpr std::array<uint32_t, 4> pwm_divisors = {1024, 683, 512, 410};
  const uint8_t pwm_freq_index = std::min(pwm_freq, static_cast<uint8_t>(3));
  uint32_t pwm_divisor = pwm_divisors[pwm_freq_index];
 
  // Calculate PWM frequency: f_PWM = 2 / (pwm_divisor * t_CLK)
  // Actually: f_PWM = 2 / (pwm_divisor * (1/f_CLK)) = 2 * f_CLK / pwm_divisor
  float f_pwm = (2.0F * static_cast<float>(f_clk)) / static_cast<float>(pwm_divisor);
 
  // Convert to Hz: f_PWM = 2 / (pwm_divisor * t_CLK) where t_CLK = 1/f_CLK
  // So: f_PWM = 2 * f_CLK / pwm_divisor (already calculated above)
 
  // Calculate lower limit: I (mA) = t_BLANK * f_PWM * V_M (mV) / R_COIL (mΩ)
  // t_BLANK is in clock cycles, so we need: t_BLANK / f_CLK (time in seconds)
  float t_blank_sec = static_cast<float>(t_blank) / static_cast<float>(f_clk);
  float i_lower_ma = (t_blank_sec * f_pwm * static_cast<float>(supply_voltage_mv)) / 
                      static_cast<float>(motor_spec.winding_resistance_mohm);
 
  return static_cast<uint16_t>(i_lower_ma);
}
 
inline uint16_t CalculateMaxCurrentForSenseResistor(uint32_t sense_resistor_mohm) noexcept {
  if (sense_resistor_mohm == 0) {
    return 0;
  }
 
  // Use shared constants
  constexpr float VFS_MV = MotorCalcConstants::VFS_MV;
  constexpr float SQRT2 = MotorCalcConstants::SQRT2;
 
  // Calculate maximum RMS current directly in milliamps
  // Converting from mV/mΩ to A: (VFS_mV/1000) / (RSENSE_mΩ/1000) = VFS_mV/RSENSE_mΩ (already in A)
  // Then convert to mA: multiply by 1000
  float i_rms_max_ma = ((VFS_MV / static_cast<float>(sense_resistor_mohm)) / SQRT2) * 1000.0F;
 
  return static_cast<uint16_t>(i_rms_max_ma);
}
 
inline uint8_t CalculateS2VSLevel(uint16_t voltage_mv) noexcept {
  if (voltage_mv == 0) {
    return 6; // Default recommended value
  }
 
  // Constrain to valid range
  if (voltage_mv < 400 || voltage_mv > 2000) {
    return 0; // Invalid
  }
 
  // Datasheet typical values for interpolation
  // S2VS_LEVEL=6: 625mV, S2VS_LEVEL=15: 1560mV
  // Linear relationship: level = 6 + (voltage - 625) * (15-6) / (1560-625)
  // Simplified: level = 6 + (voltage - 625) * 9 / 935
 
  if (voltage_mv <= 625) {
    // Below or at level 6 threshold - use linear interpolation from level 4 (approx 400mV) to level 6
    // Level 4-6 range: approximate 400-625mV
    if (voltage_mv < 400) {
      return 4; // Minimum
    }
    float level = 4.0F + ((static_cast<float>(voltage_mv - 400) / 225.0F) * 2.0F); // 4 to 6
    return static_cast<uint8_t>(std::round(level));
  }
  // Above level 6 threshold - interpolate from level 6 to level 15
  float level = 6.0F + ((static_cast<float>(voltage_mv - 625) / 935.0F) * 9.0F); // 6 to 15
  auto calculated = static_cast<uint8_t>(std::round(level));
  return std::min(static_cast<uint8_t>(15), std::max(static_cast<uint8_t>(4), calculated));
}
 
inline uint8_t CalculateS2GLevel(uint16_t voltage_mv, uint32_t supply_voltage_mv = 0) noexcept {
  if (voltage_mv == 0) {
    return 6; // Default recommended value
  }
 
  // Constrain to valid range
  if (voltage_mv < 400 || voltage_mv > 2000) {
    return 0; // Invalid
  }
 
  // For VS>52V, enforce minimum 1200mV (S2G_LEVEL=12) to prevent false triggers
  if (supply_voltage_mv > 52000 && voltage_mv < 1200) {
    voltage_mv = 1200; // Enforce minimum
  }
 
  // Datasheet typical values for interpolation
  // S2G_LEVEL=6: 625mV, S2G_LEVEL=15: 1560mV (VS<52V) or 850mV (VS<55V)
  // Use VS<52V values for interpolation (more conservative)
 
  if (voltage_mv <= 625) {
    // Below or at level 6 threshold - use linear interpolation from level 2 (approx 400mV) to level 6
    if (voltage_mv < 400) {
      return 2; // Minimum
    }
    float level = 2.0F + ((static_cast<float>(voltage_mv - 400) / 225.0F) * 4.0F); // 2 to 6
    return static_cast<uint8_t>(std::round(level));
  }
  // Above level 6 threshold - interpolate from level 6 to level 15
  float level = 6.0F + ((static_cast<float>(voltage_mv - 625) / 935.0F) * 9.0F); // 6 to 15
  auto calculated = static_cast<uint8_t>(std::round(level));
  return std::min(static_cast<uint8_t>(15), std::max(static_cast<uint8_t>(2), calculated));
}
 
inline uint8_t CalculateShortDelay(uint8_t delay_us_x10) noexcept {
  if (delay_us_x10 == 0) {
    return 0; // Default recommended value (0.85µs = shortdelay=0)
  }
 
  // Constrain to valid range (5-25 = 0.5-2.5µs)
  if (delay_us_x10 < 5) {
    delay_us_x10 = 5; // Minimum 0.5µs
  } else if (delay_us_x10 > 25) {
    delay_us_x10 = 25; // Maximum 2.5µs
  }
 
  // Threshold at ~1.0µs (10 in 0.1µs units)
  // Below 1.0µs: use shortdelay=0 (normal)
  // At or above 1.0µs: use shortdelay=1 (high delay)
  return (delay_us_x10 >= 10) ? 1 : 0;
}
 
} // namespace tmc51x0