diff --git a/CMakeLists.txt b/CMakeLists.txt index 02f724d..c808742 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,6 +67,7 @@ add_library(fgc_core STATIC src/core/HardwarePresence.cpp src/core/HardwareChecks.cpp src/core/AxisSpeed.cpp + src/core/FirmwareConfig.cpp src/core/ScanGrid.cpp src/core/TelemetryParser.cpp src/core/CaptureScheduler.cpp diff --git a/config/config.example.ini b/config/config.example.ini index b204de0..4ff9a68 100644 --- a/config/config.example.ini +++ b/config/config.example.ini @@ -241,6 +241,11 @@ mock_camera = false mock_serial = false mock_imu = false mock_env = false +; Make the simulated motor controller run the config handshake: emit NEED lines +; at startup and reject HOME/MOVE/ENABLE until every requested key is answered +; from hardware.ini. How the host half is exercised before a firmware that asks +; exists - and afterwards, on a dev box with no rig. +mock_needcfg = false [Test] ; Hardware self-test command (`test`). `profile` is the default profile used when diff --git a/config/hardware.example.ini b/config/hardware.example.ini index 157a038..b0ac06b 100644 --- a/config/hardware.example.ini +++ b/config/hardware.example.ini @@ -128,6 +128,32 @@ travel_deg = 340 ; so this follows the calibration automatically. An axis whose scale is not ; trustworthy is left on the firmware's default and the reason is logged. scan_speed_deg_s = 8.0 +; The other two motions, also in deg/s. Both happen BEFORE the first home, so +; they cannot use the counts_per_deg that homing derives - they convert through +; the scale this file implies (steps_per_rev / travel_deg), which on the IOTA +; agrees with the measured one to 0.25%. +homing_speed_deg_s = 2.0 +center_speed_deg_s = 6.0 + +; --- Told to the firmware, which holds none of it itself -------------------- +; The config handshake (docs/configuration.md): the firmware asks with NEED and +; these answer it. A key missing here leaves that axis in NEEDCFG, refusing to +; move, with the missing name logged - deliberately, because a drivetrain value +; guessed by the firmware is the duplication this removes. +; +; Expected full mechanical travel, IN COUNTS. Measured, not derived: it is the +; only guard against a PHANTOM endstop - too little homing current and the motor +; stalls on ordinary friction, trips the same latch a real stop does, and the +; bogus limit is accepted. +steps_per_rev = 1210778 +; |XACTUAL - XENC| that means "position untrustworthy". Re-measure whenever the +; homing current changes: the free-running gap is coupled to it. +enc_deviation = 1500 +; TMC SW_MODE - physical endstop switches on REFL/REFR. Yaw on this rig homes by +; StallGuard with no switches (0); an axis with real switches sets the bits. +sw_mode = 0 +; Is this axis's encoder aligned to XACTUAL and usable for hold and DIAG? +has_encoder = true [Camera.rgb] ; The serial of the physical camera fitted to this tower. A USB DEV_ id, or a diff --git a/docs/configuration.md b/docs/configuration.md index 67bdf3c..9384019 100644 --- a/docs/configuration.md +++ b/docs/configuration.md @@ -55,6 +55,7 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config | `Features` | `mock_imu` | bool | `false` | Use the simulated IMU instead of the MTi | | `Features` | `enable_env` | bool | `false` | **Legacy.** Derived from `[Hardware] env` wherever that is declared; same rule as `enable_imu` | | `Features` | `mock_env` | bool | `false` | Use the simulated env sensor instead of the SHT41 | +| `Features` | `mock_needcfg` | bool | `false` | Make the simulated motor controller run the config handshake (emit `NEED`, refuse to move until answered) | | `Features` | `strict_presence` | bool | `false` | Refuse to start when `hardware.ini` declares a subsystem this rig does not have. Off during the fleet rollout; turn it on per tower once that file is known good. `--degraded` overrides it | | `IMU` | `device` | string | — | MTi serial device (see `[IMU]` note); required when `enable_imu` | | `IMU` | `baud` | int | `115200` | MTi serial baud rate | @@ -75,6 +76,10 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config | `Motor.` | `run_current_ma` / `homing_current_ma` | float | — | Coil current (mA RMS) for normal motion, and while hunting an endstop. **hardware.ini** | | `Motor.` | `travel_deg` | float | — | Mechanical angle between the hard endstops. **hardware.ini.** Homing divides the measured travel by it to derive `_counts_per_deg` | | `Motor.` | `scan_speed_deg_s` | float | `8.0` | Production move speed **in degrees per second**. **hardware.ini.** The host converts it to VMAX through `_counts_per_deg` and sends `SPEED`; a register value would not mean the same thing on two drivetrains | +| `Motor.` | `homing_speed_deg_s` / `center_speed_deg_s` | float | — | Endstop-search and park speeds, also in °/s. **hardware.ini.** Both are needed *before* the first home, so they convert through the declared scale (`steps_per_rev / travel_deg`), not the homed one | +| `Motor.` | `steps_per_rev` | int | — | Expected full mechanical travel, **in counts**. **hardware.ini.** The firmware's only guard against a phantom endstop | +| `Motor.` | `enc_deviation` | int | — | \|XACTUAL−XENC\| that means "position untrustworthy". **hardware.ini** | +| `Motor.` | `sw_mode` / `has_encoder` | int / bool | — | TMC `SW_MODE` (physical endstop switches on REFL/REFR) and whether this axis's encoder is aligned and usable. **hardware.ini** | | `Scan` | `grid_file` | string | — | CSV of `yaw_deg,pitch_deg` waypoints; empty → generate | | `Scan` | `yaw_intervals` | int | `56` | Generated yaw positions across `[yaw_min_deg, yaw_max_deg]` | | `Scan` | `yaw_min_deg`/`yaw_max_deg` | float | `-90`/`90` | Generated yaw arc | @@ -684,6 +689,46 @@ host checks them **before** homing. If an axis declares a reduced homing current firmware has not adopted it, the host **refuses to home** rather than driving into a stop at full torque — the fire it is there to prevent. +### The config handshake — the firmware asks, `hardware.ini` answers + +Today the firmware carries a full parameter table and **guesses** when the host +says nothing, so every drivetrain value exists in two places and the two can +disagree. Section 6 of the config plan replaces that: the firmware declares what +it needs and refuses to move until it has it. + +``` +firmware -> BOOT build=abc123 +firmware -> NEED Y steps_per_rev global_scaler sw_mode enc_deviation normal_speed ... +host -> SET Y steps_per_rev 1210778 +host -> SET Y global_scaler 80 +host -> SET Y normal_speed 39830 +... +``` + +`SPEED`, `CENTER`, `MARGIN` and `CURRENT` remain as aliases, so an old firmware +and a hand-typed session both keep working; `center` and `margin` are accepted as +key names alongside `center_count` and `soft_margin` for the same reason. + +Three rules, each protecting against a different silent failure: + +- **A value the host does not have is left unanswered, never sent as zero.** A + zero here is a real setting — a zeroed current is a motor that cannot hold, a + zeroed VMAX is a stopped axis. The axis stays in `NEEDCFG` and the missing keys + are named at `ERROR`, because nothing but editing `hardware.ini` will make the + gimbal move. +- **A key the host has never heard of is reported separately** from one it simply + lacks. The first means the firmware is newer than the host and the fix is to + upgrade the host; the second means `hardware.ini` is incomplete. Conflating them + sends someone to edit the wrong file. +- **A partial answer does not unlock the axis.** Most keys landing is the common + case when a declaration is nearly complete, and it must not look like success. + +> **Status: the host half only.** The host answers `NEED` and the responder is +> verified end to end, but the firmware that *asks* is not written yet. Set +> `[Features] mock_needcfg = 1` to make the simulated controller run the +> handshake — emit `NEED` at start, reject `HOME` / `MOVE` / `ENABLE` until every +> requested key is answered — which is how this is exercised on a dev box. + ### Why speed is declared in degrees per second `VMAX` is a register value, and what it *means* depends on the gear ratio: diff --git a/docs/hardware-config-plan.md b/docs/hardware-config-plan.md index 7fe269a..c11d261 100644 --- a/docs/hardware-config-plan.md +++ b/docs/hardware-config-plan.md @@ -426,7 +426,7 @@ Phase 1 is deployable on its own. Phase 2 requires it. ## Progress Branch `worktree-hardware-config` (worktree at `.claude/worktrees/hardware-config`). -273 tests pass, up from a 204 baseline. +286 tests pass, up from a 204 baseline. | § | Item | State | |----|------|-------| @@ -438,6 +438,7 @@ Branch `worktree-hardware-config` (worktree at `.claude/worktrees/hardware-confi | 10 | Presence reconciliation (declared vs found) | **done**, 13 tests. `HardwarePresence.h` compares `[Hardware]` against what came up; warn-only by default, refusing under `[Features] strict_presence` with `--degraded` as the field-triage escape (decision 4) | | 11 | `test hardware` | **done**, 11 tests. Five leaves (`axes` / `motor` / `imu` / `env` / `camera`) under a new `hardware` subsystem; included in a bare `test`, and deliberately not gated on a homed gimbal. Nothing declared reports SKIP, never PASS | | 5 | Delete the `Application.cpp` speed literals | **done**, 13 tests. Decision 2 answered: speed is declared as `[Motor.] scan_speed_deg_s` (default **8.0 deg/s**) and converted per tower through `counts_per_deg` (`AxisSpeed.h`). The literals sent 36.4 deg/s against an intended 21.8 | +| 6 | Config handshake — **host half** | **done**, 13 tests. `FirmwareConfig.h` parses `NEED` and plans the `SET` lines; unanswered keys and unknown keys are reported separately. `[Features] mock_needcfg` makes the simulated controller run the handshake, so the host half is verified with no firmware. **The firmware half is not written** | Presence reconciliation (§10) changes nothing on a tower without a `hardware.ini`: absent is silent and `hardware.declared` stays false. Verified by running the binary both ways. diff --git a/docs/modules-reference.md b/docs/modules-reference.md index ecf309d..20e3bc4 100644 --- a/docs/modules-reference.md +++ b/docs/modules-reference.md @@ -18,6 +18,7 @@ Per-file reference for the refactored tree, plus the shared data structures. | [include/fgc/HardwarePresence.h](../include/fgc/HardwarePresence.h), [src/core/HardwarePresence.cpp](../src/core/HardwarePresence.cpp) | `reconcilePresence` (what `hardware.ini` declares vs what actually came up) + `presenceVerdict` (proceed / warn / refuse, per `strict_presence` and `--degraded`). Pure comparison, so the policy is unit-tested without a rig | | [include/fgc/HardwareChecks.h](../include/fgc/HardwareChecks.h), [src/core/HardwareChecks.cpp](../src/core/HardwareChecks.cpp) | The verdicts behind `test hardware`: `checkUsbfsMemory`, `checkUsb3Lpm`, `checkI2cFunctionality`, `checkDrivetrainAdopted`. Pure, so the thresholds that encode known-issues #2 and #19 cannot drift unnoticed | | [include/fgc/AxisSpeed.h](../include/fgc/AxisSpeed.h), [src/core/AxisSpeed.cpp](../src/core/AxisSpeed.cpp) | `planAxisSpeed` (deg/s -> TMC `VMAX` through `counts_per_deg`) + `vmaxToDegPerSec`. Pure arithmetic; refuses an uncalibrated scale rather than commanding a near-zero speed | +| [include/fgc/FirmwareConfig.h](../include/fgc/FirmwareConfig.h), [src/core/FirmwareConfig.cpp](../src/core/FirmwareConfig.cpp) | `parseNeedLine` (firmware `NEED` line -> axis + keys) + `planFirmwareConfig` (-> the `SET` lines that answer it). Reports a key it cannot answer separately from one it does not recognise: incomplete `hardware.ini` vs. a firmware newer than the host | | [include/fgc/DumpParser.h](../include/fgc/DumpParser.h), [src/core/DumpParser.cpp](../src/core/DumpParser.cpp) | `parseDump` firmware `DUMP` block → `DumpData` (per-axis state + decoded TMC registers/flags); `formatDump` | | [include/fgc/DiagParser.h](../include/fgc/DiagParser.h), [src/core/DiagParser.cpp](../src/core/DiagParser.cpp) | `parseDiag` firmware `DG` self-test stream → `DiagResult` (per-axis tests, PASS/FAIL); `formatDiag` | | [include/fgc/Calibration.h](../include/fgc/Calibration.h), [src/core/Calibration.cpp](../src/core/Calibration.cpp) | `linearFit` (least-squares, R²) + `circularMeanDeg` for the IMU-referenced calibration | diff --git a/include/fgc/Config.h b/include/fgc/Config.h index 9375fd7..861a095 100644 --- a/include/fgc/Config.h +++ b/include/fgc/Config.h @@ -84,6 +84,33 @@ struct AxisDrivetrainConfig { // The host converts through counts_per_deg and sends SPEED; see AxisSpeed.h. double scan_speed_deg_s = 8.0; // kDefaultScanSpeedDegPerSec + // Speeds for the two motions that are not production moves. Both are also + // deg/s, and both are needed BEFORE the first home - so they cannot use the + // counts_per_deg that homing derives. preHomeCountsPerDeg() below is the + // estimate they convert through. + double homing_speed_deg_s = 0.0; // 0 = undeclared -> firmware default stands + double center_speed_deg_s = 0.0; + + // The mechanism's own facts, for the config handshake (FirmwareConfig.h). + // The firmware holds none of these once it asks for them. + long steps_per_rev = 0; // expected full mechanical travel, in counts + long enc_deviation = 0; // |XACTUAL-XENC| that means "position untrustworthy" + + // Wiring: TMC SW_MODE (physical endstop switches on REFL/REFR), and whether + // this axis's encoder is aligned to XACTUAL and usable for hold and DIAG. + bool wiring_declared = false; // any of the two keys below present + unsigned sw_mode = 0; + bool has_encoder = true; + + // counts_per_deg as the DECLARATION implies it, for use before the axis has + // been homed. Homing supersedes it by measuring the travel for real; on the + // IOTA the two agree to 0.25% (1210778/340 = 3561 against a measured + // 3552.87). Returns 0 when it cannot be formed. + double preHomeCountsPerDeg() const { + if (steps_per_rev <= 0 || travel_deg <= 0.0) return 0.0; + return static_cast(steps_per_rev) / travel_deg; + } + // True when the homing current is genuinely lower than the run current, i.e. // when the reduction is load-bearing and homing without it risks the rig. bool reducesForHoming() const { @@ -181,6 +208,10 @@ struct FeaturesConfig { bool mock_imu = false; // use a simulated IMU instead of the MTi bool enable_env = false; // ambient temp/humidity sensor (off by default) bool mock_env = false; // use a simulated env sensor instead of the SHT41 + // Make the simulated motor controller run the config handshake: emit NEED + // lines at start and reject motion until they are answered. This is how the + // host half is exercised on a dev box with no firmware that asks yet. + bool mock_needcfg = false; // Refuse to start when hardware.ini declares a subsystem this rig does not // have (HardwarePresence.h). Off by default so that gaining a hardware.ini diff --git a/include/fgc/FirmwareConfig.h b/include/fgc/FirmwareConfig.h new file mode 100644 index 0000000..cbb3664 --- /dev/null +++ b/include/fgc/FirmwareConfig.h @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include + +namespace fgc { + +// The host half of the config handshake (hardware-config-plan.md section 6). +// +// Today the firmware carries a full parameter table and GUESSES when the host +// says nothing, so the same drivetrain value exists in two places and they can +// disagree. Instead the firmware declares what it needs and refuses to move +// until it has it: +// +// firmware -> BOOT build=abc123 +// firmware -> NEED Y steps_per_rev global_scaler sw_mode enc_deviation ... +// host -> SET Y steps_per_rev 1210778 +// host -> SET Y global_scaler 80 +// ... +// +// This module is the pure part: read a NEED line, and work out which SET lines +// answer it. Sending them, and deciding what to do when some cannot be answered, +// belongs to the caller. + +// A NEED line: the firmware naming the parameters it will not run without. +struct NeedRequest { + char axis = '?'; // 'Y' or 'P' + std::vector keys; +}; + +// Returns nullopt for anything that is not a well-formed NEED line. A NEED with +// no keys is well-formed and means "this axis needs nothing" - which is how a +// firmware built for fewer axes than the rig has answers for the axis it lacks. +std::optional parseNeedLine(const std::string& line); + +// Everything the host can answer for one axis. Each group carries its own +// "declared" flag because a value the host does not have must be reported as +// unanswered rather than sent as a zero - a zero here is a real setting, and a +// silently zeroed current or margin is a damaged rig. +struct AxisConfigSource { + // From [Motor.] milliamps, via TmcCurrent.h. + bool have_current = false; + int global_scaler = 0; + int run_irun = 0; + int homing_irun = 0; + + // From [Motor] calibration and margins. Always known - they have defaults + // that predate hardware.ini and are already sent as CENTER / MARGIN. + long center_count = 0; + long soft_margin = 0; + + // From [Motor.], the mechanism's own facts. + bool have_travel = false; + long steps_per_rev = 0; // expected full mechanical travel, in counts + bool have_deviation = false; + long enc_deviation = 0; + + // Speeds, already converted from deg/s to VMAX by the caller (AxisSpeed.h). + // 0 means the conversion was refused - an uncalibrated scale - and the key + // goes unanswered rather than stopping the axis with a VMAX of zero. + long normal_vmax = 0; + long homing_vmax = 0; + long center_vmax = 0; + + // Wiring facts: whether this axis has physical endstop switches on the TMC's + // REFL/REFR, and whether its encoder is aligned and usable. + bool have_wiring = false; + unsigned sw_mode = 0; + bool has_encoder = true; +}; + +struct FirmwareConfigPlan { + // Ready-to-send command lines, in the order the firmware asked for them. + std::vector sets; + // Keys the host was asked for and has no value for. The firmware stays in + // NEEDCFG until they are answered, so this is a startup failure, not a + // warning to scroll past. + std::vector unanswered; + // Keys that are not part of the vocabulary at all: a firmware newer than + // this host. Kept separate from `unanswered` because the fix is different - + // upgrade the host, rather than fill in hardware.ini. + std::vector unknown; + + bool complete() const { return unanswered.empty() && unknown.empty(); } +}; + +// Map one NEED request onto SET lines. `center` / `margin` are accepted as +// aliases for `center_count` / `soft_margin`, because those are the names the +// existing CENTER and MARGIN commands use and hand-typed sessions rely on them. +FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigSource& src); + +} // namespace fgc diff --git a/include/fgc/IMotorController.h b/include/fgc/IMotorController.h index ef3d590..c46411c 100644 --- a/include/fgc/IMotorController.h +++ b/include/fgc/IMotorController.h @@ -1,6 +1,7 @@ #pragma once #include +#include namespace fgc { @@ -70,6 +71,14 @@ public: virtual std::string lastDiag() = 0; virtual unsigned diagSeq() const = 0; + // NEED lines the firmware has emitted since they were last taken, one per + // axis, in arrival order (hardware-config-plan.md section 6). Taking them + // clears the queue: they are a request to answer once, not a state to poll. + // + // Default: none. Only the real controller sees the wire; the mock has no + // firmware to ask, and a null implementation would have nothing to say. + virtual std::vector takeNeedLines() { return {}; } + // Whether the underlying link is usable. virtual bool connected() const = 0; }; diff --git a/include/fgc/SerialMotorController.h b/include/fgc/SerialMotorController.h index 6edd44c..368ee43 100644 --- a/include/fgc/SerialMotorController.h +++ b/include/fgc/SerialMotorController.h @@ -4,6 +4,7 @@ #include #include +#include namespace fgc { @@ -23,6 +24,7 @@ public: std::string lastDump() override; std::string lastDiag() override; unsigned diagSeq() const override; + std::vector takeNeedLines() override; bool connected() const override; private: diff --git a/include/fgc/mock/MockMotorController.h b/include/fgc/mock/MockMotorController.h index 765c438..d93fd3c 100644 --- a/include/fgc/mock/MockMotorController.h +++ b/include/fgc/mock/MockMotorController.h @@ -3,9 +3,12 @@ #include "fgc/IMotorController.h" #include "fgc/Logger.h" +#include #include +#include #include #include +#include namespace fgc { @@ -16,7 +19,38 @@ namespace fgc { // move -> settle -> trigger loop runs exactly as it would against real hardware. class MockMotorController : public IMotorController { public: - void start() override { LOG_INFO << "[mock] motor controller started"; } + // `needcfg`: emit NEED lines on start and refuse to move until they are + // answered, the way the real firmware will (hardware-config-plan.md + // section 6). Off by default so every existing test sees the old mock. + // + // This is what lets the HOST half of the handshake be built and verified + // before the firmware half exists - and it is not throwaway: it stays as the + // way the handshake is exercised on a dev box with no rig. + explicit MockMotorController(bool needcfg = false) : needcfg_(needcfg) {} + + void start() override { + LOG_INFO << "[mock] motor controller started"; + if (!needcfg_) return; + std::lock_guard lock(mutex_); + static const char* kKeys[] = {"steps_per_rev", "global_scaler", "sw_mode", + "enc_deviation", "normal_speed", "homing_speed", + "center_speed", "center", "margin"}; + need_lines_.clear(); + unanswered_.clear(); + for (const char* axis : {"Y", "P"}) { + std::string line = std::string("NEED ") + axis; + for (const char* k : kKeys) { + line += std::string(" ") + k; + // Track the same set the line asks for, so the axis unlocks only + // when EVERY key has been answered - a partial answer must leave + // it in NEEDCFG, which is the behaviour worth simulating. + unanswered_.insert(std::string(axis) + "." + k); + } + need_lines_.push_back(line); + } + configured_ = false; + LOG_INFO << "[mock] NEEDCFG: asking the host for config, refusing to move until answered"; + } void stop() override { LOG_INFO << "[mock] motor controller stopped"; } void sendCommand(const std::string& cmd) override { @@ -27,6 +61,21 @@ public: in >> verb; for (auto& c : verb) c = static_cast(std::toupper(static_cast(c))); + if (verb == "SET") { + // Record what the host answered, so a test can assert the handshake + // actually put the right values on the wire. + std::string axis, key, value; + in >> axis >> key >> value; + settings_[axis + "." + key] = value; + unanswered_.erase(axis + "." + key); + if (unanswered_.empty()) configured_ = true; + return; + } + if (needcfg_ && !configured_ && + (verb == "HOME" || verb == "MOVE" || verb == "ENABLE")) { + LOG_WARN << "[mock] ERR need config (" << verb << " rejected)"; + return; + } if (verb == "HOME") { homed_ = true; deviation_ = false; // homing clears the latch, as on the real driver @@ -131,9 +180,29 @@ public: } unsigned diagSeq() const override { return diag_seq_; } + std::vector takeNeedLines() override { + std::lock_guard lock(mutex_); + std::vector out; + out.swap(need_lines_); + return out; + } + + // Test/inspection hooks: what the host SET, and whether the axis is unlocked. + std::map settings() const { + std::lock_guard lock(mutex_); + return settings_; + } + bool configured() const { return configured_; } + bool connected() const override { return true; } private: + bool needcfg_ = false; + bool configured_ = false; + std::vector need_lines_; + std::map settings_; + std::set unanswered_; + static constexpr long kStep = 80000; // counts advanced per telemetry read void step(AxisTelemetry& a, long target) { @@ -156,7 +225,7 @@ private: } catch (const std::exception&) {} } - std::mutex mutex_; + mutable std::mutex mutex_; AxisTelemetry yaw_; AxisTelemetry pitch_; long yaw_target_ = 0; diff --git a/src/core/Application.cpp b/src/core/Application.cpp index 9b4fd0a..5c104f2 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -10,6 +10,7 @@ #include "fgc/Paths.h" #include "fgc/CameraProfile.h" #include "fgc/AxisSpeed.h" +#include "fgc/FirmwareConfig.h" #include "fgc/HardwarePresence.h" #include "fgc/TmcCurrent.h" #include "fgc/ICameraSource.h" @@ -304,7 +305,7 @@ struct Application::Impl { std::unique_ptr makeMotor() { bool mock = opts.mock_serial.value_or(cfg.features.mock_serial); - if (mock) return std::make_unique(); + if (mock) return std::make_unique(cfg.features.mock_needcfg); return std::make_unique(cfg.serial.device, cfg.serial.baud); } @@ -1177,6 +1178,102 @@ struct Application::Impl { one("pitch", 'P', cfg.geometry.pitch, cfg.drivetrain.pitch); } + // Build what the host can answer for one axis, for the config handshake. + // + // Speeds are converted here rather than declared as VMAX, for the reason in + // AxisSpeed.h. Homing and centring happen BEFORE the first home, so they + // cannot use the counts_per_deg that homing derives - they use the scale the + // declaration implies (steps_per_rev / travel_deg), which on the IOTA agrees + // with the measured one to 0.25%. + AxisConfigSource axisConfigSource(const char* axis, const AxisMap& map, + const AxisDrivetrainConfig& d, const TmcCurrentPlan& cur, + long soft_margin) { + AxisConfigSource s; + s.have_current = cur.ok; + s.global_scaler = cur.global_scaler; + s.run_irun = cur.run_irun; + s.homing_irun = cur.homing_irun; + + s.center_count = map.center_count; + s.soft_margin = soft_margin; + + s.have_travel = d.steps_per_rev > 0; + s.steps_per_rev = d.steps_per_rev; + s.have_deviation = d.enc_deviation > 0; + s.enc_deviation = d.enc_deviation; + + s.have_wiring = d.wiring_declared; + s.sw_mode = d.sw_mode; + s.has_encoder = d.has_encoder; + + // The scale to convert through. The firmware asks for all three speeds at + // BOOT, before anything has been homed, so the live calibration is often + // still AxisMap's uncalibrated default - fall back to the one the + // declaration implies. Homing supersedes it: sendProductionSpeeds() re-sends + // the production speed against the measured scale once it exists. + const double live = std::fabs(map.counts_per_deg); + const double cpd = live >= kMinPlausibleCountsPerDeg ? map.counts_per_deg + : d.preHomeCountsPerDeg(); + auto vmax = [&](double deg_s) -> long { + if (deg_s <= 0.0) return 0; // undeclared: firmware default stands + const auto p = planAxisSpeed(deg_s, cpd); + if (!p.ok) { + LOG_WARN << axis << " " << deg_s << " deg/s could not be converted: " << p.error; + return 0; + } + return p.vmax; + }; + s.normal_vmax = vmax(d.scan_speed_deg_s); + s.homing_vmax = vmax(d.homing_speed_deg_s); + s.center_vmax = vmax(d.center_speed_deg_s); + return s; + } + + // Answer any NEED lines the firmware has emitted. + // + // The firmware stays in NEEDCFG and moves nothing until its required set is + // complete, so an unanswered key is a startup failure rather than a warning: + // it is named here, loudly, because the fix is in hardware.ini and nothing + // else will make the gimbal move. + void answerFirmwareNeeds() { + for (const auto& line : motor->takeNeedLines()) { + const auto need = parseNeedLine(line); + if (!need) { + LOG_WARN << "unparseable NEED line from the firmware: " << line; + continue; + } + const bool is_yaw = need->axis == 'Y'; + const auto src = is_yaw + ? axisConfigSource("yaw", cfg.geometry.yaw, cfg.drivetrain.yaw, + yaw_current_, cfg.yaw_soft_margin_counts) + : axisConfigSource("pitch", cfg.geometry.pitch, + cfg.drivetrain.pitch, pitch_current_, + cfg.pitch_soft_margin_counts); + + const auto plan = planFirmwareConfig(*need, src); + for (const auto& cmd : plan.sets) motor->sendCommand(cmd); + + if (!plan.unanswered.empty()) { + std::string keys; + for (const auto& k : plan.unanswered) keys += (keys.empty() ? "" : ", ") + k; + LOG_ERROR << "firmware asked axis " << need->axis << " for " << keys + << " and hardware.ini does not declare " + << (plan.unanswered.size() == 1 ? "it" : "them") + << "; the axis stays in NEEDCFG and will not move"; + } + if (!plan.unknown.empty()) { + std::string keys; + for (const auto& k : plan.unknown) keys += (keys.empty() ? "" : ", ") + k; + LOG_ERROR << "firmware asked axis " << need->axis << " for " << keys + << ", which this host does not know -- the firmware is newer than the " + "host; upgrade the host rather than editing hardware.ini"; + } + if (plan.complete()) + LOG_INFO << "firmware config: answered " << plan.sets.size() << " keys for axis " + << need->axis; + } + } + // 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. @@ -2490,6 +2587,7 @@ struct Application::Impl { LOG_INFO << "Entering control loop (type 'exit' to quit, or Ctrl-C)"; while (running && !g_shutdown_requested.load()) { drainCommands(); + answerFirmwareNeeds(); // the firmware asks; hardware.ini answers pollBackgroundResults(); // apply calibration result / emit DIAG summary+log scheduler->tick(); checkTrackingError(); // warn on live encoder-error excursions diff --git a/src/core/Config.cpp b/src/core/Config.cpp index cac6106..c5c4795 100644 --- a/src/core/Config.cpp +++ b/src/core/Config.cpp @@ -350,6 +350,7 @@ AppConfig ConfigLoader::fromMap(const std::map& kv) { cfg.features.mock_imu = getBool(kv, "Features.mock_imu", cfg.features.mock_imu); cfg.features.enable_env = getBool(kv, "Features.enable_env", cfg.features.enable_env); cfg.features.mock_env = getBool(kv, "Features.mock_env", cfg.features.mock_env); + cfg.features.mock_needcfg = getBool(kv, "Features.mock_needcfg", cfg.features.mock_needcfg); cfg.features.strict_presence = getBool(kv, "Features.strict_presence", cfg.features.strict_presence); @@ -465,6 +466,13 @@ AppConfig ConfigLoader::fromMap(const std::map& kv) { d.homing_current_ma = getDouble(kv, pre + "homing_current_ma", d.run_current_ma); d.travel_deg = getDouble(kv, pre + "travel_deg", d.travel_deg); d.scan_speed_deg_s = getDouble(kv, pre + "scan_speed_deg_s", d.scan_speed_deg_s); + d.homing_speed_deg_s = getDouble(kv, pre + "homing_speed_deg_s", d.homing_speed_deg_s); + d.center_speed_deg_s = getDouble(kv, pre + "center_speed_deg_s", d.center_speed_deg_s); + d.steps_per_rev = getInt(kv, pre + "steps_per_rev", static_cast(d.steps_per_rev)); + d.enc_deviation = getInt(kv, pre + "enc_deviation", static_cast(d.enc_deviation)); + d.wiring_declared = kv.count(pre + "sw_mode") || kv.count(pre + "has_encoder"); + d.sw_mode = static_cast(getInt(kv, pre + "sw_mode", static_cast(d.sw_mode))); + d.has_encoder = getBool(kv, pre + "has_encoder", d.has_encoder); }; axisDrivetrain("yaw", cfg.drivetrain.yaw); axisDrivetrain("pitch", cfg.drivetrain.pitch); diff --git a/src/core/FirmwareConfig.cpp b/src/core/FirmwareConfig.cpp new file mode 100644 index 0000000..f61b6e0 --- /dev/null +++ b/src/core/FirmwareConfig.cpp @@ -0,0 +1,92 @@ +#include "fgc/FirmwareConfig.h" + +#include + +namespace fgc { +namespace { + +// One answerable key: where its value comes from, and whether the host has it. +struct Answer { + bool have = false; + long long value = 0; +}; + +Answer answerFor(const std::string& key, const AxisConfigSource& s) { + // Motor current. All three come from the same conversion, so they are + // available together or not at all. + if (key == "global_scaler") return {s.have_current, s.global_scaler}; + if (key == "run_irun") return {s.have_current, s.run_irun}; + if (key == "homing_irun") return {s.have_current, s.homing_irun}; + + // Position anchors. `center` / `margin` are the names the existing CENTER + // and MARGIN commands use. + if (key == "center_count" || key == "center") return {true, s.center_count}; + if (key == "soft_margin" || key == "margin") return {true, s.soft_margin}; + + if (key == "steps_per_rev") return {s.have_travel, s.steps_per_rev}; + if (key == "enc_deviation") return {s.have_deviation, s.enc_deviation}; + + // A VMAX of 0 is not a slow axis, it is a stopped one, so an unconverted + // speed is reported as missing rather than sent. + if (key == "normal_speed") return {s.normal_vmax > 0, s.normal_vmax}; + if (key == "homing_speed") return {s.homing_vmax > 0, s.homing_vmax}; + if (key == "center_speed") return {s.center_vmax > 0, s.center_vmax}; + + if (key == "sw_mode") return {s.have_wiring, static_cast(s.sw_mode)}; + if (key == "has_encoder") return {s.have_wiring, s.has_encoder ? 1 : 0}; + + return {}; // not in the vocabulary +} + +bool known(const std::string& key) { + static const char* kKeys[] = { + "global_scaler", "run_irun", "homing_irun", "center_count", "center", + "soft_margin", "margin", "steps_per_rev", "enc_deviation", + "normal_speed", "homing_speed", "center_speed", "sw_mode", "has_encoder", + }; + for (const char* k : kKeys) + if (key == k) return true; + return false; +} + +} // namespace + +std::optional parseNeedLine(const std::string& line) { + std::istringstream is(line); + std::string tok; + if (!(is >> tok) || tok != "NEED") return std::nullopt; + + std::string axis; + if (!(is >> axis)) return std::nullopt; + if (axis.size() != 1) return std::nullopt; + const char a = static_cast(std::toupper(static_cast(axis[0]))); + if (a != 'Y' && a != 'P') return std::nullopt; + + NeedRequest r; + r.axis = a; + while (is >> tok) r.keys.push_back(tok); + return r; +} + +FirmwareConfigPlan planFirmwareConfig(const NeedRequest& need, const AxisConfigSource& src) { + FirmwareConfigPlan plan; + const std::string axis(1, need.axis); + + for (const auto& key : need.keys) { + if (!known(key)) { + // The firmware wants something this host has never heard of, which + // means it is newer. Naming it is the whole diagnostic. + plan.unknown.push_back(key); + continue; + } + const Answer a = answerFor(key, src); + if (!a.have) { + plan.unanswered.push_back(key); + continue; + } + plan.sets.push_back("SET " + axis + " " + key + " " + std::to_string(a.value)); + } + return plan; +} + +} // namespace fgc diff --git a/src/serial/SerialMotorController.cpp b/src/serial/SerialMotorController.cpp index fb67eb2..8faa9bb 100644 --- a/src/serial/SerialMotorController.cpp +++ b/src/serial/SerialMotorController.cpp @@ -78,6 +78,9 @@ struct SerialMotorController::Impl { std::string latest_diag; std::atomic diag_seq{0}; + // NEED capture: single lines, queued for the control thread to answer once. + std::vector need_lines; + // Write one command line (newline-terminated) to the controller. Used by // sendCommand and by the internal DUMP re-request. // @@ -137,13 +140,24 @@ struct SerialMotorController::Impl { LOG_TRACE_CAT(LogCat::Serial) << "RX " << line; LOG_WARN << "Motor controller: " << line; } else if (!line.empty()) { - // OK acks and other async output (DG/DUMP/BOOT/...). + // OK acks and other async output (DG/DUMP/BOOT/NEED/...). LOG_TRACE_CAT(LogCat::Serial) << "RX " << line; captureDump(line); captureDiag(line); + captureNeed(line); } } + // Queue a NEED line for the control thread to answer. Unlike DUMP and DIAG + // this is not a block to assemble - each line stands alone and is actionable + // immediately, so it is queued rather than accumulated, and taking it clears + // it. A firmware that re-asks (after a RESET, say) simply queues again. + void captureNeed(const std::string& line) { + if (line.rfind("NEED ", 0) != 0) return; + std::lock_guard lock(mutex); + need_lines.push_back(line); + } + // Assemble the DIAG stream. DG lines arrive interleaved with ST over many // seconds, so we accumulate only "DG " lines and log each live; on "DG DONE" // we publish the block and bump diag_seq so the app can pick up the result. @@ -281,6 +295,13 @@ std::string SerialMotorController::lastDiag() { unsigned SerialMotorController::diagSeq() const { return impl_->diag_seq.load(); } +std::vector SerialMotorController::takeNeedLines() { + std::lock_guard lock(impl_->mutex); + std::vector out; + out.swap(impl_->need_lines); + return out; +} + bool SerialMotorController::connected() const { return impl_->connected; } } // namespace fgc diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 162026a..149f523 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -26,6 +26,7 @@ add_executable(fgc_tests test_hardwarepresence.cpp test_hardwarechecks.cpp test_axisspeed.cpp + test_firmwareconfig.cpp test_cameraprofile.cpp test_camerasettingsxml.cpp test_hash.cpp diff --git a/tests/test_firmwareconfig.cpp b/tests/test_firmwareconfig.cpp new file mode 100644 index 0000000..3ae4972 --- /dev/null +++ b/tests/test_firmwareconfig.cpp @@ -0,0 +1,249 @@ +#include "doctest/doctest.h" +#include "fgc/FirmwareConfig.h" + +#include + +using namespace fgc; + +namespace { + +// Tower 121's yaw, fully declared: the case the handshake exists to serve. +AxisConfigSource iotaYaw() { + AxisConfigSource s; + s.have_current = true; + s.global_scaler = 80; + s.run_irun = 31; + s.homing_irun = 7; + s.center_count = 500000; + s.soft_margin = 5000; + s.have_travel = true; + s.steps_per_rev = 1210778; + s.have_deviation = true; + s.enc_deviation = 1500; + s.normal_vmax = 39738; // 8 deg/s at 3552.87 counts/deg + s.homing_vmax = 30000; + s.center_vmax = 25000; + s.have_wiring = true; + s.sw_mode = 0; // yaw: no physical stops, StallGuard homing + s.has_encoder = true; + return s; +} + +bool sent(const FirmwareConfigPlan& p, const std::string& text) { + return std::find(p.sets.begin(), p.sets.end(), text) != p.sets.end(); +} + +} // namespace + +TEST_CASE("a NEED line parses into an axis and its keys") { + auto r = parseNeedLine("NEED Y steps_per_rev global_scaler center margin"); + REQUIRE(r); + CHECK(r->axis == 'Y'); + REQUIRE(r->keys.size() == 4); + CHECK(r->keys[0] == "steps_per_rev"); + CHECK(r->keys[3] == "margin"); + + auto p = parseNeedLine("NEED P sw_mode"); + REQUIRE(p); + CHECK(p->axis == 'P'); +} + +TEST_CASE("an axis that needs nothing is well-formed, not a parse failure") { + // How a firmware built for fewer axes than the rig has answers for the axis + // it does not drive. + auto r = parseNeedLine("NEED P"); + REQUIRE(r); + CHECK(r->axis == 'P'); + CHECK(r->keys.empty()); + CHECK(planFirmwareConfig(*r, iotaYaw()).complete()); +} + +TEST_CASE("anything that is not a NEED line is ignored") { + CHECK_FALSE(parseNeedLine("ST Y:R,0,0,00000000,0,0,0,S")); + CHECK_FALSE(parseNeedLine("NEEDCFG")); + CHECK_FALSE(parseNeedLine("NEED")); + CHECK_FALSE(parseNeedLine("NEED X steps_per_rev")); // not an axis + CHECK_FALSE(parseNeedLine("NEED YY steps_per_rev")); + CHECK_FALSE(parseNeedLine("")); +} + +TEST_CASE("a fully declared axis is answered completely, in the order asked") { + auto r = parseNeedLine( + "NEED Y steps_per_rev global_scaler sw_mode enc_deviation normal_speed homing_speed " + "center_speed center margin"); + REQUIRE(r); + auto p = planFirmwareConfig(*r, iotaYaw()); + + CHECK(p.complete()); + CHECK(p.unanswered.empty()); + CHECK(p.unknown.empty()); + REQUIRE(p.sets.size() == 9); + CHECK(p.sets.front() == "SET Y steps_per_rev 1210778"); + CHECK(sent(p, "SET Y global_scaler 80")); + CHECK(sent(p, "SET Y enc_deviation 1500")); + CHECK(sent(p, "SET Y normal_speed 39738")); + CHECK(p.sets.back() == "SET Y margin 5000"); +} + +TEST_CASE("center/margin are accepted under both their names") { + auto legacy = parseNeedLine("NEED Y center margin"); + auto full = parseNeedLine("NEED Y center_count soft_margin"); + REQUIRE(legacy); + REQUIRE(full); + CHECK(planFirmwareConfig(*legacy, iotaYaw()).sets[0] == "SET Y center 500000"); + CHECK(planFirmwareConfig(*full, iotaYaw()).sets[0] == "SET Y center_count 500000"); +} + +TEST_CASE("an undeclared current is unanswered, never sent as zero") { + // A silently zeroed current is a motor that cannot hold, so the key must go + // unanswered and leave the firmware in NEEDCFG. + auto s = iotaYaw(); + s.have_current = false; + auto r = parseNeedLine("NEED Y global_scaler run_irun homing_irun"); + REQUIRE(r); + auto p = planFirmwareConfig(*r, s); + + CHECK_FALSE(p.complete()); + CHECK(p.sets.empty()); + CHECK(p.unanswered.size() == 3); +} + +TEST_CASE("a speed that could not be converted is unanswered, not VMAX 0") { + // VMAX 0 is a stopped axis, not a slow one. AxisSpeed refuses an + // uncalibrated scale, and that refusal has to survive into the handshake. + auto s = iotaYaw(); + s.normal_vmax = 0; + auto r = parseNeedLine("NEED Y normal_speed homing_speed"); + REQUIRE(r); + auto p = planFirmwareConfig(*r, s); + + CHECK_FALSE(p.complete()); + REQUIRE(p.unanswered.size() == 1); + CHECK(p.unanswered[0] == "normal_speed"); + CHECK(sent(p, "SET Y homing_speed 30000")); +} + +TEST_CASE("a key this host has never heard of is a version skew, not a gap") { + // The two need different fixes - upgrade the host vs. fill in hardware.ini - + // so they are reported separately. + auto r = parseNeedLine("NEED Y steps_per_rev flux_capacitor"); + REQUIRE(r); + auto p = planFirmwareConfig(*r, iotaYaw()); + + CHECK_FALSE(p.complete()); + CHECK(p.unanswered.empty()); + REQUIRE(p.unknown.size() == 1); + CHECK(p.unknown[0] == "flux_capacitor"); + // The keys it DID understand are still answered. + CHECK(sent(p, "SET Y steps_per_rev 1210778")); +} + +TEST_CASE("has_encoder is sent as 1/0, and follows the wiring declaration") { + auto s = iotaYaw(); + s.has_encoder = false; + auto r = parseNeedLine("NEED Y has_encoder"); + REQUIRE(r); + CHECK(planFirmwareConfig(*r, s).sets[0] == "SET Y has_encoder 0"); + + s.have_wiring = false; + CHECK_FALSE(planFirmwareConfig(*r, s).complete()); +} + +TEST_CASE("the pitch axis is addressed as P") { + auto r = parseNeedLine("NEED P center margin"); + REQUIRE(r); + auto p = planFirmwareConfig(*r, iotaYaw()); + CHECK(p.sets[0] == "SET P center 500000"); + CHECK(p.sets[1] == "SET P margin 5000"); +} + +// --- End to end through the mock's NEEDCFG mode ----------------------------- + +#include "fgc/mock/MockMotorController.h" + +TEST_CASE("the mock in NEEDCFG asks, and refuses to move until answered") { + MockMotorController m(/*needcfg=*/true); + m.start(); + + auto lines = m.takeNeedLines(); + REQUIRE(lines.size() == 2); + CHECK(lines[0].rfind("NEED Y ", 0) == 0); + CHECK(lines[1].rfind("NEED P ", 0) == 0); + // Taking them clears the queue: they are a request, not a state to poll. + CHECK(m.takeNeedLines().empty()); + + // Before the answer, motion commands are rejected. + CHECK_FALSE(m.configured()); + m.sendCommand("HOME"); + CHECK_FALSE(m.telemetry().yaw.ready()); + + // Answer the way the host would. + AxisConfigSource src; + src.have_current = true; + src.global_scaler = 80; + src.center_count = 500000; + src.soft_margin = 5000; + src.have_travel = true; + src.steps_per_rev = 1210778; + src.have_deviation = true; + src.enc_deviation = 1500; + src.normal_vmax = 39738; + src.homing_vmax = 30000; + src.center_vmax = 25000; + src.have_wiring = true; + + // Answer BOTH axes, as the host does - the axis unlocks only when every key + // it asked for has been answered. + for (const auto& l : lines) { + auto n = parseNeedLine(l); + REQUIRE(n); + auto plan = planFirmwareConfig(*n, src); + REQUIRE(plan.complete()); + for (const auto& cmd : plan.sets) m.sendCommand(cmd); + } + + // The values landed, under the names the firmware asked for. + const auto got = m.settings(); + CHECK(got.at("Y.steps_per_rev") == "1210778"); + CHECK(got.at("Y.global_scaler") == "80"); + CHECK(got.at("Y.normal_speed") == "39738"); + CHECK(got.at("Y.center") == "500000"); + CHECK(got.at("Y.margin") == "5000"); + + // And the axis now moves. + CHECK(m.configured()); + m.sendCommand("HOME"); + CHECK(m.telemetry().yaw.ready()); +} + +TEST_CASE("the default mock is untouched by the handshake") { + // Every existing test constructs it this way; it must behave as before. + MockMotorController m; + m.start(); + CHECK(m.takeNeedLines().empty()); + m.sendCommand("HOME"); + CHECK(m.telemetry().yaw.ready()); +} + +TEST_CASE("a partial answer leaves the axis in NEEDCFG") { + // The case that matters: hardware.ini declares most of what the firmware + // wants, so most SETs land - and the axis still must not move. + MockMotorController m(/*needcfg=*/true); + m.start(); + auto lines = m.takeNeedLines(); + REQUIRE(lines.size() == 2); + + auto need = parseNeedLine(lines[0]); + REQUIRE(need); + AxisConfigSource partial; // only the always-known anchors + partial.center_count = 500000; + partial.soft_margin = 5000; + + auto plan = planFirmwareConfig(*need, partial); + CHECK_FALSE(plan.complete()); + for (const auto& cmd : plan.sets) m.sendCommand(cmd); + + CHECK_FALSE(m.configured()); + m.sendCommand("HOME"); + CHECK_FALSE(m.telemetry().yaw.ready()); +}