GGS
/
fwt_software
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

Convert motor current from milliamps to TMC register values

This commit is contained in:
pgdalmeida 2026-09-17 17:58:45 +02:00
parent 649d0979cf
commit 3476e5adc7
Signed by: pedro.almeida
GPG Key ID: D4A6C394DF13F1D7
5 changed files with 275 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -63,6 +63,7 @@ add_library(fgc_core STATIC
src/core/SingleInstance.cpp
src/core/Logger.cpp
src/core/Geometry.cpp
src/core/TmcCurrent.cpp
src/core/ScanGrid.cpp
src/core/TelemetryParser.cpp
src/core/CaptureScheduler.cpp

50
include/fgc/TmcCurrent.h Normal file
View File

@ -0,0 +1,50 @@
#pragma once
//
// TmcCurrent — milliamps to TMC5160 register fields.
//
// The driver expresses coil current as two dimensionless knobs: GLOBALSCALER
// (0x0B) scales the whole bridge, and IRUN (a field of IHOLD_IRUN, 0x10) scales
// within that. What a given pair actually delivers depends on the sense resistor
// soldered to the board, which is why this lives on the host: hardware.ini knows
// the resistor, the firmware does not.
//
// I_rms = (GLOBALSCALER/256) * ((IRUN+1)/32) * (V_FS / (R_SENSE + 0.02)) / sqrt(2)
//
// V_FS is 0.325 V for the DRVCONF the firmware writes, and the 0.02 ohm is the
// TMC5160's internal contribution in series with the external resistor.
//
#include <string>
#include <vector>
namespace fgc {
// RMS phase current in milliamps for a given register pair. Returns 0 for a
// non-positive sense resistor rather than dividing by something meaningless.
double tmcRmsCurrentMa(int global_scaler, int irun, double r_sense_ohm,
double v_fs = 0.325);
struct TmcCurrentPlan {
bool ok = false;
int global_scaler = 0; // 32..255. Never 0: on the TMC that means 256, i.e.
// FULL SCALE, so a defaulted or truncated zero would
// silently mean maximum current.
int run_irun = 0; // 0..31, normal motion
int homing_irun = 0; // 0..31, endstop search only
// What the chosen registers actually deliver — always recomputed, never the
// requested value echoed back, so the log cannot flatter the result.
double run_ma = 0.0;
double homing_ma = 0.0;
std::vector<std::string> warnings; // caller logs each at WARN
};
// One GLOBALSCALER serves both currents: it is chosen so the RUN current lands at
// IRUN = 31 (microstep resolution is best at the top of the IRUN range, and that
// is where the axis spends its working life), and the homing IRUN is then derived
// at that same scaler. The firmware therefore switches modes with a single
// register write, and never re-scales a moving motor.
TmcCurrentPlan planTmcCurrent(double r_sense_ohm, double run_ma, double homing_ma,
double v_fs = 0.325);
} // namespace fgc

118
src/core/TmcCurrent.cpp Normal file
View File

@ -0,0 +1,118 @@
#include "fgc/TmcCurrent.h"
#include <algorithm>
#include <cmath>
#include <sstream>
namespace fgc {
namespace {
// Formatting helper: milliamps to one decimal, for warning text that has to name
// both what was asked for and what the hardware can actually do.
std::string ma(double v) {
std::ostringstream os;
os.setf(std::ios::fixed);
os.precision(1);
os << v;
return os.str();
}
constexpr int kGsMin = 32; // below this the datasheet disallows GLOBALSCALER
constexpr int kGsMax = 255; // 0 would mean 256 (full scale) - never emitted
constexpr int kIrunMax = 31;
} // namespace
double tmcRmsCurrentMa(int global_scaler, int irun, double r_sense_ohm, double v_fs) {
if (r_sense_ohm <= 0.0) return 0.0;
const double gs = (global_scaler <= 0) ? 256.0 : static_cast<double>(global_scaler);
return (gs / 256.0) * ((irun + 1) / 32.0) * (v_fs / (r_sense_ohm + 0.02)) /
std::sqrt(2.0) * 1000.0;
}
TmcCurrentPlan planTmcCurrent(double r_sense_ohm, double run_ma, double homing_ma,
double v_fs) {
TmcCurrentPlan p;
if (r_sense_ohm <= 0.0) {
p.warnings.push_back("sense resistor must be positive, got " +
ma(r_sense_ohm) + " ohm");
return p;
}
if (run_ma <= 0.0) {
p.warnings.push_back("run current must be positive, got " + ma(run_ma) + " mA");
return p;
}
if (homing_ma <= 0.0) {
p.warnings.push_back("homing current must be positive, got " + ma(homing_ma) + " mA");
return p;
}
// The most the bridge can deliver: GLOBALSCALER 256, IRUN 31.
const double i_max = tmcRmsCurrentMa(0, kIrunMax, r_sense_ohm, v_fs);
// Pick GLOBALSCALER so the RUN current sits at IRUN 31.
double gs_ideal = 256.0 * run_ma / i_max;
if (gs_ideal > kGsMax) {
p.global_scaler = kGsMax;
p.warnings.push_back("run current " + ma(run_ma) + " mA is above the " + ma(i_max) +
" mA this driver can deliver with a " + ma(r_sense_ohm) +
" ohm sense resistor; using the maximum instead");
} else if (gs_ideal < kGsMin) {
// GLOBALSCALER 1..31 is forbidden, so a very small run current has to be
// reached by lowering IRUN instead, at some cost in microstep resolution.
p.global_scaler = kGsMin;
p.warnings.push_back("run current " + ma(run_ma) +
" mA needs GLOBALSCALER below the permitted minimum of 32; "
"holding it at 32 and reducing IRUN instead, which coarsens "
"microstepping");
} else {
p.global_scaler = static_cast<int>(std::lround(gs_ideal));
}
// Full-scale current at the chosen scaler, i.e. what IRUN 31 now means.
const double full = tmcRmsCurrentMa(p.global_scaler, kIrunMax, r_sense_ohm, v_fs);
auto irunFor = [&](double target) {
return std::clamp(static_cast<int>(std::lround(32.0 * target / full)) - 1, 0, kIrunMax);
};
p.run_irun = irunFor(run_ma);
if (homing_ma > run_ma) {
// Not a feature: a homing current above the run current defeats the point
// of having a separate one.
p.homing_irun = p.run_irun;
p.warnings.push_back("homing current " + ma(homing_ma) +
" mA is above the run current " + ma(run_ma) +
" mA; clamping the search to the run current");
} else {
p.homing_irun = irunFor(homing_ma);
if (p.homing_irun > p.run_irun) p.homing_irun = p.run_irun;
}
p.run_ma = tmcRmsCurrentMa(p.global_scaler, p.run_irun, r_sense_ohm, v_fs);
p.homing_ma = tmcRmsCurrentMa(p.global_scaler, p.homing_irun, r_sense_ohm, v_fs);
// The smallest non-zero step this scaler can express. Worth naming when a
// homing target lands under it, because the result is then noticeably
// stronger than asked for - the direction that matters for a yielding stop.
const double step = full / 32.0;
if (p.homing_irun == 0 && homing_ma < step * 0.5) {
p.warnings.push_back("homing current " + ma(homing_ma) +
" mA is below the smallest step this driver can express (" +
ma(step) + " mA); using " + ma(p.homing_ma) + " mA");
}
auto driftWarn = [&](const char* what, double want, double got) {
if (want > 0.0 && std::fabs(got - want) / want > 0.10)
p.warnings.push_back(std::string(what) + " current " + ma(want) +
" mA is not reachable; nearest is " + ma(got) + " mA");
};
driftWarn("run", run_ma, p.run_ma);
if (homing_ma <= run_ma) driftWarn("homing", homing_ma, p.homing_ma);
p.ok = true;
return p;
}
} // namespace fgc

1
tests/CMakeLists.txt
View File

@ -22,6 +22,7 @@ add_executable(fgc_tests
test_paths.cpp
test_singleinstance.cpp
test_config.cpp
test_tmccurrent.cpp
test_cameraprofile.cpp
test_camerasettingsxml.cpp
test_hash.cpp

105
tests/test_tmccurrent.cpp Normal file
View File

@ -0,0 +1,105 @@
#include "doctest/doctest.h"
#include "fgc/TmcCurrent.h"
#include <cmath>
using namespace fgc;
namespace {
// The IOTA tower's driver board.
constexpr double kR = 0.075;
} // namespace
TEST_CASE("the RMS current formula matches the datasheet") {
// Full scale: GLOBALSCALER 256 (encoded 0), IRUN 31.
CHECK(tmcRmsCurrentMa(0, 31, kR) == doctest::Approx(2419.0).epsilon(0.001));
// The fleet's setting.
CHECK(tmcRmsCurrentMa(80, 31, kR) == doctest::Approx(756.0).epsilon(0.002));
// A non-positive sense resistor is a config error, not a division to attempt.
CHECK(tmcRmsCurrentMa(80, 31, 0.0) == 0.0);
}
TEST_CASE("the fleet's run current lands on the values the firmware already ships") {
// 80 is what firmware src/config.h carries for yaw, arrived at independently.
// If this test fails, either the formula or that constant has drifted.
auto p = planTmcCurrent(kR, 756.0, 200.0);
REQUIRE(p.ok);
CHECK(p.global_scaler == 80);
CHECK(p.run_irun == 31);
CHECK(p.homing_irun == 7);
CHECK(p.run_ma == doctest::Approx(756.0).epsilon(0.002));
CHECK(p.homing_ma == doctest::Approx(189.0).epsilon(0.01));
CHECK(p.warnings.empty());
}
TEST_CASE("an achieved current is recomputed from the registers, never echoed back") {
// The log must not flatter the result: what is reported has to be what the
// chosen registers deliver.
for (double r : {0.05, 0.075, 0.11}) {
for (double run : {300.0, 756.0, 1500.0}) {
auto p = planTmcCurrent(r, run, run / 4.0);
REQUIRE(p.ok);
CHECK(p.run_ma == doctest::Approx(tmcRmsCurrentMa(p.global_scaler, p.run_irun, r)));
CHECK(p.homing_ma ==
doctest::Approx(tmcRmsCurrentMa(p.global_scaler, p.homing_irun, r)));
}
}
}
TEST_CASE("GLOBALSCALER never lands in the band the datasheet forbids") {
// 1..31 is disallowed, and 0 means 256 (full scale) - emitting either would
// mean silently maximum current on a rig whose endstop yields to full torque.
for (double r : {0.05, 0.075, 0.11, 0.22}) {
for (double run = 10.0; run <= 3000.0; run += 10.0) {
auto p = planTmcCurrent(r, run, run / 2.0);
REQUIRE(p.ok);
CHECK(p.global_scaler >= 32);
CHECK(p.global_scaler <= 255);
}
}
}
TEST_CASE("a run current the driver cannot reach is clamped and named") {
auto p = planTmcCurrent(kR, 5000.0, 200.0);
REQUIRE(p.ok);
CHECK(p.global_scaler == 255);
CHECK(p.run_ma < 2500.0);
REQUIRE(p.warnings.size() >= 1);
CHECK(p.warnings[0].find("5000") != std::string::npos);
}
TEST_CASE("a homing current above the run current is refused, not obeyed") {
auto p = planTmcCurrent(kR, 756.0, 900.0);
REQUIRE(p.ok);
CHECK(p.homing_irun == p.run_irun);
REQUIRE(p.warnings.size() >= 1);
CHECK(p.warnings[0].find("above the run current") != std::string::npos);
}
TEST_CASE("a homing current below the smallest expressible step says so") {
auto p = planTmcCurrent(kR, 756.0, 5.0);
REQUIRE(p.ok);
CHECK(p.homing_irun == 0);
CHECK(p.homing_ma > 5.0); // stronger than asked - the direction worth naming
bool named = false;
for (const auto& w : p.warnings)
if (w.find("smallest step") != std::string::npos) named = true;
CHECK(named);
}
TEST_CASE("the homing current never exceeds the run current after rounding") {
for (double r : {0.05, 0.075, 0.11}) {
for (double run = 50.0; run <= 2000.0; run += 50.0) {
auto p = planTmcCurrent(r, run, run * 0.99);
REQUIRE(p.ok);
CHECK(p.homing_irun <= p.run_irun);
}
}
}
TEST_CASE("non-positive inputs are rejected rather than computed") {
CHECK_FALSE(planTmcCurrent(0.0, 756.0, 200.0).ok);
CHECK_FALSE(planTmcCurrent(kR, 0.0, 200.0).ok);
CHECK_FALSE(planTmcCurrent(kR, 756.0, 0.0).ok);
CHECK(planTmcCurrent(0.0, 756.0, 200.0).warnings.size() == 1);
}