diff --git a/CMakeLists.txt b/CMakeLists.txt index b325746..f1fb2b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/include/fgc/TmcCurrent.h b/include/fgc/TmcCurrent.h new file mode 100644 index 0000000..681f5a2 --- /dev/null +++ b/include/fgc/TmcCurrent.h @@ -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 +#include + +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 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 diff --git a/src/core/TmcCurrent.cpp b/src/core/TmcCurrent.cpp new file mode 100644 index 0000000..6478195 --- /dev/null +++ b/src/core/TmcCurrent.cpp @@ -0,0 +1,118 @@ +#include "fgc/TmcCurrent.h" + +#include +#include +#include + +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(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(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(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 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index c6307ba..f4605a4 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -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 diff --git a/tests/test_tmccurrent.cpp b/tests/test_tmccurrent.cpp new file mode 100644 index 0000000..8efa8b1 --- /dev/null +++ b/tests/test_tmccurrent.cpp @@ -0,0 +1,105 @@ +#include "doctest/doctest.h" +#include "fgc/TmcCurrent.h" + +#include + +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); +}