119 lines
4.7 KiB
C++
119 lines
4.7 KiB
C++
#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
|