From 0c1d0a6df8613f6b0a2c382618dadfaa20f36a45 Mon Sep 17 00:00:00 2001 From: pgdalmeida Date: Thu, 17 Sep 2026 17:58:46 +0200 Subject: [PATCH] Send and verify motor current, and derive counts_per_deg from the homed travel --- config/config.example.ini | 6 + src/core/Application.cpp | 240 +++++++++++++++++++++++++++++++++++--- src/core/HelpText.cpp | 6 + 3 files changed, 235 insertions(+), 17 deletions(-) diff --git a/config/config.example.ini b/config/config.example.ini index 5f2ce61..147d9dc 100644 --- a/config/config.example.ini +++ b/config/config.example.ini @@ -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.] 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). diff --git a/src/core/Application.cpp b/src/core/Application.cpp index 941b463..c885752 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -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 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 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) { - LOG_WARN << "homed travel: no firmware dump; keeping the previous degree limits"; - return; + return std::nullopt; + } + + void applyHomedTravel(const DumpData* pre = nullptr) { + using namespace std::chrono_literals; + + std::optional 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(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 (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(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(); diff --git a/src/core/HelpText.cpp b/src/core/HelpText.cpp index 6871cd1..f323039 100644 --- a/src/core/HelpText.cpp +++ b/src/core/HelpText.cpp @@ -74,6 +74,12 @@ const std::vector& helpCatalog() { "the whole 'gimbal' family is ignored while one is in progress."}}, {"gimbal speed ", "Set the max slew speed (counts/s) for an axis.", {}}, + {"gimbal current ", + "Set motor current, as raw TMC register fields.", + {"Normally sent automatically from hardware.ini [Motor.], 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 / gimbal setpos ", "Other firmware motor controls.", {}}, }},