IMU configuration readback/control + reworked calibration procedure

.deploy.env

@@ -2,4 +2,4 @@
 REMOTE_HOST=ggs@10.11.12.111
 REMOTE_DIR=/home/ggs/projects/fwt_2a/software
 CMAKE_ARGS="-DWITH_MQTT=ON -DWITH_VIMBA=ON"
-RUN_ARGS="--init --start --trace serial"
+RUN_ARGS="--start --trace serial"

README.md

@@ -101,8 +101,9 @@ While running, the program reads commands from stdin (one per line):
 | `gimbal home [y\|p]` | Run the endstop-finding home sequence. |
 | `gimbal dump` | Request a firmware state dump (shown in the gimbal `g` view). |
 | `gimbal diag [y\|p\|all]` | Run the firmware motor self-test (results in the activity strip + `logs/`). |
-| `gimbal calib` | IMU-referenced steps↔degrees calibration (needs homed axes + IMU; applied to the session). |
+| `gimbal calib` | IMU-referenced steps↔degrees calibration (auto-homes if needed; needs the IMU; applied to the session). |
 | `gimbal …` | Other motor controls — `steps`/`nudge`/`stop`/`speed`/`reset`/`enable`/`disable`/`setpos`/`status`. |
+| `gimbal raw "<command>"` | **Low-level:** send the quoted text to the firmware verbatim (newline added). Bypasses wrappers/unit conversion — use with care. |
 | `debug` | Toggle debug-level logging on/off. |
 | `set fps <n>` | Capture rate, in images per second. |
 | `set camera fps <n>` | Camera sensor frame rate. |

deploy.sh

@@ -8,6 +8,7 @@
 # Usage:
 #   ./deploy.sh                 rsync + remote configure + build
 #   ./deploy.sh --run           ... then run the app over ssh (RUN_ARGS, ctrl-c to stop)
+#   ./deploy.sh --run --home    ... and run the endstop-finding home sequence at startup
 #   ./deploy.sh --clean         wipe the remote build dir first (fresh configure)
 #   ./deploy.sh --check-deps     only check the remote build dependencies
 #   ./deploy.sh --run --force    skip the placeholder-config guard before running
@@ -38,16 +39,24 @@ CLEAN=0
 RUN=0
 CHECK_DEPS=0
 FORCE=0
+HOME_INIT=0
 for arg in "$@"; do
     case "$arg" in
         --clean)      CLEAN=1 ;;
         --run|--start) RUN=1 ;;
+        --home|--init) HOME_INIT=1 ;;
         --check-deps) CHECK_DEPS=1 ;;
         --force)      FORCE=1 ;;
         *) echo "unknown option: $arg" >&2; exit 1 ;;
     esac
 done
 
+# Homing is opt-in: append --init only when --home is passed, so a plain run
+# brings up the TUI without driving the endstop-finding sequence.
+if [[ $HOME_INIT -eq 1 && "$RUN_ARGS" != *--init* ]]; then
+    RUN_ARGS="--init $RUN_ARGS"
+fi
+
 # Locate cmake on the remote. Non-interactive ssh shells don't source .bashrc,
 # so allow an explicit REMOTE_CMAKE override in .deploy.env; otherwise search
 # the usual locations.

docs/configuration.md

@@ -59,7 +59,39 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config
 
 The firmware reports only **encoder counts**; `[Motor]` maps them to the heading/elevation degrees
 used by MQTT (`target_HDG`) and `CamEvent`. Calibrate `*_counts_per_deg` / `*_zero_count` against
-real `xenc` readings after homing (`MOVE` a known angle, read the resulting `xenc`).
+real `xenc` readings after homing (`MOVE` a known angle, read the resulting `xenc`), **or** run the
+automated IMU-referenced `gimbal calib` (below).
+
+#### `gimbal calib` — automated IMU-referenced calibration
+
+`gimbal calib` (requires the IMU) fits each axis's `counts_per_deg` / `zero_count` by sweeping it and
+correlating encoder counts with the MTi's measured angle. The sequence is deliberately ordered so the
+**yaw fit is not spoiled by the IMU's heading drift** (see the no-magnetometer discussion below):
+
+1. **Home if needed** — if the gimbal is not already `READY`, it runs the endstop-finding home first.
+2. **Pitch at the first yaw position** — yaw moves to the start of its travel and holds there while
+   pitch is swept across its soft-limit travel; at each step it dwells and records the
+   gravity-referenced IMU **pitch** (stable, absolute), then least-squares fits pitch.
+3. **Pitch → 0°** — using the just-fitted pitch map.
+4. **Switch to a no-magnetometer XKF profile** — picked from the device's own available-profiles list
+   (a `*nomag*` profile, else a `VRU` one), so the IMU's heading stops chasing the stepper-distorted
+   magnetic field. Sent in the MTi's Config state, so it **persists on the device** across power-cycles.
+   Best-effort: if the device reports no magnetometer-free profile, this step is skipped (logged) and
+   calibration continues.
+5. **Drift-correct + zero the heading** — holding the gimbal still, it runs the MTi **no-rotation**
+   gyro-bias update (cuts yaw drift), then a **heading reset** so the current pose becomes yaw 0. This
+   happens right before the yaw sweep, so any drift accrued during the slow pitch sweep is discarded.
+6. **Yaw sweep** — yaw is swept from that first (now zero-heading) position and fitted.
+
+After calibration the IMU is left on the no-mag profile (this is intentional — the gimbal's homed
+encoders are the absolute heading reference; the IMU only needs stable roll/pitch and short-term
+yaw-rate). The `i` view's **XKF profile** row will show the new selection (press `r` to refresh if you
+changed it outside calibration).
+
+Each axis fit reports an R² (shown in the gimbal `g` view + activity strip). The result is applied to
+the live session immediately; in the TUI the activity strip then offers to save it to `[Motor]` as the
+new default (`y`/`n`). `gimbal stop` cancels a run. Timings (dwell, no-rotation duration, etc.) are the
+`CalibParams` defaults in [CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h).
 
 The capture **scan grid** is the ordered `(yaw,pitch)` waypoints auto-sweep visits (ping-pong). Set
 `[Scan] grid_file` to an editable CSV ([config/scan.csv](../config/scan.csv)) to define exact
@@ -91,6 +123,10 @@ called a "scenario" in the device manual (e.g. `Machine_nomagfield`); it is the
 the wire MIDs. Older MTi firmware that does not answer the `Req*` queries simply leaves the section
 absent (the host logs `no configuration acks received`).
 
+The config is read **once at startup** and cached, so if you change the XKF profile externally the view
+stays stale until you **refresh** it: press `r` (or type `refresh`). That re-queries the device (briefly
+pausing the stream) and also requests a fresh firmware dump for the gimbal `g` view.
+
 ### Secrets
 
 `mqtt_user` / `mqtt_pw` are read from the environment variables **`FGC_MQTT_USER` / `FGC_MQTT_PW`** first,
@@ -187,8 +223,9 @@ restarts; press **`n`** to keep it for this session only. The keys are active on
 showing. (Until you answer `y`, calibration remains session-only — see
 [known-issues.md](known-issues.md).)
 
-Keys (bottom bar): `s` start · `x` stop · `h` home · `r` reset · `g` gimbal view · `i` IMU view ·
+Keys (bottom bar): `s` start · `x` stop · `h` home · `g` gimbal view · `i` IMU view · `r` refresh
-arrow keys nudge the gimbal (yaw ±5 % / pitch ±10 %) · `:` open a command line (any console/`gimbal …`
+(re-read IMU config + firmware dump) · arrow keys nudge the gimbal (yaw ±5 % / pitch ±10 %) · `:` open
+a command line (any console/`gimbal …`
 command) · `?` help · `q` quit. Plain letters are used rather than Ctrl chords so terminal
 flow-control (`Ctrl-S`/`Ctrl-Q` XON/XOFF) can't swallow them. In TUI mode all log output is diverted
 from stdout into the on-screen log pane via a `Logger` sink, so the screen is never corrupted.

docs/modules-reference.md

@@ -17,8 +17,8 @@ Per-file reference for the refactored tree, plus the shared data structures.
 | [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 |
-| [include/fgc/CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h), [src/core/CalibrationRoutine.cpp](../src/core/CalibrationRoutine.cpp) | `gimbal calib` worker thread: sweeps each axis, dwells reading the IMU, fits degrees↔counts; exposes `progress()`/`report()`/`takeResult()` |
+| [include/fgc/CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h), [src/core/CalibrationRoutine.cpp](../src/core/CalibrationRoutine.cpp) | `gimbal calib` worker thread: home-if-needed → pitch sweep at the first yaw position → pitch 0° → switch IMU to a no-mag XKF profile → IMU no-rotation + heading reset → yaw sweep, fitting degrees↔counts; tunable via `CalibParams`; exposes `progress()`/`report()`/`takeResult()` |
-| [include/fgc/MtiProtocol.h](../include/fgc/MtiProtocol.h), [src/core/MtiProtocol.cpp](../src/core/MtiProtocol.cpp) | Xsens MTi binary protocol: `MtiFramer` (checksum framing), config-message builders, `parseMTData` → `ImuSample` (temp/acc/gyr/mag/euler), config-readback query builders + `applyImuConfigAck`/`finalizeImuConfig` → `ImuDeviceConfig` (product/firmware/device-id/output mode+settings/sample rate/**XKF scenario**) |
+| [include/fgc/MtiProtocol.h](../include/fgc/MtiProtocol.h), [src/core/MtiProtocol.cpp](../src/core/MtiProtocol.cpp) | Xsens MTi binary protocol: `MtiFramer` (checksum framing), config-message builders, `parseMTData` → `ImuSample` (temp/acc/gyr/mag/euler), config-readback query builders + `applyImuConfigAck`/`finalizeImuConfig` → `ImuDeviceConfig` (product/firmware/device-id/output mode+settings/sample rate/**XKF scenario**); orientation-control builders `msgSetNoRotation` (gyro-bias update) + `msgResetOrientation` (heading reset / store) + `msgSetFilterProfile` (select XKF profile) + `pickNoMagProfile` (choose a magnetometer-free profile from the available list) |
 | [include/fgc/CaptureScheduler.h](../include/fgc/CaptureScheduler.h), [src/core/CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp) | Capture state machine over the interfaces; injectable clock; `setGeometry` adopts a recalibration |
 | [include/fgc/Application.h](../include/fgc/Application.h), [src/core/Application.cpp](../src/core/Application.cpp) | Factory (real vs mock, headless vs TUI), wiring, control loop, `gimbal …` commands, background-result polling, `buildSnapshot()` |
 | [include/fgc/ui/UiSnapshot.h](../include/fgc/ui/UiSnapshot.h), [src/ui/UiSnapshot.cpp](../src/ui/UiSnapshot.cpp) | Plain-data view model (incl. `ImuView`, `ActivityView`, `CalibResultView`, `DumpView`) + pure formatting helpers (state label/colour, degrees, time-ago, pending-sensors) |
@@ -32,7 +32,7 @@ Per-file reference for the refactored tree, plus the shared data structures.
 | [include/fgc/IMotorController.h](../include/fgc/IMotorController.h) | `IMotorController` | `MotorTelemetry` |
 | [include/fgc/IControlChannel.h](../include/fgc/IControlChannel.h) | `IControlChannel` | `ControlCommand`, `CamEvent` |
 | [include/fgc/ICameraSource.h](../include/fgc/ICameraSource.h) | `ICameraSource` | `Frame` |
-| [include/fgc/IImuSource.h](../include/fgc/IImuSource.h) | `IImuSource` | `ImuSample` (from `MtiProtocol.h`) |
+| [include/fgc/IImuSource.h](../include/fgc/IImuSource.h) | `IImuSource` (incl. `config()`, `refreshConfig()`, `noRotation()`, `headingReset()`, `setFilterProfile()`) | `ImuSample` (from `MtiProtocol.h`) |
 | [include/fgc/ui/IUserInterface.h](../include/fgc/ui/IUserInterface.h) | `IUserInterface` | `UiSnapshot` |
 
 ## Real implementations (SDK-gated)

include/fgc/CalibrationRoutine.h

@@ -39,16 +39,38 @@ struct CalibReport {
     std::vector<Axis> axes;
 };
 
+// Tunable timings/sizes for a calibration run. Defaults are the production
+// values; tests inject tiny ones so a full run completes in milliseconds.
+struct CalibParams {
+    int    positions         = 10;     // sweep steps per axis
+    int    dwell_ms          = 5000;   // hold at each position while sampling
+    int    sample_ms         = 100;    // IMU sampling cadence during a dwell
+    int    settle_timeout_ms = 15000;  // max wait for a single MOVE to settle
+    int    home_timeout_ms   = 90000;  // max wait for HOME to reach READY
+    int    norotation_s      = 3;      // no-rotation gyro-bias update duration
+    int    reset_settle_ms   = 1500;   // let the filter apply the heading reset
+    double inset_frac        = 0.05;   // keep targets off the hard endstops
+};
+
 // `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own
-// thread (the motor/imu/Logger interfaces are thread-safe), sweeping each axis
+// thread (the motor/imu/Logger interfaces are thread-safe). Sequence:
-// across its homed soft-limit travel in equal step intervals, dwelling at each to
+//   1. home the gimbal if it is not already READY;
-// record the IMU orientation, then least-squares fitting counts vs degrees. The
+//   2. move yaw to its first sweep position and calibrate PITCH there (sweep its
-// resulting Geometry is published via takeResult() for the main thread to apply
+//      soft-limit travel, dwelling to record the gravity-referenced IMU pitch);
-// to the live session; the raw samples + fit are written to a logfile. Progress
+//   3. move pitch to 0 deg;
-// is streamed to the LOG pane via LOG_INFO. Cancellable and one-at-a-time.
+//   4. switch the IMU to a no-magnetometer XKF profile (persists on the device) so
+//      its heading stops chasing the stepper-distorted magnetic field;
+//   5. with the gimbal held still, run the IMU no-rotation update (cuts yaw drift)
+//      then reset the IMU heading so the current pose is yaw 0 — this removes the
+//      drift accumulated during the slow pitch sweep right before it matters;
+//   6. sweep YAW from that first position and calibrate it.
+// Each axis fit is least-squares (counts vs degrees). The resulting Geometry is
+// published via takeResult() for the main thread to apply; raw samples + fits are
+// written to a logfile. Progress streams to the LOG pane. Cancellable, one-at-a-time.
 class CalibrationRoutine {
 public:
-    CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial);
+    CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
+                       CalibParams params = {});
     ~CalibrationRoutine();
 
     // Begin on a worker thread. Logs a reason and returns false if already
@@ -71,6 +93,7 @@ private:
     IMotorController& motor_;
     IImuSource&       imu_;
     Geometry          initial_;
+    CalibParams       params_;
 
     std::thread       thread_;
     std::atomic<bool> running_{false};

include/fgc/IImuSource.h

@@ -26,6 +26,27 @@ public:
     // sample rate, identity, XKF scenario). nullopt if not yet known or the
     // backend cannot report it.
     virtual std::optional<ImuDeviceConfig> config() const { return std::nullopt; }
+
+    // Re-read the device configuration (e.g. after the XKF profile was changed
+    // externally). May briefly pause the measurement stream. Blocking; after it
+    // returns config() reflects the device's current settings.
+    virtual void refreshConfig() {}
+
+    // Run the "no rotation" gyro-bias update for `seconds`: the device must be
+    // held perfectly still while it estimates and cancels gyro bias, which cuts
+    // heading drift (important in no-magnetometer use). Non-blocking: it sends the
+    // command; the caller is responsible for keeping the unit still that long.
+    virtual void noRotation(int seconds) { (void)seconds; }
+
+    // Redefine the *current* heading as yaw = 0 (bore-sighting / heading reset).
+    // Subsequent yaw is measured relative to this pose. Non-blocking.
+    virtual void headingReset() {}
+
+    // Select the XKF profile/scenario by its `type` number (from config()'s
+    // available_profiles). Applied in the device's Config state, so it persists to
+    // non-volatile memory. Briefly pauses the stream; blocking. Returns true if the
+    // device reports the new profile afterwards. Default: not supported.
+    virtual bool setFilterProfile(int type) { (void)type; return false; }
 };
 
 }  // namespace fgc

include/fgc/MtiImuSource.h

@@ -21,6 +21,10 @@ public:
     bool                     connected() const override;
     std::optional<ImuSample> sample() override;
     std::optional<ImuDeviceConfig> config() const override;
+    void                     refreshConfig() override;
+    bool                     setFilterProfile(int type) override;
+    void                     noRotation(int seconds) override;
+    void                     headingReset() override;
 
 private:
     struct Impl;

include/fgc/MtiProtocol.h

@@ -48,6 +48,15 @@ inline constexpr uint8_t kMidAvailFilterProf       = 0x63;
 inline constexpr uint8_t kMidReqFilterProfile      = 0x64;  // SetScenario shares this MID
 inline constexpr uint8_t kMidReqFilterProfileAck   = 0x65;
 
+// Orientation-control commands (valid in Measurement State).
+inline constexpr uint8_t kMidSetNoRotation    = 0x22;  // 2-byte duration (seconds)
+inline constexpr uint8_t kMidSetNoRotationAck = 0x23;
+inline constexpr uint8_t kMidResetOrientation    = 0xA4;  // 2-byte CODE (Table 33)
+inline constexpr uint8_t kMidResetOrientationAck = 0xA5;
+// ResetOrientation CODE values we use.
+inline constexpr uint16_t kResetHeading = 0x0001;  // current heading becomes yaw 0
+inline constexpr uint16_t kResetStore   = 0x0000;  // persist current reset (Config state)
+
 // OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04).
 inline constexpr uint16_t kOutputMode = 0x0007;
 // OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp
@@ -116,6 +125,24 @@ struct ImuDeviceConfig {
     std::vector<ImuFilterProfile> available_profiles;
 };
 
+// Orientation-control command builders.
+// SetNoRotation: run the "no rotation" gyro-bias update for `seconds` (the device
+// must be held still during it) to cut heading drift, esp. in no-magnetometer use.
+std::vector<uint8_t> msgSetNoRotation(uint16_t seconds);
+// ResetOrientation: CODE 0x0001 redefines the *current* heading as yaw 0
+// (bore-sighting); CODE 0x0000 (in Config state) stores it across power cycles.
+std::vector<uint8_t> msgResetOrientation(uint16_t code);
+
+// SetFilterProfile (classic): select the XKF profile/scenario by type number.
+// Valid in Config state; settings changed in Config state persist to the device's
+// non-volatile memory automatically.
+std::vector<uint8_t> msgSetFilterProfile(uint16_t profile);
+
+// Pick the no-magnetometer XKF profile from the device's reported list: prefers a
+// label containing "nomag"/"no_mag", else a "VRU" (gyro-tracked heading) profile.
+// Returns the profile `type` number, or -1 if none looks magnetometer-free.
+int pickNoMagProfile(const std::vector<ImuFilterProfile>& profiles);
+
 // Config-readback query builders (empty data field => "request", not "set").
 std::vector<uint8_t> msgReqProductCode();
 std::vector<uint8_t> msgReqDID();

include/fgc/mock/MockImuSource.h

@@ -44,8 +44,23 @@ public:
         return s;
     }
 
+    void refreshConfig() override { LOG_INFO << "[mock] IMU config refreshed"; }
+    void noRotation(int seconds) override {
+        LOG_INFO << "[mock] IMU no-rotation update for " << seconds << " s";
+    }
+    void headingReset() override {
+        LOG_INFO << "[mock] IMU heading reset (current direction is now yaw 0)";
+    }
+    bool setFilterProfile(int type) override {
+        scenario_type_ = type;  // observable in the next config()
+        LOG_INFO << "[mock] IMU XKF profile set to type " << type;
+        return true;
+    }
+
     // Synthetic config mirroring the real handshake (Euler + calibrated, 100 Hz),
-    // so the expanded view's IMU CONFIG section renders without hardware.
+    // so the expanded view's IMU CONFIG section renders without hardware. The
+    // available list includes a no-mag profile so the calibration's profile switch
+    // has something to pick.
     std::optional<ImuDeviceConfig> config() const override {
         ImuDeviceConfig c;
         c.valid = true;
@@ -64,18 +79,20 @@ public:
         c.has_period = true;
         c.period = 1152;  // 100 Hz
         c.sample_rate_hz = 100.0f;
-        c.has_scenario = true;
-        c.scenario_type = 39;
-        c.scenario_version = 11;
         c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
-                                {41, 11, "Dynamic"}};
+                                {41, 11, "Dynamic"}, {53, 11, "VRU_general"}};
-        c.scenario_label = "General";
+        c.has_scenario = true;
+        c.scenario_type = static_cast<uint8_t>(scenario_type_);
+        c.scenario_version = 11;
+        for (const auto& p : c.available_profiles)
+            if (p.type == c.scenario_type) c.scenario_label = p.label;
         return c;
     }
 
 private:
     using clock = std::chrono::steady_clock;
     clock::time_point start_ = clock::now();
+    int               scenario_type_ = 39;  // current XKF profile (mutable via setFilterProfile)
 };
 
 }  // namespace fgc

src/core/Application.cpp

@@ -550,7 +550,7 @@ struct Application::Impl {
         while (iss >> t) tok.push_back(t);  // tok[0] == "gimbal"
         if (tok.size() < 2) {
             LOG_WARN << "usage: gimbal <move|steps|nudge|home|stop|reset|enable|disable|"
-                        "speed|setpos|status|dump|diag|calib>";
+                        "speed|setpos|status|dump|diag|calib|raw>";
             return;
         }
         std::string sub = tok[1];
@@ -603,6 +603,25 @@ struct Application::Impl {
         } else if (sub == "dump") {
             LOG_INFO << "requesting firmware dump...";
             motor->sendCommand("DUMP");
+        } else if (sub == "raw") {
+            // Low-level escape hatch: send the text inside the quotation marks to
+            // the serial port verbatim (the transport appends the newline). This
+            // exposes any firmware verb directly, including ones with no wrapper.
+            std::string raw;
+            auto q1 = line.find('"');
+            auto q2 = line.rfind('"');
+            if (q1 != std::string::npos && q2 != std::string::npos && q2 > q1) {
+                raw = line.substr(q1 + 1, q2 - q1 - 1);  // exact text between quotes
+            } else {
+                for (size_t i = 2; i < tok.size(); ++i)  // unquoted: join remaining tokens
+                    raw += (raw.empty() ? "" : " ") + tok[i];
+            }
+            if (raw.empty()) {
+                LOG_WARN << "usage: gimbal raw \"<verbatim firmware command>\"";
+                return;
+            }
+            LOG_INFO << "gimbal raw -> " << raw;
+            motor->sendCommand(raw);
         } else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" ||
                    sub == "disable" || sub == "speed" || sub == "setpos" || sub == "status") {
             // Passthrough to the (case-insensitive) firmware verb + args.
@@ -730,6 +749,16 @@ struct Application::Impl {
             bool on = Logger::level() != LogLevel::Debug;
             Logger::setLevel(on ? LogLevel::Debug : LogLevel::Info);
             LOG_INFO << "debug logging " << (on ? "on" : "off");
+        } else if (c.verb == "refresh") {
+            // Re-read live device state surfaced in the expanded views: the IMU
+            // configuration (e.g. after the XKF profile was changed externally)
+            // and the firmware register dump shown in the gimbal 'g' view.
+            LOG_INFO << "refresh: re-reading IMU configuration + firmware dump";
+            if (imu) {
+                imu->refreshConfig();
+                imu_config_done_ = false;  // force buildSnapshot to re-read config()
+            }
+            motor->sendCommand("DUMP");
         } else if (c.verb == "trace") {
             handleTrace(c);
         } else if (c.verb == "set") {

src/core/CalibrationRoutine.cpp

@@ -19,17 +19,13 @@ namespace fgc {
 
 namespace {
 using clock = std::chrono::steady_clock;
-constexpr int    kPositions       = 10;
-constexpr int    kDwellMs         = 5000;   // hold at each position
-constexpr int    kSampleMs        = 100;    // IMU sampling cadence during dwell
-constexpr int    kSettleTimeoutMs = 15000;  // max wait for a move to settle
-constexpr double kInsetFrac       = 0.05;   // keep targets off the hard endstops
 
 void msleep(int ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); }
 }  // namespace
 
-CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial)
+CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
-    : motor_(motor), imu_(imu), initial_(std::move(initial)) {}
+                                       CalibParams params)
+    : motor_(motor), imu_(imu), initial_(std::move(initial)), params_(params) {}
 
 CalibrationRoutine::~CalibrationRoutine() {
     cancel();
@@ -86,9 +82,9 @@ long long nowMs() {
 namespace {
 
 // Wait until the axis is at `target` (standstill + within tol), or timeout/cancel.
-bool waitSettle(IMotorController& motor, char axis, long target, long tol,
+bool waitSettle(IMotorController& motor, char axis, long target, long tol, int timeout_ms,
                 const std::atomic<bool>& cancel) {
-    const auto deadline = clock::now() + std::chrono::milliseconds(kSettleTimeoutMs);
+    const auto deadline = clock::now() + std::chrono::milliseconds(timeout_ms);
     while (clock::now() < deadline) {
         if (cancel) return false;
         MotorTelemetry t = motor.telemetry();
@@ -99,16 +95,29 @@ bool waitSettle(IMotorController& motor, char axis, long target, long tol,
     return false;
 }
 
-// Dwell `kDwellMs` sampling the IMU; return the (circular for yaw) mean of the
+// Wait until both present axes report READY (homing complete), or timeout/cancel.
+bool waitHomed(IMotorController& motor, int timeout_ms, const std::atomic<bool>& cancel) {
+    const auto deadline = clock::now() + std::chrono::milliseconds(timeout_ms);
+    while (clock::now() < deadline) {
+        if (cancel) return false;
+        MotorTelemetry t = motor.telemetry();
+        if (t.yaw.ready() && (!t.pitch_present || t.pitch.ready())) return true;
+        msleep(200);
+    }
+    return false;
+}
+
+// Dwell `dwell_ms` sampling the IMU; return the (circular for yaw) mean of the
 // requested Euler angle in degrees.
-double dwellAndMeasure(IImuSource& imu, char axis, const std::atomic<bool>& cancel) {
+double dwellAndMeasure(IImuSource& imu, char axis, int dwell_ms, int sample_ms,
+                       const std::atomic<bool>& cancel) {
     std::vector<double> vals;
-    const auto end = clock::now() + std::chrono::milliseconds(kDwellMs);
+    const auto end = clock::now() + std::chrono::milliseconds(dwell_ms);
     while (clock::now() < end) {
         if (cancel) break;
         if (auto s = imu.sample())
             vals.push_back(axis == 'Y' ? s->yaw_deg : s->pitch_deg);
-        msleep(kSampleMs);
+        msleep(sample_ms);
     }
     if (vals.empty()) return 0.0;
     if (axis == 'Y') {
@@ -133,8 +142,9 @@ void CalibrationRoutine::run() {
         }
     } done{this};
 
+    const int N = params_.positions;
     LOG_INFO << "=== gimbal calibration starting ===";
-    setProgress('?', 0, kPositions, "starting");
+    setProgress('?', 0, N, "starting");
     CalibReport report;
 
     // 1. Preconditions.
@@ -142,11 +152,22 @@ void CalibrationRoutine::run() {
         LOG_WARN << "calibration aborted: IMU not connected";
         return;
     }
+
+    // 2. Home first if the gimbal is not already READY.
     MotorTelemetry t0 = motor_.telemetry();
-    if (!t0.yaw.ready() || (t0.pitch_present && !t0.pitch.ready())) {
+    bool homed = t0.yaw.ready() && (!t0.pitch_present || t0.pitch.ready());
-        LOG_WARN << "calibration aborted: axes must be homed (READY) first";
+    if (!homed) {
+        LOG_INFO << "calibration: gimbal not homed; homing first...";
+        setProgress('?', 0, N, "homing");
+        motor_.sendCommand("HOME");
+        if (!waitHomed(motor_, params_.home_timeout_ms, cancel_)) {
+            if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+            LOG_WARN << "calibration aborted: homing did not reach READY";
             return;
         }
+        t0 = motor_.telemetry();
+    }
+    const bool pitch_present = t0.pitch_present;
 
     // Fetch soft limits from a fresh dump.
     LOG_INFO << "calibration: requesting soft limits (dump)...";
@@ -166,73 +187,143 @@ void CalibrationRoutine::run() {
     std::ostringstream log;
     log << "gimbal calibration\n==================\n";
 
-    struct AxisJob { char axis; AxisMap* map; };
+    // Per-axis sweep span [a..b] (inset off the hard endstops) + a settle tolerance.
-    std::vector<AxisJob> jobs = {{'Y', &result.yaw}};
+    auto axisSpan = [&](char axis, long& a, long& b, long& tol) -> bool {
-    if (t0.pitch_present) jobs.push_back({'P', &result.pitch});
-
-    for (const auto& job : jobs) {
-        if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
         const DumpAxis* da = nullptr;
-        for (const auto& ax : dump.axes) if (ax.axis == job.axis) da = &ax;
+        for (const auto& ax : dump.axes) if (ax.axis == axis) da = &ax;
-        if (!da) { LOG_WARN << "calibration: no limits for axis " << job.axis << "; skipping"; continue; }
+        if (!da) return false;
-
         const long lo = da->lim_neg, hi = da->lim_pos;
         const long travel = std::labs(hi - lo);
-        const long inset = static_cast<long>(kInsetFrac * travel);
+        const long inset = static_cast<long>(params_.inset_frac * travel);
-        const long a = lo + inset, b = hi - inset;
+        a = lo + inset;
-        const long tol = std::max<long>(500, travel / 200);
+        b = hi - inset;
+        tol = std::max<long>(500, travel / 200);
+        return true;
+    };
 
-        LOG_INFO << "calibrating " << (job.axis == 'Y' ? "YAW" : "PITCH")
+    // Sweep one axis across [a..b] in N steps, dwelling to measure the IMU angle,
-                 << " over [" << a << ".." << b << "] counts in " << kPositions << " steps";
+    // then least-squares fit counts vs degrees and record the result. Returns
-        log << "\n[" << job.axis << "] travel=[" << lo << ".." << hi << "] tol=" << tol
+    // false on cancel; on a degenerate fit it records a failed axis and returns true.
+    auto sweepAxis = [&](char axis, AxisMap& map, long a, long b, long tol) -> bool {
+        LOG_INFO << "calibrating " << (axis == 'Y' ? "YAW" : "PITCH")
+                 << " over [" << a << ".." << b << "] counts in " << N << " steps";
+        log << "\n[" << axis << "] sweep=[" << a << ".." << b << "] tol=" << tol
             << "\n target_counts, imu_deg\n";
 
         std::vector<std::pair<double, double>> pts;  // (deg, counts)
         double prev_deg = 0.0;
         bool   have_prev = false;
-        for (int i = 0; i < kPositions; ++i) {
+        for (int i = 0; i < N; ++i) {
-            if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+            if (cancel_) { LOG_WARN << "calibration cancelled"; return false; }
-            long target = a + (b - a) * i / (kPositions - 1);
+            long target = a + (b - a) * i / (N - 1);
-            setProgress(job.axis, i + 1, kPositions, "moving");
+            setProgress(axis, i + 1, N, "moving");
-            motor_.sendCommand(std::string("MOVE ") + job.axis + " " + std::to_string(target));
+            motor_.sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
-            if (!waitSettle(motor_, job.axis, target, tol, cancel_)) {
+            if (!waitSettle(motor_, axis, target, tol, params_.settle_timeout_ms, cancel_)) {
-                if (cancel_) return;
+                if (cancel_) return false;
-                LOG_WARN << "  position " << (i + 1) << "/" << kPositions
+                LOG_WARN << "  position " << (i + 1) << "/" << N
                          << " did not settle near " << target << " (continuing)";
             }
-            setProgress(job.axis, i + 1, kPositions, "dwelling");
+            setProgress(axis, i + 1, N, "dwelling");
-            double deg = dwellAndMeasure(imu_, job.axis, cancel_);
+            double deg = dwellAndMeasure(imu_, axis, params_.dwell_ms, params_.sample_ms, cancel_);
             // Unwrap against the previous sample so a sweep crossing 0/360 stays a
             // continuous line for the fit (no ±360 jump).
             if (have_prev) deg = unwrapNear(prev_deg, deg);
             prev_deg = deg;
             have_prev = true;
             pts.emplace_back(deg, static_cast<double>(target));
-            LOG_INFO << "  " << (i + 1) << "/" << kPositions << "  counts=" << target
+            LOG_INFO << "  " << (i + 1) << "/" << N << "  counts=" << target
                      << "  imu=" << deg << " deg";
             log << "  " << target << ", " << deg << "\n";
         }
 
-        setProgress(job.axis, kPositions, kPositions, "fitting");
+        setProgress(axis, N, N, "fitting");
         LinearFit fit = linearFit(pts);
         if (!fit.ok) {
-            LOG_WARN << "  fit failed for axis " << job.axis << " (degenerate data)";
+            LOG_WARN << "  fit failed for axis " << axis << " (degenerate data)";
             log << "  FIT FAILED\n";
-            report.axes.push_back({job.axis, false, 0, 0, 0, static_cast<int>(pts.size())});
+            report.axes.push_back({axis, false, 0, 0, 0, static_cast<int>(pts.size())});
-            continue;
+            return true;
         }
-        const double old_cpd = job.map->counts_per_deg;
+        const double old_cpd = map.counts_per_deg;
-        const long   old_zc  = job.map->zero_count;
+        const long   old_zc  = map.zero_count;
-        job.map->counts_per_deg = fit.slope;
+        map.counts_per_deg = fit.slope;
-        job.map->zero_count     = static_cast<long>(std::lround(fit.intercept));
+        map.zero_count     = static_cast<long>(std::lround(fit.intercept));
         LOG_INFO << "  fit: counts_per_deg " << old_cpd << " -> " << fit.slope
-                 << ", zero_count " << old_zc << " -> " << job.map->zero_count
+                 << ", zero_count " << old_zc << " -> " << map.zero_count
                  << "  (R^2=" << fit.r2 << ")";
-        log << "  fit: counts_per_deg=" << fit.slope << " zero_count=" << job.map->zero_count
+        log << "  fit: counts_per_deg=" << fit.slope << " zero_count=" << map.zero_count
             << " R2=" << fit.r2 << " (was cpd=" << old_cpd << " zc=" << old_zc << ")\n";
-        report.axes.push_back({job.axis, true, fit.slope, job.map->zero_count, fit.r2, fit.n});
+        report.axes.push_back({axis, true, fit.slope, map.zero_count, fit.r2, fit.n});
+        return true;
+    };
+
+    // Resolve the yaw sweep span up front — we move to its first position before
+    // the pitch sweep so the whole pitch calibration happens at that fixed yaw.
+    long ya = 0, yb = 0, ytol = 0;
+    if (!axisSpan('Y', ya, yb, ytol)) {
+        LOG_WARN << "calibration aborted: no yaw soft limits in dump";
+        return;
     }
 
+    // 3. Move yaw to its first sweep position and hold it there.
+    LOG_INFO << "calibration: moving yaw to first position (counts=" << ya << ")";
+    setProgress('Y', 1, N, "moving");
+    motor_.sendCommand(std::string("MOVE Y ") + std::to_string(ya));
+    waitSettle(motor_, 'Y', ya, ytol, params_.settle_timeout_ms, cancel_);
+    if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+
+    // 4. Calibrate PITCH at this yaw position, then 5. move pitch to 0 deg.
+    if (pitch_present) {
+        long pa = 0, pb = 0, ptol = 0;
+        if (axisSpan('P', pa, pb, ptol)) {
+            if (!sweepAxis('P', result.pitch, pa, pb, ptol)) { return; }  // cancelled
+            const long pzero = result.pitch.toCounts(0.0);
+            LOG_INFO << "calibration: moving pitch to 0 deg (counts=" << pzero << ")";
+            setProgress('P', N, N, "pitch->0");
+            motor_.sendCommand(std::string("MOVE P ") + std::to_string(pzero));
+            waitSettle(motor_, 'P', pzero, ptol, params_.settle_timeout_ms, cancel_);
+        } else {
+            LOG_WARN << "calibration: no pitch soft limits in dump; skipping pitch";
+        }
+    }
+    if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+
+    // 6. Switch the IMU to a no-magnetometer XKF profile so its heading stops
+    //    chasing the (stepper-distorted) magnetic field. Selected from the device's
+    //    own available list; persists on the device. Best-effort — if none is
+    //    available we log and continue with the drift correction below.
+    if (auto cfg = imu_.config()) {
+        int nomag = pickNoMagProfile(cfg->available_profiles);
+        if (nomag >= 0 && (!cfg->has_scenario || cfg->scenario_type != nomag)) {
+            LOG_INFO << "calibration: switching IMU to no-mag XKF profile (type " << nomag << ")";
+            setProgress('Y', 0, N, "no-mag profile");
+            if (!imu_.setFilterProfile(nomag))
+                LOG_WARN << "calibration: IMU did not confirm the no-mag profile (continuing)";
+        } else if (nomag < 0) {
+            LOG_WARN << "calibration: no no-mag XKF profile available on this device (continuing)";
+        }
+    }
+    if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+
+    // 7. Hold still: correct yaw drift (no-rotation gyro-bias update) then reset
+    //    the IMU heading so this pose is yaw 0 — done right before the yaw sweep so
+    //    the drift accumulated during the long pitch sweep doesn't bias the fit.
+    LOG_INFO << "calibration: correcting yaw drift (no-rotation " << params_.norotation_s
+             << " s, holding still)...";
+    setProgress('Y', 0, N, "yaw drift correction");
+    imu_.noRotation(params_.norotation_s);
+    for (int waited = 0; waited < params_.norotation_s * 1000 + 500 && !cancel_; waited += 100)
+        msleep(100);
+    if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+    LOG_INFO << "calibration: resetting IMU heading (this pose is now yaw 0)";
+    setProgress('Y', 0, N, "yaw heading reset");
+    imu_.headingReset();
+    msleep(params_.reset_settle_ms);
+    if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+
+    // 8. Calibrate YAW from this (now zero-heading) first position.
+    if (!sweepAxis('Y', result.yaw, ya, yb, ytol)) { return; }  // cancelled
+
     if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
 
     std::string path = paths::writeLogFile(paths::timestampedLogName("calib"), log.str());

src/core/HelpText.cpp

@@ -58,12 +58,19 @@ const std::vector<HelpSection>& helpCatalog() {
                 "Each axis is swept at several speeds/directions (DG lines). A PASS/FAIL",
                 "summary is logged and saved to logs/diag_*.log. Axes must be homed first."}},
             {"gimbal calib",
-             "Calibrate steps<->degrees using the IMU (sweeps each axis, ~minutes).", {
+             "Calibrate steps<->degrees using the IMU (auto-homes; ~minutes).", {
-                "Homes-limits required + IMU enabled. Sweeps yaw then pitch in 10 steps,",
+                "Needs the IMU. Homes first if needed, calibrates PITCH at the first yaw",
-                "dwells 5 s recording IMU orientation, fits the conversion, applies it to",
+                "position, moves pitch to 0, switches the IMU to a no-mag XKF profile",
-                "the session and writes logs/calib_*.log. 'gimbal stop' cancels."}},
+                "(persists on the device), runs no-rotation + heading reset to zero yaw",
+                "drift, then calibrates YAW. Applies the fit to the session and writes",
+                "logs/calib_*.log. 'gimbal stop' cancels."}},
             {"gimbal status",
              "Ask the firmware to emit one telemetry (ST) line now.", {}},
+            {"gimbal raw \"<command>\"",
+             "Low-level: send the quoted text to the firmware verbatim (a newline is", {
+                "added automatically). Bypasses all wrappers/unit conversion for direct",
+                "firmware control — use with care; sends exactly what you type.",
+                "Example:  gimbal raw \"MOVE 100000,250000\"   /   gimbal raw \"DUMP\""}},
         }},
         {"Capture", "Control image capture and encoding.", {
             {"start", "Begin the capture scan.", {}},
@@ -81,6 +88,12 @@ const std::vector<HelpSection>& helpCatalog() {
                 "Example:  trace serial on      /      trace off"}},
         }},
         {"Session", "Help and exit.", {
+            {"refresh",
+             "Re-read live device state (TUI: press 'r').", {
+                "Re-queries the IMU configuration (XKF profile, output settings) and",
+                "requests a fresh firmware dump, updating the 'i' and 'g' expanded views.",
+                "Use after changing the XKF profile externally. Briefly pauses the IMU",
+                "stream while it re-enters the device's Config state."}},
             {"help [topic]",
              "Show this reference; 'help <topic>' expands one section.", {
                 "Example:  help positioning      /      help dump"}},

src/core/MtiProtocol.cpp

@@ -1,7 +1,9 @@
 #include "fgc/MtiProtocol.h"
 
+#include <cctype>
 #include <cstdio>
 #include <cstring>
+#include <string>
 #include <utility>
 
 namespace fgc {
@@ -75,6 +77,41 @@ std::vector<uint8_t> msgSetOutputSettings() {
     return mtiMessage(kMidSetOutputSettings, d);
 }
 
+std::vector<uint8_t> msgSetNoRotation(uint16_t seconds) {
+    std::vector<uint8_t> d;
+    putBE(d, seconds);
+    return mtiMessage(kMidSetNoRotation, d);
+}
+
+std::vector<uint8_t> msgResetOrientation(uint16_t code) {
+    std::vector<uint8_t> d;
+    putBE(d, code);
+    return mtiMessage(kMidResetOrientation, d);
+}
+
+std::vector<uint8_t> msgSetFilterProfile(uint16_t profile) {
+    std::vector<uint8_t> d;
+    putBE(d, profile);
+    return mtiMessage(kMidReqFilterProfile, d);  // same MID; non-empty data => "set"
+}
+
+int pickNoMagProfile(const std::vector<ImuFilterProfile>& profiles) {
+    auto lower = [](std::string s) {
+        for (char& c : s) c = static_cast<char>(std::tolower((unsigned char)c));
+        return s;
+    };
+    int vru = -1;
+    for (const auto& p : profiles) {
+        const std::string l = lower(p.label);
+        if (l.find("nomag") != std::string::npos || l.find("no_mag") != std::string::npos ||
+            l.find("no mag") != std::string::npos)
+            return p.type;                                  // explicit no-magnetometer
+        if (vru < 0 && l.find("vru") != std::string::npos)  // gyro-tracked heading
+            vru = p.type;
+    }
+    return vru;
+}
+
 std::vector<uint8_t> msgReqProductCode()        { return mtiMessage(kMidReqProductCode); }
 std::vector<uint8_t> msgReqDID()                { return mtiMessage(kMidReqDID); }
 std::vector<uint8_t> msgReqFWRev()              { return mtiMessage(kMidReqFWRev); }

src/serial/MtiImuSource.cpp

@@ -72,7 +72,9 @@ struct MtiImuSource::Impl {
     // Synchronous, best-effort config handshake (no ack parsing): runs on the
     // calling thread BEFORE the io_thread starts, so there is no concurrent
     // access to the serial port. Small delays let the device switch states.
-    void configure() {
+    // If set_profile >= 0, also selects that XKF profile while in Config state
+    // (persists to non-volatile memory).
+    void configure(int set_profile = -1) {
         using namespace std::chrono_literals;
         auto write = [this](const std::vector<uint8_t>& m) {
             boost::system::error_code ec;
@@ -80,6 +82,10 @@ struct MtiImuSource::Impl {
             if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
         };
         write(msgGoToConfig());        std::this_thread::sleep_for(60ms);
+        if (set_profile >= 0) {
+            write(msgSetFilterProfile(static_cast<uint16_t>(set_profile)));
+            std::this_thread::sleep_for(60ms);
+        }
         write(msgSetOutputMode());     std::this_thread::sleep_for(60ms);
         write(msgSetOutputSettings());  std::this_thread::sleep_for(60ms);
         // Read back the device configuration while still in Config State.
@@ -89,6 +95,24 @@ struct MtiImuSource::Impl {
         ::tcflush(serial.native_handle(), TCIFLUSH);
     }
 
+    // Pause the streaming io_thread, re-run the handshake (optionally setting an
+    // XKF profile), then resume streaming. Cancels + drains the outstanding read
+    // first so it can't steal the config-ack bytes during the re-query. Called
+    // from the control thread (no concurrent serial access). Used by refreshConfig
+    // and setFilterProfile.
+    void restartWithConfig(int set_profile) {
+        io.stop();
+        if (io_thread.joinable()) io_thread.join();
+        io.restart();
+        boost::system::error_code ec;
+        serial.cancel(ec);
+        static_cast<void>(io.run());  // run the aborted read handler (no reschedule)
+        io.restart();
+        configure(set_profile);
+        doRead();
+        io_thread = std::thread([this] { io.run(); });
+    }
+
     // Query the device configuration in Config State and decode the acks. Runs on
     // the calling thread before the streaming io_thread starts; it drives the
     // io_context itself with run_for() and leaves it clean (restarted, all
@@ -101,6 +125,10 @@ struct MtiImuSource::Impl {
             applyImuConfigAck(local, mid, d, n);
         });
 
+        // Drop any buffered pre-config stream (e.g. MTData from before GoToConfig
+        // took effect) so the read window below is dominated by the query acks.
+        ::tcflush(serial.native_handle(), TCIFLUSH);
+
         write(msgReqProductCode());
         write(msgReqDID());
         write(msgReqFWRev());
@@ -204,4 +232,53 @@ std::optional<ImuDeviceConfig> MtiImuSource::config() const {
     return impl_->cfg;
 }
 
+// Re-read the device configuration: briefly pause the streaming io_thread,
+// re-run the Config-state handshake (which re-queries product/firmware/output/
+// XKF profile and re-applies our output settings), then resume streaming. Mirrors
+// start() minus opening the port. Called from the control thread; sample()/config()
+// are called from that same thread when building the snapshot, so there is no
+// concurrent serial access during the refresh.
+void MtiImuSource::refreshConfig() {
+    if (!impl_->open) { LOG_WARN << "MTi: refresh ignored (not started)"; return; }
+    LOG_INFO << "MTi: refreshing configuration (briefly pausing the stream)...";
+    impl_->restartWithConfig(-1);   // re-query only, no profile change
+}
+
+bool MtiImuSource::setFilterProfile(int type) {
+    if (!impl_->open) { LOG_WARN << "MTi: setFilterProfile ignored (not started)"; return false; }
+    if (type < 0) return false;
+    LOG_INFO << "MTi: setting XKF profile to type " << type << " (persists on device)...";
+    impl_->restartWithConfig(type);
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    return impl_->have_cfg && impl_->cfg.has_scenario && impl_->cfg.scenario_type == type;
+}
+
+// Send a command while the streaming io_thread is running. The write is posted
+// onto the io_context so it runs on the io_thread, never concurrently with the
+// in-flight async_read (concurrent ops on one serial_port are unsafe — same rule
+// as SerialMotorController).
+void MtiImuSource::noRotation(int seconds) {
+    if (!impl_->open) { LOG_WARN << "MTi: noRotation ignored (not started)"; return; }
+    if (seconds < 1) seconds = 1;
+    auto msg = std::make_shared<std::vector<uint8_t>>(
+        msgSetNoRotation(static_cast<uint16_t>(seconds)));
+    boost::asio::post(impl_->io, [this, msg] {
+        boost::system::error_code ec;
+        boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
+        if (ec) LOG_WARN << "MTi noRotation write failed: " << ec.message();
+    });
+    LOG_INFO << "MTi: no-rotation update for " << seconds << " s (hold still)";
+}
+
+void MtiImuSource::headingReset() {
+    if (!impl_->open) { LOG_WARN << "MTi: headingReset ignored (not started)"; return; }
+    auto msg = std::make_shared<std::vector<uint8_t>>(msgResetOrientation(kResetHeading));
+    boost::asio::post(impl_->io, [this, msg] {
+        boost::system::error_code ec;
+        boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
+        if (ec) LOG_WARN << "MTi headingReset write failed: " << ec.message();
+    });
+    LOG_INFO << "MTi: heading reset (current direction is now yaw 0)";
+}
+
 }  // namespace fgc

src/ui/TuiUi.cpp

@@ -479,7 +479,7 @@ Element imuDetailPanel(const ImuView& v) {
 
     Element status = v.present ? (text(" MTi live ") | color(Color::Green) | bold)
                                : (text(" MTi offline ") | color(Color::Red) | bold);
-    return window(text(" IMU  (i/Esc:close) ") | bold | color(Color::Magenta),
+    return window(text(" IMU  (i/Esc:close  r:refresh config) ") | bold | color(Color::Magenta),
                   vbox({hbox({status, filler()}), separator(),
                         vbox(std::move(body)) | flex}));
 }
@@ -563,7 +563,8 @@ void TuiUi::uiLoop() {
         } else {
             bottom = hbox({
                 keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"),
-                keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"),
+                keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("r", "Refresh"),
+                keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"),
                 keyHint(":", "Cmd"), keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
             });
         }
@@ -660,7 +661,7 @@ void TuiUi::uiLoop() {
         if (c == "s") { if (sink_) sink_("start"); return true; }
         if (c == "x") { if (sink_) sink_("stop"); return true; }
         if (c == "h") { if (sink_) sink_("gimbal home"); return true; }
-        if (c == "r") { if (sink_) sink_("gimbal reset"); return true; }
+        if (c == "r") { if (sink_) sink_("refresh"); return true; }  // IMU config + dump
         if (c == ":") { command_mode = true; return true; }
         return false;
     });

tests/CMakeLists.txt

@@ -33,6 +33,7 @@ add_executable(fgc_tests
   test_helptext.cpp
   test_diagparser.cpp
   test_calibration.cpp
+  test_calibroutine.cpp
 )
 target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest)
 

tests/test_calibroutine.cpp

@@ -0,0 +1,305 @@
+#include <doctest/doctest.h>
+
+#include "fgc/CalibrationRoutine.h"
+#include "fgc/Geometry.h"
+#include "fgc/IImuSource.h"
+#include "fgc/IMotorController.h"
+
+#include <chrono>
+#include <mutex>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+
+using namespace fgc;
+
+namespace {
+
+// A two-axis firmware DUMP with soft limits (subset of the real format; enough
+// for parseDump to expose lim_neg/lim_pos per axis).
+const char* kDump =
+    "DUMP BEGIN build=test uptime=1000 mcusr=0x01 free_ram=1852\n"
+    "DUMP Y state=3 hsub=10 enabled=1 lim_neg=-80000 lim_pos=80000 hold_target=0 "
+    "speed=50000 eeprom_restored=0 has_encoder=1\n"
+    "DUMP Y TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
+    "DUMP P state=3 hsub=10 enabled=1 lim_neg=0 lim_pos=600000 hold_target=0 "
+    "speed=150000 eeprom_restored=0 has_encoder=1\n"
+    "DUMP P TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
+    "DUMP END\n";
+
+// Counts-per-degree the fake IMU reports, so a clean sweep recovers this slope.
+constexpr double kFakeCpd = 10000.0;
+
+// Fake motor: records every command in order, and "executes" HOME/MOVE instantly
+// (axis jumps to target, standstill + READY) so waitSettle/waitHomed return at
+// once. IMU angle is derived from the live encoder position (see FakeImu).
+struct FakeImu;
+struct FakeMotor : IMotorController {
+    mutable std::mutex       mtx;
+    MotorTelemetry           tel;
+    std::vector<std::string> events;   // unified, ordered log (motor + imu)
+    bool                     pitch_present = true;
+    bool                     have_dump = true;
+
+    void start() override {}
+    void stop() override {}
+
+    void record(const std::string& s) {
+        std::lock_guard<std::mutex> lk(mtx);
+        events.push_back(s);
+    }
+
+    void sendCommand(const std::string& c) override {
+        record(c);
+        std::lock_guard<std::mutex> lk(mtx);
+        std::istringstream iss(c);
+        std::string verb;
+        iss >> verb;
+        if (verb == "HOME") {
+            tel.yaw.state = AxisState::Ready;   tel.yaw.standstill = true;
+            tel.pitch.state = AxisState::Ready; tel.pitch.standstill = true;
+            tel.pitch_present = pitch_present;
+        } else if (verb == "MOVE") {
+            std::string axis;
+            long target = 0;
+            iss >> axis >> target;
+            AxisTelemetry& a = (axis == "Y") ? tel.yaw : tel.pitch;
+            a.xenc = target;
+            a.standstill = true;
+            a.state = AxisState::Ready;
+        }
+    }
+
+    MotorTelemetry telemetry() override {
+        std::lock_guard<std::mutex> lk(mtx);
+        return tel;
+    }
+    std::string lastDump() override { return have_dump ? kDump : ""; }
+    std::string lastDiag() override { return ""; }
+    unsigned    diagSeq() const override { return 0; }
+    bool        connected() const override { return true; }
+};
+
+// Fake IMU: pitch is the live pitch encoder / kFakeCpd; yaw is the live yaw
+// encoder offset by the heading-reset zero, / kFakeCpd. Records noRotation /
+// headingReset into the shared motor event log so ordering can be asserted.
+struct FakeImu : IImuSource {
+    FakeMotor& motor;
+    long       yaw_zero = 0;
+    int        scenario_type = 39;     // starts on a mag-using profile
+    bool       has_profiles = true;    // expose an available list (incl. a no-mag one)
+    explicit FakeImu(FakeMotor& m) : motor(m) {}
+
+    void start() override {}
+    void stop() override {}
+    bool connected() const override { return true; }
+
+    std::optional<ImuSample> sample() override {
+        MotorTelemetry t = motor.telemetry();
+        ImuSample s;
+        s.valid = true;
+        s.pitch_deg = static_cast<float>(t.pitch.xenc / kFakeCpd);
+        double y = (t.yaw.xenc - yaw_zero) / kFakeCpd;
+        s.yaw_deg = static_cast<float>(y);
+        return s;
+    }
+
+    std::optional<ImuDeviceConfig> config() const override {
+        ImuDeviceConfig c;
+        c.valid = true;
+        c.has_scenario = true;
+        c.scenario_type = static_cast<uint8_t>(scenario_type);
+        if (has_profiles)
+            c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
+                                    {53, 11, "VRU_general"}};
+        return c;
+    }
+
+    void noRotation(int seconds) override {
+        motor.record("noRotation:" + std::to_string(seconds));
+    }
+    void headingReset() override {
+        motor.record("headingReset");
+        std::lock_guard<std::mutex> lk(motor.mtx);
+        yaw_zero = motor.tel.yaw.xenc;  // current pose becomes yaw 0
+    }
+    bool setFilterProfile(int type) override {
+        motor.record("setProfile:" + std::to_string(type));
+        scenario_type = type;
+        return true;
+    }
+};
+
+Geometry uncalibrated() {
+    Geometry g;
+    g.yaw   = {1.0, 0, -100000.0, 100000.0};
+    g.pitch = {1.0, 0, -100000.0, 100000.0};
+    return g;
+}
+
+// Fast timings so a full run completes in well under a second.
+CalibParams fastParams() {
+    CalibParams p;
+    p.positions         = 4;
+    p.dwell_ms          = 4;
+    p.sample_ms         = 1;
+    p.settle_timeout_ms = 200;
+    p.home_timeout_ms   = 500;
+    p.norotation_s      = 0;
+    p.reset_settle_ms   = 2;
+    return p;
+}
+
+// Run to completion (or fail the test on timeout).
+void runToCompletion(CalibrationRoutine& cal) {
+    REQUIRE(cal.start());
+    auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
+    while (cal.running()) {
+        if (std::chrono::steady_clock::now() > deadline) { FAIL("calibration did not finish"); }
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+}
+
+int indexOf(const std::vector<std::string>& v, const std::string& needle) {
+    for (int i = 0; i < static_cast<int>(v.size()); ++i)
+        if (v[i].find(needle) != std::string::npos) return i;
+    return -1;
+}
+int lastIndexOf(const std::vector<std::string>& v, const std::string& needle) {
+    int found = -1;
+    for (int i = 0; i < static_cast<int>(v.size()); ++i)
+        if (v[i].find(needle) != std::string::npos) found = i;
+    return found;
+}
+
+}  // namespace
+
+TEST_CASE("calibration homes first when the gimbal is not READY") {
+    FakeMotor motor;            // starts in Boot (not ready)
+    motor.tel.pitch_present = true;
+    FakeImu imu(motor);
+
+    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
+    runToCompletion(cal);
+
+    const auto& ev = motor.events;
+    REQUIRE_FALSE(ev.empty());
+    CHECK(ev.front() == "HOME");                       // homing is the first action
+    CHECK(indexOf(ev, "HOME") < indexOf(ev, "MOVE P")); // home before any pitch move
+}
+
+TEST_CASE("calibration does NOT home when already READY") {
+    FakeMotor motor;
+    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
+    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
+    motor.tel.pitch_present = true;
+    FakeImu imu(motor);
+
+    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
+    runToCompletion(cal);
+
+    CHECK(indexOf(motor.events, "HOME") == -1);  // no homing issued
+}
+
+TEST_CASE("calibration runs pitch-at-first-yaw, then drift-correct+reset, then yaw") {
+    FakeMotor motor;
+    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
+    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
+    motor.tel.pitch_present = true;
+    FakeImu imu(motor);
+
+    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
+    runToCompletion(cal);
+
+    const auto& ev = motor.events;
+    // Yaw is parked at its first position before the pitch sweep begins.
+    const int first_yaw_move = indexOf(ev, "MOVE Y");
+    const int first_pitch_move = indexOf(ev, "MOVE P");
+    REQUIRE(first_yaw_move >= 0);
+    REQUIRE(first_pitch_move >= 0);
+    CHECK(first_yaw_move < first_pitch_move);
+
+    // No-mag profile switch, drift correction, then heading reset happen AFTER the
+    // pitch sweep and BEFORE the yaw sweep reaches its far end.
+    const int prof  = indexOf(ev, "setProfile:53");  // VRU_general from the fake list
+    const int norot = indexOf(ev, "noRotation");
+    const int reset = indexOf(ev, "headingReset");
+    REQUIRE(prof >= 0);
+    REQUIRE(norot >= 0);
+    REQUIRE(reset >= 0);
+    CHECK(first_pitch_move < prof);      // pitch calibrated before the profile switch
+    CHECK(prof < norot);                 // switch to no-mag, then correct drift
+    CHECK(norot < reset);                // correct drift, then reset
+    CHECK(reset < lastIndexOf(ev, "MOVE Y"));  // yaw sweep continues after the reset
+
+    // Pitch is commanded to 0 deg before the heading reset.
+    CHECK(indexOf(ev, "MOVE P 0") >= 0);
+    CHECK(indexOf(ev, "MOVE P 0") < reset);
+}
+
+TEST_CASE("calibration skips the profile switch when no no-mag profile exists") {
+    FakeMotor motor;
+    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
+    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
+    motor.tel.pitch_present = true;
+    FakeImu imu(motor);
+    imu.has_profiles = false;  // device reports no available profiles
+
+    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
+    runToCompletion(cal);
+
+    const auto& ev = motor.events;
+    CHECK(indexOf(ev, "setProfile") == -1);     // nothing to switch to
+    CHECK(indexOf(ev, "headingReset") >= 0);    // still resets + sweeps yaw
+    CHECK(cal.report().valid);
+}
+
+TEST_CASE("calibration recovers the fake IMU slope for both axes") {
+    FakeMotor motor;
+    motor.tel.yaw.state = AxisState::Ready;   motor.tel.yaw.standstill = true;
+    motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
+    motor.tel.pitch_present = true;
+    FakeImu imu(motor);
+
+    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
+    runToCompletion(cal);
+
+    CalibReport rep = cal.report();
+    REQUIRE(rep.valid);
+    CHECK(rep.all_ok);
+    REQUIRE(rep.axes.size() == 2);
+    for (const auto& ax : rep.axes) {
+        CHECK(ax.ok);
+        CHECK(ax.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
+        CHECK(ax.r2 == doctest::Approx(1.0).epsilon(0.001));
+    }
+
+    // The new geometry is published for the session to apply.
+    auto geo = cal.takeResult();
+    REQUIRE(geo.has_value());
+    CHECK(geo->yaw.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
+    CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
+}
+
+TEST_CASE("calibration handles a yaw-only gimbal (no pitch axis)") {
+    FakeMotor motor;
+    motor.pitch_present = false;
+    motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
+    motor.tel.pitch_present = false;
+    FakeImu imu(motor);
+
+    CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
+    runToCompletion(cal);
+
+    const auto& ev = motor.events;
+    CHECK(indexOf(ev, "MOVE P") == -1);        // no pitch moves
+    CHECK(indexOf(ev, "headingReset") >= 0);   // still resets + sweeps yaw
+    CHECK(indexOf(ev, "MOVE Y") >= 0);
+
+    CalibReport rep = cal.report();
+    REQUIRE(rep.valid);
+    REQUIRE(rep.axes.size() == 1);
+    CHECK(rep.axes[0].axis == 'Y');
+    CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
+}

tests/test_mtiprotocol.cpp

@@ -144,6 +144,59 @@ TEST_CASE("parseMTData reports yaw as a 0..360 heading") {
     CHECK(got.yaw_deg == doctest::Approx(181.0f));
 }
 
+TEST_CASE("orientation-control command builders are well-formed") {
+    SUBCASE("SetNoRotation carries the duration big-endian") {
+        auto m = msgSetNoRotation(3);
+        CHECK(m[0] == kMtiPreamble);
+        CHECK(m[1] == kMtiBid);
+        CHECK(m[2] == kMidSetNoRotation);
+        CHECK(m[3] == 2);
+        CHECK(m[4] == 0x00);
+        CHECK(m[5] == 0x03);
+        CHECK(frameChecksumOk(m));
+    }
+    SUBCASE("ResetOrientation heading reset = CODE 0x0001") {
+        auto m = msgResetOrientation(kResetHeading);
+        CHECK(m[2] == kMidResetOrientation);
+        CHECK(m[3] == 2);
+        CHECK(m[4] == 0x00);
+        CHECK(m[5] == 0x01);
+        CHECK(frameChecksumOk(m));
+    }
+    SUBCASE("ResetOrientation store = CODE 0x0000") {
+        auto m = msgResetOrientation(kResetStore);
+        CHECK(m[4] == 0x00);
+        CHECK(m[5] == 0x00);
+        CHECK(frameChecksumOk(m));
+    }
+    SUBCASE("SetFilterProfile carries the profile number (set = non-empty data)") {
+        auto m = msgSetFilterProfile(53);
+        CHECK(m[2] == kMidReqFilterProfile);  // same MID as Req; data makes it a Set
+        CHECK(m[3] == 2);
+        CHECK(m[4] == 0x00);
+        CHECK(m[5] == 53);
+        CHECK(frameChecksumOk(m));
+    }
+}
+
+TEST_CASE("pickNoMagProfile chooses a magnetometer-free profile") {
+    SUBCASE("prefers an explicit no-mag label") {
+        std::vector<ImuFilterProfile> ps = {
+            {39, 11, "General"}, {40, 11, "High_mag_dep"}, {7, 1, "Machine_nomagfield"}};
+        CHECK(pickNoMagProfile(ps) == 7);
+    }
+    SUBCASE("falls back to a VRU profile") {
+        std::vector<ImuFilterProfile> ps = {
+            {39, 11, "General"}, {53, 11, "VRU_general"}, {40, 11, "High_mag_dep"}};
+        CHECK(pickNoMagProfile(ps) == 53);
+    }
+    SUBCASE("returns -1 when every profile uses the magnetometer") {
+        std::vector<ImuFilterProfile> ps = {
+            {39, 11, "General"}, {40, 11, "High_mag_dep"}, {41, 11, "Dynamic"}};
+        CHECK(pickNoMagProfile(ps) == -1);
+    }
+}
+
 TEST_CASE("config-readback query builders are well-formed requests (empty data)") {
     struct Q { std::vector<uint8_t> m; uint8_t mid; };
     Q qs[] = {