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

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.<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).

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<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.
+        // Re-anchor the yaw zero to an endstop now that the homed limits are known,
-        if (homed) applyHomedTravel();
+        // 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() + wait;
-        const auto deadline = std::chrono::steady_clock::now() + 3s;
         while (running && std::chrono::steady_clock::now() < deadline) {
             std::this_thread::sleep_for(100ms);
             publishSnapshot();
-            dd = parseDump(motor->lastDump());
+            DumpData dd = parseDump(motor->lastDump());
-            if (dd.valid) break;
+            if (dd.valid) return dd;
         }
-        if (!dd.valid) {
+        return std::nullopt;
-            LOG_WARN << "homed travel: no firmware dump; keeping the previous degree limits";
+    }
-            return;
+
+    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();

src/core/HelpText.cpp

@@ -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.", {}},
         }},