added gimbal calibration command, added TUI prompts

CMakeLists.txt

@@ -52,6 +52,10 @@ add_library(fgc_core STATIC
   src/core/CommandParser.cpp
   src/core/HelpText.cpp
   src/core/DumpParser.cpp
+  src/core/DiagParser.cpp
+  src/core/Calibration.cpp
+  src/core/CalibrationRoutine.cpp
+  src/core/MtiProtocol.cpp
   src/ui/UiSnapshot.cpp
   src/ui/HeadlessUi.cpp
   ini.c
@@ -73,6 +77,7 @@ set(FGC_SOURCES
   src/camera/JpegXlEncoder.cpp
   src/camera/ImagePipeline.cpp
   src/serial/SerialMotorController.cpp
+  src/serial/MtiImuSource.cpp
 )
 if(WITH_MQTT)
   list(APPEND FGC_SOURCES src/mqtt/MqttControlChannel.cpp)

README.md

@@ -96,19 +96,29 @@ While running, the program reads commands from stdin (one per line):
 
 | Command | Action |
 |---------|--------|
-| `start` | Start the capture cycle. |
+| `start` / `stop` | Start / stop the capture cycle (the gimbal stays powered and homed). |
-| `stop` | Stop capturing (the gimbal stays powered and homed). |
+| `gimbal move <yaw>,<pitch>` | Aim at an absolute heading/elevation in **degrees** (calibrated). |
+| `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 …` | Other motor controls — `steps`/`nudge`/`stop`/`speed`/`reset`/`enable`/`disable`/`setpos`/`status`. |
 | `debug` | Toggle debug-level logging on/off. |
 | `set fps <n>` | Capture rate, in images per second. |
 | `set camera fps <n>` | Camera sensor frame rate. |
 | `set camera jxlq <d>` | JPEG XL quality as butteraugli **distance** (lower = higher quality / larger files). |
 | `set camera jxle <n>` | JPEG XL encoder effort (higher = slower, smaller). |
 | `set camera display <0\|1>` | Toggle the local preview window. |
-| `set motorctl <raw>` | Send a raw command string straight to the motor controller. |
 | `trace <cat> [on\|off]` | Enable/disable a wire-trace category live (e.g. `trace serial`, `trace mqtt off`). |
 | `trace all` / `trace off` | Enable every category / silence all of them. |
+| `help [topic]` | List commands / expand one section. |
 | `exit` | Stop everything and quit (Ctrl-D also works). |
 
+The **Xsens MTi IMU** (set `[Features] enable_imu` + `[IMU] device`) drives the Sensors panel's live
+roll/pitch/yaw and powers `gimbal calib`. In the TUI, `g` and `i` open full-screen **gimbal** and
+**IMU** views, and an **activity strip** below the log shows the running calibration/diagnostics/scan
+plus the last result. See [docs/configuration.md](docs/configuration.md).
+
 A typical first bring-up: `scripts/run.sh --init` to home the gimbal, then type `start` once you've confirmed
 telemetry looks sane; adjust `set fps` / `set camera jxlq` live as needed. See
 [docs/configuration.md](docs/configuration.md) for the full `config.ini` reference.

config/config.example.ini

@@ -35,6 +35,16 @@ id_Cam4 =
 device = /dev/ttyACM0
 baud = 115200
 
+[IMU]
+; Xsens MTi orientation/inertial sensor. Wired to the LattePanda's RS-232 header
+; (an onboard hardware UART), so this is a stable /dev/ttyS* node - NOT a USB
+; device and NOT /dev/ttyUSB0-3 (those are the Quectel modem). Find it with
+;   ls /dev/ttyS*  ;  dmesg | grep -iE 'ttyS|LPSS|HSUART'  (often /dev/ttyS4)
+; Enable with [Features] enable_imu = true. The host reconfigures the MTi to a
+; Euler + calibrated 100 Hz stream at startup.
+device =
+baud = 115200
+
 [Motor]
 ; Degrees<->encoder-counts calibration for each axis. The firmware speaks only
 ; in absolute encoder counts; these map them to the heading/elevation degrees
@@ -76,8 +86,10 @@ output_dir =
 enable_mqtt = true
 enable_camera = true
 enable_serial = true
+enable_imu = false
 mock_camera = false
 mock_serial = false
+mock_imu = false
 
 [UI]
 ; Full-screen terminal dashboard (sectioned, colored, live status + log pane).

docs/architecture.md

@@ -11,13 +11,19 @@ The design separates **policy** (the control logic) from **mechanism** (the I/O
 - **`fgc_core`** — an SDK-independent static library: typed configuration, path resolution, logging, the
   telemetry/command parsers, and the `CaptureScheduler` (control state machine). Depends on nothing
   proprietary, so it builds and unit-tests anywhere.
-- **Three interfaces** abstract the outside world, each with a real and a mock/null implementation:
+- **Four interfaces** abstract the outside world, each with a real and a mock/null implementation:
 
 | Interface | Real | Mock / Null |
 |-----------|------|-------------|
 | `IMotorController` | `SerialMotorController` (Boost.Asio) | `MockMotorController` (simulated sweep) |
 | `IControlChannel` | `MqttControlChannel` (Paho) | `NullControlChannel` (no broker) |
 | `ICameraSource` | `VimbaCameraSource` (Vimba X) | `MockCameraSource` (synthetic frames) |
+| `IImuSource` | `MtiImuSource` (Xsens MTi, Boost.Asio) | `MockImuSource` (synthetic orientation) |
+
+The IMU is optional (`[Features] enable_imu`); it feeds the Sensors panel and the IMU-referenced
+`gimbal calib`. Long-running operations (`gimbal calib` on a worker thread, `gimbal diag` captured from
+the firmware) publish progress + results into the `UiSnapshot` activity strip, polled on the control
+thread so all geometry/state mutation stays single-threaded.
 
 `Application` picks real vs mock from config + CLI, wires everything to the `ImagePipeline` and
 `CaptureScheduler`, and runs the loop. Selecting mocks lets the whole system run with **no hardware or broker**.

docs/configuration.md

@@ -38,8 +38,12 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config
 | `Features` | `enable_mqtt` | bool | `true` | Use MQTT (vs null channel) |
 | `Features` | `enable_camera` | bool | `true` | (reserved) |
 | `Features` | `enable_serial` | bool | `true` | (reserved) |
+| `Features` | `enable_imu` | bool | `false` | Use the Xsens MTi orientation/IMU |
 | `Features` | `mock_camera` | bool | `false` | Use the simulated camera |
 | `Features` | `mock_serial` | bool | `false` | Use the simulated motor controller |
+| `Features` | `mock_imu` | bool | `false` | Use the simulated IMU instead of the MTi |
+| `IMU` | `device` | string | — | MTi serial device (see `[IMU]` note); required when `enable_imu` |
+| `IMU` | `baud` | int | `115200` | MTi serial baud rate |
 | `Logging` | `level` | enum | `info` | Linear log level (`--log-level` overrides) |
 | `Logging` | `trace` | csv | — | Wire-trace categories, off by default (`--trace` overrides) |
 | `UI` | `enable_tui` | bool | `false` | Full-screen terminal dashboard (`--tui`/`--no-tui` override; needs `WITH_TUI=ON`) |
@@ -63,6 +67,18 @@ coordinates, or leave it blank to generate `yaw_intervals × pitch_levels` point
 
 Camera index → output subfolder defaults to `RGB`, `ACR`, `NIR` (`CameraConfig::labels`).
 
+### `[IMU]` — Xsens MTi orientation sensor
+
+Enable with `[Features] enable_imu = true` and point `[IMU] device` at the MTi's serial node. On the
+LattePanda the MTi is wired to the **RS-232 header**, i.e. an onboard hardware UART — a stable
+`/dev/ttyS*` node (e.g. `/dev/ttyS4`), **not** a USB device and not `/dev/ttyUSB0-3` (those are the
+modem). Find it with `ls /dev/ttyS*` / `dmesg | grep -iE 'ttyS|LPSS'`, or probe for the `0xFA`-framed
+stream. At startup the host reconfigures the MTi (`GoToConfig → SetOutputMode → SetOutputSettings →
+GoToMeasurement`) to a 100 Hz Euler + calibrated stream — so the device will start streaming even if it
+was left in Config state. Yaw is reported as a **0..360 heading** (not the MTi's native −180..180). Set
+`[Features] mock_imu = true` to use a synthetic IMU on dev machines (no hardware). Protocol/units are
+documented in the modules reference (`MtiProtocol`).
+
 ### Secrets
 
 `mqtt_user` / `mqtt_pw` are read from the environment variables **`FGC_MQTT_USER` / `FGC_MQTT_PW`** first,
@@ -129,17 +145,38 @@ forwards keystrokes/typed commands back through the same command queue the conso
 operation is unchanged and remains the default — the same binary runs under systemd/ssh/pipes with
 logs on stdout.
 
-Panels (MVP): **Gimbal** (per-axis state, heading, encoder counts, flag badges, target),
+Dashboard panels: **Gimbal** (per-axis state, heading, encoder counts, flag badges, target),
-**Sensors** (DHT11 + Xsens MTi — shown as *pending integration* until those drivers land),
+**Sensors** (DHT11 still *pending*; the **Xsens MTi** shows live roll/pitch/yaw + temp once
-**Camera** (count, capture state, rate, last capture), **Connectivity** (MQTT state, broker, tower,
+`enable_imu`), **Camera** (count, capture state, rate, last capture), **Connectivity** (MQTT state,
-control mode, target heading). Adding a panel later (e.g. computer vision) is a struct in
+broker, tower, control mode, target heading). Adding a panel later is a struct in `UiSnapshot.h` plus
-`UiSnapshot.h` plus one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp).
+one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp).
 
-Keys (shown in the bottom bar): `s` start · `x` stop · `h` home · `r` reset · `:` open a command
+**Expanded takeover views** replace the dashboard body full-screen (`Esc` or the same key closes):
-line (any console/`set motorctl …` command) · `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
+- **Gimbal** (`g`) — both axes side by side: live telemetry, the decoded firmware register **dump**
-output is diverted from stdout into the on-screen log pane via a `Logger` sink, so the screen is
+  (`d` requests a fresh one), the homing limits, and the **last calibration** result (per-axis
-never corrupted.
+  `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.
+
+**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
+scan) **and persists the last calibration/diagnostics result** (PASS/FAIL, age) so it doesn't scroll
+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
+was launched with (replacing those keys in place, preserving 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 ·
+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.
 
 Build without it (`-DWITH_TUI=OFF`) for a smaller, dependency-free binary; `--tui` then warns and
 runs headless.

docs/known-issues.md

@@ -50,3 +50,10 @@ doctest unit-test suite (`ctest`).
 - Make the camera index→label map and JPEG XL defaults fully config-driven.
 - Reintroduce optional image upload to the ground station, config-driven (the old hardcoded NFS/SMB upload was
   removed).
+- **`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
+  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.
+- DHT11 temperature/humidity is still a Sensors-panel placeholder (the IMU half is integrated).

docs/modules-reference.md

@@ -6,16 +6,22 @@ Per-file reference for the refactored tree, plus the shared data structures.
 
 | File | Contents |
 |------|----------|
-| [include/fgc/Config.h](../include/fgc/Config.h), [src/core/Config.cpp](../src/core/Config.cpp) | Typed `AppConfig` (General/Network/Serial/Camera/Paths/Features/Logging/Motor/Scan) + `ConfigLoader` (INI parse, env overrides, validation) |
+| [include/fgc/Config.h](../include/fgc/Config.h), [src/core/Config.cpp](../src/core/Config.cpp) | Typed `AppConfig` (General/Network/Serial/Camera/Paths/Features/Logging/UI/Motor/Scan/**IMU**) + `ConfigLoader` (INI parse, env overrides, validation) |
 | [include/fgc/Paths.h](../include/fgc/Paths.h), [src/core/Paths.cpp](../src/core/Paths.cpp) | `~`/`$ENV` expansion, executable dir, config search order, default output dir |
 | [include/fgc/Logger.h](../include/fgc/Logger.h), [src/core/Logger.cpp](../src/core/Logger.cpp) | Leveled, thread-safe logger + per-category wire trace; `LOG_TRACE..LOG_ERROR`, `LOG_TRACE_CAT` |
 | [include/fgc/Geometry.h](../include/fgc/Geometry.h), [src/core/Geometry.cpp](../src/core/Geometry.cpp) | Per-axis degrees↔encoder-counts affine map (`[Motor]` calibration) |
 | [include/fgc/ScanGrid.h](../include/fgc/ScanGrid.h), [src/core/ScanGrid.cpp](../src/core/ScanGrid.cpp) | Capture waypoints (CSV or generated) + ping-pong cursor (`[Scan]`) |
 | [include/fgc/TelemetryParser.h](../include/fgc/TelemetryParser.h), [src/core/TelemetryParser.cpp](../src/core/TelemetryParser.cpp) | `parseTelemetryLine` (firmware `ST` line) → `std::optional<MotorTelemetry>` |
 | [include/fgc/CommandParser.h](../include/fgc/CommandParser.h), [src/core/CommandParser.cpp](../src/core/CommandParser.cpp) | `parseCommand` whitespace tokenizer → `Command` |
-| [include/fgc/CaptureScheduler.h](../include/fgc/CaptureScheduler.h), [src/core/CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp) | Capture state machine over the interfaces; injectable clock |
+| [include/fgc/HelpText.h](../include/fgc/HelpText.h), [src/core/HelpText.cpp](../src/core/HelpText.cpp) | Operator command catalog (`helpCatalog`) + `renderHelp` (console & TUI help) |
-| [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, console commands, `buildSnapshot()` |
+| [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/ui/UiSnapshot.h](../include/fgc/ui/UiSnapshot.h), [src/ui/UiSnapshot.cpp](../src/ui/UiSnapshot.cpp) | Plain-data view model + pure formatting helpers (state label/colour, degrees, time-ago, pending-sensors) |
+| [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/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/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) |
 | [include/fgc/ui/HeadlessUi.h](../include/fgc/ui/HeadlessUi.h), [src/ui/HeadlessUi.cpp](../src/ui/HeadlessUi.cpp) | Default line console: stdin → command sink; logs via the default stdout/stderr writer |
 | [ini.c](../ini.c), [ini.h](../ini.h) | Bundled third-party inih INI parser |
 
@@ -26,6 +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/ui/IUserInterface.h](../include/fgc/ui/IUserInterface.h) | `IUserInterface` | `UiSnapshot` |
 
 ## Real implementations (SDK-gated)
@@ -33,6 +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/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` |
@@ -43,9 +51,10 @@ Per-file reference for the refactored tree, plus the shared data structures.
 
 | File | Implements |
 |------|-----------|
-| [include/fgc/mock/MockMotorController.h](../include/fgc/mock/MockMotorController.h) | Simulated sweeping gimbal |
+| [include/fgc/mock/MockMotorController.h](../include/fgc/mock/MockMotorController.h) | Simulated sweeping gimbal (incl. canned `DUMP`) |
 | [include/fgc/mock/NullControlChannel.h](../include/fgc/mock/NullControlChannel.h) | No-op channel; auto-sweep |
 | [include/fgc/mock/MockCameraSource.h](../include/fgc/mock/MockCameraSource.h) | Synthetic gradient frames |
+| [include/fgc/mock/MockImuSource.h](../include/fgc/mock/MockImuSource.h) | Synthetic IMU (sinusoidal orientation, gravity on accZ) |
 
 ## Entry point & scripts
 
@@ -74,11 +83,21 @@ Serialized to the CamEvent JSON payload (see [mqtt-api.md](mqtt-api.md)).
 ### `Frame` ([ICameraSource.h](../include/fgc/ICameraSource.h))
 Owned pixel buffer + `width`, `height`, `channels` (1 or 3), `timestamp_ms`, `cam_id`.
 
+### `ImuSample` ([MtiProtocol.h](../include/fgc/MtiProtocol.h))
+One decoded Xsens MTi reading: `temp_c` (°C), `acc[3]` (m/s², incl. gravity), `gyr[3]` (rad/s),
+`mag[3]` (a.u., earth-normalized), `roll_deg` (−180..180), `pitch_deg` (−90..90), `yaw_deg`
+(**0..360 heading** — the MTi's native −180..180 is shifted by `parseMTData`), `sample_counter`.
+`gimbal calib` phase-unwraps the swept yaw (`unwrapNear`) so a sweep crossing 0/360 still fits a clean
+line.
+
 ## On-disk artifacts
 
 | Artifact | Path | Format |
 |----------|------|--------|
 | Captured images | `<output_dir>/<RGB\|ACR\|NIR>/<unix_ms>.jxl` | JPEG XL, rotated 90° CCW |
 | Demo placeholder | `bin/x64/Release/test_smoke.jxl` | copied verbatim in demo mode |
+| Diagnostics log | `<data_dir>/fire_gimbal_control/logs/diag_<ts>.log` | raw `DG` stream + parsed summary |
+| Calibration log | `<data_dir>/fire_gimbal_control/logs/calib_<ts>.log` | per-axis samples + the least-squares fit |
 
-State otherwise lives in memory; diagnostics go to stdout/stderr (no log files, no database).
+Diagnostics/calibration results are also surfaced live in the TUI (activity strip + gimbal `g` view)
+and applied to the running session's geometry; other state lives in memory.

include/fgc/Application.h

@@ -22,6 +22,7 @@ struct RuntimeOptions {
 
     std::string log_level;         // empty => default
     std::string trace_categories;  // comma list (serial,mqtt,camera,control,all); empty => unset
+    std::string config_path;       // resolved config.ini path (to save calibration back); empty => none
 };
 
 // Owns the component lifecycle and the control loop. Builds the concrete

include/fgc/Calibration.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <utility>
+#include <vector>
+
+namespace fgc {
+
+// Pure math helpers for `gimbal calib`. Kept I/O-free in fgc_core so the fit can
+// be unit-tested independently of the threaded routine that drives the hardware.
+
+struct LinearFit {
+    bool   ok = false;     // false if < 2 points or x has no spread
+    double slope = 0;      // y = slope*x + intercept
+    double intercept = 0;
+    double r2 = 0;         // coefficient of determination (1 = perfect)
+    int    n = 0;
+};
+
+// Least-squares fit of y over x. For calibration: x = measured degrees,
+// y = motor encoder counts ⇒ slope = counts_per_deg, intercept = zero_count.
+LinearFit linearFit(const std::vector<std::pair<double, double>>& xy);
+
+// Circular mean of angles in degrees, robust to ±180° wrap (e.g. IMU heading).
+// Returns a value in (-180, 180]. Empty input returns 0.
+double circularMeanDeg(const std::vector<double>& degs);
+
+// Phase-unwrap `deg` relative to the previous (already-unwrapped) sample: shift it
+// by whole turns so it lands within ±180° of `prev`. Feeding a smoothly-swept
+// angle through this in sequence removes the 0/360 (or ±180) discontinuity, so the
+// calibration sees a continuous curve for the linear fit.
+double unwrapNear(double prev, double deg);
+
+}  // namespace fgc

include/fgc/CalibrationRoutine.h

@@ -0,0 +1,87 @@
+#pragma once
+
+#include "fgc/Geometry.h"
+
+#include <atomic>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace fgc {
+
+class IMotorController;
+class IImuSource;
+
+// Live progress of a running calibration, for the TUI activity strip.
+struct CalibProgress {
+    bool        running = false;
+    char        axis    = '?';   // 'Y' / 'P'
+    int         step    = 0;     // 1-based current step
+    int         total   = 0;     // steps per axis
+    std::string phase;           // "moving" / "dwelling" / "fitting" / ...
+};
+
+// Structured outcome of the last completed calibration (persists for display).
+struct CalibReport {
+    struct Axis {
+        char   axis = '?';
+        bool   ok = false;
+        double counts_per_deg = 0;
+        long   zero_count = 0;
+        double r2 = 0;
+        int    n = 0;
+    };
+    bool              valid = false;
+    long long         ts_ms = 0;      // wall-clock completion time (epoch ms)
+    bool              all_ok = false;
+    std::vector<Axis> axes;
+};
+
+// `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own
+// thread (the motor/imu/Logger interfaces are thread-safe), sweeping each axis
+// across its homed soft-limit travel in equal step intervals, dwelling at each to
+// record the IMU orientation, then least-squares fitting counts vs degrees. The
+// resulting Geometry is published via takeResult() for the main thread to apply
+// to the live session; the raw samples + fit are written to a logfile. Progress
+// is streamed to the LOG pane via LOG_INFO. Cancellable and one-at-a-time.
+class CalibrationRoutine {
+public:
+    CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial);
+    ~CalibrationRoutine();
+
+    // Begin on a worker thread. Logs a reason and returns false if already
+    // running (prechecks happen on the worker and abort there).
+    bool start();
+    void cancel();
+    bool running() const { return running_.load(); }
+
+    // If a finished run produced a new calibration, returns it once (then clears).
+    std::optional<Geometry> takeResult();
+
+    // Live progress (thread-safe copy) and the last completed report (persists).
+    CalibProgress progress() const;
+    CalibReport   report() const;
+
+private:
+    void run();
+    void setProgress(char axis, int step, int total, const char* phase);
+
+    IMotorController& motor_;
+    IImuSource&       imu_;
+    Geometry          initial_;
+
+    std::thread       thread_;
+    std::atomic<bool> running_{false};
+    std::atomic<bool> cancel_{false};
+
+    mutable std::mutex      result_mutex_;
+    std::optional<Geometry> result_;
+    CalibReport             report_;
+
+    mutable std::mutex progress_mutex_;
+    CalibProgress      progress_;
+};
+
+}  // namespace fgc

include/fgc/CaptureScheduler.h

@@ -40,6 +40,11 @@ public:
     void   setImageRate(double rate);    // images per second
     double imageRate() const { return image_rate_; }
 
+    // Replace the degrees<->counts maps (e.g. after `gimbal calib`) so subsequent
+    // scan waypoints target with the calibrated conversion. Called on the control
+    // thread, same as tick(), so no locking is needed.
+    void setGeometry(const Geometry& g) { geometry_ = g; }
+
     // Run one iteration of the control logic.
     void tick();
 

include/fgc/Config.h

@@ -4,6 +4,7 @@
 
 #include <map>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace fgc {
@@ -43,8 +44,17 @@ struct FeaturesConfig {
     bool enable_mqtt   = true;
     bool enable_camera = true;
     bool enable_serial = true;
+    bool enable_imu    = false;  // Xsens MTi orientation/IMU (off by default)
     bool mock_camera   = false;  // use a simulated camera instead of Vimba X
     bool mock_serial   = false;  // use a simulated motor controller
+    bool mock_imu      = false;  // use a simulated IMU instead of the MTi
+};
+
+// [IMU]: Xsens MTi connected over the LattePanda's RS-232 UART (a hardware
+// /dev/ttyS* node, stable across reboots — not a USB device).
+struct ImuConfig {
+    std::string  device = "";       // e.g. /dev/ttyS4; empty => required when enabled
+    unsigned int baud   = 115200;
 };
 
 struct LoggingConfig {
@@ -78,6 +88,7 @@ struct AppConfig {
     UiConfig       ui;        // [UI] terminal dashboard toggle
     Geometry       geometry;  // [Motor] degrees<->counts maps (yaw + pitch)
     ScanConfig     scan;      // [Scan] grid source
+    ImuConfig      imu;       // [IMU] Xsens MTi serial device
 
     // Capture rate in images/second (derived from general.image_interval).
     double image_rate() const;
@@ -99,4 +110,15 @@ public:
     static AppConfig fromMap(const std::map<std::string, std::string>& kv);
 };
 
+// Return `contents` with the given `key = value` pairs set under `[section]`:
+// existing keys in that section are replaced in place (comments/order preserved),
+// and any missing ones are appended in a `[section]` block at the end. Pure (no
+// I/O) so it is unit-testable.
+std::string updateIniSectionKeys(const std::string& contents, const std::string& section,
+                                 const std::vector<std::pair<std::string, std::string>>& kv);
+
+// Persist the live `[Motor]` calibration (counts_per_deg / zero_count for both
+// axes) back into the INI file at `path`. Returns true on success.
+bool saveMotorCalibration(const std::string& path, const Geometry& geo);
+
 }  // namespace fgc

include/fgc/DiagParser.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace fgc {
+
+// Structured decode of the firmware DIAG output stream. The firmware emits
+// (interleaved with periodic ST lines), per axis:
+//   DG BEGIN <Y|P>
+//   DG <Y|P> S<speed> <FWD|REV> ERR_PEAK n ERR_RMS n ERR_STILL n
+//            CS_MIN n CS_MAX n SG_MIN n PWM_AVG n FLAGS 0x.. <PASS|FAIL>   (x6)
+//   DG <Y|P> RESULT <PASS|FAIL>
+//   ... and finally:  DG DONE
+// Pure (no I/O); mirrors DumpParser so it can be unit-tested in fgc_core.
+
+struct DiagTest {
+    int      speed = 0;          // S<speed>
+    bool     fwd = true;         // FWD vs REV
+    long     err_peak = 0;       // encoder following error (counts); -1 = no encoder
+    long     err_rms = 0;
+    long     err_still = 0;
+    int      cs_min = 0, cs_max = 0;
+    int      sg_min = 0;
+    int      pwm_avg = 0;
+    unsigned flags = 0;          // fault bitmask
+    bool     pass = false;
+};
+
+struct DiagAxis {
+    char                  axis = '?';     // 'Y' / 'P'
+    bool                  has_result = false;
+    bool                  pass = false;   // axis RESULT
+    std::vector<DiagTest> tests;
+};
+
+struct DiagResult {
+    bool                  valid = false;  // at least one well-formed DG line
+    bool                  done = false;   // saw "DG DONE"
+    std::vector<DiagAxis> axes;
+
+    bool allPass() const;                 // every axis with a result passed (and >=1 axis)
+};
+
+// Parse a captured DG block (only "DG ..." lines matter; other lines are ignored).
+DiagResult parseDiag(const std::string& block);
+
+// Human-readable summary lines for a decoded diag.
+std::vector<std::string> formatDiag(const DiagResult& d);
+
+}  // namespace fgc

include/fgc/IImuSource.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "fgc/MtiProtocol.h"  // ImuSample
+
+#include <optional>
+
+namespace fgc {
+
+// Abstraction over the orientation/inertial sensor (Xsens MTi). Implemented by
+// MtiImuSource (RS-232/UART binary stream) and MockImuSource (synthetic). Runs
+// on its own thread; start() must not block the control loop.
+class IImuSource {
+public:
+    virtual ~IImuSource() = default;
+
+    virtual void start() = 0;
+    virtual void stop()  = 0;
+
+    // Whether a valid, recent reading is available.
+    virtual bool connected() const = 0;
+
+    // Latest reading, or nullopt if none/stale.
+    virtual std::optional<ImuSample> sample() = 0;
+};
+
+}  // namespace fgc

include/fgc/IMotorController.h

@@ -58,6 +58,12 @@ public:
     // Thread-safe in implementations.
     virtual std::string lastDump() = 0;
 
+    // The most recently completed firmware DIAG block (the "DG ..." lines from
+    // "DG BEGIN" through "DG DONE"), or "" if none. diagSeq() increments on each
+    // completed capture so callers can detect a fresh result. Thread-safe.
+    virtual std::string lastDiag() = 0;
+    virtual unsigned    diagSeq() const = 0;
+
     // Whether the underlying link is usable.
     virtual bool connected() const = 0;
 };

include/fgc/MtiImuSource.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "fgc/IImuSource.h"
+
+#include <memory>
+#include <string>
+
+namespace fgc {
+
+// Real IMU backend: reads the Xsens MTi binary MTData stream over a serial port
+// (RS-232 via the LattePanda UART). Configures the device to the Euler +
+// calibrated output at startup, then streams read-only. Boost.Asio detail is
+// hidden behind a pImpl (mirrors SerialMotorController).
+class MtiImuSource : public IImuSource {
+public:
+    MtiImuSource(std::string device, unsigned int baud);
+    ~MtiImuSource() override;
+
+    void                     start() override;
+    void                     stop() override;
+    bool                     connected() const override;
+    std::optional<ImuSample> sample() override;
+
+private:
+    struct Impl;
+    std::unique_ptr<Impl> impl_;
+};
+
+}  // namespace fgc

include/fgc/MtiProtocol.h

@@ -0,0 +1,93 @@
+#pragma once
+
+#include <cstdint>
+#include <functional>
+#include <optional>
+#include <vector>
+
+namespace fgc {
+
+// Xsens MTi (legacy MT0100P) binary protocol helpers. The MTi streams MTData
+// messages over RS-232/UART; this module frames the byte stream, validates
+// checksums, builds the config messages, and decodes a combined MTData payload
+// into an ImuSample. Kept pure (no I/O) so it is unit-testable and lives in
+// fgc_core (mirrors TelemetryParser / DumpParser).
+//
+// Frame layout:  PRE(0xFA) BID(0xFF) MID LEN DATA[LEN] CS
+// Checksum: (BID + MID + LEN + ΣDATA + CS) & 0xFF == 0. All multi-byte values
+// are big-endian.
+
+// Protocol constants.
+inline constexpr uint8_t kMtiPreamble = 0xFA;
+inline constexpr uint8_t kMtiBid      = 0xFF;
+inline constexpr uint8_t kMidGoToConfig       = 0x30;
+inline constexpr uint8_t kMidGoToConfigAck    = 0x31;
+inline constexpr uint8_t kMidGoToMeasurement  = 0x10;
+inline constexpr uint8_t kMidGoToMeasAck      = 0x11;
+inline constexpr uint8_t kMidSetOutputMode    = 0xD0;
+inline constexpr uint8_t kMidSetOutputModeAck = 0xD1;
+inline constexpr uint8_t kMidSetOutputSettings    = 0xD2;
+inline constexpr uint8_t kMidSetOutputSettingsAck = 0xD3;
+inline constexpr uint8_t kMidMTData = 0x32;
+inline constexpr uint8_t kMidError  = 0x42;
+
+// OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04).
+inline constexpr uint16_t kOutputMode = 0x0007;
+// OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp
+// SampleCounter (bits1:0=01 => 0x01), float, all calibrated channels enabled.
+inline constexpr uint32_t kOutputSettings = 0x00000005;
+// Expected MTData payload with the above config: Temp(4) + Acc(12) + Gyr(12) +
+// Mag(12) + Euler(12) + SampleCounter(2).
+inline constexpr uint8_t kMTDataLen = 54;
+
+// One fully decoded IMU reading.
+struct ImuSample {
+    bool     valid = false;
+    float    temp_c = 0.f;          // °C
+    float    acc[3] = {0, 0, 0};    // m/s^2 (incl. gravity), sensor frame
+    float    gyr[3] = {0, 0, 0};    // rad/s
+    float    mag[3] = {0, 0, 0};    // a.u. (normalized to earth field)
+    float    roll_deg = 0.f;          // -180..180
+    float    pitch_deg = 0.f;         // -90..90
+    float    yaw_deg = 0.f;           // 0..360 heading (MTi native -180..180 is shifted)
+    uint16_t sample_counter = 0;
+};
+
+// 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);
+
+// Build a complete message (PRE BID MID LEN DATA CS) ready to write.
+std::vector<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data = {});
+
+// The four config messages for the orientation+calibrated Euler stream.
+std::vector<uint8_t> msgGoToConfig();
+std::vector<uint8_t> msgSetOutputMode();      // kOutputMode
+std::vector<uint8_t> msgSetOutputSettings();  // kOutputSettings
+std::vector<uint8_t> msgGoToMeasurement();
+
+// Decode an MTData payload (the DATA bytes, big-endian) into an ImuSample.
+// Returns nullopt if mid != MTData or len != kMTDataLen.
+std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len);
+
+// Incremental framer: feed raw bytes; for each complete, checksum-valid frame it
+// invokes the sink with (mid, data, len). Tolerates noise/resync by re-scanning
+// for the preamble.
+class MtiFramer {
+public:
+    using FrameSink = std::function<void(uint8_t mid, const uint8_t* data, std::size_t len)>;
+
+    explicit MtiFramer(FrameSink sink) : sink_(std::move(sink)) {}
+    void feed(const uint8_t* p, std::size_t n);
+
+private:
+    enum class S { Pre, Bid, Mid, Len, Data, Cs };
+    FrameSink            sink_;
+    S                    state_ = S::Pre;
+    uint8_t              mid_ = 0;
+    uint8_t              len_ = 0;
+    std::vector<uint8_t> data_;
+    unsigned             sum_ = 0;  // running checksum sum (BID..DATA)
+};
+
+}  // namespace fgc

include/fgc/Paths.h

@@ -26,4 +26,15 @@ std::optional<std::string> resolveConfigPath(const std::string& cliArg = "");
 // ($XDG_DATA_HOME/fire_gimbal_control/images, else ~/.local/share/...).
 std::string defaultOutputDir();
 
+// Directory for diagnostic/calibration logfiles
+// ($XDG_DATA_HOME/fire_gimbal_control/logs, else ~/.local/share/...).
+std::string defaultLogDir();
+
+// "<prefix>_YYYYMMDD-HHMMSS.log" using the local clock.
+std::string timestampedLogName(const std::string& prefix);
+
+// Write `text` to `defaultLogDir()/<name>`, creating the directory if needed.
+// Returns the full path written, or "" on failure (and logs a warning).
+std::string writeLogFile(const std::string& name, const std::string& text);
+
 }  // namespace fgc::paths

include/fgc/SerialMotorController.h

@@ -21,6 +21,8 @@ public:
     void           sendCommand(const std::string& cmd) override;
     MotorTelemetry telemetry() override;
     std::string    lastDump() override;
+    std::string    lastDiag() override;
+    unsigned       diagSeq() const override;
     bool           connected() const override;
 
 private:

include/fgc/mock/MockImuSource.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "fgc/IImuSource.h"
+#include "fgc/Logger.h"
+
+#include <chrono>
+#include <cmath>
+
+namespace fgc {
+
+// Simulated IMU for development without hardware: synthesizes a slowly varying,
+// physically plausible full sample (orientation sweeps, ~1 g on accZ, small
+// gyro/mag) so the Sensors panel and its expanded view animate.
+class MockImuSource : public IImuSource {
+public:
+    void start() override {
+        start_ = clock::now();
+        LOG_INFO << "[mock] IMU started";
+    }
+    void stop() override { LOG_INFO << "[mock] IMU stopped"; }
+
+    bool connected() const override { return true; }
+
+    std::optional<ImuSample> sample() override {
+        const double t = std::chrono::duration<double>(clock::now() - start_).count();
+        ImuSample s;
+        s.valid     = true;
+        s.roll_deg  = static_cast<float>(15.0 * std::sin(t * 0.5));
+        s.pitch_deg = static_cast<float>(10.0 * std::sin(t * 0.3 + 1.0));
+        s.yaw_deg   = static_cast<float>(std::fmod(t * 8.0, 360.0));      // slow spin
+        // Gravity tilts with roll/pitch; small free accel noise.
+        const double r = s.roll_deg * M_PI / 180.0, p = s.pitch_deg * M_PI / 180.0;
+        s.acc[0] = static_cast<float>(9.81 * -std::sin(p));
+        s.acc[1] = static_cast<float>(9.81 * std::sin(r) * std::cos(p));
+        s.acc[2] = static_cast<float>(9.81 * std::cos(r) * std::cos(p));
+        s.gyr[0] = static_cast<float>(0.13 * std::cos(t * 0.5));
+        s.gyr[1] = static_cast<float>(0.05 * std::cos(t * 0.3 + 1.0));
+        s.gyr[2] = 0.14f;
+        s.mag[0] = static_cast<float>(std::cos(s.yaw_deg * M_PI / 180.0));
+        s.mag[1] = static_cast<float>(-std::sin(s.yaw_deg * M_PI / 180.0));
+        s.mag[2] = 0.35f;
+        s.temp_c = static_cast<float>(24.5 + 0.5 * std::sin(t * 0.05));
+        s.sample_counter = static_cast<uint16_t>(static_cast<unsigned>(t * 100.0) & 0xFFFF);
+        return s;
+    }
+
+private:
+    using clock = std::chrono::steady_clock;
+    clock::time_point start_ = clock::now();
+};
+
+}  // namespace fgc

include/fgc/mock/MockMotorController.h

@@ -73,6 +73,29 @@ public:
             d << "DUMP END\n";
             dump_ = d.str();
             LOG_INFO << "firmware dump:\n" << dump_;
+        } else if (verb == "DIAG") {
+            // Canned DG stream (real firmware format) so the host DiagParser /
+            // logfile path are demoable without hardware. Logged live like the
+            // real capture would be.
+            std::ostringstream d;
+            auto axis = [&](char L) {
+                d << "DG BEGIN " << L << "\n";
+                const int speeds[3] = {12500, 25000, 50000};
+                for (int s : speeds)
+                    for (bool fwd : {true, false})
+                        d << "DG " << L << " S" << s << (fwd ? " FWD" : " REV")
+                          << " ERR_PEAK 120 ERR_RMS 30 ERR_STILL 40 CS_MIN 10 CS_MAX 20"
+                          << " SG_MIN 300 PWM_AVG 96 FLAGS 0x00000000 PASS\n";
+                d << "DG " << L << " RESULT PASS\n";
+            };
+            axis('Y');
+            axis('P');
+            d << "DG DONE\n";
+            diag_ = d.str();
+            ++diag_seq_;
+            std::istringstream ds(diag_);
+            std::string ln;
+            while (std::getline(ds, ln)) LOG_INFO << ln;  // stream to LOG pane
         }
         // ENABLE/DISABLE/SPEED/SETPOS/RESET: accepted, no simulation effect.
     }
@@ -93,6 +116,12 @@ public:
         return dump_;
     }
 
+    std::string lastDiag() override {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return diag_;
+    }
+    unsigned diagSeq() const override { return diag_seq_; }
+
     bool connected() const override { return true; }
 
 private:
@@ -124,6 +153,8 @@ private:
     long          pitch_target_ = 0;
     bool          homed_        = false;
     std::string   dump_;
+    std::string   diag_;
+    unsigned      diag_seq_ = 0;
 };
 
 }  // namespace fgc

include/fgc/ui/UiSnapshot.h

@@ -58,15 +58,24 @@ struct SensorField {
     bool        present = false;
 };
 
-// DHT11 (temperature/humidity) + Xsens MTi (orientation). Neither is integrated
+// A labelled group of readings from one physical sensor, rendered as its own
-// yet, so the MVP fills `fields` with pending placeholders; flip the *_present
+// subsection in the Sensors panel.
-// flags and populate values when the drivers land.
+struct SensorGroup {
-struct SensorsView {
+    std::string              title;            // "MTi (orientation)" / "DHT11 (ambient)"
-    bool                     dht_present = false;
+    std::string              status = "pending";  // "live" / "pending" / "no fix"
-    bool                     imu_present = false;
+    bool                     present = false;  // true once the driver feeds real data
     std::vector<SensorField> fields;
 };
 
+// The Sensors panel, split by source so it is clear which parameters come from
+// which device. Each group is filled with pending placeholders until its driver
+// lands. NOTE: the MTi's Temp is the device's *internal* temperature; the DHT11
+// Temp (separate group) is *ambient* — deliberately kept apart.
+struct SensorsView {
+    SensorGroup imu;   // Xsens MTi: Roll/Pitch/Yaw + internal temperature
+    SensorGroup dht;   // DHT11: ambient temperature + humidity
+};
+
 struct CaptureView {
     bool                     present      = false;
     bool                     active       = false;
@@ -109,6 +118,47 @@ struct DumpView {
     std::string text;
 };
 
+// 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 {
+    bool     present = false;
+    float    roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
+    float    acc[3] = {0, 0, 0};
+    float    gyr[3] = {0, 0, 0};
+    float    mag[3] = {0, 0, 0};
+    float    temp_c = 0;
+    unsigned sample_counter = 0;
+};
+
+// Status of the currently-running special operation (calibration, diagnostics,
+// homing, capture) plus the last completed result, for the activity strip below
+// the log. The result block persists until replaced so it never scrolls away.
+struct ActivityView {
+    bool        active = false;        // a special op is running now
+    std::string title;                // "CALIBRATING" / "DIAGNOSTICS" / "HOMING" / "CAPTURE"
+    std::string status;               // live one-line progress
+    bool        has_result = false;
+    std::string result_title;         // e.g. "Calibration · 2m ago"
+    std::vector<std::string> result;  // summary lines
+    UiColor     result_color = UiColor::Default;  // green pass / red fail
+    std::string prompt;               // a yes/no question awaiting the operator (empty = none)
+};
+
+// Last calibration fit, per axis, for the gimbal expanded view.
+struct CalibAxisView {
+    bool   ok = false;
+    double counts_per_deg = 0;
+    long   zero_count = 0;
+    double r2 = 0;
+    int    n = 0;
+};
+struct CalibResultView {
+    bool          has = false;
+    long long     ts_ms = 0;
+    bool          pitch_present = false;
+    CalibAxisView yaw, pitch;
+};
+
 struct UiSnapshot {
     HeaderView           header;
     GimbalView           gimbal;
@@ -117,6 +167,9 @@ struct UiSnapshot {
     ConnView             conn;
     std::vector<LogLine> log;
     DumpView             dump;
+    ImuView              imu;
+    ActivityView         activity;
+    CalibResultView      calib;
 };
 
 // ---- Pure formatting helpers (unit-tested in tests/test_uisnapshot.cpp) ----

main.cpp

@@ -70,6 +70,7 @@ int main(int argc, char* argv[]) {
     if (vm["no-tui"].as<bool>())      opts.use_tui     = false;  // --no-tui wins
     if (vm.count("log-level"))        opts.log_level   = vm["log-level"].as<std::string>();
     if (vm.count("trace"))            opts.trace_categories = vm["trace"].as<std::string>();
+    opts.config_path = *cfg_path;  // so `gimbal calib` can offer to save back to it
 
     Application app(std::move(cfg), std::move(opts));
     return app.run();

src/core/Application.cpp

@@ -1,17 +1,23 @@
 #include "fgc/Application.h"
 
+#include "fgc/CalibrationRoutine.h"
 #include "fgc/CaptureScheduler.h"
 #include "fgc/CommandParser.h"
+#include "fgc/DiagParser.h"
 #include "fgc/DumpParser.h"
 #include "fgc/HelpText.h"
+#include "fgc/Paths.h"
 #include "fgc/ICameraSource.h"
 #include "fgc/IControlChannel.h"
+#include "fgc/IImuSource.h"
 #include "fgc/IMotorController.h"
 #include "fgc/ImagePipeline.h"
 #include "fgc/Logger.h"
+#include "fgc/MtiImuSource.h"
 #include "fgc/ScanGrid.h"
 #include "fgc/SerialMotorController.h"
 #include "fgc/mock/MockCameraSource.h"
+#include "fgc/mock/MockImuSource.h"
 #include "fgc/mock/MockMotorController.h"
 #include "fgc/mock/NullControlChannel.h"
 #include "fgc/ui/HeadlessUi.h"
@@ -19,7 +25,10 @@
 #include "fgc/ui/UiSnapshot.h"
 
 #include <atomic>
+#include <cctype>
 #include <chrono>
+#include <cstdio>
+#include <cstdlib>
 #include <memory>
 #include <mutex>
 #include <queue>
@@ -41,6 +50,12 @@ namespace fgc {
 
 namespace {
 
+long long nowEpochMs() {
+    return std::chrono::duration_cast<std::chrono::milliseconds>(
+               std::chrono::system_clock::now().time_since_epoch())
+        .count();
+}
+
 // Human-readable list of the enabled wire-trace categories, for echoing back.
 std::string traceNames(unsigned mask) {
     if (mask == 0) return "none";
@@ -69,11 +84,25 @@ struct Application::Impl {
     std::unique_ptr<IControlChannel>  channel;
     std::unique_ptr<IMotorController> motor;
     std::unique_ptr<ICameraSource>    camera;
+    std::unique_ptr<IImuSource>       imu;
     std::unique_ptr<ImagePipeline>    pipeline;
     std::unique_ptr<CaptureScheduler> scheduler;
     std::unique_ptr<IUserInterface>   ui;
+    std::unique_ptr<CalibrationRoutine> calib;
+    unsigned                          last_diag_seq = 0;
     ScanGrid                          grid;  // outlives scheduler (holds a reference to it)
 
+    // Persisted last-results for the TUI activity strip (survive in the log churn).
+    bool                     diag_running_ = false;
+    std::chrono::steady_clock::time_point diag_started_;
+    std::vector<std::string> last_diag_summary;
+    long long                last_diag_ts = 0;   // epoch ms
+    bool                     last_diag_pass = false;
+    CalibResultView          last_calib_view;
+    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
+
     std::atomic<bool>       running{true};
     std::mutex              cmd_mutex;
     std::queue<std::string> cmd_queue;
@@ -105,6 +134,16 @@ struct Application::Impl {
         return std::make_unique<SerialMotorController>(cfg.serial.device, cfg.serial.baud);
     }
 
+    std::unique_ptr<IImuSource> makeImu() {
+        if (!cfg.features.enable_imu) return nullptr;  // sensors panel stays "pending"
+        if (cfg.features.mock_imu) return std::make_unique<MockImuSource>();
+        if (cfg.imu.device.empty()) {
+            LOG_WARN << "IMU enabled but [IMU] device is empty; disabling IMU";
+            return nullptr;
+        }
+        return std::make_unique<MtiImuSource>(cfg.imu.device, cfg.imu.baud);
+    }
+
     std::unique_ptr<ICameraSource> makeCamera() {
         bool mock = opts.mock_camera.value_or(cfg.features.mock_camera);
 #if !FGC_WITH_VIMBA
@@ -192,8 +231,45 @@ struct Application::Impl {
             }
         }
 
-        // --- Sensors (DHT11 + MTi not integrated yet) ---
+        // --- Sensors: MTi (orientation) live if present; DHT11 (ambient) pending ---
         s.sensors = pendingSensorsView();
+        if (imu) {
+            auto fmt1 = [](float v) {
+                char b[24];
+                std::snprintf(b, sizeof(b), "%.1f", v);
+                return std::string(b);
+            };
+            auto setField = [&](SensorField& f, const std::string& v) {
+                f.value   = v;
+                f.present = true;
+            };
+            if (auto m = imu->sample()) {
+                // Full reading for the expanded view.
+                s.imu.present        = true;
+                s.imu.roll_deg       = m->roll_deg;
+                s.imu.pitch_deg      = m->pitch_deg;
+                s.imu.yaw_deg        = m->yaw_deg;
+                for (int i = 0; i < 3; ++i) {
+                    s.imu.acc[i] = m->acc[i];
+                    s.imu.gyr[i] = m->gyr[i];
+                    s.imu.mag[i] = m->mag[i];
+                }
+                s.imu.temp_c         = m->temp_c;
+                s.imu.sample_counter = m->sample_counter;
+                // Compact MTi subsection: Roll/Pitch/Yaw + the device's internal temp.
+                auto& g = s.sensors.imu;
+                g.present = true;
+                g.status  = "live";
+                if (g.fields.size() >= 4) {
+                    setField(g.fields[0], fmt1(m->roll_deg));
+                    setField(g.fields[1], fmt1(m->pitch_deg));
+                    setField(g.fields[2], fmt1(m->yaw_deg));
+                    setField(g.fields[3], fmt1(m->temp_c));  // MTi internal temp
+                }
+            } else {
+                s.sensors.imu.status = "no fix";
+            }
+        }
 
         // --- Camera / capture ---
         s.capture.present      = true;
@@ -223,9 +299,68 @@ struct Application::Impl {
         // --- Diagnostics (last firmware DUMP) ---
         s.dump.text = motor->lastDump();
         s.dump.has  = !s.dump.text.empty();
+
+        // --- Activity strip + last calibration ---
+        s.calib = last_calib_view;
+        s.calib.pitch_present = s.gimbal.pitch_present || s.calib.pitch_present;
+        fillActivity(s);
         return s;
     }
 
+    // The currently-running special op (priority: calib > diag > homing > capture)
+    // plus the most recent persisted result, for the activity strip.
+    void fillActivity(UiSnapshot& s) const {
+        ActivityView& a = s.activity;
+
+        if (calib && calib->running()) {
+            CalibProgress p = calib->progress();
+            a.active = true;
+            a.title  = "CALIBRATING";
+            std::string axis = (p.axis == 'P') ? "PITCH" : (p.axis == 'Y') ? "YAW" : "";
+            a.status = axis.empty() ? p.phase
+                     : axis + " step " + std::to_string(p.step) + "/" +
+                           std::to_string(p.total) + " — " + p.phase;
+        } else if (diag_running_) {
+            a.active = true;
+            a.title  = "DIAGNOSTICS";
+            a.status = "running motor self-test…";
+        } else if (s.gimbal.yaw.state == AxisState::Homing ||
+                   (s.gimbal.pitch_present && s.gimbal.pitch.state == AxisState::Homing)) {
+            a.active = true;
+            a.title  = "HOMING";
+            a.status = "finding endstops…";
+        } else if (scheduler && scheduler->captureActive()) {
+            a.active = true;
+            a.title  = "CAPTURE";
+            a.status = s.capture.has_last
+                           ? ("scanning · last " + s.capture.last_label)
+                           : "scanning…";
+        }
+
+        // Persisted result: the more recent of the last calibration / diagnostics.
+        const long long now = nowEpochMs();
+        if (last_calib_ts || last_diag_ts) {
+            const bool calib_newer = last_calib_ts >= last_diag_ts;
+            a.has_result = true;
+            if (calib_newer) {
+                a.result_title = "Calibration · " + formatTimeAgo(now, last_calib_ts);
+                a.result       = last_calib_summary;
+                a.result_color = last_calib_view.has &&
+                                 last_calib_view.yaw.ok ? UiColor::Green : UiColor::Yellow;
+            } else {
+                a.result_title = std::string("Diagnostics ") +
+                                 (last_diag_pass ? "PASS" : "FAIL") + " · " +
+                                 formatTimeAgo(now, last_diag_ts);
+                a.result       = last_diag_summary;
+                a.result_color = last_diag_pass ? UiColor::Green : UiColor::Red;
+            }
+        }
+
+        // Pending yes/no: persist the just-applied calibration as the new default?
+        if (calib_save_pending_)
+            a.prompt = "Save this calibration to config as the new default?  (y / n)";
+    }
+
     void publishSnapshot() {
         UiSnapshot s = buildSnapshot();
         std::lock_guard<std::mutex> lock(snapshot_mutex);
@@ -311,19 +446,186 @@ struct Application::Impl {
     // elevation in degrees. Converts to encoder counts via the operator-calibrated
     // Geometry maps (which soft-clamp to the travel limits) and issues a two-axis
     // MOVE so both axes start together.
-    void handleGoto(const std::string& line) {
+    // Parse "<a>,<b>" or "<a> <b>" (tokens from `from` onward) into two numbers.
+    static bool parsePair(const std::vector<std::string>& tok, size_t from,
+                          double& a, double& b) {
+        std::string s;
+        for (size_t i = from; i < tok.size(); ++i) s += tok[i] + " ";
+        for (char& ch : s) if (ch == ',') ch = ' ';
+        std::istringstream iss(s);
+        return static_cast<bool>(iss >> a >> b);
+    }
+
+    // Soft-limit travel (counts) for an axis from the last dump, else the
+    // configured degree range converted to counts. Returns 0 if unknown.
+    long axisRangeCounts(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 std::labs(m.toCounts(m.max_deg) - m.toCounts(m.min_deg));
+    }
+
+    // `gimbal <subcommand>` — unified, lowercase motor control. `move` is degrees
+    // (default), `steps` is raw encoder counts; the rest map to firmware verbs.
+    void handleGimbal(const std::string& line) {
         std::istringstream iss(line);
-        std::string verb;
+        std::vector<std::string> tok;
-        double yaw_deg = 0.0, pitch_deg = 0.0;
+        std::string t;
-        if (!(iss >> verb >> yaw_deg >> pitch_deg)) {
+        while (iss >> t) tok.push_back(t);  // tok[0] == "gimbal"
-            LOG_WARN << "usage: goto <yaw_deg> <pitch_deg>  (e.g. goto 30 -10)";
+        if (tok.size() < 2) {
+            LOG_WARN << "usage: gimbal <move|steps|nudge|home|stop|reset|enable|disable|"
+                        "speed|setpos|status|dump|diag|calib>";
             return;
         }
-        long yc = cfg.geometry.yaw.toCounts(yaw_deg);
+        std::string sub = tok[1];
-        long pc = cfg.geometry.pitch.toCounts(pitch_deg);
+        for (char& ch : sub) ch = static_cast<char>(std::tolower((unsigned char)ch));
-        LOG_INFO << "goto yaw=" << yaw_deg << "deg pitch=" << pitch_deg
+
-                 << "deg -> MOVE " << yc << "," << pc;
+        if (sub == "move") {
-        motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc));
+            double yaw_deg = 0, pitch_deg = 0;
+            if (!parsePair(tok, 2, yaw_deg, pitch_deg)) {
+                LOG_WARN << "usage: gimbal move <yaw_deg>,<pitch_deg>";
+                return;
+            }
+            long yc = cfg.geometry.yaw.toCounts(yaw_deg);
+            long pc = cfg.geometry.pitch.toCounts(pitch_deg);
+            LOG_INFO << "gimbal move yaw=" << yaw_deg << " pitch=" << pitch_deg
+                     << " deg -> MOVE " << yc << "," << pc;
+            motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc));
+        } else if (sub == "steps") {
+            double yc = 0, pc = 0;
+            if (!parsePair(tok, 2, yc, pc)) {
+                LOG_WARN << "usage: gimbal steps <yaw_counts>,<pitch_counts>";
+                return;
+            }
+            motor->sendCommand("MOVE " + std::to_string((long)yc) + "," + std::to_string((long)pc));
+        } else if (sub == "nudge") {
+            if (tok.size() < 4) { LOG_WARN << "usage: gimbal nudge <yaw|pitch> <+/-pct>"; return; }
+            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);
+            if (range <= 0) { LOG_WARN << "gimbal nudge: travel unknown; run gimbal home/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);
+            LOG_INFO << "gimbal nudge " << axis << " " << pct << "% -> MOVE " << axis << " " << target;
+            motor->sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
+        } else if (sub == "diag") {
+            MotorTelemetry tel = motor->telemetry();
+            if (!tel.yaw.ready() && !(tel.pitch_present && tel.pitch.ready())) {
+                LOG_WARN << "gimbal diag: axes not READY (home first)";
+                return;
+            }
+            std::string fw = "DIAG";
+            for (size_t i = 2; i < tok.size(); ++i) fw += " " + tok[i];
+            LOG_INFO << "running gimbal diagnostics...";
+            motor->sendCommand(fw);
+            diag_running_ = true;
+            diag_started_ = std::chrono::steady_clock::now();
+        } else if (sub == "calib") {
+            startCalibration();
+        } else if (sub == "dump") {
+            LOG_INFO << "requesting firmware dump...";
+            motor->sendCommand("DUMP");
+        } 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.
+            std::string fw = sub;
+            for (char& ch : fw) ch = static_cast<char>(std::toupper((unsigned char)ch));
+            for (size_t i = 2; i < tok.size(); ++i) fw += " " + tok[i];
+            motor->sendCommand(fw);
+        } else {
+            LOG_WARN << "unknown gimbal subcommand: " << sub;
+        }
+    }
+
+    void startCalibration() {
+        if (!imu) { LOG_WARN << "gimbal calib requires the IMU (set [Features] enable_imu)"; return; }
+        if (calib && calib->running()) { LOG_WARN << "calibration already running (gimbal stop to cancel)"; return; }
+        stopCapture();  // free the serial link + don't fight the routine's moves
+        calib = std::make_unique<CalibrationRoutine>(*motor, *imu, cfg.geometry);
+        calib->start();
+    }
+
+    // Persist the live (just-calibrated) [Motor] geometry back into config.ini.
+    void saveCalibration() {
+        if (!calib_save_pending_) return;
+        calib_save_pending_ = false;
+        if (saveMotorCalibration(opts.config_path, cfg.geometry))
+            LOG_INFO << "calibration saved as the new default in " << opts.config_path;
+        else
+            LOG_WARN << "could not write calibration to " << opts.config_path;
+    }
+
+    void cancelCalibration() {
+        if (calib && calib->running()) {
+            LOG_INFO << "cancelling calibration";
+            calib->cancel();
+            motor->sendCommand("STOP ALL");
+        }
+    }
+
+    // Pick up results produced by background work (calibration thread, DIAG
+    // capture) — runs on the control thread each tick so all geometry/state
+    // mutation stays single-threaded.
+    void pollBackgroundResults() {
+        if (calib) {
+            if (auto g = calib->takeResult()) {
+                cfg.geometry = *g;
+                if (scheduler) scheduler->setGeometry(cfg.geometry);  // also fix scan targeting
+                LOG_INFO << "calibration applied to live geometry";
+                // Capture the structured report for the activity strip + gimbal view.
+                CalibReport rep = calib->report();
+                CalibResultView v;
+                v.has = rep.valid;
+                v.ts_ms = rep.ts_ms;
+                last_calib_summary.clear();
+                for (const auto& ax : rep.axes) {
+                    CalibAxisView* dst = (ax.axis == 'P') ? &v.pitch : &v.yaw;
+                    if (ax.axis == 'P') v.pitch_present = true;
+                    dst->ok = ax.ok;
+                    dst->counts_per_deg = ax.counts_per_deg;
+                    dst->zero_count = ax.zero_count;
+                    dst->r2 = ax.r2;
+                    dst->n = ax.n;
+                    char b[96];
+                    std::snprintf(b, sizeof(b), "%s  cpd=%.3f  zero=%ld  R2=%.4f (n=%d)%s",
+                                  ax.axis == 'P' ? "PITCH" : "YAW", ax.counts_per_deg,
+                                  ax.zero_count, ax.r2, ax.n, ax.ok ? "" : "  FIT FAILED");
+                    last_calib_summary.emplace_back(b);
+                }
+                last_calib_view = v;
+                last_calib_ts   = nowEpochMs();
+                // Offer to persist it as the new default (answered via the activity strip).
+                if (rep.valid && !opts.config_path.empty()) calib_save_pending_ = true;
+            }
+        }
+        unsigned seq = motor->diagSeq();
+        if (seq != last_diag_seq) {
+            last_diag_seq = seq;
+            diag_running_ = false;
+            DiagResult dr = parseDiag(motor->lastDiag());
+            std::ostringstream summary;
+            for (const auto& l : formatDiag(dr)) { LOG_INFO << l; summary << l << "\n"; }
+            // Concise per-axis lines for the activity strip (full detail is logged).
+            last_diag_summary.clear();
+            for (const auto& ax : dr.axes)
+                last_diag_summary.push_back(std::string(1, ax.axis) + ": " +
+                                            (ax.pass ? "PASS" : "FAIL") + "  (" +
+                                            std::to_string(ax.tests.size()) + " tests)");
+            last_diag_pass = dr.allPass();
+            last_diag_ts   = nowEpochMs();
+            std::string path = paths::writeLogFile(
+                paths::timestampedLogName("diag"),
+                motor->lastDiag() + "\n--- parsed ---\n" + summary.str());
+            if (!path.empty()) LOG_INFO << "diagnostics log written: " << path;
+        }
+        // Safety: clear a stuck diag flag if the firmware never replied.
+        if (diag_running_ &&
+            std::chrono::steady_clock::now() - diag_started_ > std::chrono::seconds(30))
+            diag_running_ = false;
     }
 
     void handleCommand(const std::string& line) {
@@ -334,16 +636,21 @@ struct Application::Impl {
         if (c.verb == "help") {
             // c.device holds the optional topic (first token after "help").
             for (const auto& l : renderHelp(c.device)) LOG_INFO << l;
-        } else if (c.verb == "goto") {
+        } else if (c.verb == "gimbal") {
-            handleGoto(line);
+            handleGimbal(line);
-        } else if (c.verb == "dump") {
+        } else if (c.verb == "calib") {
-            LOG_INFO << "requesting firmware dump...";
+            // Answer to the "save calibration?" prompt (activity-strip y/n keys).
-            motor->sendCommand("DUMP");
+            if (c.device == "save") saveCalibration();
+            else if (c.device == "discard") {
+                calib_save_pending_ = false;
+                LOG_INFO << "calibration kept for this session only (not saved to config)";
+            }
         } else if (c.verb == "exit") {
             running = false;
         } else if (c.verb == "start") {
             startCapture();
         } else if (c.verb == "stop") {
+            cancelCalibration();  // `stop` also aborts a running calibration
             stopCapture();
         } else if (c.verb == "debug") {
             bool on = Logger::level() != LogLevel::Debug;
@@ -361,8 +668,6 @@ struct Application::Impl {
             } else if (c.device == "fps" && c.has_value) {
                 scheduler->setImageRate(c.value);
                 LOG_INFO << "capture rate set to " << c.value << " img/s";
-            } else if (c.device == "motorctl") {
-                motor->sendCommand(c.option);
             } else {
                 LOG_WARN << "unknown 'set' target: " << c.device;
             }
@@ -411,6 +716,7 @@ struct Application::Impl {
         channel = makeChannel();
         motor   = makeMotor();
         camera  = makeCamera();
+        imu     = makeImu();
 
         if (!channel->connect())
             LOG_WARN << "Control channel not connected; continuing in degraded mode";
@@ -443,6 +749,7 @@ struct Application::Impl {
                                                        cfg.geometry, grid);
 
         motor->start();
+        if (imu) imu->start();
         camera->open();
         pipeline->start();
         channel->publishStatus(0);
@@ -471,16 +778,19 @@ struct Application::Impl {
         LOG_INFO << "Entering control loop (type 'exit' to quit)";
         while (running) {
             drainCommands();
+            pollBackgroundResults();  // apply calibration result / emit DIAG summary+log
             scheduler->tick();
             publishSnapshot();
             std::this_thread::sleep_for(std::chrono::milliseconds(10));
         }
 
         LOG_INFO << "Shutting down";
+        if (calib) calib->cancel();  // stop any in-flight calibration before teardown
         if (ui) ui->stop();
         pipeline->stop();
         camera->stop();
         camera->close();
+        if (imu) imu->stop();
         motor->stop();
         channel->disconnect();
         return 0;

src/core/Calibration.cpp

@@ -0,0 +1,49 @@
+#include "fgc/Calibration.h"
+
+#include <cmath>
+
+namespace fgc {
+
+LinearFit linearFit(const std::vector<std::pair<double, double>>& xy) {
+    LinearFit f;
+    f.n = static_cast<int>(xy.size());
+    if (xy.size() < 2) return f;
+
+    double sx = 0, sy = 0;
+    for (const auto& p : xy) { sx += p.first; sy += p.second; }
+    const double mx = sx / xy.size(), my = sy / xy.size();
+
+    double sxx = 0, sxy = 0, syy = 0;
+    for (const auto& p : xy) {
+        const double dx = p.first - mx, dy = p.second - my;
+        sxx += dx * dx;
+        sxy += dx * dy;
+        syy += dy * dy;
+    }
+    if (sxx <= 0.0) return f;  // no spread in x -> undefined slope
+
+    f.slope     = sxy / sxx;
+    f.intercept = my - f.slope * mx;
+    f.r2        = (syy > 0.0) ? (sxy * sxy) / (sxx * syy) : 1.0;
+    f.ok        = true;
+    return f;
+}
+
+double circularMeanDeg(const std::vector<double>& degs) {
+    if (degs.empty()) return 0.0;
+    double sc = 0, ss = 0;
+    for (double d : degs) {
+        const double r = d * M_PI / 180.0;
+        sc += std::cos(r);
+        ss += std::sin(r);
+    }
+    return std::atan2(ss, sc) * 180.0 / M_PI;
+}
+
+double unwrapNear(double prev, double deg) {
+    while (deg - prev > 180.0) deg -= 360.0;
+    while (deg - prev < -180.0) deg += 360.0;
+    return deg;
+}
+
+}  // namespace fgc

src/core/CalibrationRoutine.cpp

@@ -0,0 +1,253 @@
+#include "fgc/CalibrationRoutine.h"
+
+#include "fgc/Calibration.h"
+#include "fgc/DumpParser.h"
+#include "fgc/IImuSource.h"
+#include "fgc/IMotorController.h"
+#include "fgc/Logger.h"
+#include "fgc/Paths.h"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdlib>
+#include <sstream>
+#include <utility>
+#include <vector>
+
+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)
+    : motor_(motor), imu_(imu), initial_(std::move(initial)) {}
+
+CalibrationRoutine::~CalibrationRoutine() {
+    cancel();
+    if (thread_.joinable()) thread_.join();
+}
+
+bool CalibrationRoutine::start() {
+    if (running_.exchange(true)) {
+        LOG_WARN << "calibration already running";
+        return false;
+    }
+    cancel_ = false;
+    if (thread_.joinable()) thread_.join();
+    thread_ = std::thread([this] { run(); });
+    return true;
+}
+
+void CalibrationRoutine::cancel() { cancel_ = true; }
+
+std::optional<Geometry> CalibrationRoutine::takeResult() {
+    std::lock_guard<std::mutex> lk(result_mutex_);
+    std::optional<Geometry> r;
+    r.swap(result_);
+    return r;
+}
+
+CalibProgress CalibrationRoutine::progress() const {
+    std::lock_guard<std::mutex> lk(progress_mutex_);
+    return progress_;
+}
+
+CalibReport CalibrationRoutine::report() const {
+    std::lock_guard<std::mutex> lk(result_mutex_);
+    return report_;
+}
+
+void CalibrationRoutine::setProgress(char axis, int step, int total, const char* phase) {
+    std::lock_guard<std::mutex> lk(progress_mutex_);
+    progress_.running = true;
+    progress_.axis    = axis;
+    progress_.step    = step;
+    progress_.total   = total;
+    progress_.phase   = phase;
+}
+
+namespace {
+long long nowMs() {
+    return std::chrono::duration_cast<std::chrono::milliseconds>(
+               std::chrono::system_clock::now().time_since_epoch())
+        .count();
+}
+}  // namespace
+
+namespace {
+
+// Wait until the axis is at `target` (standstill + within tol), or timeout/cancel.
+bool waitSettle(IMotorController& motor, char axis, long target, long tol,
+                const std::atomic<bool>& cancel) {
+    const auto deadline = clock::now() + std::chrono::milliseconds(kSettleTimeoutMs);
+    while (clock::now() < deadline) {
+        if (cancel) return false;
+        MotorTelemetry t = motor.telemetry();
+        const AxisTelemetry& a = (axis == 'Y') ? t.yaw : t.pitch;
+        if (a.standstill && std::labs(a.xenc - target) <= tol) return true;
+        msleep(100);
+    }
+    return false;
+}
+
+// Dwell `kDwellMs` 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) {
+    std::vector<double> vals;
+    const auto end = clock::now() + std::chrono::milliseconds(kDwellMs);
+    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);
+    }
+    if (vals.empty()) return 0.0;
+    if (axis == 'Y') {
+        double m = circularMeanDeg(vals);   // robust to wrap within the dwell
+        if (m < 0.0) m += 360.0;            // report a 0..360 heading (matches IMU)
+        return m;
+    }
+    double m = 0;
+    for (double v : vals) m += v;
+    return m / vals.size();
+}
+
+}  // namespace
+
+void CalibrationRoutine::run() {
+    struct Done {
+        CalibrationRoutine* self;
+        ~Done() {
+            self->running_ = false;
+            std::lock_guard<std::mutex> lk(self->progress_mutex_);
+            self->progress_.running = false;
+        }
+    } done{this};
+
+    LOG_INFO << "=== gimbal calibration starting ===";
+    setProgress('?', 0, kPositions, "starting");
+    CalibReport report;
+
+    // 1. Preconditions.
+    if (!imu_.connected()) {
+        LOG_WARN << "calibration aborted: IMU not connected";
+        return;
+    }
+    MotorTelemetry t0 = motor_.telemetry();
+    if (!t0.yaw.ready() || (t0.pitch_present && !t0.pitch.ready())) {
+        LOG_WARN << "calibration aborted: axes must be homed (READY) first";
+        return;
+    }
+
+    // Fetch soft limits from a fresh dump.
+    LOG_INFO << "calibration: requesting soft limits (dump)...";
+    motor_.sendCommand("DUMP");
+    DumpData dump;
+    for (int i = 0; i < 50 && !cancel_; ++i) {  // up to ~5 s
+        dump = parseDump(motor_.lastDump());
+        if (dump.valid && !dump.axes.empty()) break;
+        msleep(100);
+    }
+    if (!dump.valid) {
+        LOG_WARN << "calibration aborted: could not read soft limits";
+        return;
+    }
+
+    Geometry result = initial_;
+    std::ostringstream log;
+    log << "gimbal calibration\n==================\n";
+
+    struct AxisJob { char axis; AxisMap* map; };
+    std::vector<AxisJob> jobs = {{'Y', &result.yaw}};
+    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;
+        if (!da) { LOG_WARN << "calibration: no limits for axis " << job.axis << "; skipping"; continue; }
+
+        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 a = lo + inset, b = hi - inset;
+        const long tol = std::max<long>(500, travel / 200);
+
+        LOG_INFO << "calibrating " << (job.axis == 'Y' ? "YAW" : "PITCH")
+                 << " over [" << a << ".." << b << "] counts in " << kPositions << " steps";
+        log << "\n[" << job.axis << "] travel=[" << lo << ".." << hi << "] 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) {
+            if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+            long target = a + (b - a) * i / (kPositions - 1);
+            setProgress(job.axis, i + 1, kPositions, "moving");
+            motor_.sendCommand(std::string("MOVE ") + job.axis + " " + std::to_string(target));
+            if (!waitSettle(motor_, job.axis, target, tol, cancel_)) {
+                if (cancel_) return;
+                LOG_WARN << "  position " << (i + 1) << "/" << kPositions
+                         << " did not settle near " << target << " (continuing)";
+            }
+            setProgress(job.axis, i + 1, kPositions, "dwelling");
+            double deg = dwellAndMeasure(imu_, job.axis, 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
+                     << "  imu=" << deg << " deg";
+            log << "  " << target << ", " << deg << "\n";
+        }
+
+        setProgress(job.axis, kPositions, kPositions, "fitting");
+        LinearFit fit = linearFit(pts);
+        if (!fit.ok) {
+            LOG_WARN << "  fit failed for axis " << job.axis << " (degenerate data)";
+            log << "  FIT FAILED\n";
+            report.axes.push_back({job.axis, false, 0, 0, 0, static_cast<int>(pts.size())});
+            continue;
+        }
+        const double old_cpd = job.map->counts_per_deg;
+        const long   old_zc  = job.map->zero_count;
+        job.map->counts_per_deg = fit.slope;
+        job.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
+                 << "  (R^2=" << fit.r2 << ")";
+        log << "  fit: counts_per_deg=" << fit.slope << " zero_count=" << job.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});
+    }
+
+    if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
+
+    std::string path = paths::writeLogFile(paths::timestampedLogName("calib"), log.str());
+    if (!path.empty()) LOG_INFO << "calibration log written: " << path;
+
+    report.valid  = true;
+    report.ts_ms  = nowMs();
+    report.all_ok = !report.axes.empty();
+    for (const auto& ax : report.axes) report.all_ok = report.all_ok && ax.ok;
+    {
+        std::lock_guard<std::mutex> lk(result_mutex_);
+        result_ = result;
+        report_ = report;
+    }
+    LOG_INFO << "=== gimbal calibration complete (applied to session) ===";
+}
+
+}  // namespace fgc

src/core/Config.cpp

@@ -1,7 +1,10 @@
 #include "fgc/Config.h"
 #include "fgc/Paths.h"
 
+#include <cstdio>
 #include <cstdlib>
+#include <fstream>
+#include <sstream>
 #include <stdexcept>
 
 extern "C" {
@@ -106,8 +109,13 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
     cfg.features.enable_mqtt   = getBool(kv, "Features.enable_mqtt", cfg.features.enable_mqtt);
     cfg.features.enable_camera = getBool(kv, "Features.enable_camera", cfg.features.enable_camera);
     cfg.features.enable_serial = getBool(kv, "Features.enable_serial", cfg.features.enable_serial);
+    cfg.features.enable_imu    = getBool(kv, "Features.enable_imu", cfg.features.enable_imu);
     cfg.features.mock_camera   = getBool(kv, "Features.mock_camera", cfg.features.mock_camera);
     cfg.features.mock_serial   = getBool(kv, "Features.mock_serial", cfg.features.mock_serial);
+    cfg.features.mock_imu      = getBool(kv, "Features.mock_imu", cfg.features.mock_imu);
+
+    cfg.imu.device = get(kv, "IMU.device", cfg.imu.device);
+    cfg.imu.baud   = static_cast<unsigned>(getInt(kv, "IMU.baud", cfg.imu.baud));
 
     cfg.logging.level = get(kv, "Logging.level", cfg.logging.level);
     cfg.logging.trace = get(kv, "Logging.trace", cfg.logging.trace);
@@ -151,4 +159,78 @@ AppConfig ConfigLoader::loadFromFile(const std::string& path) {
     return fromMap(kv);
 }
 
+namespace {
+std::string trimmed(const std::string& s) {
+    const char* ws = " \t\r\n";
+    size_t a = s.find_first_not_of(ws);
+    if (a == std::string::npos) return "";
+    size_t b = s.find_last_not_of(ws);
+    return s.substr(a, b - a + 1);
+}
+}  // namespace
+
+std::string updateIniSectionKeys(const std::string& contents, const std::string& section,
+                                 const std::vector<std::pair<std::string, std::string>>& kv) {
+    std::vector<bool> written(kv.size(), false);
+    std::istringstream in(contents);
+    std::ostringstream out;
+    std::string line, cur;
+    while (std::getline(in, line)) {
+        const std::string t = trimmed(line);
+        if (t.size() >= 2 && t.front() == '[' && t.back() == ']') {
+            cur = trimmed(t.substr(1, t.size() - 2));
+        } else if (cur == section && !t.empty() && t[0] != ';' && t[0] != '#') {
+            const size_t eq = line.find('=');
+            if (eq != std::string::npos) {
+                const std::string key = trimmed(line.substr(0, eq));
+                for (size_t i = 0; i < kv.size(); ++i) {
+                    if (!written[i] && key == kv[i].first) {
+                        line = kv[i].first + " = " + kv[i].second;
+                        written[i] = true;
+                        break;
+                    }
+                }
+            }
+        }
+        out << line << "\n";
+    }
+    // Any keys not present in the section get appended in a fresh [section] block
+    // (inih merges duplicate sections on read).
+    bool missing = false;
+    for (bool w : written) missing = missing || !w;
+    if (missing) {
+        out << "\n[" << section << "]\n";
+        for (size_t i = 0; i < kv.size(); ++i)
+            if (!written[i]) out << kv[i].first << " = " << kv[i].second << "\n";
+    }
+    return out.str();
+}
+
+bool saveMotorCalibration(const std::string& path, const Geometry& geo) {
+    if (path.empty()) return false;
+    std::ifstream f(path);
+    if (!f) return false;
+    std::stringstream buf;
+    buf << f.rdbuf();
+    f.close();
+
+    auto num = [](double v) {
+        char b[32];
+        std::snprintf(b, sizeof(b), "%.10g", v);
+        return std::string(b);
+    };
+    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)},
+        {"pitch_counts_per_deg", num(geo.pitch.counts_per_deg)},
+        {"pitch_zero_count", std::to_string(geo.pitch.zero_count)},
+    };
+    const std::string updated = updateIniSectionKeys(buf.str(), "Motor", kv);
+
+    std::ofstream out(path, std::ios::trunc);
+    if (!out) return false;
+    out << updated;
+    return static_cast<bool>(out);
+}
+
 }  // namespace fgc

src/core/DiagParser.cpp

@@ -0,0 +1,124 @@
+#include "fgc/DiagParser.h"
+
+#include <map>
+#include <sstream>
+
+namespace fgc {
+
+namespace {
+
+long toLong(const std::string& s, long fallback = 0) {
+    if (s.empty()) return fallback;
+    try {
+        bool hex = s.size() > 1 && s[0] == '0' && (s[1] == 'x' || s[1] == 'X');
+        return std::stol(s, nullptr, hex ? 16 : 10);
+    } catch (const std::exception&) {
+        return fallback;
+    }
+}
+
+}  // namespace
+
+bool DiagResult::allPass() const {
+    bool any = false;
+    for (const auto& ax : axes) {
+        if (!ax.has_result) continue;
+        any = true;
+        if (!ax.pass) return false;
+    }
+    return any;
+}
+
+DiagResult parseDiag(const std::string& block) {
+    DiagResult d;
+    std::istringstream iss(block);
+    std::string line;
+    DiagAxis* cur = nullptr;
+
+    auto axisFor = [&d](char a) -> DiagAxis& {
+        for (auto& ax : d.axes)
+            if (ax.axis == a) return ax;
+        d.axes.push_back(DiagAxis{});
+        d.axes.back().axis = a;
+        return d.axes.back();
+    };
+
+    while (std::getline(iss, line)) {
+        if (!line.empty() && line.back() == '\r') line.pop_back();
+        // Only "DG ..." lines are part of the diag (ST/other lines interleave).
+        auto pos = line.find("DG ");
+        if (pos == std::string::npos) continue;
+        std::istringstream ls(line.substr(pos));
+        std::vector<std::string> tok;
+        std::string t;
+        while (ls >> t) tok.push_back(t);
+        if (tok.size() < 2) continue;  // "DG" + something
+
+        d.valid = true;
+        if (tok[1] == "DONE") { d.done = true; continue; }
+        if (tok[1] == "BEGIN" && tok.size() >= 3) { cur = &axisFor(tok[2][0]); continue; }
+
+        // Otherwise tok[1] is the axis letter.
+        char axis = tok[1][0];
+        if (axis != 'Y' && axis != 'P') continue;
+        DiagAxis& ax = axisFor(axis);
+        cur = &ax;
+
+        if (tok.size() >= 4 && tok[2] == "RESULT") {
+            ax.has_result = true;
+            ax.pass = (tok[3] == "PASS");
+            continue;
+        }
+
+        // Test line: "DG <axis> S<speed> <FWD|REV> KEY val KEY val ... <PASS|FAIL>"
+        if (tok.size() < 4 || tok[2].empty() || (tok[2][0] != 'S' && tok[2][0] != 's'))
+            continue;
+        DiagTest test;
+        test.speed = static_cast<int>(toLong(tok[2].substr(1)));
+        test.fwd   = (tok[3] != "REV");
+        test.pass  = (tok.back() == "PASS");
+        // Remaining KEY value pairs (between dir and the trailing PASS/FAIL).
+        std::map<std::string, std::string> kv;
+        for (size_t i = 4; i + 1 < tok.size(); i += 2) kv[tok[i]] = tok[i + 1];
+        auto g = [&](const char* k) { auto it = kv.find(k); return it == kv.end() ? std::string() : it->second; };
+        test.err_peak  = toLong(g("ERR_PEAK"), 0);
+        test.err_rms   = toLong(g("ERR_RMS"), 0);
+        test.err_still = toLong(g("ERR_STILL"), 0);
+        test.cs_min    = static_cast<int>(toLong(g("CS_MIN")));
+        test.cs_max    = static_cast<int>(toLong(g("CS_MAX")));
+        test.sg_min    = static_cast<int>(toLong(g("SG_MIN")));
+        test.pwm_avg   = static_cast<int>(toLong(g("PWM_AVG")));
+        test.flags     = static_cast<unsigned>(toLong(g("FLAGS")));
+        ax.tests.push_back(test);
+    }
+    return d;
+}
+
+std::vector<std::string> formatDiag(const DiagResult& d) {
+    std::vector<std::string> out;
+    if (!d.valid) {
+        out.emplace_back("(no diagnostic data)");
+        return out;
+    }
+    for (const auto& ax : d.axes) {
+        std::ostringstream hdr;
+        hdr << "[" << ax.axis << "] " << ax.tests.size() << " tests  RESULT="
+            << (ax.has_result ? (ax.pass ? "PASS" : "FAIL") : "?");
+        out.push_back(hdr.str());
+        for (const auto& t : ax.tests) {
+            std::ostringstream os;
+            os << "    S" << t.speed << " " << (t.fwd ? "FWD" : "REV")
+               << "  err_peak=" << t.err_peak << " err_rms=" << t.err_rms
+               << " err_still=" << t.err_still << "  cs=" << t.cs_min << ".." << t.cs_max
+               << " sg_min=" << t.sg_min << " pwm=" << t.pwm_avg
+               << " flags=0x" << std::hex << t.flags << std::dec
+               << "  " << (t.pass ? "PASS" : "FAIL");
+            out.push_back(os.str());
+        }
+    }
+    out.push_back(std::string("overall: ") + (d.allPass() ? "PASS" : "FAIL") +
+                  (d.done ? "" : "  (incomplete - no DG DONE)"));
+    return out;
+}
+
+}  // namespace fgc

src/core/HelpText.cpp

@@ -24,37 +24,45 @@ std::string verbOf(const std::string& syntax) {
 const std::vector<HelpSection>& helpCatalog() {
     // clang-format off
     static const std::vector<HelpSection> catalog = {
-        {"Positioning", "Aim the gimbal. 'goto' is degrees; raw MOVE is encoder counts.", {
+        {"Positioning", "Aim the gimbal. 'move' is degrees; 'steps' is raw encoder counts.", {
-            {"goto <yaw_deg> <pitch_deg>",
+            {"gimbal move <yaw>,<pitch>",
-             "Point the gimbal at an absolute heading/elevation in degrees.", {
+             "Point the gimbal at an absolute heading/elevation in DEGREES.", {
                 "Converts degrees to encoder counts (operator-calibrated) and sends a",
-                "two-axis MOVE so both axes start together. Degrees are soft-clamped to",
+                "two-axis move so both axes start together; soft-clamped to travel limits.",
-                "the configured travel limits.",
+                "Example:  gimbal move 30,-10    (yaw 30 deg, pitch -10 deg)"}},
-                "Example:  goto 30 -10     (yaw 30 deg, pitch -10 deg)"}},
+            {"gimbal steps <yaw>,<pitch>",
-            {"set motorctl MOVE <yaw>,<pitch>",
+             "Move both axes to absolute encoder COUNTS (no degree conversion).", {
-             "Move both axes to absolute encoder counts (no degree conversion).", {
+                "Example:  gimbal steps 100000,250000"}},
-                "Example:  set motorctl MOVE 100000,250000",
+            {"gimbal nudge <yaw|pitch> <+/-pct>",
-                "Single axis:  set motorctl MOVE Y 100000   /   set motorctl MOVE P 250000"}},
+             "Relative step move by a percent of the axis travel.", {
-            {"set motorctl HOME [Y|P]",
+                "Arrow keys do this: Left/Right = yaw -/+5%, Up/Down = pitch +/-10%.",
+                "Example:  gimbal nudge yaw -5"}},
+            {"gimbal home [y|p]",
              "Run the endstop-finding home sequence (both axes, or one).", {
-                "Example:  set motorctl HOME      /      set motorctl HOME Y"}},
+                "Example:  gimbal home      /      gimbal home y"}},
-            {"set motorctl STOP <Y|P|ALL>",
+            {"gimbal stop [y|p|all]",
-             "Stop motion immediately on an axis or both.", {}},
+             "Stop motion immediately (also cancels a running calibration).", {}},
-            {"set motorctl SPEED <Y|P> <vel>",
+            {"gimbal speed <y|p> <vel>",
              "Set the max slew speed (counts/s) for an axis.", {}},
+            {"gimbal reset [y|p] / gimbal enable|disable <y|p> / gimbal setpos <y|p> <v>",
+             "Other firmware motor controls.", {}},
         }},
-        {"Diagnostics", "Inspect the firmware/driver state for debugging.", {
+        {"Diagnostics", "Inspect, self-test and calibrate the gimbal.", {
-            {"dump",
+            {"gimbal dump",
              "Request a full firmware state dump and show it here.", {
-                "Sends DUMP to the firmware; the captured DUMP BEGIN..END block (build,",
+                "Captured DUMP BEGIN..END block (build, uptime, reset cause, per-axis",
-                "uptime, reset cause, and per-axis TMC5160 registers) is logged and, in",
+                "TMC5160 registers); shown in the gimbal 'g' expanded view.",
-                "the TUI, shown in the Diagnostics help section.",
                 "Equivalent offline tool:  ./firmware/dump.sh"}},
-            {"set motorctl DIAG [Y|P|ALL]",
+            {"gimbal diag [y|p|all]",
-             "Run the motor self-test (emits DG lines, ends with DG DONE).", {
+             "Run the firmware motor self-test; results stream to the LOG + a logfile.", {
-                "Each axis is swept at several speeds/directions; results stream as DG",
+                "Each axis is swept at several speeds/directions (DG lines). A PASS/FAIL",
-                "lines in the log. Axis must be homed first."}},
+                "summary is logged and saved to logs/diag_*.log. Axes must be homed first."}},
-            {"set motorctl STATUS",
+            {"gimbal calib",
+             "Calibrate steps<->degrees using the IMU (sweeps each axis, ~minutes).", {
+                "Homes-limits required + IMU enabled. Sweeps yaw then pitch in 10 steps,",
+                "dwells 5 s recording IMU orientation, fits the conversion, applies it to",
+                "the session and writes logs/calib_*.log. 'gimbal stop' cancels."}},
+            {"gimbal status",
              "Ask the firmware to emit one telemetry (ST) line now.", {}},
         }},
         {"Capture", "Control image capture and encoding.", {

src/core/MtiProtocol.cpp

@@ -0,0 +1,123 @@
+#include "fgc/MtiProtocol.h"
+
+#include <cstring>
+
+namespace fgc {
+
+namespace {
+
+// Big-endian readers (MTi is big-endian; host x86 is little-endian).
+float beFloat(const uint8_t* p) {
+    uint32_t u = (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) |
+                 (uint32_t(p[2]) << 8) | uint32_t(p[3]);
+    float f;
+    std::memcpy(&f, &u, sizeof(f));
+    return f;
+}
+
+uint16_t beU16(const uint8_t* p) {
+    return static_cast<uint16_t>((uint16_t(p[0]) << 8) | uint16_t(p[1]));
+}
+
+// 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));
+    v.push_back(static_cast<uint8_t>(x & 0xFF));
+}
+void putBE(std::vector<uint8_t>& v, uint32_t x) {
+    v.push_back(static_cast<uint8_t>((x >> 24) & 0xFF));
+    v.push_back(static_cast<uint8_t>((x >> 16) & 0xFF));
+    v.push_back(static_cast<uint8_t>((x >> 8) & 0xFF));
+    v.push_back(static_cast<uint8_t>(x & 0xFF));
+}
+
+}  // namespace
+
+uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len) {
+    unsigned sum = 0;
+    for (std::size_t i = 0; i < len; ++i) sum += from_bid[i];
+    return static_cast<uint8_t>(sum & 0xFF);
+}
+
+std::vector<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data) {
+    std::vector<uint8_t> m;
+    m.reserve(5 + data.size());
+    m.push_back(kMtiPreamble);
+    m.push_back(kMtiBid);
+    m.push_back(mid);
+    m.push_back(static_cast<uint8_t>(data.size()));
+    m.insert(m.end(), data.begin(), data.end());
+    // Checksum covers BID..DATA; the CS byte makes the total ≡ 0 (mod 256).
+    uint8_t s = mtiChecksum(m.data() + 1, m.size() - 1);
+    m.push_back(static_cast<uint8_t>((0x100 - s) & 0xFF));
+    return m;
+}
+
+std::vector<uint8_t> msgGoToConfig() { return mtiMessage(kMidGoToConfig); }
+std::vector<uint8_t> msgGoToMeasurement() { return mtiMessage(kMidGoToMeasurement); }
+
+std::vector<uint8_t> msgSetOutputMode() {
+    std::vector<uint8_t> d;
+    putBE(d, kOutputMode);
+    return mtiMessage(kMidSetOutputMode, d);
+}
+
+std::vector<uint8_t> msgSetOutputSettings() {
+    std::vector<uint8_t> d;
+    putBE(d, kOutputSettings);
+    return mtiMessage(kMidSetOutputSettings, d);
+}
+
+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;
+    std::size_t o = 0;
+    s.temp_c = beFloat(data + o);  o += 4;
+    for (int i = 0; i < 3; ++i) { s.acc[i] = beFloat(data + o); o += 4; }
+    for (int i = 0; i < 3; ++i) { s.gyr[i] = beFloat(data + o); o += 4; }
+    for (int i = 0; i < 3; ++i) { s.mag[i] = beFloat(data + o); o += 4; }
+    s.roll_deg  = beFloat(data + o); o += 4;
+    s.pitch_deg = beFloat(data + o); o += 4;
+    s.yaw_deg   = beFloat(data + o); o += 4;
+    // Report yaw as a 0..360 heading rather than the MTi's native -180..180.
+    if (s.yaw_deg < 0.f) s.yaw_deg += 360.f;
+    s.sample_counter = beU16(data + o); o += 2;
+    s.valid = true;
+    return s;
+}
+
+void MtiFramer::feed(const uint8_t* p, std::size_t n) {
+    for (std::size_t i = 0; i < n; ++i) {
+        uint8_t b = p[i];
+        switch (state_) {
+            case S::Pre:
+                if (b == kMtiPreamble) state_ = S::Bid;
+                break;
+            case S::Bid:
+                // After PRE we expect BID; otherwise resync (allow back-to-back PRE).
+                if (b == kMtiBid) { sum_ = b; state_ = S::Mid; }
+                else if (b == kMtiPreamble) { /* stay */ }
+                else state_ = S::Pre;
+                break;
+            case S::Mid:
+                mid_ = b; sum_ += b; state_ = S::Len;
+                break;
+            case S::Len:
+                len_ = b; sum_ += b; data_.clear();
+                state_ = (len_ == 0) ? S::Cs : S::Data;
+                break;
+            case S::Data:
+                data_.push_back(b); sum_ += b;
+                if (data_.size() == len_) state_ = S::Cs;
+                break;
+            case S::Cs:
+                sum_ += b;
+                if ((sum_ & 0xFF) == 0 && sink_)
+                    sink_(mid_, data_.data(), data_.size());
+                state_ = S::Pre;
+                break;
+        }
+    }
+}
+
+}  // namespace fgc

src/core/Paths.cpp

@@ -1,7 +1,12 @@
 #include "fgc/Paths.h"
 
+#include "fgc/Logger.h"
+
+#include <cstdio>
 #include <cstdlib>
+#include <ctime>
 #include <filesystem>
+#include <system_error>
 #include <unistd.h>
 
 namespace fs = std::filesystem;
@@ -92,4 +97,37 @@ std::string defaultOutputDir() {
     return (fs::path(base) / "fire_gimbal_control" / "images").string();
 }
 
+std::string defaultLogDir() {
+    std::string base = envOr("XDG_DATA_HOME", expandUser("~/.local/share"));
+    return (fs::path(base) / "fire_gimbal_control" / "logs").string();
+}
+
+std::string timestampedLogName(const std::string& prefix) {
+    std::time_t t = std::time(nullptr);
+    std::tm tm{};
+    localtime_r(&t, &tm);
+    char buf[32];
+    std::strftime(buf, sizeof(buf), "%Y%m%d-%H%M%S", &tm);
+    return prefix + "_" + buf + ".log";
+}
+
+std::string writeLogFile(const std::string& name, const std::string& text) {
+    std::error_code ec;
+    fs::path dir = defaultLogDir();
+    fs::create_directories(dir, ec);
+    fs::path path = dir / name;
+    FILE* f = std::fopen(path.c_str(), "wb");
+    if (!f) {
+        LOG_WARN << "could not open logfile " << path.string() << " for writing";
+        return "";
+    }
+    bool ok = std::fwrite(text.data(), 1, text.size(), f) == text.size();
+    if (std::fclose(f) != 0) ok = false;
+    if (!ok) {
+        LOG_WARN << "failed to write logfile " << path.string();
+        return "";
+    }
+    return path.string();
+}
+
 }  // namespace fgc::paths

src/serial/MtiImuSource.cpp

@@ -0,0 +1,139 @@
+#include "fgc/MtiImuSource.h"
+
+#include "fgc/Logger.h"
+#include "fgc/MtiProtocol.h"
+
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <mutex>
+#include <thread>
+
+#include <boost/asio.hpp>
+#include <termios.h>
+#include <unistd.h>
+
+namespace fgc {
+
+struct MtiImuSource::Impl {
+    Impl(std::string dev, unsigned int b)
+        : device(std::move(dev)), baud(b), serial(io),
+          framer([this](uint8_t mid, const uint8_t* d, std::size_t n) { onFrame(mid, d, n); }) {}
+
+    using clock = std::chrono::steady_clock;
+
+    std::string              device;
+    unsigned int             baud;
+    boost::asio::io_context  io;
+    boost::asio::serial_port serial;
+    std::thread              io_thread;
+    std::array<uint8_t, 256> rxbuf{};
+
+    std::mutex        mutex;
+    ImuSample         latest;
+    clock::time_point last_rx{};
+    std::atomic<bool> open{false};
+
+    MtiFramer framer;
+    unsigned  bad_len_warned = 0;
+
+    void onFrame(uint8_t mid, const uint8_t* d, std::size_t n) {
+        if (mid != kMidMTData) return;  // ignore acks/other during streaming
+        auto s = parseMTData(mid, d, n);
+        if (!s) {
+            if (bad_len_warned++ < 1)
+                LOG_WARN << "MTi: unexpected MTData length " << n << " (expected "
+                         << int(kMTDataLen) << "); device may not be in the configured mode";
+            return;
+        }
+        std::lock_guard<std::mutex> lock(mutex);
+        latest  = *s;
+        last_rx = clock::now();
+    }
+
+    void doRead() {
+        serial.async_read_some(
+            boost::asio::buffer(rxbuf),
+            [this](const boost::system::error_code& ec, std::size_t n) {
+                if (ec) {
+                    if (ec != boost::asio::error::operation_aborted)
+                        LOG_WARN << "MTi serial read failed: " << ec.message();
+                    return;
+                }
+                framer.feed(rxbuf.data(), n);
+                doRead();
+            });
+    }
+
+    // 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() {
+        using namespace std::chrono_literals;
+        auto write = [this](const std::vector<uint8_t>& m) {
+            boost::system::error_code ec;
+            boost::asio::write(serial, boost::asio::buffer(m), ec);
+            if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
+        };
+        write(msgGoToConfig());        std::this_thread::sleep_for(60ms);
+        write(msgSetOutputMode());     std::this_thread::sleep_for(60ms);
+        write(msgSetOutputSettings());  std::this_thread::sleep_for(60ms);
+        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);
+    }
+};
+
+MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
+    : impl_(std::make_unique<Impl>(std::move(device), baud)) {}
+
+MtiImuSource::~MtiImuSource() { stop(); }
+
+void MtiImuSource::start() {
+    namespace asio = boost::asio;
+    boost::system::error_code ec;
+    impl_->serial.open(impl_->device, ec);
+    if (ec) {
+        LOG_ERROR << "MTi: failed to open " << impl_->device << ": " << ec.message();
+        return;
+    }
+    impl_->serial.set_option(asio::serial_port_base::baud_rate(impl_->baud));
+    impl_->serial.set_option(asio::serial_port_base::character_size(8));
+    impl_->serial.set_option(asio::serial_port_base::parity(asio::serial_port_base::parity::none));
+    impl_->serial.set_option(
+        asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
+    impl_->serial.set_option(
+        asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));
+
+    impl_->open = true;
+    impl_->configure();
+    impl_->doRead();
+    impl_->io_thread = std::thread([this] { impl_->io.run(); });
+    LOG_INFO << "MTi IMU started on " << impl_->device << " @ " << impl_->baud;
+}
+
+void MtiImuSource::stop() {
+    if (!impl_) return;
+    impl_->io.stop();
+    if (impl_->io_thread.joinable()) impl_->io_thread.join();
+    boost::system::error_code ec;
+    if (impl_->serial.is_open()) impl_->serial.close(ec);
+    impl_->open = false;
+}
+
+bool MtiImuSource::connected() const {
+    if (!impl_->open) return false;
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    return impl_->latest.valid &&
+           (Impl::clock::now() - impl_->last_rx) < std::chrono::seconds(1);
+}
+
+std::optional<ImuSample> MtiImuSource::sample() {
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    if (!impl_->latest.valid ||
+        (Impl::clock::now() - impl_->last_rx) >= std::chrono::seconds(1))
+        return std::nullopt;
+    return impl_->latest;
+}
+
+}  // namespace fgc

src/serial/SerialMotorController.cpp

@@ -70,6 +70,14 @@ struct SerialMotorController::Impl {
     std::string                  dump_buf;
     std::string                  latest_dump;
 
+    // DIAG capture: the firmware streams "DG ..." lines (interleaved with ST)
+    // from "DG BEGIN" to "DG DONE". We accumulate the DG lines, log each one live
+    // (so it appears in the LOG pane), and publish the completed block.
+    bool                         diagging = false;
+    std::string                  diag_buf;
+    std::string                  latest_diag;
+    std::atomic<unsigned>        diag_seq{0};
+
     // Write one command line (newline-terminated) to the controller. Used by
     // sendCommand and by the internal DUMP re-request.
     //
@@ -132,6 +140,30 @@ struct SerialMotorController::Impl {
             // OK acks and other async output (DG/DUMP/BOOT/...).
             LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
             captureDump(line);
+            captureDiag(line);
+        }
+    }
+
+    // Assemble the DIAG stream. DG lines arrive interleaved with ST over many
+    // seconds, so we accumulate only "DG " lines and log each live; on "DG DONE"
+    // we publish the block and bump diag_seq so the app can pick up the result.
+    void captureDiag(const std::string& line) {
+        const bool begin = line.find("DG BEGIN") != std::string::npos;
+        // Start on the FIRST BEGIN only; a per-axis "DG BEGIN P" mid-run must not
+        // wipe the already-captured first axis.
+        if (begin && !diagging) { diagging = true; diag_buf.clear(); }
+        if (!diagging) return;
+        if (line.rfind("DG ", 0) != 0 && !begin) return;  // ignore non-DG lines
+        diag_buf += line + "\n";
+        LOG_INFO << line;  // stream to the LOG pane
+        if (line.find("DG DONE") != std::string::npos) {
+            {
+                std::lock_guard<std::mutex> lock(mutex);
+                latest_diag = diag_buf;
+            }
+            ++diag_seq;
+            diagging = false;
+            diag_buf.clear();
         }
     }
 
@@ -242,6 +274,13 @@ std::string SerialMotorController::lastDump() {
     return impl_->latest_dump;
 }
 
+std::string SerialMotorController::lastDiag() {
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    return impl_->latest_diag;
+}
+
+unsigned SerialMotorController::diagSeq() const { return impl_->diag_seq.load(); }
+
 bool SerialMotorController::connected() const { return impl_->connected; }
 
 }  // namespace fgc

src/ui/TuiUi.cpp

@@ -77,19 +77,30 @@ Element gimbalPanel(const GimbalView& g) {
         rows.push_back(separator());
         rows.push_back(axisRow(g.pitch));
     }
-    return panel("GIMBAL", Color::Cyan, vbox(std::move(rows)));
+    return panel("[g] GIMBAL", Color::Cyan, vbox(std::move(rows)));
 }
 
 Element sensorsPanel(const SensorsView& s) {
-    std::vector<Element> rows;
+    // One labelled subsection per physical sensor: a title + status header, then
-    rows.push_back(text("pending integration") | dim);
+    // its readings. Keeps MTi (orientation/device temp) and DHT11 (ambient)
-    rows.push_back(separator());
+    // clearly separated.
-    for (const auto& f : s.fields) {
+    auto group = [](const SensorGroup& g, const std::string& hint) {
-        Element val = text(f.value + (f.unit.empty() ? "" : " " + f.unit));
+        std::vector<Element> rows;
-        val = f.present ? (val | bold) : (val | dim);
+        Element status = g.present ? (text(g.status) | color(Color::Green))
-        rows.push_back(hbox({text(f.label) | dim, filler(), val}));
+                                   : (text(g.status) | dim);
-    }
+        rows.push_back(hbox({text(g.title) | bold, text("  "), status, filler(),
-    return panel("SENSORS  (DHT11 · MTi)", Color::Magenta, vbox(std::move(rows)));
+                             text(hint) | dim}));
+        for (const auto& f : g.fields) {
+            Element val = text(f.value + (f.unit.empty() ? "" : " " + f.unit));
+            val = f.present ? (val | bold) : (val | dim);
+            rows.push_back(hbox({text("  "), text(f.label) | dim, filler(), val}));
+        }
+        return vbox(std::move(rows));
+    };
+    return panel("[i] SENSORS", Color::Magenta,
+                 vbox({group(s.imu, "i to expand"),
+                       separator(),
+                       group(s.dht, "")}));
 }
 
 Element cameraPanel(const CaptureView& c) {
@@ -151,6 +162,32 @@ Element logPanel(const std::vector<LogLine>& lines) {
                   vbox(std::move(rows)) | focusPositionRelative(0, 1) | yframe);
 }
 
+// Compact strip below the log: the running special op (live) + the last
+// calibration/diagnostics result (persists so it doesn't scroll away).
+Element activityPanel(const ActivityView& a) {
+    std::vector<Element> rows;
+    if (a.active) {
+        rows.push_back(hbox({
+            text(" \xE2\x96\xB6 ") | color(Color::Cyan) | bold,
+            text(a.title + ": ") | bold,
+            text(a.status) | color(Color::Cyan),
+        }));
+    } else if (a.has_result) {
+        rows.push_back(text(" idle") | dim);
+    }
+    if (a.has_result) {
+        rows.push_back(hbox({text("last: ") | dim,
+                             text(a.result_title) | color(toColor(a.result_color)) | bold}));
+        for (const auto& l : a.result)
+            rows.push_back(text("   " + l) | color(toColor(a.result_color)));
+    }
+    if (!a.prompt.empty()) {
+        rows.push_back(hbox({text(" " + a.prompt + " ") | bold | color(Color::Black) |
+                                 bgcolor(Color::Yellow)}));
+    }
+    return window(text(" ACTIVITY ") | bold | color(Color::Cyan), vbox(std::move(rows)));
+}
+
 // Inline help pane (toggled with '?'). Lists every command section; the
 // `sel`-th section is expanded to show each entry's detail. The Diagnostics
 // section additionally renders the last captured firmware DUMP block.
@@ -281,7 +318,12 @@ Element axisDumpDetail(const DumpAxis& ax) {
 // registers. Rendered per axis so the two axes sit side by side and every row
 // (incl. RAMP_STAT, the last one) is visible without scrolling.
 Element axisColumn(const std::string& title, const AxisView& live, const DumpData& d,
-                   char letter) {
+                   char letter, bool calib_has, const CalibAxisView& cal) {
+    auto fmt = [](const char* f, double v) {
+        char b[32];
+        std::snprintf(b, sizeof(b), f, v);
+        return std::string(b);
+    };
     std::vector<Element> col;
     col.push_back(axisLiveDetail(live));
     col.push_back(separator());
@@ -296,20 +338,39 @@ Element axisColumn(const std::string& title, const AxisView& live, const DumpDat
         col.push_back(text(d.valid ? "(axis absent from dump)"
                                    : "(awaiting dump - press 'd')") | dim);
 
+    // Last calibration result for this axis.
+    col.push_back(separator());
+    col.push_back(text("CALIBRATION") | bold | color(Color::Magenta));
+    if (calib_has && cal.ok) {
+        col.push_back(kvRow("cnt/deg", fmt("%.3f", cal.counts_per_deg)));
+        col.push_back(kvRow("zero", std::to_string(cal.zero_count)));
+        col.push_back(kvRow("R2", fmt("%.4f", cal.r2), cal.r2 >= 0.99 ? Color::Green : Color::Yellow));
+        col.push_back(kvRow("points", std::to_string(cal.n)));
+    } else {
+        col.push_back(text(calib_has ? "fit failed" : "uncalibrated this session") | dim);
+    }
+
     return panel(title, Color::Cyan, vbox(std::move(col)) | yframe);
 }
 
 // Full-screen gimbal view (toggled with 'g'): one column per axis, each with
-// live telemetry above its decoded firmware register dump.
+// live telemetry above its decoded firmware register dump + last calibration.
-Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump) {
+Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump, const CalibResultView& calib) {
     DumpData d = parseDump(dump.text);
 
     std::string reset;
     for (size_t i = 0; i < d.reset_flags.size(); ++i)
         reset += (i ? " " : "") + d.reset_flags[i];
+    std::string calib_note = "uncalibrated this session";
+    if (calib.has) {
+        long long now = std::chrono::duration_cast<std::chrono::milliseconds>(
+                            std::chrono::system_clock::now().time_since_epoch()).count();
+        calib_note = "calibrated " + formatTimeAgo(now, calib.ts_ms);
+    }
     Element header = hbox({
         (g.present ? text(" link up ") | color(Color::Green)
                    : text(" link down ") | color(Color::Red) | bold),
+        text("  " + calib_note) | color(calib.has ? Color::Magenta : Color::GrayDark),
         filler(),
         text(d.valid ? ("build " + d.build + "  up " + std::to_string(d.uptime_ms) +
                         "ms  reset:" + reset)
@@ -317,13 +378,64 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump) {
     });
 
     std::vector<Element> cols;
-    cols.push_back(axisColumn("YAW", g.yaw, d, 'Y'));
+    cols.push_back(axisColumn("YAW", g.yaw, d, 'Y', calib.has, calib.yaw));
-    if (g.pitch_present) cols.push_back(axisColumn("PITCH", g.pitch, d, 'P'));
+    if (g.pitch_present) cols.push_back(axisColumn("PITCH", g.pitch, d, 'P', calib.has, calib.pitch));
 
     return window(text(" GIMBAL  (g/Esc:close  d:refresh dump) ") | bold | color(Color::Cyan),
                   vbox({header, separator(), hbox(std::move(cols)) | flex}));
 }
 
+// Full-screen IMU view (toggled with 'i'): every MTi channel with units.
+Element imuDetailPanel(const ImuView& v) {
+    auto f2 = [](float x) {
+        char b[24];
+        std::snprintf(b, sizeof(b), "%.2f", x);
+        return std::string(b);
+    };
+    // 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("y " + f2(xyz[1])) | color(c) | size(WIDTH, EQUAL, 12),
+            text("z " + f2(xyz[2])) | color(c) | size(WIDTH, EQUAL, 12),
+        });
+    };
+    const float ori[3] = {v.roll_deg, v.pitch_deg, v.yaw_deg};
+
+    std::vector<Element> body;
+    if (!v.present) {
+        body.push_back(text("(no IMU data)") | color(Color::Red) | bold);
+        body.push_back(text("check [Features] enable_imu and [IMU] device, or --mock-imu") | dim);
+    } else {
+        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("pitch " + f2(ori[1])) | bold | size(WIDTH, EQUAL, 14),
+            text("yaw " + f2(ori[2])) | bold | size(WIDTH, EQUAL, 14),
+        }));
+        body.push_back(separator());
+        body.push_back(vecRow("ACCEL", "m/s2", v.acc, Color::Cyan));
+        body.push_back(vecRow("RATE OF TURN", "rad/s", v.gyr, Color::Cyan));
+        body.push_back(vecRow("MAG FIELD", "a.u.", v.mag, Color::Cyan));
+        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("sample #" + std::to_string(v.sample_counter)) | 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),
+                  vbox({hbox({status, filler()}), separator(),
+                        vbox(std::move(body)) | flex}));
+}
+
 }  // namespace
 
 TuiUi::TuiUi() = default;
@@ -368,10 +480,11 @@ void TuiUi::refreshLoop() {
 void TuiUi::uiLoop() {
     std::string cmd_buffer;
     bool        command_mode = false;
-    enum class Overlay { None, Help, Gimbal };
+    enum class Overlay { None, Help, Gimbal, Sensors };
     Overlay     overlay      = Overlay::None;  // which takeover panel owns the main area
     int         help_sel     = 0;
     bool        gimbal_dump_requested = false;  // auto-pull a dump the first time
+    bool        calib_prompt = false;           // a yes/no calib-save question is showing
 
     auto input = Input(&cmd_buffer, "type a command, Enter to run, Esc to cancel");
 
@@ -381,6 +494,7 @@ void TuiUi::uiLoop() {
             std::lock_guard<std::mutex> lock(log_mutex_);
             s.log.assign(log_.begin(), log_.end());
         }
+        calib_prompt = !s.activity.prompt.empty();
 
         std::string mode = s.header.live ? "LIVE" : "MOCK";
         Element header = hbox({
@@ -401,8 +515,8 @@ void TuiUi::uiLoop() {
         } else {
             bottom = hbox({
                 keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"),
-                keyHint("r", "Reset"), keyHint("g", "Gimbal"), keyHint(":", "Cmd"),
+                keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"),
-                keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
+                keyHint(":", "Cmd"), keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
             });
         }
 
@@ -414,9 +528,19 @@ void TuiUi::uiLoop() {
                 return vbox({header, separator(), helpPanel(help_sel, s.dump) | flex, bottom});
             case Overlay::Gimbal:
                 return vbox({header, separator(),
-                             gimbalDetailPanel(s.gimbal, s.dump) | flex, bottom});
+                             gimbalDetailPanel(s.gimbal, s.dump, s.calib) | flex, bottom});
-            default:
+            case Overlay::Sensors:
-                return vbox({header, separator(), top, middle, logPanel(s.log) | flex, bottom});
+                return vbox({header, separator(), imuDetailPanel(s.imu) | flex, bottom});
+            default: {
+                // Activity strip sits between the log and the key bar; shown only
+                // once a special op has run or is running, else it costs no space.
+                std::vector<Element> col = {header, separator(), top, middle,
+                                            logPanel(s.log) | flex};
+                if (s.activity.active || s.activity.has_result || !s.activity.prompt.empty())
+                    col.push_back(activityPanel(s.activity));
+                col.push_back(bottom);
+                return vbox(std::move(col));
+            }
         }
     });
 
@@ -441,8 +565,21 @@ void TuiUi::uiLoop() {
             if (e == Event::ArrowUp)   { help_sel = (help_sel - 1 + n) % n; return true; }
         }
         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.
+        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::ArrowDown)  { sink_("gimbal nudge pitch -10"); return true; }
+        }
         if (!e.is_character()) return false;
         const std::string& c = e.character();
+        // Answer the activity-strip "save calibration?" yes/no prompt.
+        if (calib_prompt && overlay == Overlay::None && sink_) {
+            if (c == "y" || c == "Y") { sink_("calib save");    return true; }
+            if (c == "n" || c == "N") { sink_("calib discard"); return true; }
+        }
         if (c == "?") {
             overlay = (overlay == Overlay::Help) ? Overlay::None : Overlay::Help;
             return true;
@@ -453,14 +590,18 @@ void TuiUi::uiLoop() {
             } else {
                 overlay = Overlay::Gimbal;
                 if (!gimbal_dump_requested) {  // auto-pull a dump the first time
-                    if (sink_) sink_("dump");
+                    if (sink_) sink_("gimbal dump");
                     gimbal_dump_requested = true;
                 }
             }
             return true;
         }
         if (overlay == Overlay::Gimbal && c == "d") {  // manual dump refresh
-            if (sink_) sink_("dump");
+            if (sink_) sink_("gimbal dump");
+            return true;
+        }
+        if (c == "i") {
+            overlay = (overlay == Overlay::Sensors) ? Overlay::None : Overlay::Sensors;
             return true;
         }
         if (overlay == Overlay::Help) {  // vim-style section nav while help is open
@@ -470,8 +611,8 @@ void TuiUi::uiLoop() {
         if (c == "q") { if (sink_) sink_("exit"); return true; }
         if (c == "s") { if (sink_) sink_("start"); return true; }
         if (c == "x") { if (sink_) sink_("stop"); return true; }
-        if (c == "h") { if (sink_) sink_("set motorctl HOME"); return true; }
+        if (c == "h") { if (sink_) sink_("gimbal home"); return true; }
-        if (c == "r") { if (sink_) sink_("set motorctl RESET"); return true; }
+        if (c == "r") { if (sink_) sink_("gimbal reset"); return true; }
         if (c == ":") { command_mode = true; return true; }
         return false;
     });

src/ui/UiSnapshot.cpp

@@ -49,16 +49,18 @@ std::string formatTimeAgo(long long now_ms, long long then_ms) {
 
 SensorsView pendingSensorsView() {
     SensorsView v;
-    v.dht_present = false;
+    // Xsens MTi-28 AHRS: drift-free Euler orientation + device internal temp.
-    v.imu_present = false;
+    v.imu.title  = "MTi (orientation)";
-    // DHT11 temperature & humidity (Aosong): 0-50 °C ±2 °C, 20-90 %RH ±5 %.
+    v.imu.status = "pending";
-    v.fields.push_back({"Temp",  "--.-", "\xC2\xB0""C", false});
+    v.imu.fields.push_back({"Roll",  "--.-", "\xC2\xB0", false});
-    v.fields.push_back({"Humid", "--",   "%RH",         false});
+    v.imu.fields.push_back({"Pitch", "--.-", "\xC2\xB0", false});
-    // Xsens MTi-28 AHRS: drift-free Euler orientation (roll/pitch/yaw).
+    v.imu.fields.push_back({"Yaw",   "--.-", "\xC2\xB0", false});
-    v.fields.push_back({"Roll",  "--.-", "\xC2\xB0", false});
+    v.imu.fields.push_back({"Temp",  "--.-", "\xC2\xB0""C", false});  // MTi internal
-    v.fields.push_back({"Pitch", "--.-", "\xC2\xB0", false});
+    // DHT11 temperature & humidity (Aosong): 0-50 °C ±2 °C, 20-90 %RH ±5 % (ambient).
-    v.fields.push_back({"Yaw",   "--.-", "\xC2\xB0", false});
+    v.dht.title  = "DHT11 (ambient)";
-    v.fields.push_back({"IMU",   "pending", "",      false});
+    v.dht.status = "pending";
+    v.dht.fields.push_back({"Temp",  "--.-", "\xC2\xB0""C", false});
+    v.dht.fields.push_back({"Humid", "--",   "%RH",         false});
     return v;
 }
 

tests/CMakeLists.txt

@@ -28,6 +28,11 @@ add_executable(fgc_tests
   test_geometry.cpp
   test_scangrid.cpp
   test_uisnapshot.cpp
+  test_mtiprotocol.cpp
+  test_dumpparser.cpp
+  test_helptext.cpp
+  test_diagparser.cpp
+  test_calibration.cpp
 )
 target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest)
 

tests/test_calibration.cpp

@@ -0,0 +1,73 @@
+#include <doctest/doctest.h>
+
+#include "fgc/Calibration.h"
+
+#include <cmath>
+
+using namespace fgc;
+
+TEST_CASE("linearFit recovers slope/intercept exactly for collinear points") {
+    // counts = 983.33*deg + 500000  (a yaw-like calibration)
+    std::vector<std::pair<double, double>> xy;
+    for (int deg = -45; deg <= 45; deg += 10)
+        xy.emplace_back(deg, 983.33 * deg + 500000.0);
+
+    LinearFit f = linearFit(xy);
+    REQUIRE(f.ok);
+    CHECK(f.slope == doctest::Approx(983.33));
+    CHECK(f.intercept == doctest::Approx(500000.0));
+    CHECK(f.r2 == doctest::Approx(1.0));
+}
+
+TEST_CASE("linearFit is robust to noise (slope close, r2 high)") {
+    std::vector<std::pair<double, double>> xy = {
+        {-40, -39500}, {-20, -20100}, {0, 300}, {20, 19800}, {40, 40200}};
+    LinearFit f = linearFit(xy);
+    REQUIRE(f.ok);
+    CHECK(f.slope == doctest::Approx(1000.0).epsilon(0.05));
+    CHECK(f.r2 > 0.99);
+}
+
+TEST_CASE("linearFit rejects degenerate input") {
+    CHECK_FALSE(linearFit({}).ok);
+    CHECK_FALSE(linearFit({{1.0, 2.0}}).ok);          // single point
+    CHECK_FALSE(linearFit({{5.0, 1.0}, {5.0, 9.0}}).ok);  // no x spread
+}
+
+TEST_CASE("circularMeanDeg handles the +/-180 wrap") {
+    CHECK(circularMeanDeg({10, 20, 30}) == doctest::Approx(20.0));
+    // Mean of 170 and -170 is 180 (not 0) — must not average naively to 0.
+    CHECK(std::abs(circularMeanDeg({170, -170})) == doctest::Approx(180.0));
+    CHECK(circularMeanDeg({}) == doctest::Approx(0.0));
+}
+
+TEST_CASE("unwrapNear shifts an angle to within +/-180 of the previous sample") {
+    CHECK(unwrapNear(10.0, 20.0)   == doctest::Approx(20.0));    // already near
+    CHECK(unwrapNear(350.0, 355.0) == doctest::Approx(355.0));
+    CHECK(unwrapNear(350.0, 5.0)   == doctest::Approx(365.0));   // crossed 360 going up
+    CHECK(unwrapNear(10.0, 355.0)  == doctest::Approx(-5.0));    // crossed 0 going down
+    CHECK(unwrapNear(720.0, 10.0)  == doctest::Approx(730.0));   // multiple turns up
+    CHECK(unwrapNear(-360.0, 10.0) == doctest::Approx(-350.0));  // multiple turns down
+}
+
+TEST_CASE("unwrapping a 0..360 sweep across the wrap yields a clean linear fit") {
+    // A yaw sweep whose true (unwrapped) heading is 300..420 deg, but the IMU
+    // reports it wrapped into 0..360. With per-sample unwrapNear the fit recovers
+    // the straight line; without it the 360->0 jump would wreck R^2 (the bug from
+    // the field calibration log).
+    std::vector<std::pair<double, double>> pts;  // (deg, counts), counts = 100*heading
+    double prev = 0.0;
+    bool   have = false;
+    for (int i = 0; i <= 12; ++i) {
+        const double heading   = 300.0 + i * 10.0;          // 300..420 (true)
+        const double reported  = std::fmod(heading, 360.0);  // 0..360 (wrapped)
+        double d = have ? unwrapNear(prev, reported) : reported;
+        prev = d;
+        have = true;
+        pts.emplace_back(d, heading * 100.0);
+    }
+    LinearFit f = linearFit(pts);
+    CHECK(f.ok);
+    CHECK(f.slope == doctest::Approx(100.0));
+    CHECK(f.r2 > 0.9999);
+}

tests/test_config.cpp

@@ -47,3 +47,34 @@ TEST_CASE("ConfigLoader validates input") {
     CHECK_THROWS(ConfigLoader::fromMap({{"General.image_interval", "abc"}}));
     CHECK_THROWS(ConfigLoader::fromMap({{"General.debug", "maybe"}}));
 }
+
+TEST_CASE("updateIniSectionKeys replaces in-section keys, leaving others intact") {
+    const std::string in =
+        "[General]\n"
+        "tower_name = Foo\n"
+        "[Motor]\n"
+        "; calibrate me\n"
+        "yaw_counts_per_deg = 983.33\n"
+        "yaw_zero_count = 500000\n"
+        "yaw_min_deg = -90\n"
+        "[Scan]\n"
+        "yaw_min_deg = -90\n";  // same key name, different section — must NOT change
+
+    auto out = updateIniSectionKeys(in, "Motor",
+                                    {{"yaw_counts_per_deg", "1000"}, {"yaw_zero_count", "12345"}});
+
+    CHECK(out.find("yaw_counts_per_deg = 1000") != std::string::npos);
+    CHECK(out.find("yaw_zero_count = 12345") != std::string::npos);
+    CHECK(out.find("yaw_counts_per_deg = 983.33") == std::string::npos);  // replaced
+    CHECK(out.find("; calibrate me") != std::string::npos);               // comment kept
+    CHECK(out.find("tower_name = Foo") != std::string::npos);             // other section kept
+    CHECK(out.find("[Motor]\nyaw_min_deg") == std::string::npos);         // Motor's other key kept in place
+    CHECK(out.find("[Scan]\nyaw_min_deg = -90") != std::string::npos);    // Scan untouched
+}
+
+TEST_CASE("updateIniSectionKeys appends keys missing from the section") {
+    const std::string in = "[Motor]\nyaw_counts_per_deg = 1\n";
+    auto out = updateIniSectionKeys(in, "Motor", {{"pitch_zero_count", "7"}});
+    // re-parsing must see the appended key under [Motor]
+    CHECK(out.find("pitch_zero_count = 7") != std::string::npos);
+}

tests/test_diagparser.cpp

@@ -0,0 +1,81 @@
+#include <doctest/doctest.h>
+
+#include "fgc/DiagParser.h"
+
+using namespace fgc;
+
+namespace {
+// A DIAG stream as captured on the wire: DG lines interleaved with an ST line.
+const char* kDiag =
+    "DG BEGIN Y\n"
+    "ST Y:A,5,5,80084000,0,8,8,S P:A,5,5,80084000,0,8,8,S\n"
+    "DG Y S12500 FWD ERR_PEAK 234 ERR_RMS 45 ERR_STILL 89 CS_MIN 10 CS_MAX 20 "
+    "SG_MIN 456 PWM_AVG 128 FLAGS 0x00000000 PASS\n"
+    "DG Y S25000 REV ERR_PEAK 300 ERR_RMS 50 ERR_STILL 100 CS_MIN 11 CS_MAX 21 "
+    "SG_MIN 400 PWM_AVG 130 FLAGS 0x00000000 PASS\n"
+    "DG Y RESULT PASS\n"
+    "DG BEGIN P\n"
+    "DG P S12500 FWD ERR_PEAK -1 ERR_RMS -1 ERR_STILL -1 CS_MIN 12 CS_MAX 22 "
+    "SG_MIN 0 PWM_AVG 64 FLAGS 0x00000020 FAIL\n"
+    "DG P RESULT FAIL\n"
+    "DG DONE\n";
+}  // namespace
+
+TEST_CASE("parseDiag decodes axes, tests and results") {
+    DiagResult d = parseDiag(kDiag);
+    REQUIRE(d.valid);
+    CHECK(d.done);
+    REQUIRE(d.axes.size() == 2);
+
+    const DiagAxis& y = d.axes[0];
+    CHECK(y.axis == 'Y');
+    CHECK(y.has_result);
+    CHECK(y.pass);
+    REQUIRE(y.tests.size() == 2);
+    CHECK(y.tests[0].speed == 12500);
+    CHECK(y.tests[0].fwd);
+    CHECK(y.tests[0].err_peak == 234);
+    CHECK(y.tests[0].err_still == 89);
+    CHECK(y.tests[0].cs_min == 10);
+    CHECK(y.tests[0].cs_max == 20);
+    CHECK(y.tests[0].sg_min == 456);
+    CHECK(y.tests[0].pass);
+    CHECK_FALSE(y.tests[1].fwd);  // REV
+
+    const DiagAxis& p = d.axes[1];
+    CHECK(p.axis == 'P');
+    CHECK_FALSE(p.pass);          // RESULT FAIL
+    REQUIRE(p.tests.size() == 1);
+    CHECK(p.tests[0].err_peak == -1);   // no encoder
+    CHECK(p.tests[0].flags == 0x20u);
+    CHECK_FALSE(p.tests[0].pass);
+}
+
+TEST_CASE("DiagResult::allPass requires every axis result to pass") {
+    CHECK_FALSE(parseDiag(kDiag).allPass());        // P failed
+    DiagResult ok = parseDiag(
+        "DG BEGIN Y\nDG Y RESULT PASS\nDG BEGIN P\nDG P RESULT PASS\nDG DONE\n");
+    CHECK(ok.allPass());
+}
+
+TEST_CASE("parseDiag handles empty/garbage and incomplete streams") {
+    CHECK_FALSE(parseDiag("").valid);
+    CHECK_FALSE(parseDiag("ST Y:A,1,1,0,0,0,0,S\nnothing\n").valid);
+
+    DiagResult inc = parseDiag("DG BEGIN Y\nDG Y RESULT PASS\n");  // no DG DONE
+    CHECK(inc.valid);
+    CHECK_FALSE(inc.done);
+}
+
+TEST_CASE("formatDiag renders a summary") {
+    auto bad = formatDiag(DiagResult{});
+    REQUIRE(bad.size() == 1);
+    CHECK(bad[0].find("no diagnostic") != std::string::npos);
+
+    auto good = formatDiag(parseDiag(kDiag));
+    std::string joined;
+    for (const auto& l : good) joined += l + "\n";
+    CHECK(joined.find("[Y]") != std::string::npos);
+    CHECK(joined.find("[P]") != std::string::npos);
+    CHECK(joined.find("overall: FAIL") != std::string::npos);
+}

tests/test_dumpparser.cpp

@@ -0,0 +1,131 @@
+#include <doctest/doctest.h>
+
+#include "fgc/DumpParser.h"
+
+#include <algorithm>
+#include <string>
+
+using namespace fgc;
+
+namespace {
+
+// A realistic two-axis firmware DUMP block (the on-wire format from
+// firmware/src/motor.cpp printDump): header, then per axis a state line and a
+// TMC register line, bracketed by DUMP BEGIN / DUMP END.
+const char* kDump =
+    "DUMP BEGIN build=11dd3ce-dirty uptime=11740067 mcusr=0x01 free_ram=1852\n"
+    "DUMP Y state=3 hsub=10 enabled=1 lim_neg=-82919 lim_pos=98687 hold_target=-90 "
+    "speed=50000 eeprom_restored=0 has_encoder=1\n"
+    "DUMP Y TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
+    "RAMPMODE=0x00000000 XACTUAL=0xFFFFFFA6 VACTUAL=0x00000000 XTARGET=0xFFFFFFA6 "
+    "SW_MODE=0x000008A0 RAMP_STAT=0x00001680 X_ENC=0xFFFFFF7F ENC_STATUS=0x00000002 "
+    "CHOPCONF=0x00410043 DRV_STATUS=0x80084000 PWM_SCALE=0x00000011 PWM_AUTO=0x003F0039\n"
+    "DUMP P state=3 hsub=10 enabled=1 lim_neg=495183 lim_pos=1069042 hold_target=500183 "
+    "speed=150000 eeprom_restored=0 has_encoder=1\n"
+    "DUMP P TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
+    "RAMPMODE=0x00000000 XACTUAL=0x0007A1C8 VACTUAL=0x00000000 XTARGET=0x0007A1C8 "
+    "SW_MODE=0x000008A3 RAMP_STAT=0x00001688 X_ENC=0x0007A231 ENC_STATUS=0x00000002 "
+    "CHOPCONF=0x00410043 DRV_STATUS=0x80084000 PWM_SCALE=0x00010011 PWM_AUTO=0x0021003C\n"
+    "DUMP END\n";
+
+bool has(const std::vector<std::string>& v, const std::string& s) {
+    return std::find(v.begin(), v.end(), s) != v.end();
+}
+
+const DumpAxis* axis(const DumpData& d, char a) {
+    for (const auto& ax : d.axes)
+        if (ax.axis == a) return &ax;
+    return nullptr;
+}
+
+}  // namespace
+
+TEST_CASE("parseDump decodes the header") {
+    DumpData d = parseDump(kDump);
+    REQUIRE(d.valid);
+    CHECK(d.build == "11dd3ce-dirty");
+    CHECK(d.uptime_ms == 11740067);
+    CHECK(d.mcusr == 0x01u);
+    CHECK(d.free_ram == 1852);
+    CHECK(has(d.reset_flags, "PORF (power-on)"));
+    REQUIRE(d.axes.size() == 2);
+}
+
+TEST_CASE("parseDump decodes the per-axis state line") {
+    DumpData d = parseDump(kDump);
+    const DumpAxis* y = axis(d, 'Y');
+    REQUIRE(y != nullptr);
+    CHECK(y->state_name == "READY");   // state=3
+    CHECK(y->enabled);
+    CHECK(y->has_encoder);
+    CHECK_FALSE(y->eeprom_restored);
+    CHECK(y->lim_neg == -82919);
+    CHECK(y->lim_pos == 98687);
+    CHECK(y->hold_target == -90);
+    CHECK(y->speed == 50000);
+    CHECK(y->hsub == 10);
+
+    const DumpAxis* p = axis(d, 'P');
+    REQUIRE(p != nullptr);
+    CHECK(p->lim_neg == 495183);
+    CHECK(p->lim_pos == 1069042);
+}
+
+TEST_CASE("parseDump keeps all 16 TMC registers (regression: TMC-line whitespace)") {
+    // The "DUMP Y TMC ..." line was once misclassified due to a leading-space
+    // slice, dropping every register. Guard that all of them survive.
+    DumpData d = parseDump(kDump);
+    const DumpAxis* y = axis(d, 'Y');
+    REQUIRE(y != nullptr);
+    for (const char* r : {"GCONF", "GSTAT", "IOIN", "TSTEP", "RAMPMODE", "XACTUAL",
+                          "VACTUAL", "XTARGET", "SW_MODE", "RAMP_STAT", "X_ENC",
+                          "ENC_STATUS", "CHOPCONF", "DRV_STATUS", "PWM_SCALE", "PWM_AUTO"}) {
+        CHECK_MESSAGE(y->regs.count(r) == 1, "missing register ", r);
+    }
+    CHECK(y->regs.at("RAMP_STAT") == "0x00001680");
+    CHECK(y->regs.at("DRV_STATUS") == "0x80084000");
+}
+
+TEST_CASE("parseDump decodes status-register bit flags") {
+    DumpData d = parseDump(kDump);
+    const DumpAxis* y = axis(d, 'Y');
+    REQUIRE(y != nullptr);
+
+    // DRV_STATUS = 0x80084000 -> bit31 stst, bit14 stealth; CS_ACTUAL=(>>16)&0x1F=8.
+    CHECK(y->drv_status == 0x80084000u);
+    CHECK(has(y->drv_flags, "stst"));
+    CHECK(has(y->drv_flags, "stealth"));
+    CHECK(y->cs_actual == 8);
+    CHECK(y->sg_result == 0);
+
+    // GSTAT = 0 -> no flags (placeholder "-").
+    CHECK(has(y->gstat_flags, "-"));
+
+    // RAMP_STAT = 0x1680 -> bit7 event_pos_reached, bit9 position_reached, bit10 vzero.
+    CHECK(y->ramp_stat == 0x00001680u);
+    CHECK(has(y->ramp_flags, "event_pos_reached"));
+    CHECK(has(y->ramp_flags, "position_reached"));
+    CHECK(has(y->ramp_flags, "vzero"));
+}
+
+TEST_CASE("parseDump tolerates a corrupted/merged DUMP END (best-effort)") {
+    // The host detects DUMP END anywhere in a line; a clean header still parses
+    // even when later content is mangled — but a block with no BEGIN/END is invalid.
+    CHECK_FALSE(parseDump("").valid);
+    CHECK_FALSE(parseDump("garbage with no markers\n").valid);
+    CHECK_FALSE(parseDump("DUMP Y state=3\nno end marker\n").valid);
+}
+
+TEST_CASE("formatDump renders valid and invalid dumps") {
+    auto bad = formatDump(DumpData{});
+    REQUIRE(bad.size() == 1);
+    CHECK(bad[0].find("no firmware dump") != std::string::npos);
+
+    auto good = formatDump(parseDump(kDump));
+    std::string joined;
+    for (const auto& l : good) joined += l + "\n";
+    CHECK(joined.find("build=11dd3ce-dirty") != std::string::npos);
+    CHECK(joined.find("[Y]") != std::string::npos);
+    CHECK(joined.find("[P]") != std::string::npos);
+    CHECK(joined.find("RAMP_STAT") != std::string::npos);
+}

tests/test_helptext.cpp

@@ -0,0 +1,62 @@
+#include <doctest/doctest.h>
+
+#include "fgc/HelpText.h"
+
+#include <string>
+
+using namespace fgc;
+
+namespace {
+std::string join(const std::vector<std::string>& v) {
+    std::string s;
+    for (const auto& l : v) s += l + "\n";
+    return s;
+}
+}  // namespace
+
+TEST_CASE("helpCatalog is well-formed") {
+    const auto& cat = helpCatalog();
+    REQUIRE_FALSE(cat.empty());
+    for (const auto& sec : cat) {
+        CHECK_FALSE(sec.title.empty());
+        CHECK_FALSE(sec.entries.empty());
+        for (const auto& e : sec.entries) {
+            CHECK_FALSE(e.syntax.empty());
+            CHECK_FALSE(e.summary.empty());
+        }
+    }
+}
+
+TEST_CASE("renderHelp() with no topic lists every section and entry") {
+    std::string out = join(renderHelp(""));
+    CHECK(out.find("help <topic>") != std::string::npos);  // the usage hint
+    // Each catalog section title and entry syntax should appear.
+    for (const auto& sec : helpCatalog()) {
+        CHECK(out.find(sec.title) != std::string::npos);
+        for (const auto& e : sec.entries)
+            CHECK(out.find(e.syntax) != std::string::npos);
+    }
+    // A couple of the commands.
+    CHECK(out.find("gimbal move") != std::string::npos);
+    CHECK(out.find("gimbal calib") != std::string::npos);
+}
+
+TEST_CASE("renderHelp(<section>) expands that section with detail") {
+    std::string out = join(renderHelp("positioning"));
+    CHECK(out.find("gimbal move <yaw>,<pitch>") != std::string::npos);
+    // Detail lines (example) are only emitted in topic mode.
+    CHECK(out.find("gimbal move 30,-10") != std::string::npos);
+}
+
+TEST_CASE("renderHelp(<verb>) matches the gimbal commands, case-insensitively") {
+    std::string lower = join(renderHelp("gimbal"));
+    std::string upper = join(renderHelp("GIMBAL"));
+    CHECK(lower.find("gimbal move <yaw>,<pitch>") != std::string::npos);
+    CHECK(lower.find("gimbal calib") != std::string::npos);
+    CHECK(lower == upper);
+}
+
+TEST_CASE("renderHelp(unknown) reports no match") {
+    std::string out = join(renderHelp("definitely-not-a-command"));
+    CHECK(out.find("No help topic") != std::string::npos);
+}

tests/test_mtiprotocol.cpp

@@ -0,0 +1,163 @@
+#include <doctest/doctest.h>
+
+#include "fgc/MtiProtocol.h"
+
+#include <cstring>
+#include <vector>
+
+using namespace fgc;
+
+namespace {
+void putBEFloat(std::vector<uint8_t>& v, float f) {
+    uint32_t u;
+    std::memcpy(&u, &f, 4);
+    v.push_back(static_cast<uint8_t>(u >> 24));
+    v.push_back(static_cast<uint8_t>(u >> 16));
+    v.push_back(static_cast<uint8_t>(u >> 8));
+    v.push_back(static_cast<uint8_t>(u));
+}
+
+// Sum of all bytes from BID through CS must be ≡ 0 (mod 256) for a valid frame.
+bool frameChecksumOk(const std::vector<uint8_t>& m) {
+    unsigned s = 0;
+    for (size_t i = 1; i < m.size(); ++i) s += m[i];
+    return (s & 0xFF) == 0;
+}
+}  // namespace
+
+TEST_CASE("config messages are well-formed with correct payloads") {
+    auto cfg  = msgGoToConfig();
+    auto mode = msgSetOutputMode();
+    auto set  = msgSetOutputSettings();
+    auto meas = msgGoToMeasurement();
+
+    for (const auto& m : {cfg, mode, set, meas}) {
+        CHECK(m[0] == kMtiPreamble);
+        CHECK(m[1] == kMtiBid);
+        CHECK(frameChecksumOk(m));
+    }
+    // GoToConfig / GoToMeasurement: no data.
+    CHECK(cfg[2] == kMidGoToConfig);
+    CHECK(cfg[3] == 0);
+    CHECK(meas[2] == kMidGoToMeasurement);
+    CHECK(meas[3] == 0);
+    // SetOutputMode = 0x0007 (Temp|Calibrated|Orientation), 2-byte big-endian.
+    CHECK(mode[2] == kMidSetOutputMode);
+    CHECK(mode[3] == 2);
+    CHECK(mode[4] == 0x00);
+    CHECK(mode[5] == 0x07);
+    // SetOutputSettings = 0x00000005 (Euler + sample counter), 4-byte big-endian.
+    CHECK(set[2] == kMidSetOutputSettings);
+    CHECK(set[3] == 4);
+    CHECK(set[4] == 0x00);
+    CHECK(set[5] == 0x00);
+    CHECK(set[6] == 0x00);
+    CHECK(set[7] == 0x05);
+}
+
+TEST_CASE("framer decodes a combined MTData frame into a full sample") {
+    std::vector<uint8_t> d;
+    putBEFloat(d, 24.5f);                                       // temp
+    putBEFloat(d, 0.10f); putBEFloat(d, -0.20f); putBEFloat(d, 9.81f);   // acc
+    putBEFloat(d, 0.01f); putBEFloat(d, 0.02f); putBEFloat(d, -0.03f);   // gyr
+    putBEFloat(d, 0.45f); putBEFloat(d, -0.88f); putBEFloat(d, 0.21f);   // mag
+    putBEFloat(d, -1.5f); putBEFloat(d, 3.25f); putBEFloat(d, 187.0f);   // roll/pitch/yaw
+    d.push_back(0x12); d.push_back(0x34);                       // sample counter
+    REQUIRE(d.size() == kMTDataLen);
+
+    auto frame = mtiMessage(kMidMTData, d);
+
+    ImuSample got;
+    bool fired = false;
+    MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
+        if (auto s = parseMTData(mid, p, n)) { got = *s; fired = true; }
+    });
+
+    // Leading noise must not break resync.
+    const uint8_t noise[] = {0x00, 0xAB, 0xFA, 0x01};
+    fr.feed(noise, sizeof(noise));
+    fr.feed(frame.data(), frame.size());
+
+    REQUIRE(fired);
+    CHECK(got.valid);
+    CHECK(got.temp_c == doctest::Approx(24.5f));
+    CHECK(got.acc[2] == doctest::Approx(9.81f));
+    CHECK(got.gyr[0] == doctest::Approx(0.01f));
+    CHECK(got.mag[1] == doctest::Approx(-0.88f));
+    CHECK(got.roll_deg == doctest::Approx(-1.5f));
+    CHECK(got.pitch_deg == doctest::Approx(3.25f));
+    CHECK(got.yaw_deg == doctest::Approx(187.0f));
+    CHECK(got.sample_counter == 0x1234);
+}
+
+TEST_CASE("framer reassembles frames split across feeds and back-to-back frames") {
+    // Real serial reads arrive in arbitrary chunks; the framer must not depend
+    // on frame boundaries aligning with feed() calls.
+    std::vector<uint8_t> d;
+    putBEFloat(d, 7.5f);                  // temp
+    d.resize(kMTDataLen, 0);              // rest of the 54-byte payload = 0
+    auto frame = mtiMessage(kMidMTData, d);
+
+    int    count = 0;
+    float  last_temp = 0;
+    MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
+        if (auto s = parseMTData(mid, p, n)) { ++count; last_temp = s->temp_c; }
+    });
+
+    // Two frames fed one byte at a time (worst-case fragmentation).
+    for (uint8_t b : frame) fr.feed(&b, 1);
+    for (uint8_t b : frame) fr.feed(&b, 1);
+    CHECK(count == 2);
+    CHECK(last_temp == doctest::Approx(7.5f));
+
+    // Two frames concatenated in a single feed.
+    std::vector<uint8_t> two = frame;
+    two.insert(two.end(), frame.begin(), frame.end());
+    count = 0;
+    MtiFramer fr2([&](uint8_t mid, const uint8_t* p, size_t n) {
+        if (parseMTData(mid, p, n)) ++count;
+    });
+    fr2.feed(two.data(), two.size());
+    CHECK(count == 2);
+}
+
+TEST_CASE("parseMTData reports yaw as a 0..360 heading") {
+    auto frameWithYaw = [](float yaw) {
+        std::vector<uint8_t> d;
+        for (int i = 0; i < 10; ++i) putBEFloat(d, 0.f);  // temp + acc3 + gyr3 + mag3
+        putBEFloat(d, 0.f);                                // roll
+        putBEFloat(d, 0.f);                                // pitch
+        putBEFloat(d, yaw);                                // yaw
+        d.push_back(0); d.push_back(0);                    // counter
+        return mtiMessage(kMidMTData, d);
+    };
+    ImuSample got;
+    MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
+        if (auto s = parseMTData(mid, p, n)) got = *s;
+    });
+
+    auto fn = frameWithYaw(-90.f);   fr.feed(fn.data(), fn.size());
+    CHECK(got.yaw_deg == doctest::Approx(270.0f));
+    auto fp = frameWithYaw(45.f);    fr.feed(fp.data(), fp.size());
+    CHECK(got.yaw_deg == doctest::Approx(45.0f));
+    auto fb = frameWithYaw(-179.f);  fr.feed(fb.data(), fb.size());
+    CHECK(got.yaw_deg == doctest::Approx(181.0f));
+}
+
+TEST_CASE("framer rejects a bad checksum and a wrong-length payload") {
+    std::vector<uint8_t> d(kMTDataLen, 0);
+    auto frame = mtiMessage(kMidMTData, d);
+
+    SUBCASE("corrupt checksum") {
+        auto bad = frame;
+        bad.back() ^= 0xFF;
+        bool fired = false;
+        MtiFramer fr([&](uint8_t, const uint8_t*, size_t) { fired = true; });
+        fr.feed(bad.data(), bad.size());
+        CHECK_FALSE(fired);
+    }
+    SUBCASE("wrong-length MTData parses to nullopt") {
+        std::vector<uint8_t> shortData(10, 0);
+        CHECK_FALSE(parseMTData(kMidMTData, shortData.data(), shortData.size()).has_value());
+    }
+}

tests/test_scheduler.cpp

@@ -17,6 +17,8 @@ struct FakeMotor : IMotorController {
     void           sendCommand(const std::string& c) override { cmds.push_back(c); }
     MotorTelemetry telemetry() override { return tel; }
     std::string    lastDump() override { return ""; }
+    std::string    lastDiag() override { return ""; }
+    unsigned       diagSeq() const override { return 0; }
     bool           connected() const override { return true; }
 };
 
@@ -145,6 +147,43 @@ TEST_CASE("CaptureScheduler clamps the target heading to the soft range") {
     CHECK(motor.cmds.back() == "MOVE 900,0");
 }
 
+TEST_CASE("CaptureScheduler::setGeometry re-targets with the recalibrated conversion") {
+    // A `gimbal calib` updates the live geometry; the scheduler must adopt it so
+    // the scan/directed targets use the calibrated counts/deg (not the stale copy).
+    long long   clock = 0;
+    FakeMotor   motor;
+    FakeCamera  cam;
+    FakeChannel chan;
+    ScanGrid    grid({{0.0, 0.0}});
+    CaptureScheduler sch(motor, cam, chan, 1.0, testGeometry(), grid, [&] { return clock; });
+    sch.setCaptureActive(true);
+
+    chan.next.control_code_available = true;
+    chan.next.control_code = 1;
+    chan.next.heading_available = true;
+    chan.next.target_heading = "45";  // 45 deg
+
+    clock = 1600;
+    sch.tick();
+    CHECK(motor.cmds.back() == "MOVE 450,0");  // 45 * 10 counts/deg
+
+    // Settle + trigger so the scheduler is ready to move again.
+    settleAt(motor, 450, 0);
+    clock = 1700;
+    sch.tick();
+
+    // Recalibrate to 20 counts/deg; the same persisted heading must now resolve
+    // to twice the counts — no new heading sent.
+    Geometry g2;
+    g2.yaw   = {20.0, 0, -90.0, 90.0};
+    g2.pitch = {10.0, 0, 0.0, 60.0};
+    sch.setGeometry(g2);
+
+    clock = 2800;
+    sch.tick();
+    CHECK(motor.cmds.back() == "MOVE 900,0");  // 45 * 20 counts/deg
+}
+
 TEST_CASE("CaptureScheduler stays idle when capture inactive") {
     long long   clock = 0;
     FakeMotor   motor;

tests/test_uisnapshot.cpp

@@ -36,13 +36,18 @@ TEST_CASE("formatTimeAgo: buckets and the never case") {
     CHECK(formatTimeAgo(5'000, 9'000) == "0s ago");           // future clamps to 0
 }
 
-TEST_CASE("pendingSensorsView: all fields absent until drivers land") {
+TEST_CASE("pendingSensorsView: grouped by sensor, all absent until drivers land") {
     SensorsView v = pendingSensorsView();
-    CHECK_FALSE(v.dht_present);
+    CHECK_FALSE(v.imu.present);
-    CHECK_FALSE(v.imu_present);
+    CHECK_FALSE(v.dht.present);
-    CHECK(v.fields.size() >= 2);
+    CHECK_FALSE(v.imu.title.empty());
-    for (const auto& f : v.fields) {
+    CHECK_FALSE(v.dht.title.empty());
-        CHECK_FALSE(f.present);
+    CHECK(v.imu.fields.size() >= 3);  // roll/pitch/yaw (+ device temp)
-        CHECK_FALSE(f.label.empty());
+    CHECK(v.dht.fields.size() >= 2);  // ambient temp + humidity
+    for (const auto* g : {&v.imu, &v.dht}) {
+        for (const auto& f : g->fields) {
+            CHECK_FALSE(f.present);
+            CHECK_FALSE(f.label.empty());
+        }
     }
 }