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

Send and verify motor current, and derive counts_per_deg from the homed travel

This commit is contained in:
pgdalmeida 2026-09-17 17:58:46 +02:00
parent 9a61167aa6
commit 0c1d0a6df8
Signed by: pedro.almeida
GPG Key ID: D4A6C394DF13F1D7
3 changed files with 235 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
config/config.example.ini
View File

@ -168,6 +168,8 @@ period_ms = 2000
; Travel limits are NOT configured here - they are derived from the firmware's
; homed soft limits after every home, the only place they can be correct.
;
; Mechanical travel between the hard endstops now belongs in hardware.ini as
; [Motor.<axis>] travel_deg, where homing uses it to derive counts_per_deg below.
; Mechanical travel of this gimbal (between hard endstops, found at homing):
; Yaw ~320 deg (NOT a full revolution: there is an unreachable arc)
; Pitch 40 deg
@ -190,6 +192,10 @@ pitch_angle_at_center = 10.0
pitch_soft_margin_counts = 5000
; Live encoder-error WARN threshold in counts (0 = off).
enc_error_warn_counts = 400
; How long a full two-end homing may take before the host calls it failed. A
; property of the drivetrain: a high gear ratio searching at a gentle speed takes
; minutes, and too tight a value reports a healthy axis as broken.
homing_timeout_s = 65
[Scan]
; Capture scan grid (the (yaw,pitch) waypoints auto-sweep steps through).

236
src/core/Application.cpp
View File

@ -9,6 +9,7 @@
#include "fgc/HelpText.h"
#include "fgc/Paths.h"
#include "fgc/CameraProfile.h"
#include "fgc/TmcCurrent.h"
#include "fgc/ICameraSource.h"
#include "fgc/IControlChannel.h"
#include "fgc/IEnvSensor.h"
@ -237,6 +238,9 @@ struct Application::Impl {
CalibResultView last_calib_view;
std::vector<std::string> last_calib_summary;
long long last_calib_ts = 0; // epoch ms
// Motor-current registers computed once from hardware.ini, sent to the
// firmware before every home, and checked against the DUMP echo afterwards.
TmcCurrentPlan yaw_current_, pitch_current_;
bool calib_save_pending_ = false; // awaiting y/n to persist to config
// Which command produced the pending values, recorded into the `;prev`
// comment when they are saved so the file says what superseded what.
@ -987,7 +991,12 @@ struct Application::Impl {
// Pending yes/no: persist the just-applied calibration as the new default?
if (calib_save_pending_)
a.prompt = "Save the new [Motor] calibration to config as the default? (y / n)";
// Name the source: a rough derivation and a landmark fit both arrive
// here, and they do not deserve the same confidence.
a.prompt = "Save the new [Motor] calibration (" +
(calib_save_source_.empty() ? std::string("calibration")
: calib_save_source_) +
") to config as the default? (y / n)";
}
void publishSnapshot() {
@ -1037,6 +1046,19 @@ struct Application::Impl {
// the centre count to re-origin, so a late send would apply to the next
// home, not this one.
sendAnchorConfig();
// Confirm the current landed BEFORE driving into anything. This is the
// only check standing between a stale firmware and an endstop that cannot
// survive full torque.
if (yaw_current_.ok || pitch_current_.ok) {
if (auto dd = freshDump(std::chrono::seconds(3))) {
if (!verifyCurrentAdoption(*dd)) return;
} else {
LOG_WARN << "no firmware dump before homing; cannot confirm the motor "
"current was adopted";
}
}
motor->sendCommand("HOME"); // homes all axes; firmware runs it non-blocking
auto allReady = [](const MotorTelemetry& t) {
@ -1057,7 +1079,12 @@ struct Application::Impl {
// Wait for completion (both axes READY again) or the homing timeout.
bool homed = false;
const auto deadline = std::chrono::steady_clock::now() + 65s;
// How long a full two-end search may take is a property of the drivetrain,
// not of this program: a high gear ratio homing at a deliberately gentle
// speed takes minutes, and a fixed 65 s ceiling reported those towers as
// failed while the firmware was still searching perfectly happily.
const auto deadline =
std::chrono::steady_clock::now() + std::chrono::seconds(cfg.homing_timeout_s);
while (running && !cancel_procedure_ && std::chrono::steady_clock::now() < deadline) {
std::this_thread::sleep_for(250ms);
drainAndDiscardCommands();
@ -1079,14 +1106,102 @@ struct Application::Impl {
motor->sendCommand("STOP ALL");
return;
}
// Re-anchor the yaw zero to an endstop now that the homed limits are known.
if (homed) applyHomedTravel();
// Re-anchor the yaw zero to an endstop now that the homed limits are known,
// and take the chance to check the degree scale against the travel we just
// measured. Derivation first: applyHomedTravel() converts the soft limits
// through the map, so it wants the corrected scale.
if (homed) {
using namespace std::chrono_literals;
if (auto dd = freshDump(3s)) {
deriveCountsPerDeg(*dd);
applyHomedTravel(&*dd);
} else {
LOG_WARN << "no firmware dump after homing; degree limits and the derived "
"counts_per_deg both skipped";
}
}
// Production move speeds for subsequent MOVE commands.
motor->sendCommand("SPEED Y 50000");
motor->sendCommand("SPEED P 150000");
LOG_INFO << "Gimbal init finished";
}
// Convert this tower's declared milliamps into TMC register fields, once.
// The host owns this because hardware.ini is where the sense resistor lives;
// the firmware is told registers and keeps working compile-time defaults.
void planMotorCurrents() {
auto plan = [&](const char* name, const AxisDrivetrainConfig& d, TmcCurrentPlan& out) {
if (!d.declared || d.run_current_ma <= 0.0) return;
out = planTmcCurrent(d.r_sense_ohm, d.run_current_ma, d.homing_current_ma);
for (const auto& w : out.warnings) LOG_WARN << name << " motor current: " << w;
if (!out.ok) {
LOG_ERROR << name << " motor current could not be computed; the firmware "
"will keep its built-in defaults";
return;
}
// Log what the registers actually deliver, not what was asked for.
LOG_INFO << name << " motor current: run " << std::lround(out.run_ma)
<< " mA (GLOBALSCALER " << out.global_scaler << ", IRUN " << out.run_irun
<< "), homing " << std::lround(out.homing_ma) << " mA (IRUN "
<< out.homing_irun << ") -- R_sense " << d.r_sense_ohm
<< " ohm from hardware.ini [Motor." << name << ']';
};
plan("yaw", cfg.drivetrain.yaw, yaw_current_);
plan("pitch", cfg.drivetrain.pitch, pitch_current_);
}
// Did the firmware actually adopt the current we sent? GLOBALSCALER and
// IHOLD_IRUN are write-only on the TMC5160, so the DUMP echo is the only
// channel - and sendCommand() is fire-and-forget, so an 'ERR unknown command'
// from an older firmware arrives as an uncorrelated warning we cannot pin to
// this command. Returns false only when a reduction was REQUIRED and is
// demonstrably absent.
bool verifyCurrentAdoption(const DumpData& dd) {
bool safe = true;
auto check = [&](const char* name, char axis, const AxisDrivetrainConfig& d,
const TmcCurrentPlan& want) {
if (!want.ok) return;
const DumpAxis* ax = nullptr;
for (const auto& a : dd.axes)
if (a.axis == axis) ax = &a;
if (!ax) return;
const bool absent = (ax->run_irun < 0);
const bool wrong = !absent && (ax->global_scaler != want.global_scaler ||
ax->run_irun != want.run_irun ||
ax->homing_irun != want.homing_irun);
if (!absent && !wrong) return;
std::ostringstream detail;
if (absent)
detail << "the firmware does not report a motor current at all, so it "
"predates the CURRENT command";
else
detail << "the firmware reports GLOBALSCALER " << ax->global_scaler << ", IRUN "
<< ax->run_irun << '/' << ax->homing_irun << " but was sent "
<< want.global_scaler << ", IRUN " << want.run_irun << '/'
<< want.homing_irun;
if (d.reducesForHoming()) {
// The reduction is load-bearing: this tower's endstop yields to
// full torque. Homing anyway is how the stop gets destroyed.
LOG_ERROR << name << " motor current was NOT adopted -- " << detail.str()
<< ". Refusing to home: this axis declares a reduced homing "
"current because its endstop cannot survive full torque. "
"Flash the firmware, or remove homing_current_ma from "
"hardware.ini to accept the risk deliberately.";
safe = false;
} else {
LOG_WARN << name << " motor current was not adopted -- " << detail.str()
<< ". Continuing: this axis asks for no reduction, so homing is "
"unaffected.";
}
};
check("yaw", 'Y', cfg.drivetrain.yaw, yaw_current_);
check("pitch", 'P', cfg.drivetrain.pitch, pitch_current_);
return safe;
}
// Tell the firmware the anchor point and endstop clearance for each axis.
// The centre count is what finishHoming() re-origins onto, so all counts stay
// positive and reproducible across power cycles; the margin is what
@ -1096,6 +1211,17 @@ struct Application::Impl {
motor->sendCommand("CENTER P " + std::to_string(cfg.geometry.pitch.center_count));
motor->sendCommand("MARGIN Y " + std::to_string(cfg.yaw_soft_margin_counts));
motor->sendCommand("MARGIN P " + std::to_string(cfg.pitch_soft_margin_counts));
// Motor current belongs here too: it governs homing torque, so it has to
// land before the search, not after it.
auto sendCurrent = [&](char axis, const TmcCurrentPlan& p) {
if (!p.ok) return;
motor->sendCommand(std::string("CURRENT ") + axis + ' ' +
std::to_string(p.global_scaler) + ' ' +
std::to_string(p.run_irun) + ' ' +
std::to_string(p.homing_irun));
};
sendCurrent('Y', yaw_current_);
sendCurrent('P', pitch_current_);
}
// Derive each axis's reachable degree window from the freshly homed soft
@ -1109,24 +1235,34 @@ struct Application::Impl {
// not take effect - firmware predating it answers "ERR unknown command" and
// homes to its own compiled default, which would leave every published
// bearing silently wrong.
void applyHomedTravel() {
// Request a DUMP and wait briefly for a parseable one. Several callers need
// the firmware's own view of itself - the homed limits, and whether it adopted
// what we sent - so the round trip lives in one place.
std::optional<DumpData> freshDump(std::chrono::milliseconds wait) {
using namespace std::chrono_literals;
// The homed limits live in the firmware DUMP; pull a fresh one and wait
// briefly for it to arrive.
motor->sendCommand("DUMP");
DumpData dd;
const auto deadline = std::chrono::steady_clock::now() + 3s;
const auto deadline = std::chrono::steady_clock::now() + wait;
while (running && std::chrono::steady_clock::now() < deadline) {
std::this_thread::sleep_for(100ms);
publishSnapshot();
dd = parseDump(motor->lastDump());
if (dd.valid) break;
DumpData dd = parseDump(motor->lastDump());
if (dd.valid) return dd;
}
if (!dd.valid) {
return std::nullopt;
}
void applyHomedTravel(const DumpData* pre = nullptr) {
using namespace std::chrono_literals;
std::optional<DumpData> got;
if (!pre) {
got = freshDump(3s);
if (!got) {
LOG_WARN << "homed travel: no firmware dump; keeping the previous degree limits";
return;
}
}
const DumpData& dd = pre ? *pre : *got;
bool any = false;
for (const auto& a : dd.axes) {
@ -1386,6 +1522,71 @@ struct Application::Impl {
return static_cast<bool>(iss >> a >> b);
}
// Cross-check the degree scale against the travel homing just measured, and
// bootstrap it when it is obviously wrong.
//
// Called ONLY from the homing path. applyHomedTravel() runs after every
// heading fit and offset too, and deriving there would overwrite a landmark
// calibration with this rougher figure on every tweak.
void deriveCountsPerDeg(const DumpData& dd) {
struct AxisRef {
const char* name;
char axis;
const AxisDrivetrainConfig& drive;
AxisMap& map;
};
AxisRef axes[] = {
{"yaw", 'Y', cfg.drivetrain.yaw, cfg.geometry.yaw},
{"pitch", 'P', cfg.drivetrain.pitch, cfg.geometry.pitch},
};
bool changed = false;
for (auto& a : axes) {
if (a.drive.travel_deg <= 0.0) continue; // nothing declared to divide by
// Measured travel from THIS dump. Taking it from motor->lastDump()
// instead would read whatever dump happened to be cached - which,
// before applyHomedTravel() fetches a fresh one, is the pre-homing
// dump whose limits are still zero, so the derivation would skip in
// silence on every single home.
long counts = 0;
for (const auto& ax : dd.axes)
if (ax.axis == a.axis) counts = std::labs(ax.lim_pos - ax.lim_neg);
if (counts <= 0) continue; // that axis did not home
// The angle is unsigned; which way the axis counts is a property of
// the map, so carry the existing sign rather than inventing one.
const double derived =
std::copysign(counts / a.drive.travel_deg, a.map.counts_per_deg);
const double current = a.map.counts_per_deg;
const double ratio =
std::fabs(current) > 1e-9 ? std::fabs(derived / current) : 1e9;
// Only bootstrap a scale that is obviously wrong. Within a factor of
// two this is a health check, not a correction: a landmark fit is a
// better measurement than this one and must not be clobbered by a
// routine re-home.
if (ratio > 2.0 || ratio < 0.5) {
LOG_WARN << a.name << " counts_per_deg derived from homing: " << derived
<< " (was " << current << ") = " << counts << " counts / "
<< a.drive.travel_deg << " deg of travel declared in hardware.ini ["
<< "Motor." << a.name << "] travel_deg. That angle is a NAMEPLATE "
"figure: every bearing carries its error until it is measured. "
"Run 'gimbal heading setup' to supersede this with a landmark fit.";
a.map.counts_per_deg = derived;
changed = true;
} else {
LOG_INFO << a.name << " counts_per_deg " << current << " agrees with the homed "
<< "travel (" << counts << " counts / " << a.drive.travel_deg
<< " deg = " << derived << "); leaving the calibration alone";
}
}
if (changed && !opts.config_path.empty()) {
calib_save_source_ = "homed travel / hardware.ini travel_deg (nameplate)";
calib_save_pending_ = true;
}
}
// Homed physical travel (counts) for an axis: the endstop-to-endstop span from
// the last firmware dump. Returns 0 if no dump has been captured yet (caller
// should request one). Deliberately does NOT fall back to the configured degree
@ -1408,7 +1609,8 @@ struct Application::Impl {
while (iss >> t) tok.push_back(t); // tok[0] == "gimbal"
if (tok.size() < 2) {
LOG_WARN << "usage: gimbal <move|steps|nudge|jog|heading|pitch|home|stop|reset|"
"enable|disable|speed|setpos|status|dump|raw> (self-test: 'test')";
"enable|disable|speed|current|setpos|status|dump|raw> "
"(self-test: 'test')";
return;
}
std::string sub = tok[1];
@ -1505,7 +1707,8 @@ struct Application::Impl {
// home rather than a fire-and-forget HOME that skips the re-anchor.
runInitSequence();
} else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" ||
sub == "disable" || sub == "speed" || sub == "setpos" || sub == "status") {
sub == "disable" || sub == "speed" || sub == "setpos" ||
sub == "status" || sub == "current") {
// Passthrough to the (case-insensitive) firmware verb + args.
std::string fw = sub;
for (char& ch : fw) ch = static_cast<char>(std::toupper((unsigned char)ch));
@ -2102,6 +2305,9 @@ struct Application::Impl {
// the wait is the longest across them, not the first one's.
scheduler->setSettleWait(cfg.camera.maxSettleWaitMs(), cfg.camera.maxFrameTimeoutMs());
// Before the firmware is talked to: the registers are wanted by the very
// first sendAnchorConfig(), on both the --init and the no---init path.
planMotorCurrents();
motor->start();
if (imu) imu->start();
if (env) env->start();

6
src/core/HelpText.cpp
View File

@ -74,6 +74,12 @@ const std::vector<HelpSection>& helpCatalog() {
"the whole 'gimbal' family is ignored while one is in progress."}},
{"gimbal speed <y|p> <vel>",
"Set the max slew speed (counts/s) for an axis.", {}},
{"gimbal current <y|p> <scaler> <irun> <irun_home>",
"Set motor current, as raw TMC register fields.",
{"Normally sent automatically from hardware.ini [Motor.<axis>], which states",
"the current in milliamps and lets the host convert it using this board's",
"sense resistor. Setting it by hand does not stay secret: the next home",
"checks the firmware's reported current against the configured one."}},
{"gimbal reset [y|p] / gimbal enable|disable <y|p> / gimbal setpos <y|p> <v>",
"Other firmware motor controls.", {}},
}},