From 9a25bbcc487d8a22f4be1288cafeb6a3071b8feb Mon Sep 17 00:00:00 2001 From: pgdalmeida Date: Mon, 29 Jun 2026 06:27:36 +0200 Subject: [PATCH] added gimbal calibration command, added TUI prompts --- CMakeLists.txt | 3 + README.md | 16 +- docs/architecture.md | 8 +- docs/configuration.md | 57 ++++- docs/known-issues.md | 7 + docs/modules-reference.md | 31 ++- include/fgc/Application.h | 1 + include/fgc/Calibration.h | 33 +++ include/fgc/CalibrationRoutine.h | 87 +++++++ include/fgc/CaptureScheduler.h | 5 + include/fgc/Config.h | 12 + include/fgc/DiagParser.h | 51 ++++ include/fgc/IMotorController.h | 6 + include/fgc/MtiProtocol.h | 6 +- include/fgc/Paths.h | 11 + include/fgc/SerialMotorController.h | 2 + include/fgc/mock/MockMotorController.h | 31 +++ include/fgc/ui/UiSnapshot.h | 52 +++- main.cpp | 1 + src/core/Application.cpp | 322 ++++++++++++++++++++++--- src/core/Calibration.cpp | 49 ++++ src/core/CalibrationRoutine.cpp | 253 +++++++++++++++++++ src/core/Config.cpp | 77 ++++++ src/core/DiagParser.cpp | 124 ++++++++++ src/core/HelpText.cpp | 58 +++-- src/core/MtiProtocol.cpp | 2 + src/core/Paths.cpp | 38 +++ src/serial/SerialMotorController.cpp | 39 +++ src/ui/TuiUi.cpp | 131 ++++++++-- src/ui/UiSnapshot.cpp | 22 +- tests/CMakeLists.txt | 2 + tests/test_calibration.cpp | 73 ++++++ tests/test_config.cpp | 31 +++ tests/test_diagparser.cpp | 81 +++++++ tests/test_helptext.cpp | 20 +- tests/test_mtiprotocol.cpp | 54 +++++ tests/test_scheduler.cpp | 39 +++ tests/test_uisnapshot.cpp | 19 +- 38 files changed, 1714 insertions(+), 140 deletions(-) create mode 100644 include/fgc/Calibration.h create mode 100644 include/fgc/CalibrationRoutine.h create mode 100644 include/fgc/DiagParser.h create mode 100644 src/core/Calibration.cpp create mode 100644 src/core/CalibrationRoutine.cpp create mode 100644 src/core/DiagParser.cpp create mode 100644 tests/test_calibration.cpp create mode 100644 tests/test_diagparser.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index dfd3cd5..aad1e63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,9 @@ 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 diff --git a/README.md b/README.md index c91b695..ad31b1e 100644 --- a/README.md +++ b/README.md @@ -96,19 +96,29 @@ While running, the program reads commands from stdin (one per line): | Command | Action | |---------|--------| -| `start` | Start the capture cycle. | -| `stop` | Stop capturing (the gimbal stays powered and homed). | +| `start` / `stop` | Start / stop the capture cycle (the gimbal stays powered and homed). | +| `gimbal move ,` | 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 ` | Capture rate, in images per second. | | `set camera fps ` | Camera sensor frame rate. | | `set camera jxlq ` | JPEG XL quality as butteraugli **distance** (lower = higher quality / larger files). | | `set camera jxle ` | JPEG XL encoder effort (higher = slower, smaller). | | `set camera display <0\|1>` | Toggle the local preview window. | -| `set motorctl ` | Send a raw command string straight to the motor controller. | | `trace [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. diff --git a/docs/architecture.md b/docs/architecture.md index db12d18..1e37861 100644 --- a/docs/architecture.md +++ b/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**. diff --git a/docs/configuration.md b/docs/configuration.md index 6b9b6a2..886e29b 100644 --- a/docs/configuration.md +++ b/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), -**Sensors** (DHT11 + Xsens MTi — shown as *pending integration* until those drivers land), -**Camera** (count, capture state, rate, last capture), **Connectivity** (MQTT state, broker, tower, -control mode, target heading). Adding a panel later (e.g. computer vision) is a struct in -`UiSnapshot.h` plus one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp). +Dashboard panels: **Gimbal** (per-axis state, heading, encoder counts, flag badges, target), +**Sensors** (DHT11 still *pending*; the **Xsens MTi** shows live roll/pitch/yaw + temp once +`enable_imu`), **Camera** (count, capture state, rate, last capture), **Connectivity** (MQTT state, +broker, tower, control mode, target heading). Adding a panel later is a struct in `UiSnapshot.h` plus +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 -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 -output is diverted from stdout into the on-screen log pane via a `Logger` sink, so the screen is -never corrupted. +**Expanded takeover views** replace the dashboard body full-screen (`Esc` or the same key closes): + +- **Gimbal** (`g`) — both axes side by side: live telemetry, the decoded firmware register **dump** + (`d` requests a fresh one), the homing limits, and the **last calibration** result (per-axis + `counts_per_deg` / `zero_count` / R² / age). +- **IMU** (`i`) — every MTi channel with units: orientation (°), acceleration (m/s²), rate-of-turn + (rad/s), magnetic field (a.u.), temperature, sample counter. + +**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. diff --git a/docs/known-issues.md b/docs/known-issues.md index ef5366e..9e29cf6 100644 --- a/docs/known-issues.md +++ b/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). diff --git a/docs/modules-reference.md b/docs/modules-reference.md index 58d5fdc..87d4a3f 100644 --- a/docs/modules-reference.md +++ b/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` | | [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/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/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/HelpText.h](../include/fgc/HelpText.h), [src/core/HelpText.cpp](../src/core/HelpText.cpp) | Operator command catalog (`helpCatalog`) + `renderHelp` (console & TUI help) | +| [include/fgc/DumpParser.h](../include/fgc/DumpParser.h), [src/core/DumpParser.cpp](../src/core/DumpParser.cpp) | `parseDump` firmware `DUMP` block → `DumpData` (per-axis state + decoded TMC registers/flags); `formatDump` | +| [include/fgc/DiagParser.h](../include/fgc/DiagParser.h), [src/core/DiagParser.cpp](../src/core/DiagParser.cpp) | `parseDiag` firmware `DG` self-test stream → `DiagResult` (per-axis tests, PASS/FAIL); `formatDiag` | +| [include/fgc/Calibration.h](../include/fgc/Calibration.h), [src/core/Calibration.cpp](../src/core/Calibration.cpp) | `linearFit` (least-squares, R²) + `circularMeanDeg` for the IMU-referenced calibration | +| [include/fgc/CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h), [src/core/CalibrationRoutine.cpp](../src/core/CalibrationRoutine.cpp) | `gimbal calib` worker thread: sweeps each axis, dwells reading the IMU, fits degrees↔counts; exposes `progress()`/`report()`/`takeResult()` | +| [include/fgc/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 | `//.jxl` | JPEG XL, rotated 90° CCW | | Demo placeholder | `bin/x64/Release/test_smoke.jxl` | copied verbatim in demo mode | +| Diagnostics log | `/fire_gimbal_control/logs/diag_.log` | raw `DG` stream + parsed summary | +| Calibration log | `/fire_gimbal_control/logs/calib_.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. diff --git a/include/fgc/Application.h b/include/fgc/Application.h index 7ac0c6b..c6a643a 100644 --- a/include/fgc/Application.h +++ b/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 diff --git a/include/fgc/Calibration.h b/include/fgc/Calibration.h new file mode 100644 index 0000000..931f036 --- /dev/null +++ b/include/fgc/Calibration.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +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>& 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& 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 diff --git a/include/fgc/CalibrationRoutine.h b/include/fgc/CalibrationRoutine.h new file mode 100644 index 0000000..6b8e151 --- /dev/null +++ b/include/fgc/CalibrationRoutine.h @@ -0,0 +1,87 @@ +#pragma once + +#include "fgc/Geometry.h" + +#include +#include +#include +#include +#include +#include + +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 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 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 running_{false}; + std::atomic cancel_{false}; + + mutable std::mutex result_mutex_; + std::optional result_; + CalibReport report_; + + mutable std::mutex progress_mutex_; + CalibProgress progress_; +}; + +} // namespace fgc diff --git a/include/fgc/CaptureScheduler.h b/include/fgc/CaptureScheduler.h index af30fce..3bec674 100644 --- a/include/fgc/CaptureScheduler.h +++ b/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(); diff --git a/include/fgc/Config.h b/include/fgc/Config.h index 7cd4f52..1f5dc6b 100644 --- a/include/fgc/Config.h +++ b/include/fgc/Config.h @@ -4,6 +4,7 @@ #include #include +#include #include namespace fgc { @@ -109,4 +110,15 @@ public: static AppConfig fromMap(const std::map& 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>& 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 diff --git a/include/fgc/DiagParser.h b/include/fgc/DiagParser.h new file mode 100644 index 0000000..15ee190 --- /dev/null +++ b/include/fgc/DiagParser.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include + +namespace fgc { + +// Structured decode of the firmware DIAG output stream. The firmware emits +// (interleaved with periodic ST lines), per axis: +// DG BEGIN +// DG S ERR_PEAK n ERR_RMS n ERR_STILL n +// CS_MIN n CS_MAX n SG_MIN n PWM_AVG n FLAGS 0x.. (x6) +// DG RESULT +// ... 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 + 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 tests; +}; + +struct DiagResult { + bool valid = false; // at least one well-formed DG line + bool done = false; // saw "DG DONE" + std::vector 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 formatDiag(const DiagResult& d); + +} // namespace fgc diff --git a/include/fgc/IMotorController.h b/include/fgc/IMotorController.h index dd36433..075f213 100644 --- a/include/fgc/IMotorController.h +++ b/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; }; diff --git a/include/fgc/MtiProtocol.h b/include/fgc/MtiProtocol.h index 80a08fd..ac4c4e8 100644 --- a/include/fgc/MtiProtocol.h +++ b/include/fgc/MtiProtocol.h @@ -47,9 +47,9 @@ struct ImuSample { 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; - float pitch_deg = 0.f; - float yaw_deg = 0.f; + 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; }; diff --git a/include/fgc/Paths.h b/include/fgc/Paths.h index 39a3fcd..ad7f0e3 100644 --- a/include/fgc/Paths.h +++ b/include/fgc/Paths.h @@ -26,4 +26,15 @@ std::optional 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(); + +// "_YYYYMMDD-HHMMSS.log" using the local clock. +std::string timestampedLogName(const std::string& prefix); + +// Write `text` to `defaultLogDir()/`, 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 diff --git a/include/fgc/SerialMotorController.h b/include/fgc/SerialMotorController.h index 997e50a..6edd44c 100644 --- a/include/fgc/SerialMotorController.h +++ b/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: diff --git a/include/fgc/mock/MockMotorController.h b/include/fgc/mock/MockMotorController.h index 49d7e0a..5112dd1 100644 --- a/include/fgc/mock/MockMotorController.h +++ b/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 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 diff --git a/include/fgc/ui/UiSnapshot.h b/include/fgc/ui/UiSnapshot.h index dfcba35..01d34a2 100644 --- a/include/fgc/ui/UiSnapshot.h +++ b/include/fgc/ui/UiSnapshot.h @@ -58,15 +58,24 @@ struct SensorField { bool present = false; }; -// DHT11 (temperature/humidity) + Xsens MTi (orientation). Neither is integrated -// yet, so the MVP fills `fields` with pending placeholders; flip the *_present -// flags and populate values when the drivers land. -struct SensorsView { - bool dht_present = false; - bool imu_present = false; +// A labelled group of readings from one physical sensor, rendered as its own +// subsection in the Sensors panel. +struct SensorGroup { + std::string title; // "MTi (orientation)" / "DHT11 (ambient)" + std::string status = "pending"; // "live" / "pending" / "no fix" + bool present = false; // true once the driver feeds real data std::vector 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; @@ -121,6 +130,35 @@ struct ImuView { 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 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; @@ -130,6 +168,8 @@ struct UiSnapshot { std::vector log; DumpView dump; ImuView imu; + ActivityView activity; + CalibResultView calib; }; // ---- Pure formatting helpers (unit-tested in tests/test_uisnapshot.cpp) ---- diff --git a/main.cpp b/main.cpp index c0ef4fd..890d464 100644 --- a/main.cpp +++ b/main.cpp @@ -70,6 +70,7 @@ int main(int argc, char* argv[]) { if (vm["no-tui"].as()) opts.use_tui = false; // --no-tui wins if (vm.count("log-level")) opts.log_level = vm["log-level"].as(); if (vm.count("trace")) opts.trace_categories = vm["trace"].as(); + 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(); diff --git a/src/core/Application.cpp b/src/core/Application.cpp index 2219fff..d66f924 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -1,9 +1,12 @@ #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" @@ -22,8 +25,10 @@ #include "fgc/ui/UiSnapshot.h" #include +#include #include #include +#include #include #include #include @@ -45,6 +50,12 @@ namespace fgc { namespace { +long long nowEpochMs() { + return std::chrono::duration_cast( + 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"; @@ -77,8 +88,21 @@ struct Application::Impl { std::unique_ptr pipeline; std::unique_ptr scheduler; std::unique_ptr ui; + std::unique_ptr 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 last_diag_summary; + long long last_diag_ts = 0; // epoch ms + bool last_diag_pass = false; + CalibResultView last_calib_view; + std::vector 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 running{true}; std::mutex cmd_mutex; std::queue cmd_queue; @@ -207,7 +231,7 @@ struct Application::Impl { } } - // --- Sensors: DHT11 still pending; MTi orientation/IMU live if present --- + // --- Sensors: MTi (orientation) live if present; DHT11 (ambient) pending --- s.sensors = pendingSensorsView(); if (imu) { auto fmt1 = [](float v) { @@ -215,11 +239,9 @@ struct Application::Impl { std::snprintf(b, sizeof(b), "%.1f", v); return std::string(b); }; - auto setField = [&](size_t i, const std::string& v) { - if (i < s.sensors.fields.size()) { - s.sensors.fields[i].value = v; - s.sensors.fields[i].present = true; - } + 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. @@ -234,19 +256,18 @@ struct Application::Impl { } s.imu.temp_c = m->temp_c; s.imu.sample_counter = m->sample_counter; - // Compact panel: Temp (field 0), Roll/Pitch/Yaw (2-4), status (5). - // Field units are already set, so values are bare numbers. - s.sensors.imu_present = true; - setField(0, fmt1(m->temp_c)); - setField(2, fmt1(m->roll_deg)); - setField(3, fmt1(m->pitch_deg)); - setField(4, fmt1(m->yaw_deg)); - if (s.sensors.fields.size() > 5) { - s.sensors.fields[5].value = "live"; - s.sensors.fields[5].present = true; + // 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 if (s.sensors.fields.size() > 5) { - s.sensors.fields[5].value = "no fix"; + } else { + s.sensors.imu.status = "no fix"; } } @@ -278,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 lock(snapshot_mutex); @@ -366,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 "," or " " (tokens from `from` onward) into two numbers. + static bool parsePair(const std::vector& 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(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 ` — 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; - double yaw_deg = 0.0, pitch_deg = 0.0; - if (!(iss >> verb >> yaw_deg >> pitch_deg)) { - LOG_WARN << "usage: goto (e.g. goto 30 -10)"; + std::vector tok; + std::string t; + while (iss >> t) tok.push_back(t); // tok[0] == "gimbal" + if (tok.size() < 2) { + LOG_WARN << "usage: gimbal "; return; } - long yc = cfg.geometry.yaw.toCounts(yaw_deg); - long pc = cfg.geometry.pitch.toCounts(pitch_deg); - LOG_INFO << "goto yaw=" << yaw_deg << "deg pitch=" << pitch_deg - << "deg -> MOVE " << yc << "," << pc; - motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc)); + std::string sub = tok[1]; + for (char& ch : sub) ch = static_cast(std::tolower((unsigned char)ch)); + + if (sub == "move") { + double yaw_deg = 0, pitch_deg = 0; + if (!parsePair(tok, 2, yaw_deg, pitch_deg)) { + LOG_WARN << "usage: gimbal move ,"; + 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 ,"; + 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 <+/-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(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(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(*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) { @@ -389,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") { - handleGoto(line); - } else if (c.verb == "dump") { - LOG_INFO << "requesting firmware dump..."; - motor->sendCommand("DUMP"); + } else if (c.verb == "gimbal") { + handleGimbal(line); + } else if (c.verb == "calib") { + // Answer to the "save calibration?" prompt (activity-strip y/n keys). + 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; @@ -416,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; } @@ -528,12 +778,14 @@ 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(); diff --git a/src/core/Calibration.cpp b/src/core/Calibration.cpp new file mode 100644 index 0000000..de36ce2 --- /dev/null +++ b/src/core/Calibration.cpp @@ -0,0 +1,49 @@ +#include "fgc/Calibration.h" + +#include + +namespace fgc { + +LinearFit linearFit(const std::vector>& xy) { + LinearFit f; + f.n = static_cast(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& 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 diff --git a/src/core/CalibrationRoutine.cpp b/src/core/CalibrationRoutine.cpp new file mode 100644 index 0000000..2a4f65c --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +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 CalibrationRoutine::takeResult() { + std::lock_guard lk(result_mutex_); + std::optional r; + r.swap(result_); + return r; +} + +CalibProgress CalibrationRoutine::progress() const { + std::lock_guard lk(progress_mutex_); + return progress_; +} + +CalibReport CalibrationRoutine::report() const { + std::lock_guard lk(result_mutex_); + return report_; +} + +void CalibrationRoutine::setProgress(char axis, int step, int total, const char* phase) { + std::lock_guard 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::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& 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& cancel) { + std::vector 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 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 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(kInsetFrac * travel); + const long a = lo + inset, b = hi - inset; + const long tol = std::max(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> 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(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(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(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 lk(result_mutex_); + result_ = result; + report_ = report; + } + LOG_INFO << "=== gimbal calibration complete (applied to session) ==="; +} + +} // namespace fgc diff --git a/src/core/Config.cpp b/src/core/Config.cpp index 13f5e09..ec1b435 100644 --- a/src/core/Config.cpp +++ b/src/core/Config.cpp @@ -1,7 +1,10 @@ #include "fgc/Config.h" #include "fgc/Paths.h" +#include #include +#include +#include #include extern "C" { @@ -156,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>& kv) { + std::vector 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> 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(out); +} + } // namespace fgc diff --git a/src/core/DiagParser.cpp b/src/core/DiagParser.cpp new file mode 100644 index 0000000..9613164 --- /dev/null +++ b/src/core/DiagParser.cpp @@ -0,0 +1,124 @@ +#include "fgc/DiagParser.h" + +#include +#include + +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 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 S KEY val KEY val ... " + if (tok.size() < 4 || tok[2].empty() || (tok[2][0] != 'S' && tok[2][0] != 's')) + continue; + DiagTest test; + test.speed = static_cast(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 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(toLong(g("CS_MIN"))); + test.cs_max = static_cast(toLong(g("CS_MAX"))); + test.sg_min = static_cast(toLong(g("SG_MIN"))); + test.pwm_avg = static_cast(toLong(g("PWM_AVG"))); + test.flags = static_cast(toLong(g("FLAGS"))); + ax.tests.push_back(test); + } + return d; +} + +std::vector formatDiag(const DiagResult& d) { + std::vector 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 diff --git a/src/core/HelpText.cpp b/src/core/HelpText.cpp index 6d85432..6d50a59 100644 --- a/src/core/HelpText.cpp +++ b/src/core/HelpText.cpp @@ -24,37 +24,45 @@ std::string verbOf(const std::string& syntax) { const std::vector& helpCatalog() { // clang-format off static const std::vector catalog = { - {"Positioning", "Aim the gimbal. 'goto' is degrees; raw MOVE is encoder counts.", { - {"goto ", - "Point the gimbal at an absolute heading/elevation in degrees.", { + {"Positioning", "Aim the gimbal. 'move' is degrees; 'steps' is raw encoder counts.", { + {"gimbal move ,", + "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", - "the configured travel limits.", - "Example: goto 30 -10 (yaw 30 deg, pitch -10 deg)"}}, - {"set motorctl MOVE ,", - "Move both axes to absolute encoder counts (no degree conversion).", { - "Example: set motorctl MOVE 100000,250000", - "Single axis: set motorctl MOVE Y 100000 / set motorctl MOVE P 250000"}}, - {"set motorctl HOME [Y|P]", + "two-axis move so both axes start together; soft-clamped to travel limits.", + "Example: gimbal move 30,-10 (yaw 30 deg, pitch -10 deg)"}}, + {"gimbal steps ,", + "Move both axes to absolute encoder COUNTS (no degree conversion).", { + "Example: gimbal steps 100000,250000"}}, + {"gimbal nudge <+/-pct>", + "Relative step move by a percent of the axis travel.", { + "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"}}, - {"set motorctl STOP ", - "Stop motion immediately on an axis or both.", {}}, - {"set motorctl SPEED ", + "Example: gimbal home / gimbal home y"}}, + {"gimbal stop [y|p|all]", + "Stop motion immediately (also cancels a running calibration).", {}}, + {"gimbal speed ", "Set the max slew speed (counts/s) for an axis.", {}}, + {"gimbal reset [y|p] / gimbal enable|disable / gimbal setpos ", + "Other firmware motor controls.", {}}, }}, - {"Diagnostics", "Inspect the firmware/driver state for debugging.", { - {"dump", + {"Diagnostics", "Inspect, self-test and calibrate the gimbal.", { + {"gimbal dump", "Request a full firmware state dump and show it here.", { - "Sends DUMP to the firmware; the captured DUMP BEGIN..END block (build,", - "uptime, reset cause, and per-axis TMC5160 registers) is logged and, in", - "the TUI, shown in the Diagnostics help section.", + "Captured DUMP BEGIN..END block (build, uptime, reset cause, per-axis", + "TMC5160 registers); shown in the gimbal 'g' expanded view.", "Equivalent offline tool: ./firmware/dump.sh"}}, - {"set motorctl DIAG [Y|P|ALL]", - "Run the motor self-test (emits DG lines, ends with DG DONE).", { - "Each axis is swept at several speeds/directions; results stream as DG", - "lines in the log. Axis must be homed first."}}, - {"set motorctl STATUS", + {"gimbal diag [y|p|all]", + "Run the firmware motor self-test; results stream to the LOG + a logfile.", { + "Each axis is swept at several speeds/directions (DG lines). A PASS/FAIL", + "summary is logged and saved to logs/diag_*.log. Axes must be homed first."}}, + {"gimbal calib", + "Calibrate steps<->degrees using the IMU (sweeps each axis, ~minutes).", { + "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.", { diff --git a/src/core/MtiProtocol.cpp b/src/core/MtiProtocol.cpp index 0f114fc..d5861ae 100644 --- a/src/core/MtiProtocol.cpp +++ b/src/core/MtiProtocol.cpp @@ -79,6 +79,8 @@ std::optional parseMTData(uint8_t mid, const uint8_t* data, std::size 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; diff --git a/src/core/Paths.cpp b/src/core/Paths.cpp index eddbf39..d1783ad 100644 --- a/src/core/Paths.cpp +++ b/src/core/Paths.cpp @@ -1,7 +1,12 @@ #include "fgc/Paths.h" +#include "fgc/Logger.h" + +#include #include +#include #include +#include #include 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 diff --git a/src/serial/SerialMotorController.cpp b/src/serial/SerialMotorController.cpp index 0b9cdf6..fb67eb2 100644 --- a/src/serial/SerialMotorController.cpp +++ b/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 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 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 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 diff --git a/src/ui/TuiUi.cpp b/src/ui/TuiUi.cpp index b69089a..f582a74 100644 --- a/src/ui/TuiUi.cpp +++ b/src/ui/TuiUi.cpp @@ -77,20 +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 rows; - rows.push_back(s.imu_present ? (text("MTi: live (i to expand)") | color(Color::Green)) - : (text("MTi: -- DHT11 pending") | dim)); - rows.push_back(separator()); - for (const auto& f : s.fields) { - Element val = text(f.value + (f.unit.empty() ? "" : " " + f.unit)); - val = f.present ? (val | bold) : (val | dim); - rows.push_back(hbox({text(f.label) | dim, filler(), val})); - } - return panel("SENSORS (DHT11 · MTi)", Color::Magenta, vbox(std::move(rows))); + // One labelled subsection per physical sensor: a title + status header, then + // its readings. Keeps MTi (orientation/device temp) and DHT11 (ambient) + // clearly separated. + auto group = [](const SensorGroup& g, const std::string& hint) { + std::vector rows; + Element status = g.present ? (text(g.status) | color(Color::Green)) + : (text(g.status) | dim); + rows.push_back(hbox({text(g.title) | bold, text(" "), status, filler(), + 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) { @@ -152,6 +162,32 @@ Element logPanel(const std::vector& 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 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. @@ -282,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 col; col.push_back(axisLiveDetail(live)); col.push_back(separator()); @@ -297,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. -Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump) { +// live telemetry above its decoded firmware register dump + last calibration. +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::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) @@ -318,8 +378,8 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump) { }); std::vector cols; - cols.push_back(axisColumn("YAW", g.yaw, d, 'Y')); - if (g.pitch_present) cols.push_back(axisColumn("PITCH", g.pitch, d, 'P')); + 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', 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})); @@ -424,6 +484,7 @@ void TuiUi::uiLoop() { 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"); @@ -433,6 +494,7 @@ void TuiUi::uiLoop() { std::lock_guard 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({ @@ -453,7 +515,7 @@ void TuiUi::uiLoop() { } else { bottom = hbox({ keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"), - keyHint("r", "Reset"), keyHint("g", "Gimbal"), keyHint("i", "IMU"), + keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"), keyHint(":", "Cmd"), keyHint("?", "Help"), filler(), keyHint("q", "Quit"), }); } @@ -466,11 +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}); case Overlay::Sensors: return vbox({header, separator(), imuDetailPanel(s.imu) | flex, bottom}); - default: - return vbox({header, separator(), top, middle, logPanel(s.log) | 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 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)); + } } }); @@ -495,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; @@ -507,14 +590,14 @@ 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") { @@ -528,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 == "r") { if (sink_) sink_("set motorctl RESET"); return true; } + if (c == "h") { if (sink_) sink_("gimbal home"); return true; } + if (c == "r") { if (sink_) sink_("gimbal reset"); return true; } if (c == ":") { command_mode = true; return true; } return false; }); diff --git a/src/ui/UiSnapshot.cpp b/src/ui/UiSnapshot.cpp index e78f5bc..8038cfd 100644 --- a/src/ui/UiSnapshot.cpp +++ b/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; - v.imu_present = false; - // DHT11 temperature & humidity (Aosong): 0-50 °C ±2 °C, 20-90 %RH ±5 %. - v.fields.push_back({"Temp", "--.-", "\xC2\xB0""C", false}); - v.fields.push_back({"Humid", "--", "%RH", false}); - // Xsens MTi-28 AHRS: drift-free Euler orientation (roll/pitch/yaw). - v.fields.push_back({"Roll", "--.-", "\xC2\xB0", false}); - v.fields.push_back({"Pitch", "--.-", "\xC2\xB0", false}); - v.fields.push_back({"Yaw", "--.-", "\xC2\xB0", false}); - v.fields.push_back({"IMU", "pending", "", false}); + // Xsens MTi-28 AHRS: drift-free Euler orientation + device internal temp. + v.imu.title = "MTi (orientation)"; + v.imu.status = "pending"; + v.imu.fields.push_back({"Roll", "--.-", "\xC2\xB0", false}); + v.imu.fields.push_back({"Pitch", "--.-", "\xC2\xB0", false}); + v.imu.fields.push_back({"Yaw", "--.-", "\xC2\xB0", false}); + v.imu.fields.push_back({"Temp", "--.-", "\xC2\xB0""C", false}); // MTi internal + // DHT11 temperature & humidity (Aosong): 0-50 °C ±2 °C, 20-90 %RH ±5 % (ambient). + v.dht.title = "DHT11 (ambient)"; + v.dht.status = "pending"; + v.dht.fields.push_back({"Temp", "--.-", "\xC2\xB0""C", false}); + v.dht.fields.push_back({"Humid", "--", "%RH", false}); return v; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 582dae0..1a715c8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -31,6 +31,8 @@ add_executable(fgc_tests 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) diff --git a/tests/test_calibration.cpp b/tests/test_calibration.cpp new file mode 100644 index 0000000..21e2481 --- /dev/null +++ b/tests/test_calibration.cpp @@ -0,0 +1,73 @@ +#include + +#include "fgc/Calibration.h" + +#include + +using namespace fgc; + +TEST_CASE("linearFit recovers slope/intercept exactly for collinear points") { + // counts = 983.33*deg + 500000 (a yaw-like calibration) + std::vector> 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> 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> 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); +} diff --git a/tests/test_config.cpp b/tests/test_config.cpp index a0f0148..019e4ce 100644 --- a/tests/test_config.cpp +++ b/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); +} diff --git a/tests/test_diagparser.cpp b/tests/test_diagparser.cpp new file mode 100644 index 0000000..76116d9 --- /dev/null +++ b/tests/test_diagparser.cpp @@ -0,0 +1,81 @@ +#include + +#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); +} diff --git a/tests/test_helptext.cpp b/tests/test_helptext.cpp index 775717a..8bd3c4c 100644 --- a/tests/test_helptext.cpp +++ b/tests/test_helptext.cpp @@ -36,23 +36,23 @@ TEST_CASE("renderHelp() with no topic lists every section and entry") { for (const auto& e : sec.entries) CHECK(out.find(e.syntax) != std::string::npos); } - // A couple of the commands added this session. - CHECK(out.find("goto") != std::string::npos); - CHECK(out.find("dump") != 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(
) expands that section with detail") { std::string out = join(renderHelp("positioning")); - CHECK(out.find("goto ") != std::string::npos); + CHECK(out.find("gimbal move ,") != std::string::npos); // Detail lines (example) are only emitted in topic mode. - CHECK(out.find("goto 30 -10") != std::string::npos); + CHECK(out.find("gimbal move 30,-10") != std::string::npos); } -TEST_CASE("renderHelp() matches a single command, case-insensitively") { - std::string lower = join(renderHelp("goto")); - std::string upper = join(renderHelp("GOTO")); - CHECK(lower.find("goto ") != std::string::npos); - CHECK(upper.find("goto ") != std::string::npos); +TEST_CASE("renderHelp() matches the gimbal commands, case-insensitively") { + std::string lower = join(renderHelp("gimbal")); + std::string upper = join(renderHelp("GIMBAL")); + CHECK(lower.find("gimbal move ,") != std::string::npos); + CHECK(lower.find("gimbal calib") != std::string::npos); CHECK(lower == upper); } diff --git a/tests/test_mtiprotocol.cpp b/tests/test_mtiprotocol.cpp index 393b40d..7e7a54c 100644 --- a/tests/test_mtiprotocol.cpp +++ b/tests/test_mtiprotocol.cpp @@ -90,6 +90,60 @@ TEST_CASE("framer decodes a combined MTData frame into a full sample") { 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 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 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 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 d(kMTDataLen, 0); auto frame = mtiMessage(kMidMTData, d); diff --git a/tests/test_scheduler.cpp b/tests/test_scheduler.cpp index cad4822..b413a84 100644 --- a/tests/test_scheduler.cpp +++ b/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; diff --git a/tests/test_uisnapshot.cpp b/tests/test_uisnapshot.cpp index c90f64d..7868442 100644 --- a/tests/test_uisnapshot.cpp +++ b/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(v.fields.size() >= 2); - for (const auto& f : v.fields) { - CHECK_FALSE(f.present); - CHECK_FALSE(f.label.empty()); + CHECK_FALSE(v.imu.present); + CHECK_FALSE(v.dht.present); + CHECK_FALSE(v.imu.title.empty()); + CHECK_FALSE(v.dht.title.empty()); + CHECK(v.imu.fields.size() >= 3); // roll/pitch/yaw (+ device temp) + 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()); + } } }