Fixed bug nudging befor calibration is run

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

.gitignore

@@ -29,3 +29,5 @@ bin/x64/Release/NIR/
 # Real configs hold plaintext MQTT credentials. Commit config/config.example.ini instead.
 config.ini
 bin/x64/Release/config.ini
+build-test/
+build/

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
@@ -79,6 +111,22 @@ was left in Config state. Yaw is reported as a **0..360 heading** (not the MTi's
 `[Features] mock_imu = true` to use a synthetic IMU on dev machines (no hardware). Protocol/units are
 documented in the modules reference (`MtiProtocol`).
 
+During that same Config-state handshake the host also **reads back the device configuration**
+(`ReqProductCode`, `ReqDID`, `ReqFWRev`, `ReqPeriod`, `ReqOutputMode`, `ReqOutputSettings`, and the
+filter-profile / **XKF profile** via `ReqFilterProfile` + `ReqAvailableFilterProfiles`). The decoded
+values — product code, firmware, device ID, output mode/format, calibration channels, sample rate, and
+the list of supported **Xsens Kalman Filter (XKF) profiles** with the active one marked — are surfaced
+in the **IMU CONFIG** section of the expanded Sensors view (press `i`). This is read-only: it reports
+what the device is actually configured to do, which is the place to confirm the active XKF profile
+(e.g. `General` vs `VRU_general`) when diagnosing yaw drift. On the legacy MTi the same concept is
+called a "scenario" in the device manual (e.g. `Machine_nomagfield`); it is the same setting and shares
+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,
@@ -157,7 +205,10 @@ one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp).
   (`d` requests a fresh one), the homing limits, and the **last calibration** result (per-axis
   `counts_per_deg` / `zero_count` / R² / age).
 - **IMU** (`i`) — every MTi channel with units: orientation (°), acceleration (m/s²), rate-of-turn
-  (rad/s), magnetic field (a.u.), temperature, sample counter.
+  (rad/s), magnetic field (a.u.), temperature, sample counter, plus an **IMU CONFIG** section
+  (read back from the device at startup): product code, firmware, device ID, output mode/format,
+  calibration channels, sample rate, and the **Xsens Kalman Filter (XKF) profile** list — every
+  profile the device supports, by name, with the active one marked `●` (selected).
 
 **Activity strip** — a compact section between the log and the key bar that shows the
 currently-running special operation with live progress (`gimbal calib`, `gimbal diag`, homing, capture
@@ -166,14 +217,15 @@ away in the log. It only appears once something has run.
 
 When a `gimbal calib` completes it is applied to the live session and the strip shows a highlighted
 yes/no prompt — **`Save this calibration to config as the new default? (y / n)`**. Press **`y`** to
-write the fitted `[Motor]` `*_counts_per_deg` / `*_zero_count` back into the `config.ini` the program
+write the full `[Motor]` per-axis map — `*_counts_per_deg`, `*_zero_count`, and `*_min_deg`/`*_max_deg`
-was launched with (replacing those keys in place, preserving everything else) so it persists across
+— back into the `config.ini` the program was launched with (replacing those keys in place, preserving
-restarts; press **`n`** to keep it for this session only. The keys are active only while the prompt is
+everything else) so it persists across restarts; press **`n`** to keep it for this session only. The keys are active only while the prompt is
 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/known-issues.md

@@ -53,7 +53,13 @@ doctest unit-test suite (`ctest`).
 - **`gimbal calib` persistence**: the fitted `counts_per_deg`/`zero_count` are always applied to the live
   session (display, manual moves, MQTT heading, **and** the capture scheduler) and written to
   `logs/calib_*.log`. In the **TUI**, the activity strip then prompts `Save … as the new default? (y/n)`
-  — `y` writes them into `[Motor]` in `config.ini` (persists across restarts), `n` keeps them
+  — `y` writes the full per-axis `[Motor]` map (`*_counts_per_deg`, `*_zero_count`,
+  `*_min_deg`/`*_max_deg`) into `config.ini` (persists across restarts), `n` keeps them
   session-only. In the **headless** console there is no prompt, so calibration stays session-only there;
   copy the logged values into `[Motor]` by hand to keep them.
+- **`gimbal nudge`** moves a fixed fraction of the **homed** endstop-to-endstop travel (from the
+  firmware dump), so it is independent of the degrees↔counts calibration. If no dump has been captured
+  yet (not homed / no `gimbal dump`), nudge requests one and does nothing that press — it never falls
+  back to the configured degree clamps (which, if `*_min_deg`/`*_max_deg` were unset, produced absurd
+  ±100000° steps).
 - DHT11 temperature/humidity is still a Sensors-panel placeholder (the IMU half is integrated).

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) |
+| [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)
@@ -40,7 +40,7 @@ Per-file reference for the refactored tree, plus the shared data structures.
 | File | Implements | Built when |
 |------|-----------|-----------|
 | [src/serial/SerialMotorController.cpp](../src/serial/SerialMotorController.cpp) | `IMotorController` over Boost.Asio serial (pImpl) | always |
-| [src/serial/MtiImuSource.cpp](../src/serial/MtiImuSource.cpp) | `IImuSource` over Boost.Asio serial: configures the MTi to Euler+calibrated, then frames the MTData stream | always |
+| [src/serial/MtiImuSource.cpp](../src/serial/MtiImuSource.cpp) | `IImuSource` over Boost.Asio serial: configures the MTi to Euler+calibrated, reads back its config (`config()` → `ImuDeviceConfig`), then frames the MTData stream | always |
 | [src/mqtt/MqttControlChannel.cpp](../src/mqtt/MqttControlChannel.cpp) | `IControlChannel` over Eclipse Paho | `WITH_MQTT` |
 | [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp) | `IUserInterface` over FTXUI: panels, key bar, log pane | `WITH_TUI` |
 | [src/camera/VimbaCameraSource.cpp](../src/camera/VimbaCameraSource.cpp) | `ICameraSource` over Vimba X (pImpl) | `WITH_VIMBA` |
@@ -90,6 +90,16 @@ One decoded Xsens MTi reading: `temp_c` (°C), `acc[3]` (m/s², incl. gravity),
 `gimbal calib` phase-unwraps the swept yaw (`unwrapNear`) so a sweep crossing 0/360 still fits a clean
 line.
 
+### `ImuDeviceConfig` ([MtiProtocol.h](../include/fgc/MtiProtocol.h))
+The device configuration read back during the Config-state handshake (each `has_*` flag marks whether
+the device actually answered): `product_code`, `device_id`, `firmware`, output mode flags
+(temp/calibrated/orientation/aux/status), output settings (`orientation_mode`, `timestamp_mode`,
+per-channel `acc/gyr/mag_enabled`, `data_format`), `period`/`sample_rate_hz`, and the **XKF scenario**
+(`scenario_type`/`version`, resolved to `scenario_label` against `available_profiles`). Built by
+`applyImuConfigAck` (one ack frame at a time) + `finalizeImuConfig` (derive rate, resolve label).
+Surfaced read-only in the TUI's `i` view (IMU CONFIG section); the legacy "scenario" is the modern
+"filter profile" — same MIDs (`0x62/0x64/0x65`).
+
 ## On-disk artifacts
 
 | Artifact | Path | Format |

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

@@ -21,6 +21,32 @@ public:
 
     // Latest reading, or nullopt if none/stale.
     virtual std::optional<ImuSample> sample() = 0;
+
+    // Device configuration read back during start-up (output mode/settings,
+    // 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

@@ -20,6 +20,11 @@ public:
     void                     stop() override;
     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

@@ -3,6 +3,7 @@
 #include <cstdint>
 #include <functional>
 #include <optional>
+#include <string>
 #include <vector>
 
 namespace fgc {
@@ -31,6 +32,31 @@ inline constexpr uint8_t kMidSetOutputSettingsAck = 0xD3;
 inline constexpr uint8_t kMidMTData = 0x32;
 inline constexpr uint8_t kMidError  = 0x42;
 
+// Config-readback queries (sent with an empty data field in Config State; the
+// device replies with the matching ack MID = request MID + 1). ReqOutputMode /
+// ReqOutputSettings reuse the Set MIDs above (len 0 => request, not set).
+inline constexpr uint8_t kMidReqDID                = 0x00;
+inline constexpr uint8_t kMidDeviceID              = 0x01;
+inline constexpr uint8_t kMidReqPeriod             = 0x04;
+inline constexpr uint8_t kMidReqPeriodAck          = 0x05;
+inline constexpr uint8_t kMidReqFWRev              = 0x12;
+inline constexpr uint8_t kMidFirmwareRev           = 0x13;
+inline constexpr uint8_t kMidReqProductCode        = 0x1C;
+inline constexpr uint8_t kMidProductCode           = 0x1D;
+inline constexpr uint8_t kMidReqAvailFilterProf    = 0x62;
+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
@@ -53,6 +79,91 @@ struct ImuSample {
     uint16_t sample_counter = 0;
 };
 
+// One available filter profile (a.k.a. XKF "scenario") as reported by the device
+// in the AvailableFilterProfiles message.
+struct ImuFilterProfile {
+    uint8_t     type    = 0;
+    uint8_t     version = 0;
+    std::string label;   // human name, e.g. "General" / "VRU_general"
+};
+
+// Device configuration read back during the Config-state handshake. Each `has_*`
+// flag marks whether the corresponding ack was actually received and decoded, so
+// the UI can show "—" for anything the device did not answer.
+struct ImuDeviceConfig {
+    bool        valid = false;  // at least one field was populated
+
+    // Identity.
+    std::string product_code;             // e.g. "MTi-28A33G85"
+    bool        has_device_id = false;
+    uint32_t    device_id = 0;            // serial / device ID
+    std::string firmware;                 // "2.3.1 build 25" ("" if unknown)
+
+    // Output mode (which data the device streams).
+    bool        has_output_mode = false;
+    uint16_t    output_mode = 0;
+    bool        out_temperature = false, out_calibrated = false;
+    bool        out_orientation = false, out_auxiliary = false, out_status = false;
+
+    // Output settings (how the data is formatted).
+    bool        has_output_settings = false;
+    uint32_t    output_settings = 0;
+    std::string orientation_mode;         // "Quaternion" / "Euler" / "Matrix"
+    std::string timestamp_mode;           // "Sample counter" / "None"
+    bool        acc_enabled = true, gyr_enabled = true, mag_enabled = true;
+    std::string data_format;              // "Float" / "Fixed 12.20"
+
+    // Sample rate.
+    bool        has_period = false;
+    uint16_t    period = 0;               // raw, resolution 1/115200 s
+    float       sample_rate_hz = 0.f;
+
+    // Filter profile / XKF scenario.
+    bool        has_scenario = false;
+    uint8_t     scenario_type = 0, scenario_version = 0;
+    std::string scenario_label;           // resolved from available_profiles, else ""
+    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();
+std::vector<uint8_t> msgReqFWRev();
+std::vector<uint8_t> msgReqPeriod();
+std::vector<uint8_t> msgReqOutputMode();          // kMidSetOutputMode, len 0
+std::vector<uint8_t> msgReqOutputSettings();      // kMidSetOutputSettings, len 0
+std::vector<uint8_t> msgReqFilterProfile();
+std::vector<uint8_t> msgReqAvailFilterProfiles();
+
+// Decode a single config-ack frame into `c`. Recognizes DeviceID(0x01),
+// ProductCode(0x1D), FirmwareRev(0x13), ReqPeriodAck(0x05), output-mode ack
+// (0xD1), output-settings ack (0xD3), filter-profile ack (0x65), and the
+// available-profiles list (0x63). Unknown MIDs are ignored. Returns true if the
+// frame was recognized and applied.
+bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n);
+
+// Resolve derived fields once all acks are applied: the scenario label (matched
+// against available_profiles by type) and sample_rate_hz from the period.
+void finalizeImuConfig(ImuDeviceConfig& c);
+
 // Lower byte of the sum of all bytes from BID through the end of DATA. The CS
 // byte that makes the running total ≡ 0 (mod 256) is (256 - mtiChecksum) & 0xFF.
 uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len);

include/fgc/mock/MockImuSource.h

@@ -44,9 +44,55 @@ 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. 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;
+        c.product_code = "MTi-28A53G35 (mock)";
+        c.has_device_id = true;
+        c.device_id = 0x00990ABC;
+        c.firmware = "2.8.1 build 0";
+        c.has_output_mode = true;
+        c.output_mode = kOutputMode;
+        c.out_temperature = c.out_calibrated = c.out_orientation = true;
+        c.has_output_settings = true;
+        c.output_settings = kOutputSettings;
+        c.orientation_mode = "Euler";
+        c.timestamp_mode = "Sample counter";
+        c.data_format = "Float";
+        c.has_period = true;
+        c.period = 1152;  // 100 Hz
+        c.sample_rate_hz = 100.0f;
+        c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
+                                {41, 11, "Dynamic"}, {53, 11, "VRU_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

include/fgc/ui/UiSnapshot.h

@@ -118,6 +118,30 @@ struct DumpView {
     std::string text;
 };
 
+// Device configuration read back from the MTi at startup, formatted for the
+// expanded Sensors view. `present` is false until the handshake reports it (or
+// for backends that cannot report it).
+// One XKF (Xsens Kalman Filter) profile the device supports, with whether it is
+// the active one.
+struct ImuProfileView {
+    std::string name;            // human label, no numeric IDs ("General")
+    bool        selected = false;  // the profile currently in use
+};
+
+struct ImuConfigView {
+    bool        present = false;
+    std::string product_code;     // "MTi-28A53G35"
+    std::string device_id;        // "0x00990ABC"
+    std::string firmware;         // "2.8.1 build 0"
+    std::string output_mode;      // "Temp · Calibrated · Orientation"
+    std::string output_settings;  // "Euler · Sample counter · Float"
+    std::string channels;         // "acc gyr mag"
+    std::string sample_rate;      // "100 Hz"
+    // Available XKF profiles with the active one flagged. Empty if the device
+    // did not report them.
+    std::vector<ImuProfileView> xkf_profiles;
+};
+
 // Full Xsens MTi reading for the expanded Sensors view (units: acc m/s^2,
 // gyr rad/s, mag a.u., angles deg, temp °C).
 struct ImuView {
@@ -128,6 +152,7 @@ struct ImuView {
     float         mag[3] = {0, 0, 0};
     float         temp_c = 0;
     unsigned      sample_counter = 0;
+    ImuConfigView config;
 };
 
 // Status of the currently-running special operation (calibration, diagnostics,

src/core/Application.cpp

@@ -73,6 +73,70 @@ std::string traceNames(unsigned mask) {
     return out;
 }
 
+// Build the expanded-view ImuConfigView from the device's decoded config.
+ImuConfigView formatImuConfig(const ImuDeviceConfig& c) {
+    ImuConfigView v;
+    v.present = true;
+    v.product_code = c.product_code.empty() ? "—" : c.product_code;
+    if (c.has_device_id) {
+        char b[16];
+        std::snprintf(b, sizeof(b), "0x%08X", c.device_id);
+        v.device_id = b;
+    } else {
+        v.device_id = "—";
+    }
+    v.firmware = c.firmware.empty() ? "—" : c.firmware;
+
+    if (c.has_output_mode) {
+        std::string m;
+        auto add = [&](bool on, const char* name) {
+            if (on) { if (!m.empty()) m += " · "; m += name; }
+        };
+        add(c.out_temperature, "Temp");
+        add(c.out_calibrated, "Calibrated");
+        add(c.out_orientation, "Orientation");
+        add(c.out_auxiliary, "Auxiliary");
+        add(c.out_status, "Status");
+        v.output_mode = m.empty() ? "—" : m;
+    } else {
+        v.output_mode = "—";
+    }
+
+    if (c.has_output_settings) {
+        v.output_settings = c.orientation_mode + " · " + c.timestamp_mode + " · " + c.data_format;
+        std::string ch;
+        if (c.acc_enabled) ch += "acc ";
+        if (c.gyr_enabled) ch += "gyr ";
+        if (c.mag_enabled) ch += "mag";
+        v.channels = ch.empty() ? "none" : ch;
+    } else {
+        v.output_settings = "—";
+        v.channels = "—";
+    }
+
+    if (c.has_period && c.sample_rate_hz > 0) {
+        char b[24];
+        std::snprintf(b, sizeof(b), "%.0f Hz", c.sample_rate_hz);
+        v.sample_rate = b;
+    } else {
+        v.sample_rate = "—";
+    }
+
+    // XKF profiles: list every available profile by name (no numeric IDs) and
+    // flag the active one (matched by profile type).
+    for (const auto& p : c.available_profiles) {
+        ImuProfileView pv;
+        pv.name = p.label.empty() ? "(unnamed)" : p.label;
+        pv.selected = c.has_scenario && p.type == c.scenario_type;
+        v.xkf_profiles.push_back(std::move(pv));
+    }
+    // If the device reported the active profile but not the full list, still show
+    // the one in use (by its resolved label, when known).
+    if (v.xkf_profiles.empty() && c.has_scenario && !c.scenario_label.empty())
+        v.xkf_profiles.push_back({c.scenario_label, true});
+    return v;
+}
+
 }  // namespace
 
 struct Application::Impl {
@@ -102,6 +166,8 @@ struct Application::Impl {
     std::vector<std::string> last_calib_summary;
     long long                last_calib_ts = 0;   // epoch ms
     bool                     calib_save_pending_ = false;  // awaiting y/n to persist to config
+    mutable ImuConfigView    imu_config_view_;     // formatted MTi config (read once)
+    mutable bool             imu_config_done_ = false;
 
     std::atomic<bool>       running{true};
     std::mutex              cmd_mutex;
@@ -243,6 +309,14 @@ struct Application::Impl {
                 f.value   = v;
                 f.present = true;
             };
+            // Device configuration (read once at startup): format and cache.
+            if (!imu_config_done_) {
+                if (auto c = imu->config()) {
+                    imu_config_view_ = formatImuConfig(*c);
+                    imu_config_done_ = true;
+                }
+            }
+            s.imu.config = imu_config_view_;
             if (auto m = imu->sample()) {
                 // Full reading for the expanded view.
                 s.imu.present        = true;
@@ -456,15 +530,17 @@ struct Application::Impl {
         return static_cast<bool>(iss >> a >> b);
     }
 
-    // Soft-limit travel (counts) for an axis from the last dump, else the
+    // Homed physical travel (counts) for an axis: the endstop-to-endstop span from
-    // configured degree range converted to counts. Returns 0 if unknown.
+    // the last firmware dump. Returns 0 if no dump has been captured yet (caller
-    long axisRangeCounts(char axis) {
+    // should request one). Deliberately does NOT fall back to the configured degree
+    // range × counts_per_deg — that conflates calibration with physical travel and,
+    // when min/max are unset, yields an absurd span.
+    long homedTravelCounts(char axis) {
         DumpData d = parseDump(motor->lastDump());
         if (d.valid)
             for (const auto& ax : d.axes)
                 if (ax.axis == axis) return std::labs(ax.lim_pos - ax.lim_neg);
-        const AxisMap& m = (axis == 'Y') ? cfg.geometry.yaw : cfg.geometry.pitch;
+        return 0;
-        return std::labs(m.toCounts(m.max_deg) - m.toCounts(m.min_deg));
     }
 
     // `gimbal <subcommand>` — unified, lowercase motor control. `move` is degrees
@@ -476,7 +552,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];
@@ -505,8 +581,16 @@ struct Application::Impl {
             char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
             double pct = 0;
             try { pct = std::stod(tok[3]); } catch (...) { LOG_WARN << "gimbal nudge: bad percent"; return; }
-            long range = axisRangeCounts(axis);
+            // Nudge is a fraction of the *homed* physical travel (endstop-to-endstop
-            if (range <= 0) { LOG_WARN << "gimbal nudge: travel unknown; run gimbal home/dump first"; motor->sendCommand("DUMP"); return; }
+            // from the firmware dump), so it is independent of calibration and never
+            // a fraction of the configured degree clamps. If no dump is available
+            // yet, request one and bail rather than guessing.
+            long range = homedTravelCounts(axis);
+            if (range <= 0) {
+                LOG_WARN << "gimbal nudge: homed travel unknown; home or 'gimbal dump' first";
+                motor->sendCommand("DUMP");
+                return;
+            }
             MotorTelemetry tel = motor->telemetry();
             long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
             long target = cur + static_cast<long>(pct / 100.0 * range);
@@ -529,6 +613,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.
@@ -656,6 +759,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/Config.cpp

@@ -219,11 +219,19 @@ bool saveMotorCalibration(const std::string& path, const Geometry& geo) {
         std::snprintf(b, sizeof(b), "%.10g", v);
         return std::string(b);
     };
+    // Persist the full per-axis map, not just the fitted counts_per_deg/zero_count:
+    // min_deg/max_deg are part of the geometry and consumers (e.g. travel-range
+    // estimates) rely on them, so writing them keeps the [Motor] block complete and
+    // self-consistent rather than depending on pre-existing lines.
     const std::vector<std::pair<std::string, std::string>> kv = {
         {"yaw_counts_per_deg", num(geo.yaw.counts_per_deg)},
         {"yaw_zero_count", std::to_string(geo.yaw.zero_count)},
+        {"yaw_min_deg", num(geo.yaw.min_deg)},
+        {"yaw_max_deg", num(geo.yaw.max_deg)},
         {"pitch_counts_per_deg", num(geo.pitch.counts_per_deg)},
         {"pitch_zero_count", std::to_string(geo.pitch.zero_count)},
+        {"pitch_min_deg", num(geo.pitch.min_deg)},
+        {"pitch_max_deg", num(geo.pitch.max_deg)},
     };
     const std::string updated = updateIniSectionKeys(buf.str(), "Motor", kv);
 

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,6 +1,10 @@
 #include "fgc/MtiProtocol.h"
 
+#include <cctype>
+#include <cstdio>
 #include <cstring>
+#include <string>
+#include <utility>
 
 namespace fgc {
 
@@ -19,6 +23,11 @@ uint16_t beU16(const uint8_t* p) {
     return static_cast<uint16_t>((uint16_t(p[0]) << 8) | uint16_t(p[1]));
 }
 
+uint32_t beU32(const uint8_t* p) {
+    return (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) | (uint32_t(p[2]) << 8) |
+           uint32_t(p[3]);
+}
+
 // Append a big-endian value to a byte vector.
 void putBE(std::vector<uint8_t>& v, uint16_t x) {
     v.push_back(static_cast<uint8_t>(x >> 8));
@@ -68,6 +77,168 @@ 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); }
+std::vector<uint8_t> msgReqPeriod()             { return mtiMessage(kMidReqPeriod); }
+std::vector<uint8_t> msgReqOutputMode()         { return mtiMessage(kMidSetOutputMode); }
+std::vector<uint8_t> msgReqOutputSettings()     { return mtiMessage(kMidSetOutputSettings); }
+std::vector<uint8_t> msgReqFilterProfile()      { return mtiMessage(kMidReqFilterProfile); }
+std::vector<uint8_t> msgReqAvailFilterProfiles(){ return mtiMessage(kMidReqAvailFilterProf); }
+
+bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n) {
+    switch (mid) {
+        case kMidDeviceID:
+            if (n < 4) return false;
+            c.device_id = beU32(d);
+            c.has_device_id = true;
+            c.valid = true;
+            return true;
+
+        case kMidProductCode: {
+            // ASCII string, possibly space-padded; trim trailing spaces/NULs.
+            std::size_t end = n;
+            while (end > 0 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
+            c.product_code.assign(reinterpret_cast<const char*>(d), end);
+            c.valid = true;
+            return true;
+        }
+
+        case kMidFirmwareRev: {
+            // MAJOR MINOR REV [BUILDNR(4) SCMREF(4)] — older firmware sends only 3.
+            if (n < 3) return false;
+            char buf[48];
+            if (n >= 7) {
+                uint32_t build = beU32(d + 3);
+                std::snprintf(buf, sizeof(buf), "%u.%u.%u build %u", d[0], d[1], d[2], build);
+            } else {
+                std::snprintf(buf, sizeof(buf), "%u.%u.%u", d[0], d[1], d[2]);
+            }
+            c.firmware = buf;
+            c.valid = true;
+            return true;
+        }
+
+        case kMidReqPeriodAck:
+            if (n < 2) return false;
+            c.period = beU16(d);
+            c.has_period = true;
+            c.valid = true;
+            return true;
+
+        case kMidSetOutputModeAck: {  // 0xD1, ack to ReqOutputMode
+            if (n < 2) return false;
+            uint16_t m = beU16(d);
+            c.output_mode = m;
+            c.out_temperature = m & 0x0001;
+            c.out_calibrated  = m & 0x0002;
+            c.out_orientation = m & 0x0004;
+            c.out_auxiliary   = m & 0x0008;
+            c.out_status      = m & 0x0800;
+            c.has_output_mode = true;
+            c.valid = true;
+            return true;
+        }
+
+        case kMidSetOutputSettingsAck: {  // 0xD3, ack to ReqOutputSettings
+            if (n < 4) return false;
+            uint32_t s = beU32(d);
+            c.output_settings = s;
+            switch (s & 0x0003) {
+                case 0x1: c.timestamp_mode = "Sample counter"; break;
+                default:  c.timestamp_mode = "None"; break;
+            }
+            switch ((s >> 2) & 0x0003) {
+                case 0x0: c.orientation_mode = "Quaternion"; break;
+                case 0x1: c.orientation_mode = "Euler"; break;
+                case 0x2: c.orientation_mode = "Matrix"; break;
+                default:  c.orientation_mode = "?"; break;
+            }
+            // Bits 4/5/6: 1 = output DISABLED.
+            c.acc_enabled = !(s & 0x0010);
+            c.gyr_enabled = !(s & 0x0020);
+            c.mag_enabled = !(s & 0x0040);
+            c.data_format = ((s >> 8) & 0x0003) == 0x1 ? "Fixed 12.20" : "Float";
+            c.has_output_settings = true;
+            c.valid = true;
+            return true;
+        }
+
+        case kMidReqFilterProfileAck:  // VERSION, FILTERPROFILE(type)
+            if (n < 2) return false;
+            c.scenario_version = d[0];
+            c.scenario_type    = d[1];
+            c.has_scenario = true;
+            c.valid = true;
+            return true;
+
+        case kMidAvailFilterProf: {
+            // Repeating 22-byte records: TYPE(1) VERSION(1) LABEL(20, space-padded).
+            c.available_profiles.clear();
+            for (std::size_t o = 0; o + 22 <= n; o += 22) {
+                ImuFilterProfile p;
+                p.type    = d[o];
+                p.version = d[o + 1];
+                if (p.type == 0) continue;  // empty slot
+                std::size_t end = o + 22;
+                while (end > o + 2 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
+                p.label.assign(reinterpret_cast<const char*>(d + o + 2), end - (o + 2));
+                c.available_profiles.push_back(std::move(p));
+            }
+            c.valid = true;
+            return true;
+        }
+
+        default:
+            return false;
+    }
+}
+
+void finalizeImuConfig(ImuDeviceConfig& c) {
+    if (c.has_period && c.period > 0)
+        c.sample_rate_hz = 115200.0f / static_cast<float>(c.period);
+    if (c.has_scenario) {
+        for (const auto& p : c.available_profiles) {
+            if (p.type == c.scenario_type) { c.scenario_label = p.label; break; }
+        }
+    }
+}
+
 std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) {
     if (mid != kMidMTData || len != kMTDataLen) return std::nullopt;
     ImuSample s;

src/serial/MtiImuSource.cpp

@@ -6,6 +6,7 @@
 #include <array>
 #include <atomic>
 #include <chrono>
+#include <functional>
 #include <mutex>
 #include <thread>
 
@@ -34,6 +35,9 @@ struct MtiImuSource::Impl {
     clock::time_point last_rx{};
     std::atomic<bool> open{false};
 
+    ImuDeviceConfig   cfg;          // device config read back at startup
+    bool              have_cfg = false;
+
     MtiFramer framer;
     unsigned  bad_len_warned = 0;
 
@@ -68,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;
@@ -76,12 +82,96 @@ 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.
+        queryConfig(write);
         write(msgGoToMeasurement());    std::this_thread::sleep_for(60ms);
         // Drop any pre-config (old-format) bytes so the framer starts clean.
         ::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
+    // pending ops cancelled) so the later io.run() streams normally.
+    template <class WriteFn>
+    void queryConfig(WriteFn&& write) {
+        using namespace std::chrono_literals;
+        ImuDeviceConfig local;
+        MtiFramer cf([&](uint8_t mid, const uint8_t* d, std::size_t n) {
+            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());
+        write(msgReqPeriod());
+        write(msgReqOutputMode());
+        write(msgReqOutputSettings());
+        write(msgReqFilterProfile());
+        write(msgReqAvailFilterProfiles());
+
+        std::array<uint8_t, 256> buf{};
+        std::function<void()> rd = [&]() {
+            serial.async_read_some(boost::asio::buffer(buf),
+                                   [&](const boost::system::error_code& ec, std::size_t n) {
+                                       if (ec) return;  // cancelled / error: stop
+                                       cf.feed(buf.data(), n);
+                                       rd();
+                                   });
+        };
+        io.restart();
+        rd();
+        static_cast<void>(io.run_for(500ms));  // collect acks for up to half a second
+        // Drain the pending read so its by-ref handler can't fire later with the
+        // local buffer/framer already destroyed.
+        boost::system::error_code ec;
+        serial.cancel(ec);
+        io.restart();
+        static_cast<void>(io.run());
+        io.restart();
+
+        finalizeImuConfig(local);
+        if (local.valid) {
+            std::lock_guard<std::mutex> lock(mutex);
+            cfg = local;
+            have_cfg = true;
+            LOG_INFO << "MTi config: " << (local.product_code.empty() ? "?" : local.product_code)
+                     << " fw " << (local.firmware.empty() ? "?" : local.firmware)
+                     << " scenario " << int(local.scenario_type)
+                     << (local.scenario_label.empty() ? "" : " (" + local.scenario_label + ")")
+                     << " @ " << local.sample_rate_hz << " Hz";
+        } else {
+            LOG_WARN << "MTi: no configuration acks received (device may not answer Req* in this firmware)";
+        }
+    }
 };
 
 MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
@@ -136,4 +226,59 @@ std::optional<ImuSample> MtiImuSource::sample() {
     return impl_->latest;
 }
 
+std::optional<ImuDeviceConfig> MtiImuSource::config() const {
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    if (!impl_->have_cfg) return std::nullopt;
+    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

@@ -387,20 +387,30 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump, const Calib
 
 // Full-screen IMU view (toggled with 'i'): every MTi channel with units.
 Element imuDetailPanel(const ImuView& v) {
-    auto f2 = [](float x) {
+    // Fixed-width, right-aligned to 2 decimals. The constant width keeps the sign
+    // column and decimal point from jumping as values cross zero or change digit
+    // count, so the readout stays steady instead of flickering. The width is kept
+    // just wide enough for the field's range so the number sits close to its
+    // x/y/z label: 6 for the vectors (accel/gyro/mag stay well under ±100), 7 for
+    // orientation (so a 3-digit "-180.00" still fits without widening).
+    auto f2 = [](float x, int w) {
         char b[24];
-        std::snprintf(b, sizeof(b), "%.2f", x);
+        std::snprintf(b, sizeof(b), "%*.2f", w, x);
         return std::string(b);
     };
-    // One "LABEL (unit)  x=.. y=.. z=.." row for a 3-vector.
+    // A dim " │ " divider between value columns.
+    auto vsep = [] { return text(" \xE2\x94\x82 ") | dim; };
+    // One "LABEL (unit)  x:.. │ y:.. │ z:.." row for a 3-vector.
     auto vecRow = [&](const std::string& label, const char* unit, const float xyz[3],
                       Color c = Color::Default) {
         return hbox({
             text(label) | dim | size(WIDTH, EQUAL, 14),
             text(std::string(unit)) | dim | size(WIDTH, EQUAL, 9),
-            text("x " + f2(xyz[0])) | color(c) | size(WIDTH, EQUAL, 12),
+            text("x:" + f2(xyz[0], 6)) | color(c) | size(WIDTH, EQUAL, 8),
-            text("y " + f2(xyz[1])) | color(c) | size(WIDTH, EQUAL, 12),
+            vsep(),
-            text("z " + f2(xyz[2])) | color(c) | size(WIDTH, EQUAL, 12),
+            text("y:" + f2(xyz[1], 6)) | color(c) | size(WIDTH, EQUAL, 8),
+            vsep(),
+            text("z:" + f2(xyz[2], 6)) | color(c) | size(WIDTH, EQUAL, 8),
         });
     };
     const float ori[3] = {v.roll_deg, v.pitch_deg, v.yaw_deg};
@@ -413,9 +423,11 @@ Element imuDetailPanel(const ImuView& v) {
         body.push_back(hbox({
             text("ORIENTATION") | dim | size(WIDTH, EQUAL, 14),
             text("deg") | dim | size(WIDTH, EQUAL, 9),
-            text("roll " + f2(ori[0])) | bold | size(WIDTH, EQUAL, 14),
+            text("roll:" + f2(ori[0], 6)) | bold | size(WIDTH, EQUAL, 11),
-            text("pitch " + f2(ori[1])) | bold | size(WIDTH, EQUAL, 14),
+            vsep(),
-            text("yaw " + f2(ori[2])) | bold | size(WIDTH, EQUAL, 14),
+            text("pitch:" + f2(ori[1], 6)) | bold | size(WIDTH, EQUAL, 12),
+            vsep(),
+            text("yaw:" + f2(ori[2], 6)) | bold | size(WIDTH, EQUAL, 10),
         }));
         body.push_back(separator());
         body.push_back(vecRow("ACCEL", "m/s2", v.acc, Color::Cyan));
@@ -424,14 +436,50 @@ Element imuDetailPanel(const ImuView& v) {
         body.push_back(separator());
         body.push_back(hbox({
             text("TEMP") | dim | size(WIDTH, EQUAL, 14),
-            text(f2(v.temp_c) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18),
+            text(f2(v.temp_c, 6) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18),
             text("sample #" + std::to_string(v.sample_counter)) | dim,
         }));
     }
 
+    // IMU CONFIG section (device configuration read back at startup). Shown
+    // whenever it is known, even if the live stream is offline.
+    const auto& c = v.config;
+    if (c.present) {
+        auto cfgRow = [](const std::string& k, const std::string& val,
+                         Color vc = Color::Default) {
+            return hbox({text(k) | dim | size(WIDTH, EQUAL, 16),
+                         text(val) | color(vc)});
+        };
+        body.push_back(separator());
+        body.push_back(text("IMU CONFIG") | bold | color(Color::Magenta));
+        body.push_back(cfgRow("Product", c.product_code));
+        body.push_back(cfgRow("Firmware", c.firmware));
+        body.push_back(cfgRow("Device ID", c.device_id));
+        body.push_back(cfgRow("Output mode", c.output_mode));
+        body.push_back(cfgRow("Output fmt", c.output_settings));
+        body.push_back(cfgRow("Calib channels", c.channels));
+        body.push_back(cfgRow("Sample rate", c.sample_rate));
+        // XKF profile list: the active one is marked "●" and highlighted; the
+        // rest are dim "○". (Numeric profile IDs are intentionally hidden.)
+        body.push_back(text("Xsens Kalman Filter (XKF) profile") | dim);
+        if (c.xkf_profiles.empty()) {
+            body.push_back(hbox({text("  "), text("(not reported by device)") | dim}));
+        } else {
+            for (const auto& p : c.xkf_profiles) {
+                if (p.selected)
+                    body.push_back(hbox({text("  \xE2\x97\x8F ") | color(Color::Yellow),
+                                         text(p.name) | color(Color::Yellow) | bold,
+                                         text("  (selected)") | dim}));
+                else
+                    body.push_back(hbox({text("  \xE2\x97\x8B ") | dim,
+                                         text(p.name) | dim}));
+            }
+        }
+    }
+
     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}));
 }
@@ -515,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"),
             });
         }
@@ -566,12 +615,12 @@ void TuiUi::uiLoop() {
         }
         if (overlay != Overlay::None && e == Event::Escape) { overlay = Overlay::None; return true; }
         // Arrow keys nudge the gimbal in steps (only when no overlay is open):
-        // Left/Right = yaw -/+5%, Up/Down = pitch +/-10% of travel.
+        // Left/Right = yaw -/+5%, Up/Down = pitch -/+10% of travel.
         if (overlay == Overlay::None && sink_) {
             if (e == Event::ArrowLeft)  { sink_("gimbal nudge yaw -5");  return true; }
             if (e == Event::ArrowRight) { sink_("gimbal nudge yaw 5");   return true; }
-            if (e == Event::ArrowUp)    { sink_("gimbal nudge pitch 10"); return true; }
+            if (e == Event::ArrowUp)    { sink_("gimbal nudge pitch -10"); return true; }
-            if (e == Event::ArrowDown)  { sink_("gimbal nudge pitch -10"); return true; }
+            if (e == Event::ArrowDown)  { sink_("gimbal nudge pitch 10"); return true; }
         }
         if (!e.is_character()) return false;
         const std::string& c = e.character();
@@ -612,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_config.cpp

@@ -3,7 +3,9 @@
 #include "fgc/Config.h"
 
 #include <cmath>
+#include <cstdio>
 #include <cstdlib>
+#include <fstream>
 
 using namespace fgc;
 
@@ -78,3 +80,41 @@ TEST_CASE("updateIniSectionKeys appends keys missing from the section") {
     // re-parsing must see the appended key under [Motor]
     CHECK(out.find("pitch_zero_count = 7") != std::string::npos);
 }
+
+TEST_CASE("saveMotorCalibration round-trips the full per-axis map") {
+    // Write a starting config, save a calibrated Geometry into it, reload, and
+    // confirm every [Motor] parameter (incl. min/max_deg) survives, while another
+    // section is left untouched.
+    const std::string path = "test_calib_roundtrip.ini";
+    {
+        std::ofstream f(path, std::ios::trunc);
+        f << "[General]\ntower_name = rig7\n\n"
+             "[Motor]\n"
+             "yaw_counts_per_deg = 1.0\n"
+             "yaw_zero_count = 0\n"
+             "yaw_min_deg = -90\n"
+             "yaw_max_deg = 90\n"
+             "pitch_counts_per_deg = 1.0\n"
+             "pitch_zero_count = 0\n"
+             "pitch_min_deg = 0\n"
+             "pitch_max_deg = 60\n";
+    }
+
+    Geometry geo;
+    geo.yaw   = {983.33, 500123, -88.0, 92.0};
+    geo.pitch = {8333.33, -4200, -2.0, 61.5};
+    REQUIRE(saveMotorCalibration(path, geo));
+
+    AppConfig cfg = ConfigLoader::loadFromFile(path);
+    CHECK(cfg.general.tower_name == "rig7");  // unrelated section preserved
+    CHECK(cfg.geometry.yaw.counts_per_deg == doctest::Approx(983.33));
+    CHECK(cfg.geometry.yaw.zero_count == 500123);
+    CHECK(cfg.geometry.yaw.min_deg == doctest::Approx(-88.0));
+    CHECK(cfg.geometry.yaw.max_deg == doctest::Approx(92.0));
+    CHECK(cfg.geometry.pitch.counts_per_deg == doctest::Approx(8333.33));
+    CHECK(cfg.geometry.pitch.zero_count == -4200);
+    CHECK(cfg.geometry.pitch.min_deg == doctest::Approx(-2.0));
+    CHECK(cfg.geometry.pitch.max_deg == doctest::Approx(61.5));
+
+    std::remove(path.c_str());
+}

tests/test_mtiprotocol.cpp

@@ -144,6 +144,209 @@ 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[] = {
+        {msgReqProductCode(), kMidReqProductCode},
+        {msgReqDID(), kMidReqDID},
+        {msgReqFWRev(), kMidReqFWRev},
+        {msgReqPeriod(), kMidReqPeriod},
+        {msgReqOutputMode(), kMidSetOutputMode},
+        {msgReqOutputSettings(), kMidSetOutputSettings},
+        {msgReqFilterProfile(), kMidReqFilterProfile},
+        {msgReqAvailFilterProfiles(), kMidReqAvailFilterProf},
+    };
+    for (const auto& q : qs) {
+        CHECK(q.m[0] == kMtiPreamble);
+        CHECK(q.m[1] == kMtiBid);
+        CHECK(q.m[2] == q.mid);
+        CHECK(q.m[3] == 0);   // request => empty data field
+        CHECK(frameChecksumOk(q.m));
+    }
+}
+
+TEST_CASE("applyImuConfigAck decodes each ack type") {
+    ImuDeviceConfig c;
+
+    SUBCASE("DeviceID is a 32-bit big-endian serial") {
+        uint8_t d[] = {0x00, 0x99, 0x0A, 0xBC};
+        CHECK(applyImuConfigAck(c, kMidDeviceID, d, sizeof(d)));
+        CHECK(c.has_device_id);
+        CHECK(c.device_id == 0x00990ABCu);
+    }
+
+    SUBCASE("ProductCode trims trailing spaces") {
+        const char* s = "MTi-28A53G35   ";
+        CHECK(applyImuConfigAck(c, kMidProductCode,
+                                reinterpret_cast<const uint8_t*>(s), 15));
+        CHECK(c.product_code == "MTi-28A53G35");
+    }
+
+    SUBCASE("FirmwareRev with build number") {
+        uint8_t d[] = {2, 8, 1, 0, 0, 0, 25};  // 2.8.1 build 25
+        CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d)));
+        CHECK(c.firmware == "2.8.1 build 25");
+    }
+
+    SUBCASE("FirmwareRev short form (major.minor.rev only)") {
+        uint8_t d[] = {1, 2, 3};
+        CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d)));
+        CHECK(c.firmware == "1.2.3");
+    }
+
+    SUBCASE("Period yields 100 Hz from 0x0480") {
+        uint8_t d[] = {0x04, 0x80};  // 1152 => 115200/1152 = 100 Hz
+        CHECK(applyImuConfigAck(c, kMidReqPeriodAck, d, sizeof(d)));
+        finalizeImuConfig(c);
+        CHECK(c.has_period);
+        CHECK(c.sample_rate_hz == doctest::Approx(100.0f));
+    }
+
+    SUBCASE("OutputMode 0x0007 = Temp + Calibrated + Orientation") {
+        uint8_t d[] = {0x00, 0x07};
+        CHECK(applyImuConfigAck(c, kMidSetOutputModeAck, d, sizeof(d)));
+        CHECK(c.out_temperature);
+        CHECK(c.out_calibrated);
+        CHECK(c.out_orientation);
+        CHECK_FALSE(c.out_auxiliary);
+    }
+
+    SUBCASE("OutputSettings 0x00000005 = Euler + sample counter + float, all channels") {
+        uint8_t d[] = {0x00, 0x00, 0x00, 0x05};
+        CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d)));
+        CHECK(c.orientation_mode == "Euler");
+        CHECK(c.timestamp_mode == "Sample counter");
+        CHECK(c.data_format == "Float");
+        CHECK(c.acc_enabled);
+        CHECK(c.gyr_enabled);
+        CHECK(c.mag_enabled);
+    }
+
+    SUBCASE("OutputSettings with disabled mag and fixed-point format") {
+        // bit6 set (disable mag), output format bits 9:8 = 01 (Fixed 12.20).
+        uint32_t s = 0x05 | 0x40 | 0x100;
+        uint8_t d[] = {uint8_t(s >> 24), uint8_t(s >> 16), uint8_t(s >> 8), uint8_t(s)};
+        CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d)));
+        CHECK(c.acc_enabled);
+        CHECK(c.gyr_enabled);
+        CHECK_FALSE(c.mag_enabled);
+        CHECK(c.data_format == "Fixed 12.20");
+    }
+
+    SUBCASE("unknown MID is ignored") {
+        uint8_t d[] = {0};
+        CHECK_FALSE(applyImuConfigAck(c, 0x99, d, sizeof(d)));
+        CHECK_FALSE(c.valid);
+    }
+}
+
+TEST_CASE("scenario label resolves against the available-profiles list") {
+    ImuDeviceConfig c;
+    // AvailableFilterProfiles: two 22-byte records (type, version, 20-byte label).
+    std::vector<uint8_t> d;
+    auto addProfile = [&](uint8_t type, uint8_t ver, const std::string& label) {
+        d.push_back(type);
+        d.push_back(ver);
+        std::string padded = label;
+        padded.resize(20, ' ');
+        d.insert(d.end(), padded.begin(), padded.end());
+    };
+    addProfile(39, 11, "General");
+    addProfile(40, 11, "High_mag_dep");
+    CHECK(applyImuConfigAck(c, kMidAvailFilterProf, d.data(), d.size()));
+    REQUIRE(c.available_profiles.size() == 2);
+    CHECK(c.available_profiles[0].label == "General");
+    CHECK(c.available_profiles[1].label == "High_mag_dep");
+
+    // Current filter profile ack: VERSION, FILTERPROFILE(type).
+    uint8_t fp[] = {11, 40};
+    CHECK(applyImuConfigAck(c, kMidReqFilterProfileAck, fp, sizeof(fp)));
+    finalizeImuConfig(c);
+    CHECK(c.has_scenario);
+    CHECK(c.scenario_type == 40);
+    CHECK(c.scenario_label == "High_mag_dep");
+}
+
+TEST_CASE("a full ack stream decodes through the framer") {
+    // Concatenate realistic acks and run them through MtiFramer, mirroring how
+    // the live handshake collects them.
+    ImuDeviceConfig c;
+    MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
+        applyImuConfigAck(c, mid, p, n);
+    });
+    auto feed = [&](const std::vector<uint8_t>& m) { fr.feed(m.data(), m.size()); };
+
+    feed(mtiMessage(kMidProductCode,
+                    {'M','T','i','-','2','8'}));
+    feed(mtiMessage(kMidDeviceID, {0x00, 0x99, 0x0A, 0xBC}));
+    feed(mtiMessage(kMidSetOutputModeAck, {0x00, 0x07}));
+    feed(mtiMessage(kMidSetOutputSettingsAck, {0x00, 0x00, 0x00, 0x05}));
+    feed(mtiMessage(kMidReqPeriodAck, {0x04, 0x80}));
+    feed(mtiMessage(kMidReqFilterProfileAck, {11, 39}));
+    finalizeImuConfig(c);
+
+    CHECK(c.valid);
+    CHECK(c.product_code == "MTi-28");
+    CHECK(c.device_id == 0x00990ABCu);
+    CHECK(c.out_orientation);
+    CHECK(c.orientation_mode == "Euler");
+    CHECK(c.sample_rate_hz == doctest::Approx(100.0f));
+    CHECK(c.scenario_type == 39);
+}
+
 TEST_CASE("framer rejects a bad checksum and a wrong-length payload") {
     std::vector<uint8_t> d(kMTDataLen, 0);
     auto frame = mtiMessage(kMidMTData, d);