Compare commits
2 Commits
5fb1d3103b
...
9a25bbcc48
| Author | SHA1 | Date |
|---|---|---|
 pgdalmeida
|
9a25bbcc48
|
|
|
pgdalmeida
|
e33781c545
|
|
|
@ -52,6 +52,10 @@ add_library(fgc_core STATIC
|
|||
src/core/CommandParser.cpp
|
||||
src/core/HelpText.cpp
|
||||
src/core/DumpParser.cpp
|
||||
src/core/DiagParser.cpp
|
||||
src/core/Calibration.cpp
|
||||
src/core/CalibrationRoutine.cpp
|
||||
src/core/MtiProtocol.cpp
|
||||
src/ui/UiSnapshot.cpp
|
||||
src/ui/HeadlessUi.cpp
|
||||
ini.c
|
||||
|
|
@ -73,6 +77,7 @@ set(FGC_SOURCES
|
|||
src/camera/JpegXlEncoder.cpp
|
||||
src/camera/ImagePipeline.cpp
|
||||
src/serial/SerialMotorController.cpp
|
||||
src/serial/MtiImuSource.cpp
|
||||
)
|
||||
if(WITH_MQTT)
|
||||
list(APPEND FGC_SOURCES src/mqtt/MqttControlChannel.cpp)
|
||||
|
|
|
|||
16
README.md
16
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 <yaw>,<pitch>` | Aim at an absolute heading/elevation in **degrees** (calibrated). |
|
||||
| `gimbal home [y\|p]` | Run the endstop-finding home sequence. |
|
||||
| `gimbal dump` | Request a firmware state dump (shown in the gimbal `g` view). |
|
||||
| `gimbal diag [y\|p\|all]` | Run the firmware motor self-test (results in the activity strip + `logs/`). |
|
||||
| `gimbal calib` | IMU-referenced steps↔degrees calibration (needs homed axes + IMU; applied to the session). |
|
||||
| `gimbal …` | Other motor controls — `steps`/`nudge`/`stop`/`speed`/`reset`/`enable`/`disable`/`setpos`/`status`. |
|
||||
| `debug` | Toggle debug-level logging on/off. |
|
||||
| `set fps <n>` | Capture rate, in images per second. |
|
||||
| `set camera fps <n>` | Camera sensor frame rate. |
|
||||
| `set camera jxlq <d>` | JPEG XL quality as butteraugli **distance** (lower = higher quality / larger files). |
|
||||
| `set camera jxle <n>` | JPEG XL encoder effort (higher = slower, smaller). |
|
||||
| `set camera display <0\|1>` | Toggle the local preview window. |
|
||||
| `set motorctl <raw>` | Send a raw command string straight to the motor controller. |
|
||||
| `trace <cat> [on\|off]` | Enable/disable a wire-trace category live (e.g. `trace serial`, `trace mqtt off`). |
|
||||
| `trace all` / `trace off` | Enable every category / silence all of them. |
|
||||
| `help [topic]` | List commands / expand one section. |
|
||||
| `exit` | Stop everything and quit (Ctrl-D also works). |
|
||||
|
||||
The **Xsens MTi IMU** (set `[Features] enable_imu` + `[IMU] device`) drives the Sensors panel's live
|
||||
roll/pitch/yaw and powers `gimbal calib`. In the TUI, `g` and `i` open full-screen **gimbal** and
|
||||
**IMU** views, and an **activity strip** below the log shows the running calibration/diagnostics/scan
|
||||
plus the last result. See [docs/configuration.md](docs/configuration.md).
|
||||
|
||||
A typical first bring-up: `scripts/run.sh --init` to home the gimbal, then type `start` once you've confirmed
|
||||
telemetry looks sane; adjust `set fps` / `set camera jxlq` live as needed. See
|
||||
[docs/configuration.md](docs/configuration.md) for the full `config.ini` reference.
|
||||
|
|
|
|||
|
|
@ -35,6 +35,16 @@ id_Cam4 =
|
|||
device = /dev/ttyACM0
|
||||
baud = 115200
|
||||
|
||||
[IMU]
|
||||
; Xsens MTi orientation/inertial sensor. Wired to the LattePanda's RS-232 header
|
||||
; (an onboard hardware UART), so this is a stable /dev/ttyS* node - NOT a USB
|
||||
; device and NOT /dev/ttyUSB0-3 (those are the Quectel modem). Find it with
|
||||
; ls /dev/ttyS* ; dmesg | grep -iE 'ttyS|LPSS|HSUART' (often /dev/ttyS4)
|
||||
; Enable with [Features] enable_imu = true. The host reconfigures the MTi to a
|
||||
; Euler + calibrated 100 Hz stream at startup.
|
||||
device =
|
||||
baud = 115200
|
||||
|
||||
[Motor]
|
||||
; Degrees<->encoder-counts calibration for each axis. The firmware speaks only
|
||||
; in absolute encoder counts; these map them to the heading/elevation degrees
|
||||
|
|
@ -76,8 +86,10 @@ output_dir =
|
|||
enable_mqtt = true
|
||||
enable_camera = true
|
||||
enable_serial = true
|
||||
enable_imu = false
|
||||
mock_camera = false
|
||||
mock_serial = false
|
||||
mock_imu = false
|
||||
|
||||
[UI]
|
||||
; Full-screen terminal dashboard (sectioned, colored, live status + log pane).
|
||||
|
|
|
|||
|
|
@ -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**.
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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).
|
||||
|
|
|
|||
|
|
@ -6,16 +6,22 @@ Per-file reference for the refactored tree, plus the shared data structures.
|
|||
|
||||
| File | Contents |
|
||||
|------|----------|
|
||||
| [include/fgc/Config.h](../include/fgc/Config.h), [src/core/Config.cpp](../src/core/Config.cpp) | Typed `AppConfig` (General/Network/Serial/Camera/Paths/Features/Logging/Motor/Scan) + `ConfigLoader` (INI parse, env overrides, validation) |
|
||||
| [include/fgc/Config.h](../include/fgc/Config.h), [src/core/Config.cpp](../src/core/Config.cpp) | Typed `AppConfig` (General/Network/Serial/Camera/Paths/Features/Logging/UI/Motor/Scan/**IMU**) + `ConfigLoader` (INI parse, env overrides, validation) |
|
||||
| [include/fgc/Paths.h](../include/fgc/Paths.h), [src/core/Paths.cpp](../src/core/Paths.cpp) | `~`/`$ENV` expansion, executable dir, config search order, default output dir |
|
||||
| [include/fgc/Logger.h](../include/fgc/Logger.h), [src/core/Logger.cpp](../src/core/Logger.cpp) | Leveled, thread-safe logger + per-category wire trace; `LOG_TRACE..LOG_ERROR`, `LOG_TRACE_CAT` |
|
||||
| [include/fgc/Geometry.h](../include/fgc/Geometry.h), [src/core/Geometry.cpp](../src/core/Geometry.cpp) | Per-axis degrees↔encoder-counts affine map (`[Motor]` calibration) |
|
||||
| [include/fgc/ScanGrid.h](../include/fgc/ScanGrid.h), [src/core/ScanGrid.cpp](../src/core/ScanGrid.cpp) | Capture waypoints (CSV or generated) + ping-pong cursor (`[Scan]`) |
|
||||
| [include/fgc/TelemetryParser.h](../include/fgc/TelemetryParser.h), [src/core/TelemetryParser.cpp](../src/core/TelemetryParser.cpp) | `parseTelemetryLine` (firmware `ST` line) → `std::optional<MotorTelemetry>` |
|
||||
| [include/fgc/CommandParser.h](../include/fgc/CommandParser.h), [src/core/CommandParser.cpp](../src/core/CommandParser.cpp) | `parseCommand` whitespace tokenizer → `Command` |
|
||||
| [include/fgc/CaptureScheduler.h](../include/fgc/CaptureScheduler.h), [src/core/CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp) | Capture state machine over the interfaces; injectable clock |
|
||||
| [include/fgc/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 | `<output_dir>/<RGB\|ACR\|NIR>/<unix_ms>.jxl` | JPEG XL, rotated 90° CCW |
|
||||
| Demo placeholder | `bin/x64/Release/test_smoke.jxl` | copied verbatim in demo mode |
|
||||
| Diagnostics log | `<data_dir>/fire_gimbal_control/logs/diag_<ts>.log` | raw `DG` stream + parsed summary |
|
||||
| Calibration log | `<data_dir>/fire_gimbal_control/logs/calib_<ts>.log` | per-axis samples + the least-squares fit |
|
||||
|
||||
State otherwise lives in memory; diagnostics go to stdout/stderr (no log files, no database).
|
||||
Diagnostics/calibration results are also surfaced live in the TUI (activity strip + gimbal `g` view)
|
||||
and applied to the running session's geometry; other state lives in memory.
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,33 @@
|
|||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Pure math helpers for `gimbal calib`. Kept I/O-free in fgc_core so the fit can
|
||||
// be unit-tested independently of the threaded routine that drives the hardware.
|
||||
|
||||
struct LinearFit {
|
||||
bool ok = false; // false if < 2 points or x has no spread
|
||||
double slope = 0; // y = slope*x + intercept
|
||||
double intercept = 0;
|
||||
double r2 = 0; // coefficient of determination (1 = perfect)
|
||||
int n = 0;
|
||||
};
|
||||
|
||||
// Least-squares fit of y over x. For calibration: x = measured degrees,
|
||||
// y = motor encoder counts ⇒ slope = counts_per_deg, intercept = zero_count.
|
||||
LinearFit linearFit(const std::vector<std::pair<double, double>>& xy);
|
||||
|
||||
// Circular mean of angles in degrees, robust to ±180° wrap (e.g. IMU heading).
|
||||
// Returns a value in (-180, 180]. Empty input returns 0.
|
||||
double circularMeanDeg(const std::vector<double>& degs);
|
||||
|
||||
// Phase-unwrap `deg` relative to the previous (already-unwrapped) sample: shift it
|
||||
// by whole turns so it lands within ±180° of `prev`. Feeding a smoothly-swept
|
||||
// angle through this in sequence removes the 0/360 (or ±180) discontinuity, so the
|
||||
// calibration sees a continuous curve for the linear fit.
|
||||
double unwrapNear(double prev, double deg);
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
#pragma once
|
||||
|
||||
#include "fgc/Geometry.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
class IMotorController;
|
||||
class IImuSource;
|
||||
|
||||
// Live progress of a running calibration, for the TUI activity strip.
|
||||
struct CalibProgress {
|
||||
bool running = false;
|
||||
char axis = '?'; // 'Y' / 'P'
|
||||
int step = 0; // 1-based current step
|
||||
int total = 0; // steps per axis
|
||||
std::string phase; // "moving" / "dwelling" / "fitting" / ...
|
||||
};
|
||||
|
||||
// Structured outcome of the last completed calibration (persists for display).
|
||||
struct CalibReport {
|
||||
struct Axis {
|
||||
char axis = '?';
|
||||
bool ok = false;
|
||||
double counts_per_deg = 0;
|
||||
long zero_count = 0;
|
||||
double r2 = 0;
|
||||
int n = 0;
|
||||
};
|
||||
bool valid = false;
|
||||
long long ts_ms = 0; // wall-clock completion time (epoch ms)
|
||||
bool all_ok = false;
|
||||
std::vector<Axis> axes;
|
||||
};
|
||||
|
||||
// `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own
|
||||
// thread (the motor/imu/Logger interfaces are thread-safe), sweeping each axis
|
||||
// across its homed soft-limit travel in equal step intervals, dwelling at each to
|
||||
// record the IMU orientation, then least-squares fitting counts vs degrees. The
|
||||
// resulting Geometry is published via takeResult() for the main thread to apply
|
||||
// to the live session; the raw samples + fit are written to a logfile. Progress
|
||||
// is streamed to the LOG pane via LOG_INFO. Cancellable and one-at-a-time.
|
||||
class CalibrationRoutine {
|
||||
public:
|
||||
CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial);
|
||||
~CalibrationRoutine();
|
||||
|
||||
// Begin on a worker thread. Logs a reason and returns false if already
|
||||
// running (prechecks happen on the worker and abort there).
|
||||
bool start();
|
||||
void cancel();
|
||||
bool running() const { return running_.load(); }
|
||||
|
||||
// If a finished run produced a new calibration, returns it once (then clears).
|
||||
std::optional<Geometry> takeResult();
|
||||
|
||||
// Live progress (thread-safe copy) and the last completed report (persists).
|
||||
CalibProgress progress() const;
|
||||
CalibReport report() const;
|
||||
|
||||
private:
|
||||
void run();
|
||||
void setProgress(char axis, int step, int total, const char* phase);
|
||||
|
||||
IMotorController& motor_;
|
||||
IImuSource& imu_;
|
||||
Geometry initial_;
|
||||
|
||||
std::thread thread_;
|
||||
std::atomic<bool> running_{false};
|
||||
std::atomic<bool> cancel_{false};
|
||||
|
||||
mutable std::mutex result_mutex_;
|
||||
std::optional<Geometry> result_;
|
||||
CalibReport report_;
|
||||
|
||||
mutable std::mutex progress_mutex_;
|
||||
CalibProgress progress_;
|
||||
};
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
|
@ -43,8 +44,17 @@ struct FeaturesConfig {
|
|||
bool enable_mqtt = true;
|
||||
bool enable_camera = true;
|
||||
bool enable_serial = true;
|
||||
bool enable_imu = false; // Xsens MTi orientation/IMU (off by default)
|
||||
bool mock_camera = false; // use a simulated camera instead of Vimba X
|
||||
bool mock_serial = false; // use a simulated motor controller
|
||||
bool mock_imu = false; // use a simulated IMU instead of the MTi
|
||||
};
|
||||
|
||||
// [IMU]: Xsens MTi connected over the LattePanda's RS-232 UART (a hardware
|
||||
// /dev/ttyS* node, stable across reboots — not a USB device).
|
||||
struct ImuConfig {
|
||||
std::string device = ""; // e.g. /dev/ttyS4; empty => required when enabled
|
||||
unsigned int baud = 115200;
|
||||
};
|
||||
|
||||
struct LoggingConfig {
|
||||
|
|
@ -78,6 +88,7 @@ struct AppConfig {
|
|||
UiConfig ui; // [UI] terminal dashboard toggle
|
||||
Geometry geometry; // [Motor] degrees<->counts maps (yaw + pitch)
|
||||
ScanConfig scan; // [Scan] grid source
|
||||
ImuConfig imu; // [IMU] Xsens MTi serial device
|
||||
|
||||
// Capture rate in images/second (derived from general.image_interval).
|
||||
double image_rate() const;
|
||||
|
|
@ -99,4 +110,15 @@ public:
|
|||
static AppConfig fromMap(const std::map<std::string, std::string>& kv);
|
||||
};
|
||||
|
||||
// Return `contents` with the given `key = value` pairs set under `[section]`:
|
||||
// existing keys in that section are replaced in place (comments/order preserved),
|
||||
// and any missing ones are appended in a `[section]` block at the end. Pure (no
|
||||
// I/O) so it is unit-testable.
|
||||
std::string updateIniSectionKeys(const std::string& contents, const std::string& section,
|
||||
const std::vector<std::pair<std::string, std::string>>& kv);
|
||||
|
||||
// Persist the live `[Motor]` calibration (counts_per_deg / zero_count for both
|
||||
// axes) back into the INI file at `path`. Returns true on success.
|
||||
bool saveMotorCalibration(const std::string& path, const Geometry& geo);
|
||||
|
||||
} // namespace fgc
|
||||
|
|
|
|||
|
|
@ -0,0 +1,51 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Structured decode of the firmware DIAG output stream. The firmware emits
|
||||
// (interleaved with periodic ST lines), per axis:
|
||||
// DG BEGIN <Y|P>
|
||||
// DG <Y|P> S<speed> <FWD|REV> ERR_PEAK n ERR_RMS n ERR_STILL n
|
||||
// CS_MIN n CS_MAX n SG_MIN n PWM_AVG n FLAGS 0x.. <PASS|FAIL> (x6)
|
||||
// DG <Y|P> RESULT <PASS|FAIL>
|
||||
// ... and finally: DG DONE
|
||||
// Pure (no I/O); mirrors DumpParser so it can be unit-tested in fgc_core.
|
||||
|
||||
struct DiagTest {
|
||||
int speed = 0; // S<speed>
|
||||
bool fwd = true; // FWD vs REV
|
||||
long err_peak = 0; // encoder following error (counts); -1 = no encoder
|
||||
long err_rms = 0;
|
||||
long err_still = 0;
|
||||
int cs_min = 0, cs_max = 0;
|
||||
int sg_min = 0;
|
||||
int pwm_avg = 0;
|
||||
unsigned flags = 0; // fault bitmask
|
||||
bool pass = false;
|
||||
};
|
||||
|
||||
struct DiagAxis {
|
||||
char axis = '?'; // 'Y' / 'P'
|
||||
bool has_result = false;
|
||||
bool pass = false; // axis RESULT
|
||||
std::vector<DiagTest> tests;
|
||||
};
|
||||
|
||||
struct DiagResult {
|
||||
bool valid = false; // at least one well-formed DG line
|
||||
bool done = false; // saw "DG DONE"
|
||||
std::vector<DiagAxis> axes;
|
||||
|
||||
bool allPass() const; // every axis with a result passed (and >=1 axis)
|
||||
};
|
||||
|
||||
// Parse a captured DG block (only "DG ..." lines matter; other lines are ignored).
|
||||
DiagResult parseDiag(const std::string& block);
|
||||
|
||||
// Human-readable summary lines for a decoded diag.
|
||||
std::vector<std::string> formatDiag(const DiagResult& d);
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
#pragma once
|
||||
|
||||
#include "fgc/MtiProtocol.h" // ImuSample
|
||||
|
||||
#include <optional>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Abstraction over the orientation/inertial sensor (Xsens MTi). Implemented by
|
||||
// MtiImuSource (RS-232/UART binary stream) and MockImuSource (synthetic). Runs
|
||||
// on its own thread; start() must not block the control loop.
|
||||
class IImuSource {
|
||||
public:
|
||||
virtual ~IImuSource() = default;
|
||||
|
||||
virtual void start() = 0;
|
||||
virtual void stop() = 0;
|
||||
|
||||
// Whether a valid, recent reading is available.
|
||||
virtual bool connected() const = 0;
|
||||
|
||||
// Latest reading, or nullopt if none/stale.
|
||||
virtual std::optional<ImuSample> sample() = 0;
|
||||
};
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,29 @@
|
|||
#pragma once
|
||||
|
||||
#include "fgc/IImuSource.h"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Real IMU backend: reads the Xsens MTi binary MTData stream over a serial port
|
||||
// (RS-232 via the LattePanda UART). Configures the device to the Euler +
|
||||
// calibrated output at startup, then streams read-only. Boost.Asio detail is
|
||||
// hidden behind a pImpl (mirrors SerialMotorController).
|
||||
class MtiImuSource : public IImuSource {
|
||||
public:
|
||||
MtiImuSource(std::string device, unsigned int baud);
|
||||
~MtiImuSource() override;
|
||||
|
||||
void start() override;
|
||||
void stop() override;
|
||||
bool connected() const override;
|
||||
std::optional<ImuSample> sample() override;
|
||||
|
||||
private:
|
||||
struct Impl;
|
||||
std::unique_ptr<Impl> impl_;
|
||||
};
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Xsens MTi (legacy MT0100P) binary protocol helpers. The MTi streams MTData
|
||||
// messages over RS-232/UART; this module frames the byte stream, validates
|
||||
// checksums, builds the config messages, and decodes a combined MTData payload
|
||||
// into an ImuSample. Kept pure (no I/O) so it is unit-testable and lives in
|
||||
// fgc_core (mirrors TelemetryParser / DumpParser).
|
||||
//
|
||||
// Frame layout: PRE(0xFA) BID(0xFF) MID LEN DATA[LEN] CS
|
||||
// Checksum: (BID + MID + LEN + ΣDATA + CS) & 0xFF == 0. All multi-byte values
|
||||
// are big-endian.
|
||||
|
||||
// Protocol constants.
|
||||
inline constexpr uint8_t kMtiPreamble = 0xFA;
|
||||
inline constexpr uint8_t kMtiBid = 0xFF;
|
||||
inline constexpr uint8_t kMidGoToConfig = 0x30;
|
||||
inline constexpr uint8_t kMidGoToConfigAck = 0x31;
|
||||
inline constexpr uint8_t kMidGoToMeasurement = 0x10;
|
||||
inline constexpr uint8_t kMidGoToMeasAck = 0x11;
|
||||
inline constexpr uint8_t kMidSetOutputMode = 0xD0;
|
||||
inline constexpr uint8_t kMidSetOutputModeAck = 0xD1;
|
||||
inline constexpr uint8_t kMidSetOutputSettings = 0xD2;
|
||||
inline constexpr uint8_t kMidSetOutputSettingsAck = 0xD3;
|
||||
inline constexpr uint8_t kMidMTData = 0x32;
|
||||
inline constexpr uint8_t kMidError = 0x42;
|
||||
|
||||
// OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04).
|
||||
inline constexpr uint16_t kOutputMode = 0x0007;
|
||||
// OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp
|
||||
// SampleCounter (bits1:0=01 => 0x01), float, all calibrated channels enabled.
|
||||
inline constexpr uint32_t kOutputSettings = 0x00000005;
|
||||
// Expected MTData payload with the above config: Temp(4) + Acc(12) + Gyr(12) +
|
||||
// Mag(12) + Euler(12) + SampleCounter(2).
|
||||
inline constexpr uint8_t kMTDataLen = 54;
|
||||
|
||||
// One fully decoded IMU reading.
|
||||
struct ImuSample {
|
||||
bool valid = false;
|
||||
float temp_c = 0.f; // °C
|
||||
float acc[3] = {0, 0, 0}; // m/s^2 (incl. gravity), sensor frame
|
||||
float gyr[3] = {0, 0, 0}; // rad/s
|
||||
float mag[3] = {0, 0, 0}; // a.u. (normalized to earth field)
|
||||
float roll_deg = 0.f; // -180..180
|
||||
float pitch_deg = 0.f; // -90..90
|
||||
float yaw_deg = 0.f; // 0..360 heading (MTi native -180..180 is shifted)
|
||||
uint16_t sample_counter = 0;
|
||||
};
|
||||
|
||||
// Lower byte of the sum of all bytes from BID through the end of DATA. The CS
|
||||
// byte that makes the running total ≡ 0 (mod 256) is (256 - mtiChecksum) & 0xFF.
|
||||
uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len);
|
||||
|
||||
// Build a complete message (PRE BID MID LEN DATA CS) ready to write.
|
||||
std::vector<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data = {});
|
||||
|
||||
// The four config messages for the orientation+calibrated Euler stream.
|
||||
std::vector<uint8_t> msgGoToConfig();
|
||||
std::vector<uint8_t> msgSetOutputMode(); // kOutputMode
|
||||
std::vector<uint8_t> msgSetOutputSettings(); // kOutputSettings
|
||||
std::vector<uint8_t> msgGoToMeasurement();
|
||||
|
||||
// Decode an MTData payload (the DATA bytes, big-endian) into an ImuSample.
|
||||
// Returns nullopt if mid != MTData or len != kMTDataLen.
|
||||
std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len);
|
||||
|
||||
// Incremental framer: feed raw bytes; for each complete, checksum-valid frame it
|
||||
// invokes the sink with (mid, data, len). Tolerates noise/resync by re-scanning
|
||||
// for the preamble.
|
||||
class MtiFramer {
|
||||
public:
|
||||
using FrameSink = std::function<void(uint8_t mid, const uint8_t* data, std::size_t len)>;
|
||||
|
||||
explicit MtiFramer(FrameSink sink) : sink_(std::move(sink)) {}
|
||||
void feed(const uint8_t* p, std::size_t n);
|
||||
|
||||
private:
|
||||
enum class S { Pre, Bid, Mid, Len, Data, Cs };
|
||||
FrameSink sink_;
|
||||
S state_ = S::Pre;
|
||||
uint8_t mid_ = 0;
|
||||
uint8_t len_ = 0;
|
||||
std::vector<uint8_t> data_;
|
||||
unsigned sum_ = 0; // running checksum sum (BID..DATA)
|
||||
};
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -26,4 +26,15 @@ std::optional<std::string> resolveConfigPath(const std::string& cliArg = "");
|
|||
// ($XDG_DATA_HOME/fire_gimbal_control/images, else ~/.local/share/...).
|
||||
std::string defaultOutputDir();
|
||||
|
||||
// Directory for diagnostic/calibration logfiles
|
||||
// ($XDG_DATA_HOME/fire_gimbal_control/logs, else ~/.local/share/...).
|
||||
std::string defaultLogDir();
|
||||
|
||||
// "<prefix>_YYYYMMDD-HHMMSS.log" using the local clock.
|
||||
std::string timestampedLogName(const std::string& prefix);
|
||||
|
||||
// Write `text` to `defaultLogDir()/<name>`, creating the directory if needed.
|
||||
// Returns the full path written, or "" on failure (and logs a warning).
|
||||
std::string writeLogFile(const std::string& name, const std::string& text);
|
||||
|
||||
} // namespace fgc::paths
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -0,0 +1,52 @@
|
|||
#pragma once
|
||||
|
||||
#include "fgc/IImuSource.h"
|
||||
#include "fgc/Logger.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
// Simulated IMU for development without hardware: synthesizes a slowly varying,
|
||||
// physically plausible full sample (orientation sweeps, ~1 g on accZ, small
|
||||
// gyro/mag) so the Sensors panel and its expanded view animate.
|
||||
class MockImuSource : public IImuSource {
|
||||
public:
|
||||
void start() override {
|
||||
start_ = clock::now();
|
||||
LOG_INFO << "[mock] IMU started";
|
||||
}
|
||||
void stop() override { LOG_INFO << "[mock] IMU stopped"; }
|
||||
|
||||
bool connected() const override { return true; }
|
||||
|
||||
std::optional<ImuSample> sample() override {
|
||||
const double t = std::chrono::duration<double>(clock::now() - start_).count();
|
||||
ImuSample s;
|
||||
s.valid = true;
|
||||
s.roll_deg = static_cast<float>(15.0 * std::sin(t * 0.5));
|
||||
s.pitch_deg = static_cast<float>(10.0 * std::sin(t * 0.3 + 1.0));
|
||||
s.yaw_deg = static_cast<float>(std::fmod(t * 8.0, 360.0)); // slow spin
|
||||
// Gravity tilts with roll/pitch; small free accel noise.
|
||||
const double r = s.roll_deg * M_PI / 180.0, p = s.pitch_deg * M_PI / 180.0;
|
||||
s.acc[0] = static_cast<float>(9.81 * -std::sin(p));
|
||||
s.acc[1] = static_cast<float>(9.81 * std::sin(r) * std::cos(p));
|
||||
s.acc[2] = static_cast<float>(9.81 * std::cos(r) * std::cos(p));
|
||||
s.gyr[0] = static_cast<float>(0.13 * std::cos(t * 0.5));
|
||||
s.gyr[1] = static_cast<float>(0.05 * std::cos(t * 0.3 + 1.0));
|
||||
s.gyr[2] = 0.14f;
|
||||
s.mag[0] = static_cast<float>(std::cos(s.yaw_deg * M_PI / 180.0));
|
||||
s.mag[1] = static_cast<float>(-std::sin(s.yaw_deg * M_PI / 180.0));
|
||||
s.mag[2] = 0.35f;
|
||||
s.temp_c = static_cast<float>(24.5 + 0.5 * std::sin(t * 0.05));
|
||||
s.sample_counter = static_cast<uint16_t>(static_cast<unsigned>(t * 100.0) & 0xFFFF);
|
||||
return s;
|
||||
}
|
||||
|
||||
private:
|
||||
using clock = std::chrono::steady_clock;
|
||||
clock::time_point start_ = clock::now();
|
||||
};
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -73,6 +73,29 @@ public:
|
|||
d << "DUMP END\n";
|
||||
dump_ = d.str();
|
||||
LOG_INFO << "firmware dump:\n" << dump_;
|
||||
} else if (verb == "DIAG") {
|
||||
// Canned DG stream (real firmware format) so the host DiagParser /
|
||||
// logfile path are demoable without hardware. Logged live like the
|
||||
// real capture would be.
|
||||
std::ostringstream d;
|
||||
auto axis = [&](char L) {
|
||||
d << "DG BEGIN " << L << "\n";
|
||||
const int speeds[3] = {12500, 25000, 50000};
|
||||
for (int s : speeds)
|
||||
for (bool fwd : {true, false})
|
||||
d << "DG " << L << " S" << s << (fwd ? " FWD" : " REV")
|
||||
<< " ERR_PEAK 120 ERR_RMS 30 ERR_STILL 40 CS_MIN 10 CS_MAX 20"
|
||||
<< " SG_MIN 300 PWM_AVG 96 FLAGS 0x00000000 PASS\n";
|
||||
d << "DG " << L << " RESULT PASS\n";
|
||||
};
|
||||
axis('Y');
|
||||
axis('P');
|
||||
d << "DG DONE\n";
|
||||
diag_ = d.str();
|
||||
++diag_seq_;
|
||||
std::istringstream ds(diag_);
|
||||
std::string ln;
|
||||
while (std::getline(ds, ln)) LOG_INFO << ln; // stream to LOG pane
|
||||
}
|
||||
// ENABLE/DISABLE/SPEED/SETPOS/RESET: accepted, no simulation effect.
|
||||
}
|
||||
|
|
@ -93,6 +116,12 @@ public:
|
|||
return dump_;
|
||||
}
|
||||
|
||||
std::string lastDiag() override {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return diag_;
|
||||
}
|
||||
unsigned diagSeq() const override { return diag_seq_; }
|
||||
|
||||
bool connected() const override { return true; }
|
||||
|
||||
private:
|
||||
|
|
@ -124,6 +153,8 @@ private:
|
|||
long pitch_target_ = 0;
|
||||
bool homed_ = false;
|
||||
std::string dump_;
|
||||
std::string diag_;
|
||||
unsigned diag_seq_ = 0;
|
||||
};
|
||||
|
||||
} // namespace fgc
|
||||
|
|
|
|||
|
|
@ -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<SensorField> fields;
|
||||
};
|
||||
|
||||
// The Sensors panel, split by source so it is clear which parameters come from
|
||||
// which device. Each group is filled with pending placeholders until its driver
|
||||
// lands. NOTE: the MTi's Temp is the device's *internal* temperature; the DHT11
|
||||
// Temp (separate group) is *ambient* — deliberately kept apart.
|
||||
struct SensorsView {
|
||||
SensorGroup imu; // Xsens MTi: Roll/Pitch/Yaw + internal temperature
|
||||
SensorGroup dht; // DHT11: ambient temperature + humidity
|
||||
};
|
||||
|
||||
struct CaptureView {
|
||||
bool present = false;
|
||||
bool active = false;
|
||||
|
|
@ -109,6 +118,47 @@ struct DumpView {
|
|||
std::string text;
|
||||
};
|
||||
|
||||
// Full Xsens MTi reading for the expanded Sensors view (units: acc m/s^2,
|
||||
// gyr rad/s, mag a.u., angles deg, temp °C).
|
||||
struct ImuView {
|
||||
bool present = false;
|
||||
float roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
|
||||
float acc[3] = {0, 0, 0};
|
||||
float gyr[3] = {0, 0, 0};
|
||||
float mag[3] = {0, 0, 0};
|
||||
float temp_c = 0;
|
||||
unsigned sample_counter = 0;
|
||||
};
|
||||
|
||||
// Status of the currently-running special operation (calibration, diagnostics,
|
||||
// homing, capture) plus the last completed result, for the activity strip below
|
||||
// the log. The result block persists until replaced so it never scrolls away.
|
||||
struct ActivityView {
|
||||
bool active = false; // a special op is running now
|
||||
std::string title; // "CALIBRATING" / "DIAGNOSTICS" / "HOMING" / "CAPTURE"
|
||||
std::string status; // live one-line progress
|
||||
bool has_result = false;
|
||||
std::string result_title; // e.g. "Calibration · 2m ago"
|
||||
std::vector<std::string> result; // summary lines
|
||||
UiColor result_color = UiColor::Default; // green pass / red fail
|
||||
std::string prompt; // a yes/no question awaiting the operator (empty = none)
|
||||
};
|
||||
|
||||
// Last calibration fit, per axis, for the gimbal expanded view.
|
||||
struct CalibAxisView {
|
||||
bool ok = false;
|
||||
double counts_per_deg = 0;
|
||||
long zero_count = 0;
|
||||
double r2 = 0;
|
||||
int n = 0;
|
||||
};
|
||||
struct CalibResultView {
|
||||
bool has = false;
|
||||
long long ts_ms = 0;
|
||||
bool pitch_present = false;
|
||||
CalibAxisView yaw, pitch;
|
||||
};
|
||||
|
||||
struct UiSnapshot {
|
||||
HeaderView header;
|
||||
GimbalView gimbal;
|
||||
|
|
@ -117,6 +167,9 @@ struct UiSnapshot {
|
|||
ConnView conn;
|
||||
std::vector<LogLine> log;
|
||||
DumpView dump;
|
||||
ImuView imu;
|
||||
ActivityView activity;
|
||||
CalibResultView calib;
|
||||
};
|
||||
|
||||
// ---- Pure formatting helpers (unit-tested in tests/test_uisnapshot.cpp) ----
|
||||
|
|
|
|||
1
main.cpp
1
main.cpp
|
|
@ -70,6 +70,7 @@ int main(int argc, char* argv[]) {
|
|||
if (vm["no-tui"].as<bool>()) opts.use_tui = false; // --no-tui wins
|
||||
if (vm.count("log-level")) opts.log_level = vm["log-level"].as<std::string>();
|
||||
if (vm.count("trace")) opts.trace_categories = vm["trace"].as<std::string>();
|
||||
opts.config_path = *cfg_path; // so `gimbal calib` can offer to save back to it
|
||||
|
||||
Application app(std::move(cfg), std::move(opts));
|
||||
return app.run();
|
||||
|
|
|
|||
|
|
@ -1,17 +1,23 @@
|
|||
#include "fgc/Application.h"
|
||||
|
||||
#include "fgc/CalibrationRoutine.h"
|
||||
#include "fgc/CaptureScheduler.h"
|
||||
#include "fgc/CommandParser.h"
|
||||
#include "fgc/DiagParser.h"
|
||||
#include "fgc/DumpParser.h"
|
||||
#include "fgc/HelpText.h"
|
||||
#include "fgc/Paths.h"
|
||||
#include "fgc/ICameraSource.h"
|
||||
#include "fgc/IControlChannel.h"
|
||||
#include "fgc/IImuSource.h"
|
||||
#include "fgc/IMotorController.h"
|
||||
#include "fgc/ImagePipeline.h"
|
||||
#include "fgc/Logger.h"
|
||||
#include "fgc/MtiImuSource.h"
|
||||
#include "fgc/ScanGrid.h"
|
||||
#include "fgc/SerialMotorController.h"
|
||||
#include "fgc/mock/MockCameraSource.h"
|
||||
#include "fgc/mock/MockImuSource.h"
|
||||
#include "fgc/mock/MockMotorController.h"
|
||||
#include "fgc/mock/NullControlChannel.h"
|
||||
#include "fgc/ui/HeadlessUi.h"
|
||||
|
|
@ -19,7 +25,10 @@
|
|||
#include "fgc/ui/UiSnapshot.h"
|
||||
|
||||
#include <atomic>
|
||||
#include <cctype>
|
||||
#include <chrono>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <queue>
|
||||
|
|
@ -41,6 +50,12 @@ namespace fgc {
|
|||
|
||||
namespace {
|
||||
|
||||
long long nowEpochMs() {
|
||||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch())
|
||||
.count();
|
||||
}
|
||||
|
||||
// Human-readable list of the enabled wire-trace categories, for echoing back.
|
||||
std::string traceNames(unsigned mask) {
|
||||
if (mask == 0) return "none";
|
||||
|
|
@ -69,11 +84,25 @@ struct Application::Impl {
|
|||
std::unique_ptr<IControlChannel> channel;
|
||||
std::unique_ptr<IMotorController> motor;
|
||||
std::unique_ptr<ICameraSource> camera;
|
||||
std::unique_ptr<IImuSource> imu;
|
||||
std::unique_ptr<ImagePipeline> pipeline;
|
||||
std::unique_ptr<CaptureScheduler> scheduler;
|
||||
std::unique_ptr<IUserInterface> ui;
|
||||
std::unique_ptr<CalibrationRoutine> calib;
|
||||
unsigned last_diag_seq = 0;
|
||||
ScanGrid grid; // outlives scheduler (holds a reference to it)
|
||||
|
||||
// Persisted last-results for the TUI activity strip (survive in the log churn).
|
||||
bool diag_running_ = false;
|
||||
std::chrono::steady_clock::time_point diag_started_;
|
||||
std::vector<std::string> last_diag_summary;
|
||||
long long last_diag_ts = 0; // epoch ms
|
||||
bool last_diag_pass = false;
|
||||
CalibResultView last_calib_view;
|
||||
std::vector<std::string> last_calib_summary;
|
||||
long long last_calib_ts = 0; // epoch ms
|
||||
bool calib_save_pending_ = false; // awaiting y/n to persist to config
|
||||
|
||||
std::atomic<bool> running{true};
|
||||
std::mutex cmd_mutex;
|
||||
std::queue<std::string> cmd_queue;
|
||||
|
|
@ -105,6 +134,16 @@ struct Application::Impl {
|
|||
return std::make_unique<SerialMotorController>(cfg.serial.device, cfg.serial.baud);
|
||||
}
|
||||
|
||||
std::unique_ptr<IImuSource> makeImu() {
|
||||
if (!cfg.features.enable_imu) return nullptr; // sensors panel stays "pending"
|
||||
if (cfg.features.mock_imu) return std::make_unique<MockImuSource>();
|
||||
if (cfg.imu.device.empty()) {
|
||||
LOG_WARN << "IMU enabled but [IMU] device is empty; disabling IMU";
|
||||
return nullptr;
|
||||
}
|
||||
return std::make_unique<MtiImuSource>(cfg.imu.device, cfg.imu.baud);
|
||||
}
|
||||
|
||||
std::unique_ptr<ICameraSource> makeCamera() {
|
||||
bool mock = opts.mock_camera.value_or(cfg.features.mock_camera);
|
||||
#if !FGC_WITH_VIMBA
|
||||
|
|
@ -192,8 +231,45 @@ struct Application::Impl {
|
|||
}
|
||||
}
|
||||
|
||||
// --- Sensors (DHT11 + MTi not integrated yet) ---
|
||||
// --- Sensors: MTi (orientation) live if present; DHT11 (ambient) pending ---
|
||||
s.sensors = pendingSensorsView();
|
||||
if (imu) {
|
||||
auto fmt1 = [](float v) {
|
||||
char b[24];
|
||||
std::snprintf(b, sizeof(b), "%.1f", v);
|
||||
return std::string(b);
|
||||
};
|
||||
auto setField = [&](SensorField& f, const std::string& v) {
|
||||
f.value = v;
|
||||
f.present = true;
|
||||
};
|
||||
if (auto m = imu->sample()) {
|
||||
// Full reading for the expanded view.
|
||||
s.imu.present = true;
|
||||
s.imu.roll_deg = m->roll_deg;
|
||||
s.imu.pitch_deg = m->pitch_deg;
|
||||
s.imu.yaw_deg = m->yaw_deg;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
s.imu.acc[i] = m->acc[i];
|
||||
s.imu.gyr[i] = m->gyr[i];
|
||||
s.imu.mag[i] = m->mag[i];
|
||||
}
|
||||
s.imu.temp_c = m->temp_c;
|
||||
s.imu.sample_counter = m->sample_counter;
|
||||
// Compact MTi subsection: Roll/Pitch/Yaw + the device's internal temp.
|
||||
auto& g = s.sensors.imu;
|
||||
g.present = true;
|
||||
g.status = "live";
|
||||
if (g.fields.size() >= 4) {
|
||||
setField(g.fields[0], fmt1(m->roll_deg));
|
||||
setField(g.fields[1], fmt1(m->pitch_deg));
|
||||
setField(g.fields[2], fmt1(m->yaw_deg));
|
||||
setField(g.fields[3], fmt1(m->temp_c)); // MTi internal temp
|
||||
}
|
||||
} else {
|
||||
s.sensors.imu.status = "no fix";
|
||||
}
|
||||
}
|
||||
|
||||
// --- Camera / capture ---
|
||||
s.capture.present = true;
|
||||
|
|
@ -223,9 +299,68 @@ struct Application::Impl {
|
|||
// --- Diagnostics (last firmware DUMP) ---
|
||||
s.dump.text = motor->lastDump();
|
||||
s.dump.has = !s.dump.text.empty();
|
||||
|
||||
// --- Activity strip + last calibration ---
|
||||
s.calib = last_calib_view;
|
||||
s.calib.pitch_present = s.gimbal.pitch_present || s.calib.pitch_present;
|
||||
fillActivity(s);
|
||||
return s;
|
||||
}
|
||||
|
||||
// The currently-running special op (priority: calib > diag > homing > capture)
|
||||
// plus the most recent persisted result, for the activity strip.
|
||||
void fillActivity(UiSnapshot& s) const {
|
||||
ActivityView& a = s.activity;
|
||||
|
||||
if (calib && calib->running()) {
|
||||
CalibProgress p = calib->progress();
|
||||
a.active = true;
|
||||
a.title = "CALIBRATING";
|
||||
std::string axis = (p.axis == 'P') ? "PITCH" : (p.axis == 'Y') ? "YAW" : "";
|
||||
a.status = axis.empty() ? p.phase
|
||||
: axis + " step " + std::to_string(p.step) + "/" +
|
||||
std::to_string(p.total) + " — " + p.phase;
|
||||
} else if (diag_running_) {
|
||||
a.active = true;
|
||||
a.title = "DIAGNOSTICS";
|
||||
a.status = "running motor self-test…";
|
||||
} else if (s.gimbal.yaw.state == AxisState::Homing ||
|
||||
(s.gimbal.pitch_present && s.gimbal.pitch.state == AxisState::Homing)) {
|
||||
a.active = true;
|
||||
a.title = "HOMING";
|
||||
a.status = "finding endstops…";
|
||||
} else if (scheduler && scheduler->captureActive()) {
|
||||
a.active = true;
|
||||
a.title = "CAPTURE";
|
||||
a.status = s.capture.has_last
|
||||
? ("scanning · last " + s.capture.last_label)
|
||||
: "scanning…";
|
||||
}
|
||||
|
||||
// Persisted result: the more recent of the last calibration / diagnostics.
|
||||
const long long now = nowEpochMs();
|
||||
if (last_calib_ts || last_diag_ts) {
|
||||
const bool calib_newer = last_calib_ts >= last_diag_ts;
|
||||
a.has_result = true;
|
||||
if (calib_newer) {
|
||||
a.result_title = "Calibration · " + formatTimeAgo(now, last_calib_ts);
|
||||
a.result = last_calib_summary;
|
||||
a.result_color = last_calib_view.has &&
|
||||
last_calib_view.yaw.ok ? UiColor::Green : UiColor::Yellow;
|
||||
} else {
|
||||
a.result_title = std::string("Diagnostics ") +
|
||||
(last_diag_pass ? "PASS" : "FAIL") + " · " +
|
||||
formatTimeAgo(now, last_diag_ts);
|
||||
a.result = last_diag_summary;
|
||||
a.result_color = last_diag_pass ? UiColor::Green : UiColor::Red;
|
||||
}
|
||||
}
|
||||
|
||||
// Pending yes/no: persist the just-applied calibration as the new default?
|
||||
if (calib_save_pending_)
|
||||
a.prompt = "Save this calibration to config as the new default? (y / n)";
|
||||
}
|
||||
|
||||
void publishSnapshot() {
|
||||
UiSnapshot s = buildSnapshot();
|
||||
std::lock_guard<std::mutex> lock(snapshot_mutex);
|
||||
|
|
@ -311,19 +446,186 @@ struct Application::Impl {
|
|||
// elevation in degrees. Converts to encoder counts via the operator-calibrated
|
||||
// Geometry maps (which soft-clamp to the travel limits) and issues a two-axis
|
||||
// MOVE so both axes start together.
|
||||
void handleGoto(const std::string& line) {
|
||||
// Parse "<a>,<b>" or "<a> <b>" (tokens from `from` onward) into two numbers.
|
||||
static bool parsePair(const std::vector<std::string>& tok, size_t from,
|
||||
double& a, double& b) {
|
||||
std::string s;
|
||||
for (size_t i = from; i < tok.size(); ++i) s += tok[i] + " ";
|
||||
for (char& ch : s) if (ch == ',') ch = ' ';
|
||||
std::istringstream iss(s);
|
||||
return static_cast<bool>(iss >> a >> b);
|
||||
}
|
||||
|
||||
// Soft-limit travel (counts) for an axis from the last dump, else the
|
||||
// configured degree range converted to counts. Returns 0 if unknown.
|
||||
long axisRangeCounts(char axis) {
|
||||
DumpData d = parseDump(motor->lastDump());
|
||||
if (d.valid)
|
||||
for (const auto& ax : d.axes)
|
||||
if (ax.axis == axis) return std::labs(ax.lim_pos - ax.lim_neg);
|
||||
const AxisMap& m = (axis == 'Y') ? cfg.geometry.yaw : cfg.geometry.pitch;
|
||||
return std::labs(m.toCounts(m.max_deg) - m.toCounts(m.min_deg));
|
||||
}
|
||||
|
||||
// `gimbal <subcommand>` — unified, lowercase motor control. `move` is degrees
|
||||
// (default), `steps` is raw encoder counts; the rest map to firmware verbs.
|
||||
void handleGimbal(const std::string& line) {
|
||||
std::istringstream iss(line);
|
||||
std::string verb;
|
||||
double yaw_deg = 0.0, pitch_deg = 0.0;
|
||||
if (!(iss >> verb >> yaw_deg >> pitch_deg)) {
|
||||
LOG_WARN << "usage: goto <yaw_deg> <pitch_deg> (e.g. goto 30 -10)";
|
||||
std::vector<std::string> tok;
|
||||
std::string t;
|
||||
while (iss >> t) tok.push_back(t); // tok[0] == "gimbal"
|
||||
if (tok.size() < 2) {
|
||||
LOG_WARN << "usage: gimbal <move|steps|nudge|home|stop|reset|enable|disable|"
|
||||
"speed|setpos|status|dump|diag|calib>";
|
||||
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<char>(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 <yaw_deg>,<pitch_deg>";
|
||||
return;
|
||||
}
|
||||
long yc = cfg.geometry.yaw.toCounts(yaw_deg);
|
||||
long pc = cfg.geometry.pitch.toCounts(pitch_deg);
|
||||
LOG_INFO << "gimbal move yaw=" << yaw_deg << " pitch=" << pitch_deg
|
||||
<< " deg -> MOVE " << yc << "," << pc;
|
||||
motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc));
|
||||
} else if (sub == "steps") {
|
||||
double yc = 0, pc = 0;
|
||||
if (!parsePair(tok, 2, yc, pc)) {
|
||||
LOG_WARN << "usage: gimbal steps <yaw_counts>,<pitch_counts>";
|
||||
return;
|
||||
}
|
||||
motor->sendCommand("MOVE " + std::to_string((long)yc) + "," + std::to_string((long)pc));
|
||||
} else if (sub == "nudge") {
|
||||
if (tok.size() < 4) { LOG_WARN << "usage: gimbal nudge <yaw|pitch> <+/-pct>"; return; }
|
||||
char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
|
||||
double pct = 0;
|
||||
try { pct = std::stod(tok[3]); } catch (...) { LOG_WARN << "gimbal nudge: bad percent"; return; }
|
||||
long range = axisRangeCounts(axis);
|
||||
if (range <= 0) { LOG_WARN << "gimbal nudge: travel unknown; run gimbal home/dump first"; motor->sendCommand("DUMP"); return; }
|
||||
MotorTelemetry tel = motor->telemetry();
|
||||
long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
|
||||
long target = cur + static_cast<long>(pct / 100.0 * range);
|
||||
LOG_INFO << "gimbal nudge " << axis << " " << pct << "% -> MOVE " << axis << " " << target;
|
||||
motor->sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
|
||||
} else if (sub == "diag") {
|
||||
MotorTelemetry tel = motor->telemetry();
|
||||
if (!tel.yaw.ready() && !(tel.pitch_present && tel.pitch.ready())) {
|
||||
LOG_WARN << "gimbal diag: axes not READY (home first)";
|
||||
return;
|
||||
}
|
||||
std::string fw = "DIAG";
|
||||
for (size_t i = 2; i < tok.size(); ++i) fw += " " + tok[i];
|
||||
LOG_INFO << "running gimbal diagnostics...";
|
||||
motor->sendCommand(fw);
|
||||
diag_running_ = true;
|
||||
diag_started_ = std::chrono::steady_clock::now();
|
||||
} else if (sub == "calib") {
|
||||
startCalibration();
|
||||
} else if (sub == "dump") {
|
||||
LOG_INFO << "requesting firmware dump...";
|
||||
motor->sendCommand("DUMP");
|
||||
} else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" ||
|
||||
sub == "disable" || sub == "speed" || sub == "setpos" || sub == "status") {
|
||||
// Passthrough to the (case-insensitive) firmware verb + args.
|
||||
std::string fw = sub;
|
||||
for (char& ch : fw) ch = static_cast<char>(std::toupper((unsigned char)ch));
|
||||
for (size_t i = 2; i < tok.size(); ++i) fw += " " + tok[i];
|
||||
motor->sendCommand(fw);
|
||||
} else {
|
||||
LOG_WARN << "unknown gimbal subcommand: " << sub;
|
||||
}
|
||||
}
|
||||
|
||||
void startCalibration() {
|
||||
if (!imu) { LOG_WARN << "gimbal calib requires the IMU (set [Features] enable_imu)"; return; }
|
||||
if (calib && calib->running()) { LOG_WARN << "calibration already running (gimbal stop to cancel)"; return; }
|
||||
stopCapture(); // free the serial link + don't fight the routine's moves
|
||||
calib = std::make_unique<CalibrationRoutine>(*motor, *imu, cfg.geometry);
|
||||
calib->start();
|
||||
}
|
||||
|
||||
// Persist the live (just-calibrated) [Motor] geometry back into config.ini.
|
||||
void saveCalibration() {
|
||||
if (!calib_save_pending_) return;
|
||||
calib_save_pending_ = false;
|
||||
if (saveMotorCalibration(opts.config_path, cfg.geometry))
|
||||
LOG_INFO << "calibration saved as the new default in " << opts.config_path;
|
||||
else
|
||||
LOG_WARN << "could not write calibration to " << opts.config_path;
|
||||
}
|
||||
|
||||
void cancelCalibration() {
|
||||
if (calib && calib->running()) {
|
||||
LOG_INFO << "cancelling calibration";
|
||||
calib->cancel();
|
||||
motor->sendCommand("STOP ALL");
|
||||
}
|
||||
}
|
||||
|
||||
// Pick up results produced by background work (calibration thread, DIAG
|
||||
// capture) — runs on the control thread each tick so all geometry/state
|
||||
// mutation stays single-threaded.
|
||||
void pollBackgroundResults() {
|
||||
if (calib) {
|
||||
if (auto g = calib->takeResult()) {
|
||||
cfg.geometry = *g;
|
||||
if (scheduler) scheduler->setGeometry(cfg.geometry); // also fix scan targeting
|
||||
LOG_INFO << "calibration applied to live geometry";
|
||||
// Capture the structured report for the activity strip + gimbal view.
|
||||
CalibReport rep = calib->report();
|
||||
CalibResultView v;
|
||||
v.has = rep.valid;
|
||||
v.ts_ms = rep.ts_ms;
|
||||
last_calib_summary.clear();
|
||||
for (const auto& ax : rep.axes) {
|
||||
CalibAxisView* dst = (ax.axis == 'P') ? &v.pitch : &v.yaw;
|
||||
if (ax.axis == 'P') v.pitch_present = true;
|
||||
dst->ok = ax.ok;
|
||||
dst->counts_per_deg = ax.counts_per_deg;
|
||||
dst->zero_count = ax.zero_count;
|
||||
dst->r2 = ax.r2;
|
||||
dst->n = ax.n;
|
||||
char b[96];
|
||||
std::snprintf(b, sizeof(b), "%s cpd=%.3f zero=%ld R2=%.4f (n=%d)%s",
|
||||
ax.axis == 'P' ? "PITCH" : "YAW", ax.counts_per_deg,
|
||||
ax.zero_count, ax.r2, ax.n, ax.ok ? "" : " FIT FAILED");
|
||||
last_calib_summary.emplace_back(b);
|
||||
}
|
||||
last_calib_view = v;
|
||||
last_calib_ts = nowEpochMs();
|
||||
// Offer to persist it as the new default (answered via the activity strip).
|
||||
if (rep.valid && !opts.config_path.empty()) calib_save_pending_ = true;
|
||||
}
|
||||
}
|
||||
unsigned seq = motor->diagSeq();
|
||||
if (seq != last_diag_seq) {
|
||||
last_diag_seq = seq;
|
||||
diag_running_ = false;
|
||||
DiagResult dr = parseDiag(motor->lastDiag());
|
||||
std::ostringstream summary;
|
||||
for (const auto& l : formatDiag(dr)) { LOG_INFO << l; summary << l << "\n"; }
|
||||
// Concise per-axis lines for the activity strip (full detail is logged).
|
||||
last_diag_summary.clear();
|
||||
for (const auto& ax : dr.axes)
|
||||
last_diag_summary.push_back(std::string(1, ax.axis) + ": " +
|
||||
(ax.pass ? "PASS" : "FAIL") + " (" +
|
||||
std::to_string(ax.tests.size()) + " tests)");
|
||||
last_diag_pass = dr.allPass();
|
||||
last_diag_ts = nowEpochMs();
|
||||
std::string path = paths::writeLogFile(
|
||||
paths::timestampedLogName("diag"),
|
||||
motor->lastDiag() + "\n--- parsed ---\n" + summary.str());
|
||||
if (!path.empty()) LOG_INFO << "diagnostics log written: " << path;
|
||||
}
|
||||
// Safety: clear a stuck diag flag if the firmware never replied.
|
||||
if (diag_running_ &&
|
||||
std::chrono::steady_clock::now() - diag_started_ > std::chrono::seconds(30))
|
||||
diag_running_ = false;
|
||||
}
|
||||
|
||||
void handleCommand(const std::string& line) {
|
||||
|
|
@ -334,16 +636,21 @@ struct Application::Impl {
|
|||
if (c.verb == "help") {
|
||||
// c.device holds the optional topic (first token after "help").
|
||||
for (const auto& l : renderHelp(c.device)) LOG_INFO << l;
|
||||
} else if (c.verb == "goto") {
|
||||
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;
|
||||
|
|
@ -361,8 +668,6 @@ struct Application::Impl {
|
|||
} else if (c.device == "fps" && c.has_value) {
|
||||
scheduler->setImageRate(c.value);
|
||||
LOG_INFO << "capture rate set to " << c.value << " img/s";
|
||||
} else if (c.device == "motorctl") {
|
||||
motor->sendCommand(c.option);
|
||||
} else {
|
||||
LOG_WARN << "unknown 'set' target: " << c.device;
|
||||
}
|
||||
|
|
@ -411,6 +716,7 @@ struct Application::Impl {
|
|||
channel = makeChannel();
|
||||
motor = makeMotor();
|
||||
camera = makeCamera();
|
||||
imu = makeImu();
|
||||
|
||||
if (!channel->connect())
|
||||
LOG_WARN << "Control channel not connected; continuing in degraded mode";
|
||||
|
|
@ -443,6 +749,7 @@ struct Application::Impl {
|
|||
cfg.geometry, grid);
|
||||
|
||||
motor->start();
|
||||
if (imu) imu->start();
|
||||
camera->open();
|
||||
pipeline->start();
|
||||
channel->publishStatus(0);
|
||||
|
|
@ -471,16 +778,19 @@ struct Application::Impl {
|
|||
LOG_INFO << "Entering control loop (type 'exit' to quit)";
|
||||
while (running) {
|
||||
drainCommands();
|
||||
pollBackgroundResults(); // apply calibration result / emit DIAG summary+log
|
||||
scheduler->tick();
|
||||
publishSnapshot();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
LOG_INFO << "Shutting down";
|
||||
if (calib) calib->cancel(); // stop any in-flight calibration before teardown
|
||||
if (ui) ui->stop();
|
||||
pipeline->stop();
|
||||
camera->stop();
|
||||
camera->close();
|
||||
if (imu) imu->stop();
|
||||
motor->stop();
|
||||
channel->disconnect();
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,49 @@
|
|||
#include "fgc/Calibration.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
LinearFit linearFit(const std::vector<std::pair<double, double>>& xy) {
|
||||
LinearFit f;
|
||||
f.n = static_cast<int>(xy.size());
|
||||
if (xy.size() < 2) return f;
|
||||
|
||||
double sx = 0, sy = 0;
|
||||
for (const auto& p : xy) { sx += p.first; sy += p.second; }
|
||||
const double mx = sx / xy.size(), my = sy / xy.size();
|
||||
|
||||
double sxx = 0, sxy = 0, syy = 0;
|
||||
for (const auto& p : xy) {
|
||||
const double dx = p.first - mx, dy = p.second - my;
|
||||
sxx += dx * dx;
|
||||
sxy += dx * dy;
|
||||
syy += dy * dy;
|
||||
}
|
||||
if (sxx <= 0.0) return f; // no spread in x -> undefined slope
|
||||
|
||||
f.slope = sxy / sxx;
|
||||
f.intercept = my - f.slope * mx;
|
||||
f.r2 = (syy > 0.0) ? (sxy * sxy) / (sxx * syy) : 1.0;
|
||||
f.ok = true;
|
||||
return f;
|
||||
}
|
||||
|
||||
double circularMeanDeg(const std::vector<double>& degs) {
|
||||
if (degs.empty()) return 0.0;
|
||||
double sc = 0, ss = 0;
|
||||
for (double d : degs) {
|
||||
const double r = d * M_PI / 180.0;
|
||||
sc += std::cos(r);
|
||||
ss += std::sin(r);
|
||||
}
|
||||
return std::atan2(ss, sc) * 180.0 / M_PI;
|
||||
}
|
||||
|
||||
double unwrapNear(double prev, double deg) {
|
||||
while (deg - prev > 180.0) deg -= 360.0;
|
||||
while (deg - prev < -180.0) deg += 360.0;
|
||||
return deg;
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -0,0 +1,253 @@
|
|||
#include "fgc/CalibrationRoutine.h"
|
||||
|
||||
#include "fgc/Calibration.h"
|
||||
#include "fgc/DumpParser.h"
|
||||
#include "fgc/IImuSource.h"
|
||||
#include "fgc/IMotorController.h"
|
||||
#include "fgc/Logger.h"
|
||||
#include "fgc/Paths.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <sstream>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
namespace {
|
||||
using clock = std::chrono::steady_clock;
|
||||
constexpr int kPositions = 10;
|
||||
constexpr int kDwellMs = 5000; // hold at each position
|
||||
constexpr int kSampleMs = 100; // IMU sampling cadence during dwell
|
||||
constexpr int kSettleTimeoutMs = 15000; // max wait for a move to settle
|
||||
constexpr double kInsetFrac = 0.05; // keep targets off the hard endstops
|
||||
|
||||
void msleep(int ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); }
|
||||
} // namespace
|
||||
|
||||
CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial)
|
||||
: motor_(motor), imu_(imu), initial_(std::move(initial)) {}
|
||||
|
||||
CalibrationRoutine::~CalibrationRoutine() {
|
||||
cancel();
|
||||
if (thread_.joinable()) thread_.join();
|
||||
}
|
||||
|
||||
bool CalibrationRoutine::start() {
|
||||
if (running_.exchange(true)) {
|
||||
LOG_WARN << "calibration already running";
|
||||
return false;
|
||||
}
|
||||
cancel_ = false;
|
||||
if (thread_.joinable()) thread_.join();
|
||||
thread_ = std::thread([this] { run(); });
|
||||
return true;
|
||||
}
|
||||
|
||||
void CalibrationRoutine::cancel() { cancel_ = true; }
|
||||
|
||||
std::optional<Geometry> CalibrationRoutine::takeResult() {
|
||||
std::lock_guard<std::mutex> lk(result_mutex_);
|
||||
std::optional<Geometry> r;
|
||||
r.swap(result_);
|
||||
return r;
|
||||
}
|
||||
|
||||
CalibProgress CalibrationRoutine::progress() const {
|
||||
std::lock_guard<std::mutex> lk(progress_mutex_);
|
||||
return progress_;
|
||||
}
|
||||
|
||||
CalibReport CalibrationRoutine::report() const {
|
||||
std::lock_guard<std::mutex> lk(result_mutex_);
|
||||
return report_;
|
||||
}
|
||||
|
||||
void CalibrationRoutine::setProgress(char axis, int step, int total, const char* phase) {
|
||||
std::lock_guard<std::mutex> lk(progress_mutex_);
|
||||
progress_.running = true;
|
||||
progress_.axis = axis;
|
||||
progress_.step = step;
|
||||
progress_.total = total;
|
||||
progress_.phase = phase;
|
||||
}
|
||||
|
||||
namespace {
|
||||
long long nowMs() {
|
||||
return std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch())
|
||||
.count();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
namespace {
|
||||
|
||||
// Wait until the axis is at `target` (standstill + within tol), or timeout/cancel.
|
||||
bool waitSettle(IMotorController& motor, char axis, long target, long tol,
|
||||
const std::atomic<bool>& cancel) {
|
||||
const auto deadline = clock::now() + std::chrono::milliseconds(kSettleTimeoutMs);
|
||||
while (clock::now() < deadline) {
|
||||
if (cancel) return false;
|
||||
MotorTelemetry t = motor.telemetry();
|
||||
const AxisTelemetry& a = (axis == 'Y') ? t.yaw : t.pitch;
|
||||
if (a.standstill && std::labs(a.xenc - target) <= tol) return true;
|
||||
msleep(100);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Dwell `kDwellMs` sampling the IMU; return the (circular for yaw) mean of the
|
||||
// requested Euler angle in degrees.
|
||||
double dwellAndMeasure(IImuSource& imu, char axis, const std::atomic<bool>& cancel) {
|
||||
std::vector<double> vals;
|
||||
const auto end = clock::now() + std::chrono::milliseconds(kDwellMs);
|
||||
while (clock::now() < end) {
|
||||
if (cancel) break;
|
||||
if (auto s = imu.sample())
|
||||
vals.push_back(axis == 'Y' ? s->yaw_deg : s->pitch_deg);
|
||||
msleep(kSampleMs);
|
||||
}
|
||||
if (vals.empty()) return 0.0;
|
||||
if (axis == 'Y') {
|
||||
double m = circularMeanDeg(vals); // robust to wrap within the dwell
|
||||
if (m < 0.0) m += 360.0; // report a 0..360 heading (matches IMU)
|
||||
return m;
|
||||
}
|
||||
double m = 0;
|
||||
for (double v : vals) m += v;
|
||||
return m / vals.size();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void CalibrationRoutine::run() {
|
||||
struct Done {
|
||||
CalibrationRoutine* self;
|
||||
~Done() {
|
||||
self->running_ = false;
|
||||
std::lock_guard<std::mutex> lk(self->progress_mutex_);
|
||||
self->progress_.running = false;
|
||||
}
|
||||
} done{this};
|
||||
|
||||
LOG_INFO << "=== gimbal calibration starting ===";
|
||||
setProgress('?', 0, kPositions, "starting");
|
||||
CalibReport report;
|
||||
|
||||
// 1. Preconditions.
|
||||
if (!imu_.connected()) {
|
||||
LOG_WARN << "calibration aborted: IMU not connected";
|
||||
return;
|
||||
}
|
||||
MotorTelemetry t0 = motor_.telemetry();
|
||||
if (!t0.yaw.ready() || (t0.pitch_present && !t0.pitch.ready())) {
|
||||
LOG_WARN << "calibration aborted: axes must be homed (READY) first";
|
||||
return;
|
||||
}
|
||||
|
||||
// Fetch soft limits from a fresh dump.
|
||||
LOG_INFO << "calibration: requesting soft limits (dump)...";
|
||||
motor_.sendCommand("DUMP");
|
||||
DumpData dump;
|
||||
for (int i = 0; i < 50 && !cancel_; ++i) { // up to ~5 s
|
||||
dump = parseDump(motor_.lastDump());
|
||||
if (dump.valid && !dump.axes.empty()) break;
|
||||
msleep(100);
|
||||
}
|
||||
if (!dump.valid) {
|
||||
LOG_WARN << "calibration aborted: could not read soft limits";
|
||||
return;
|
||||
}
|
||||
|
||||
Geometry result = initial_;
|
||||
std::ostringstream log;
|
||||
log << "gimbal calibration\n==================\n";
|
||||
|
||||
struct AxisJob { char axis; AxisMap* map; };
|
||||
std::vector<AxisJob> jobs = {{'Y', &result.yaw}};
|
||||
if (t0.pitch_present) jobs.push_back({'P', &result.pitch});
|
||||
|
||||
for (const auto& job : jobs) {
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
const DumpAxis* da = nullptr;
|
||||
for (const auto& ax : dump.axes) if (ax.axis == job.axis) da = &ax;
|
||||
if (!da) { LOG_WARN << "calibration: no limits for axis " << job.axis << "; skipping"; continue; }
|
||||
|
||||
const long lo = da->lim_neg, hi = da->lim_pos;
|
||||
const long travel = std::labs(hi - lo);
|
||||
const long inset = static_cast<long>(kInsetFrac * travel);
|
||||
const long a = lo + inset, b = hi - inset;
|
||||
const long tol = std::max<long>(500, travel / 200);
|
||||
|
||||
LOG_INFO << "calibrating " << (job.axis == 'Y' ? "YAW" : "PITCH")
|
||||
<< " over [" << a << ".." << b << "] counts in " << kPositions << " steps";
|
||||
log << "\n[" << job.axis << "] travel=[" << lo << ".." << hi << "] tol=" << tol
|
||||
<< "\n target_counts, imu_deg\n";
|
||||
|
||||
std::vector<std::pair<double, double>> pts; // (deg, counts)
|
||||
double prev_deg = 0.0;
|
||||
bool have_prev = false;
|
||||
for (int i = 0; i < kPositions; ++i) {
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
long target = a + (b - a) * i / (kPositions - 1);
|
||||
setProgress(job.axis, i + 1, kPositions, "moving");
|
||||
motor_.sendCommand(std::string("MOVE ") + job.axis + " " + std::to_string(target));
|
||||
if (!waitSettle(motor_, job.axis, target, tol, cancel_)) {
|
||||
if (cancel_) return;
|
||||
LOG_WARN << " position " << (i + 1) << "/" << kPositions
|
||||
<< " did not settle near " << target << " (continuing)";
|
||||
}
|
||||
setProgress(job.axis, i + 1, kPositions, "dwelling");
|
||||
double deg = dwellAndMeasure(imu_, job.axis, cancel_);
|
||||
// Unwrap against the previous sample so a sweep crossing 0/360 stays a
|
||||
// continuous line for the fit (no ±360 jump).
|
||||
if (have_prev) deg = unwrapNear(prev_deg, deg);
|
||||
prev_deg = deg;
|
||||
have_prev = true;
|
||||
pts.emplace_back(deg, static_cast<double>(target));
|
||||
LOG_INFO << " " << (i + 1) << "/" << kPositions << " counts=" << target
|
||||
<< " imu=" << deg << " deg";
|
||||
log << " " << target << ", " << deg << "\n";
|
||||
}
|
||||
|
||||
setProgress(job.axis, kPositions, kPositions, "fitting");
|
||||
LinearFit fit = linearFit(pts);
|
||||
if (!fit.ok) {
|
||||
LOG_WARN << " fit failed for axis " << job.axis << " (degenerate data)";
|
||||
log << " FIT FAILED\n";
|
||||
report.axes.push_back({job.axis, false, 0, 0, 0, static_cast<int>(pts.size())});
|
||||
continue;
|
||||
}
|
||||
const double old_cpd = job.map->counts_per_deg;
|
||||
const long old_zc = job.map->zero_count;
|
||||
job.map->counts_per_deg = fit.slope;
|
||||
job.map->zero_count = static_cast<long>(std::lround(fit.intercept));
|
||||
LOG_INFO << " fit: counts_per_deg " << old_cpd << " -> " << fit.slope
|
||||
<< ", zero_count " << old_zc << " -> " << job.map->zero_count
|
||||
<< " (R^2=" << fit.r2 << ")";
|
||||
log << " fit: counts_per_deg=" << fit.slope << " zero_count=" << job.map->zero_count
|
||||
<< " R2=" << fit.r2 << " (was cpd=" << old_cpd << " zc=" << old_zc << ")\n";
|
||||
report.axes.push_back({job.axis, true, fit.slope, job.map->zero_count, fit.r2, fit.n});
|
||||
}
|
||||
|
||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||
|
||||
std::string path = paths::writeLogFile(paths::timestampedLogName("calib"), log.str());
|
||||
if (!path.empty()) LOG_INFO << "calibration log written: " << path;
|
||||
|
||||
report.valid = true;
|
||||
report.ts_ms = nowMs();
|
||||
report.all_ok = !report.axes.empty();
|
||||
for (const auto& ax : report.axes) report.all_ok = report.all_ok && ax.ok;
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(result_mutex_);
|
||||
result_ = result;
|
||||
report_ = report;
|
||||
}
|
||||
LOG_INFO << "=== gimbal calibration complete (applied to session) ===";
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -1,7 +1,10 @@
|
|||
#include "fgc/Config.h"
|
||||
#include "fgc/Paths.h"
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
|
||||
extern "C" {
|
||||
|
|
@ -106,8 +109,13 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
|
|||
cfg.features.enable_mqtt = getBool(kv, "Features.enable_mqtt", cfg.features.enable_mqtt);
|
||||
cfg.features.enable_camera = getBool(kv, "Features.enable_camera", cfg.features.enable_camera);
|
||||
cfg.features.enable_serial = getBool(kv, "Features.enable_serial", cfg.features.enable_serial);
|
||||
cfg.features.enable_imu = getBool(kv, "Features.enable_imu", cfg.features.enable_imu);
|
||||
cfg.features.mock_camera = getBool(kv, "Features.mock_camera", cfg.features.mock_camera);
|
||||
cfg.features.mock_serial = getBool(kv, "Features.mock_serial", cfg.features.mock_serial);
|
||||
cfg.features.mock_imu = getBool(kv, "Features.mock_imu", cfg.features.mock_imu);
|
||||
|
||||
cfg.imu.device = get(kv, "IMU.device", cfg.imu.device);
|
||||
cfg.imu.baud = static_cast<unsigned>(getInt(kv, "IMU.baud", cfg.imu.baud));
|
||||
|
||||
cfg.logging.level = get(kv, "Logging.level", cfg.logging.level);
|
||||
cfg.logging.trace = get(kv, "Logging.trace", cfg.logging.trace);
|
||||
|
|
@ -151,4 +159,78 @@ AppConfig ConfigLoader::loadFromFile(const std::string& path) {
|
|||
return fromMap(kv);
|
||||
}
|
||||
|
||||
namespace {
|
||||
std::string trimmed(const std::string& s) {
|
||||
const char* ws = " \t\r\n";
|
||||
size_t a = s.find_first_not_of(ws);
|
||||
if (a == std::string::npos) return "";
|
||||
size_t b = s.find_last_not_of(ws);
|
||||
return s.substr(a, b - a + 1);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
std::string updateIniSectionKeys(const std::string& contents, const std::string& section,
|
||||
const std::vector<std::pair<std::string, std::string>>& kv) {
|
||||
std::vector<bool> written(kv.size(), false);
|
||||
std::istringstream in(contents);
|
||||
std::ostringstream out;
|
||||
std::string line, cur;
|
||||
while (std::getline(in, line)) {
|
||||
const std::string t = trimmed(line);
|
||||
if (t.size() >= 2 && t.front() == '[' && t.back() == ']') {
|
||||
cur = trimmed(t.substr(1, t.size() - 2));
|
||||
} else if (cur == section && !t.empty() && t[0] != ';' && t[0] != '#') {
|
||||
const size_t eq = line.find('=');
|
||||
if (eq != std::string::npos) {
|
||||
const std::string key = trimmed(line.substr(0, eq));
|
||||
for (size_t i = 0; i < kv.size(); ++i) {
|
||||
if (!written[i] && key == kv[i].first) {
|
||||
line = kv[i].first + " = " + kv[i].second;
|
||||
written[i] = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
out << line << "\n";
|
||||
}
|
||||
// Any keys not present in the section get appended in a fresh [section] block
|
||||
// (inih merges duplicate sections on read).
|
||||
bool missing = false;
|
||||
for (bool w : written) missing = missing || !w;
|
||||
if (missing) {
|
||||
out << "\n[" << section << "]\n";
|
||||
for (size_t i = 0; i < kv.size(); ++i)
|
||||
if (!written[i]) out << kv[i].first << " = " << kv[i].second << "\n";
|
||||
}
|
||||
return out.str();
|
||||
}
|
||||
|
||||
bool saveMotorCalibration(const std::string& path, const Geometry& geo) {
|
||||
if (path.empty()) return false;
|
||||
std::ifstream f(path);
|
||||
if (!f) return false;
|
||||
std::stringstream buf;
|
||||
buf << f.rdbuf();
|
||||
f.close();
|
||||
|
||||
auto num = [](double v) {
|
||||
char b[32];
|
||||
std::snprintf(b, sizeof(b), "%.10g", v);
|
||||
return std::string(b);
|
||||
};
|
||||
const std::vector<std::pair<std::string, std::string>> kv = {
|
||||
{"yaw_counts_per_deg", num(geo.yaw.counts_per_deg)},
|
||||
{"yaw_zero_count", std::to_string(geo.yaw.zero_count)},
|
||||
{"pitch_counts_per_deg", num(geo.pitch.counts_per_deg)},
|
||||
{"pitch_zero_count", std::to_string(geo.pitch.zero_count)},
|
||||
};
|
||||
const std::string updated = updateIniSectionKeys(buf.str(), "Motor", kv);
|
||||
|
||||
std::ofstream out(path, std::ios::trunc);
|
||||
if (!out) return false;
|
||||
out << updated;
|
||||
return static_cast<bool>(out);
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
|
|||
|
|
@ -0,0 +1,124 @@
|
|||
#include "fgc/DiagParser.h"
|
||||
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
namespace {
|
||||
|
||||
long toLong(const std::string& s, long fallback = 0) {
|
||||
if (s.empty()) return fallback;
|
||||
try {
|
||||
bool hex = s.size() > 1 && s[0] == '0' && (s[1] == 'x' || s[1] == 'X');
|
||||
return std::stol(s, nullptr, hex ? 16 : 10);
|
||||
} catch (const std::exception&) {
|
||||
return fallback;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool DiagResult::allPass() const {
|
||||
bool any = false;
|
||||
for (const auto& ax : axes) {
|
||||
if (!ax.has_result) continue;
|
||||
any = true;
|
||||
if (!ax.pass) return false;
|
||||
}
|
||||
return any;
|
||||
}
|
||||
|
||||
DiagResult parseDiag(const std::string& block) {
|
||||
DiagResult d;
|
||||
std::istringstream iss(block);
|
||||
std::string line;
|
||||
DiagAxis* cur = nullptr;
|
||||
|
||||
auto axisFor = [&d](char a) -> DiagAxis& {
|
||||
for (auto& ax : d.axes)
|
||||
if (ax.axis == a) return ax;
|
||||
d.axes.push_back(DiagAxis{});
|
||||
d.axes.back().axis = a;
|
||||
return d.axes.back();
|
||||
};
|
||||
|
||||
while (std::getline(iss, line)) {
|
||||
if (!line.empty() && line.back() == '\r') line.pop_back();
|
||||
// Only "DG ..." lines are part of the diag (ST/other lines interleave).
|
||||
auto pos = line.find("DG ");
|
||||
if (pos == std::string::npos) continue;
|
||||
std::istringstream ls(line.substr(pos));
|
||||
std::vector<std::string> tok;
|
||||
std::string t;
|
||||
while (ls >> t) tok.push_back(t);
|
||||
if (tok.size() < 2) continue; // "DG" + something
|
||||
|
||||
d.valid = true;
|
||||
if (tok[1] == "DONE") { d.done = true; continue; }
|
||||
if (tok[1] == "BEGIN" && tok.size() >= 3) { cur = &axisFor(tok[2][0]); continue; }
|
||||
|
||||
// Otherwise tok[1] is the axis letter.
|
||||
char axis = tok[1][0];
|
||||
if (axis != 'Y' && axis != 'P') continue;
|
||||
DiagAxis& ax = axisFor(axis);
|
||||
cur = &ax;
|
||||
|
||||
if (tok.size() >= 4 && tok[2] == "RESULT") {
|
||||
ax.has_result = true;
|
||||
ax.pass = (tok[3] == "PASS");
|
||||
continue;
|
||||
}
|
||||
|
||||
// Test line: "DG <axis> S<speed> <FWD|REV> KEY val KEY val ... <PASS|FAIL>"
|
||||
if (tok.size() < 4 || tok[2].empty() || (tok[2][0] != 'S' && tok[2][0] != 's'))
|
||||
continue;
|
||||
DiagTest test;
|
||||
test.speed = static_cast<int>(toLong(tok[2].substr(1)));
|
||||
test.fwd = (tok[3] != "REV");
|
||||
test.pass = (tok.back() == "PASS");
|
||||
// Remaining KEY value pairs (between dir and the trailing PASS/FAIL).
|
||||
std::map<std::string, std::string> kv;
|
||||
for (size_t i = 4; i + 1 < tok.size(); i += 2) kv[tok[i]] = tok[i + 1];
|
||||
auto g = [&](const char* k) { auto it = kv.find(k); return it == kv.end() ? std::string() : it->second; };
|
||||
test.err_peak = toLong(g("ERR_PEAK"), 0);
|
||||
test.err_rms = toLong(g("ERR_RMS"), 0);
|
||||
test.err_still = toLong(g("ERR_STILL"), 0);
|
||||
test.cs_min = static_cast<int>(toLong(g("CS_MIN")));
|
||||
test.cs_max = static_cast<int>(toLong(g("CS_MAX")));
|
||||
test.sg_min = static_cast<int>(toLong(g("SG_MIN")));
|
||||
test.pwm_avg = static_cast<int>(toLong(g("PWM_AVG")));
|
||||
test.flags = static_cast<unsigned>(toLong(g("FLAGS")));
|
||||
ax.tests.push_back(test);
|
||||
}
|
||||
return d;
|
||||
}
|
||||
|
||||
std::vector<std::string> formatDiag(const DiagResult& d) {
|
||||
std::vector<std::string> out;
|
||||
if (!d.valid) {
|
||||
out.emplace_back("(no diagnostic data)");
|
||||
return out;
|
||||
}
|
||||
for (const auto& ax : d.axes) {
|
||||
std::ostringstream hdr;
|
||||
hdr << "[" << ax.axis << "] " << ax.tests.size() << " tests RESULT="
|
||||
<< (ax.has_result ? (ax.pass ? "PASS" : "FAIL") : "?");
|
||||
out.push_back(hdr.str());
|
||||
for (const auto& t : ax.tests) {
|
||||
std::ostringstream os;
|
||||
os << " S" << t.speed << " " << (t.fwd ? "FWD" : "REV")
|
||||
<< " err_peak=" << t.err_peak << " err_rms=" << t.err_rms
|
||||
<< " err_still=" << t.err_still << " cs=" << t.cs_min << ".." << t.cs_max
|
||||
<< " sg_min=" << t.sg_min << " pwm=" << t.pwm_avg
|
||||
<< " flags=0x" << std::hex << t.flags << std::dec
|
||||
<< " " << (t.pass ? "PASS" : "FAIL");
|
||||
out.push_back(os.str());
|
||||
}
|
||||
}
|
||||
out.push_back(std::string("overall: ") + (d.allPass() ? "PASS" : "FAIL") +
|
||||
(d.done ? "" : " (incomplete - no DG DONE)"));
|
||||
return out;
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -24,37 +24,45 @@ std::string verbOf(const std::string& syntax) {
|
|||
const std::vector<HelpSection>& helpCatalog() {
|
||||
// clang-format off
|
||||
static const std::vector<HelpSection> catalog = {
|
||||
{"Positioning", "Aim the gimbal. 'goto' is degrees; raw MOVE is encoder counts.", {
|
||||
{"goto <yaw_deg> <pitch_deg>",
|
||||
"Point the gimbal at an absolute heading/elevation in degrees.", {
|
||||
{"Positioning", "Aim the gimbal. 'move' is degrees; 'steps' is raw encoder counts.", {
|
||||
{"gimbal move <yaw>,<pitch>",
|
||||
"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 <yaw>,<pitch>",
|
||||
"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 <yaw>,<pitch>",
|
||||
"Move both axes to absolute encoder COUNTS (no degree conversion).", {
|
||||
"Example: gimbal steps 100000,250000"}},
|
||||
{"gimbal nudge <yaw|pitch> <+/-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 <Y|P|ALL>",
|
||||
"Stop motion immediately on an axis or both.", {}},
|
||||
{"set motorctl SPEED <Y|P> <vel>",
|
||||
"Example: gimbal home / gimbal home y"}},
|
||||
{"gimbal stop [y|p|all]",
|
||||
"Stop motion immediately (also cancels a running calibration).", {}},
|
||||
{"gimbal speed <y|p> <vel>",
|
||||
"Set the max slew speed (counts/s) for an axis.", {}},
|
||||
{"gimbal reset [y|p] / gimbal enable|disable <y|p> / gimbal setpos <y|p> <v>",
|
||||
"Other firmware motor controls.", {}},
|
||||
}},
|
||||
{"Diagnostics", "Inspect the firmware/driver state for debugging.", {
|
||||
{"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.", {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,123 @@
|
|||
#include "fgc/MtiProtocol.h"
|
||||
|
||||
#include <cstring>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
namespace {
|
||||
|
||||
// Big-endian readers (MTi is big-endian; host x86 is little-endian).
|
||||
float beFloat(const uint8_t* p) {
|
||||
uint32_t u = (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) |
|
||||
(uint32_t(p[2]) << 8) | uint32_t(p[3]);
|
||||
float f;
|
||||
std::memcpy(&f, &u, sizeof(f));
|
||||
return f;
|
||||
}
|
||||
|
||||
uint16_t beU16(const uint8_t* p) {
|
||||
return static_cast<uint16_t>((uint16_t(p[0]) << 8) | uint16_t(p[1]));
|
||||
}
|
||||
|
||||
// Append a big-endian value to a byte vector.
|
||||
void putBE(std::vector<uint8_t>& v, uint16_t x) {
|
||||
v.push_back(static_cast<uint8_t>(x >> 8));
|
||||
v.push_back(static_cast<uint8_t>(x & 0xFF));
|
||||
}
|
||||
void putBE(std::vector<uint8_t>& v, uint32_t x) {
|
||||
v.push_back(static_cast<uint8_t>((x >> 24) & 0xFF));
|
||||
v.push_back(static_cast<uint8_t>((x >> 16) & 0xFF));
|
||||
v.push_back(static_cast<uint8_t>((x >> 8) & 0xFF));
|
||||
v.push_back(static_cast<uint8_t>(x & 0xFF));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len) {
|
||||
unsigned sum = 0;
|
||||
for (std::size_t i = 0; i < len; ++i) sum += from_bid[i];
|
||||
return static_cast<uint8_t>(sum & 0xFF);
|
||||
}
|
||||
|
||||
std::vector<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data) {
|
||||
std::vector<uint8_t> m;
|
||||
m.reserve(5 + data.size());
|
||||
m.push_back(kMtiPreamble);
|
||||
m.push_back(kMtiBid);
|
||||
m.push_back(mid);
|
||||
m.push_back(static_cast<uint8_t>(data.size()));
|
||||
m.insert(m.end(), data.begin(), data.end());
|
||||
// Checksum covers BID..DATA; the CS byte makes the total ≡ 0 (mod 256).
|
||||
uint8_t s = mtiChecksum(m.data() + 1, m.size() - 1);
|
||||
m.push_back(static_cast<uint8_t>((0x100 - s) & 0xFF));
|
||||
return m;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> msgGoToConfig() { return mtiMessage(kMidGoToConfig); }
|
||||
std::vector<uint8_t> msgGoToMeasurement() { return mtiMessage(kMidGoToMeasurement); }
|
||||
|
||||
std::vector<uint8_t> msgSetOutputMode() {
|
||||
std::vector<uint8_t> d;
|
||||
putBE(d, kOutputMode);
|
||||
return mtiMessage(kMidSetOutputMode, d);
|
||||
}
|
||||
|
||||
std::vector<uint8_t> msgSetOutputSettings() {
|
||||
std::vector<uint8_t> d;
|
||||
putBE(d, kOutputSettings);
|
||||
return mtiMessage(kMidSetOutputSettings, d);
|
||||
}
|
||||
|
||||
std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) {
|
||||
if (mid != kMidMTData || len != kMTDataLen) return std::nullopt;
|
||||
ImuSample s;
|
||||
std::size_t o = 0;
|
||||
s.temp_c = beFloat(data + o); o += 4;
|
||||
for (int i = 0; i < 3; ++i) { s.acc[i] = beFloat(data + o); o += 4; }
|
||||
for (int i = 0; i < 3; ++i) { s.gyr[i] = beFloat(data + o); o += 4; }
|
||||
for (int i = 0; i < 3; ++i) { s.mag[i] = beFloat(data + o); o += 4; }
|
||||
s.roll_deg = beFloat(data + o); o += 4;
|
||||
s.pitch_deg = beFloat(data + o); o += 4;
|
||||
s.yaw_deg = beFloat(data + o); o += 4;
|
||||
// Report yaw as a 0..360 heading rather than the MTi's native -180..180.
|
||||
if (s.yaw_deg < 0.f) s.yaw_deg += 360.f;
|
||||
s.sample_counter = beU16(data + o); o += 2;
|
||||
s.valid = true;
|
||||
return s;
|
||||
}
|
||||
|
||||
void MtiFramer::feed(const uint8_t* p, std::size_t n) {
|
||||
for (std::size_t i = 0; i < n; ++i) {
|
||||
uint8_t b = p[i];
|
||||
switch (state_) {
|
||||
case S::Pre:
|
||||
if (b == kMtiPreamble) state_ = S::Bid;
|
||||
break;
|
||||
case S::Bid:
|
||||
// After PRE we expect BID; otherwise resync (allow back-to-back PRE).
|
||||
if (b == kMtiBid) { sum_ = b; state_ = S::Mid; }
|
||||
else if (b == kMtiPreamble) { /* stay */ }
|
||||
else state_ = S::Pre;
|
||||
break;
|
||||
case S::Mid:
|
||||
mid_ = b; sum_ += b; state_ = S::Len;
|
||||
break;
|
||||
case S::Len:
|
||||
len_ = b; sum_ += b; data_.clear();
|
||||
state_ = (len_ == 0) ? S::Cs : S::Data;
|
||||
break;
|
||||
case S::Data:
|
||||
data_.push_back(b); sum_ += b;
|
||||
if (data_.size() == len_) state_ = S::Cs;
|
||||
break;
|
||||
case S::Cs:
|
||||
sum_ += b;
|
||||
if ((sum_ & 0xFF) == 0 && sink_)
|
||||
sink_(mid_, data_.data(), data_.size());
|
||||
state_ = S::Pre;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -1,7 +1,12 @@
|
|||
#include "fgc/Paths.h"
|
||||
|
||||
#include "fgc/Logger.h"
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
#include <filesystem>
|
||||
#include <system_error>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace fs = std::filesystem;
|
||||
|
|
@ -92,4 +97,37 @@ std::string defaultOutputDir() {
|
|||
return (fs::path(base) / "fire_gimbal_control" / "images").string();
|
||||
}
|
||||
|
||||
std::string defaultLogDir() {
|
||||
std::string base = envOr("XDG_DATA_HOME", expandUser("~/.local/share"));
|
||||
return (fs::path(base) / "fire_gimbal_control" / "logs").string();
|
||||
}
|
||||
|
||||
std::string timestampedLogName(const std::string& prefix) {
|
||||
std::time_t t = std::time(nullptr);
|
||||
std::tm tm{};
|
||||
localtime_r(&t, &tm);
|
||||
char buf[32];
|
||||
std::strftime(buf, sizeof(buf), "%Y%m%d-%H%M%S", &tm);
|
||||
return prefix + "_" + buf + ".log";
|
||||
}
|
||||
|
||||
std::string writeLogFile(const std::string& name, const std::string& text) {
|
||||
std::error_code ec;
|
||||
fs::path dir = defaultLogDir();
|
||||
fs::create_directories(dir, ec);
|
||||
fs::path path = dir / name;
|
||||
FILE* f = std::fopen(path.c_str(), "wb");
|
||||
if (!f) {
|
||||
LOG_WARN << "could not open logfile " << path.string() << " for writing";
|
||||
return "";
|
||||
}
|
||||
bool ok = std::fwrite(text.data(), 1, text.size(), f) == text.size();
|
||||
if (std::fclose(f) != 0) ok = false;
|
||||
if (!ok) {
|
||||
LOG_WARN << "failed to write logfile " << path.string();
|
||||
return "";
|
||||
}
|
||||
return path.string();
|
||||
}
|
||||
|
||||
} // namespace fgc::paths
|
||||
|
|
|
|||
|
|
@ -0,0 +1,139 @@
|
|||
#include "fgc/MtiImuSource.h"
|
||||
|
||||
#include "fgc/Logger.h"
|
||||
#include "fgc/MtiProtocol.h"
|
||||
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include <boost/asio.hpp>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
struct MtiImuSource::Impl {
|
||||
Impl(std::string dev, unsigned int b)
|
||||
: device(std::move(dev)), baud(b), serial(io),
|
||||
framer([this](uint8_t mid, const uint8_t* d, std::size_t n) { onFrame(mid, d, n); }) {}
|
||||
|
||||
using clock = std::chrono::steady_clock;
|
||||
|
||||
std::string device;
|
||||
unsigned int baud;
|
||||
boost::asio::io_context io;
|
||||
boost::asio::serial_port serial;
|
||||
std::thread io_thread;
|
||||
std::array<uint8_t, 256> rxbuf{};
|
||||
|
||||
std::mutex mutex;
|
||||
ImuSample latest;
|
||||
clock::time_point last_rx{};
|
||||
std::atomic<bool> open{false};
|
||||
|
||||
MtiFramer framer;
|
||||
unsigned bad_len_warned = 0;
|
||||
|
||||
void onFrame(uint8_t mid, const uint8_t* d, std::size_t n) {
|
||||
if (mid != kMidMTData) return; // ignore acks/other during streaming
|
||||
auto s = parseMTData(mid, d, n);
|
||||
if (!s) {
|
||||
if (bad_len_warned++ < 1)
|
||||
LOG_WARN << "MTi: unexpected MTData length " << n << " (expected "
|
||||
<< int(kMTDataLen) << "); device may not be in the configured mode";
|
||||
return;
|
||||
}
|
||||
std::lock_guard<std::mutex> lock(mutex);
|
||||
latest = *s;
|
||||
last_rx = clock::now();
|
||||
}
|
||||
|
||||
void doRead() {
|
||||
serial.async_read_some(
|
||||
boost::asio::buffer(rxbuf),
|
||||
[this](const boost::system::error_code& ec, std::size_t n) {
|
||||
if (ec) {
|
||||
if (ec != boost::asio::error::operation_aborted)
|
||||
LOG_WARN << "MTi serial read failed: " << ec.message();
|
||||
return;
|
||||
}
|
||||
framer.feed(rxbuf.data(), n);
|
||||
doRead();
|
||||
});
|
||||
}
|
||||
|
||||
// Synchronous, best-effort config handshake (no ack parsing): runs on the
|
||||
// calling thread BEFORE the io_thread starts, so there is no concurrent
|
||||
// access to the serial port. Small delays let the device switch states.
|
||||
void configure() {
|
||||
using namespace std::chrono_literals;
|
||||
auto write = [this](const std::vector<uint8_t>& m) {
|
||||
boost::system::error_code ec;
|
||||
boost::asio::write(serial, boost::asio::buffer(m), ec);
|
||||
if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
|
||||
};
|
||||
write(msgGoToConfig()); std::this_thread::sleep_for(60ms);
|
||||
write(msgSetOutputMode()); std::this_thread::sleep_for(60ms);
|
||||
write(msgSetOutputSettings()); std::this_thread::sleep_for(60ms);
|
||||
write(msgGoToMeasurement()); std::this_thread::sleep_for(60ms);
|
||||
// Drop any pre-config (old-format) bytes so the framer starts clean.
|
||||
::tcflush(serial.native_handle(), TCIFLUSH);
|
||||
}
|
||||
};
|
||||
|
||||
MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
|
||||
: impl_(std::make_unique<Impl>(std::move(device), baud)) {}
|
||||
|
||||
MtiImuSource::~MtiImuSource() { stop(); }
|
||||
|
||||
void MtiImuSource::start() {
|
||||
namespace asio = boost::asio;
|
||||
boost::system::error_code ec;
|
||||
impl_->serial.open(impl_->device, ec);
|
||||
if (ec) {
|
||||
LOG_ERROR << "MTi: failed to open " << impl_->device << ": " << ec.message();
|
||||
return;
|
||||
}
|
||||
impl_->serial.set_option(asio::serial_port_base::baud_rate(impl_->baud));
|
||||
impl_->serial.set_option(asio::serial_port_base::character_size(8));
|
||||
impl_->serial.set_option(asio::serial_port_base::parity(asio::serial_port_base::parity::none));
|
||||
impl_->serial.set_option(
|
||||
asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
|
||||
impl_->serial.set_option(
|
||||
asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));
|
||||
|
||||
impl_->open = true;
|
||||
impl_->configure();
|
||||
impl_->doRead();
|
||||
impl_->io_thread = std::thread([this] { impl_->io.run(); });
|
||||
LOG_INFO << "MTi IMU started on " << impl_->device << " @ " << impl_->baud;
|
||||
}
|
||||
|
||||
void MtiImuSource::stop() {
|
||||
if (!impl_) return;
|
||||
impl_->io.stop();
|
||||
if (impl_->io_thread.joinable()) impl_->io_thread.join();
|
||||
boost::system::error_code ec;
|
||||
if (impl_->serial.is_open()) impl_->serial.close(ec);
|
||||
impl_->open = false;
|
||||
}
|
||||
|
||||
bool MtiImuSource::connected() const {
|
||||
if (!impl_->open) return false;
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
return impl_->latest.valid &&
|
||||
(Impl::clock::now() - impl_->last_rx) < std::chrono::seconds(1);
|
||||
}
|
||||
|
||||
std::optional<ImuSample> MtiImuSource::sample() {
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
if (!impl_->latest.valid ||
|
||||
(Impl::clock::now() - impl_->last_rx) >= std::chrono::seconds(1))
|
||||
return std::nullopt;
|
||||
return impl_->latest;
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -70,6 +70,14 @@ struct SerialMotorController::Impl {
|
|||
std::string dump_buf;
|
||||
std::string latest_dump;
|
||||
|
||||
// DIAG capture: the firmware streams "DG ..." lines (interleaved with ST)
|
||||
// from "DG BEGIN" to "DG DONE". We accumulate the DG lines, log each one live
|
||||
// (so it appears in the LOG pane), and publish the completed block.
|
||||
bool diagging = false;
|
||||
std::string diag_buf;
|
||||
std::string latest_diag;
|
||||
std::atomic<unsigned> diag_seq{0};
|
||||
|
||||
// Write one command line (newline-terminated) to the controller. Used by
|
||||
// sendCommand and by the internal DUMP re-request.
|
||||
//
|
||||
|
|
@ -132,6 +140,30 @@ struct SerialMotorController::Impl {
|
|||
// OK acks and other async output (DG/DUMP/BOOT/...).
|
||||
LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
|
||||
captureDump(line);
|
||||
captureDiag(line);
|
||||
}
|
||||
}
|
||||
|
||||
// Assemble the DIAG stream. DG lines arrive interleaved with ST over many
|
||||
// seconds, so we accumulate only "DG " lines and log each live; on "DG DONE"
|
||||
// we publish the block and bump diag_seq so the app can pick up the result.
|
||||
void captureDiag(const std::string& line) {
|
||||
const bool begin = line.find("DG BEGIN") != std::string::npos;
|
||||
// Start on the FIRST BEGIN only; a per-axis "DG BEGIN P" mid-run must not
|
||||
// wipe the already-captured first axis.
|
||||
if (begin && !diagging) { diagging = true; diag_buf.clear(); }
|
||||
if (!diagging) return;
|
||||
if (line.rfind("DG ", 0) != 0 && !begin) return; // ignore non-DG lines
|
||||
diag_buf += line + "\n";
|
||||
LOG_INFO << line; // stream to the LOG pane
|
||||
if (line.find("DG DONE") != std::string::npos) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex);
|
||||
latest_diag = diag_buf;
|
||||
}
|
||||
++diag_seq;
|
||||
diagging = false;
|
||||
diag_buf.clear();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -242,6 +274,13 @@ std::string SerialMotorController::lastDump() {
|
|||
return impl_->latest_dump;
|
||||
}
|
||||
|
||||
std::string SerialMotorController::lastDiag() {
|
||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
||||
return impl_->latest_diag;
|
||||
}
|
||||
|
||||
unsigned SerialMotorController::diagSeq() const { return impl_->diag_seq.load(); }
|
||||
|
||||
bool SerialMotorController::connected() const { return impl_->connected; }
|
||||
|
||||
} // namespace fgc
|
||||
|
|
|
|||
191
src/ui/TuiUi.cpp
191
src/ui/TuiUi.cpp
|
|
@ -77,19 +77,30 @@ Element gimbalPanel(const GimbalView& g) {
|
|||
rows.push_back(separator());
|
||||
rows.push_back(axisRow(g.pitch));
|
||||
}
|
||||
return panel("GIMBAL", Color::Cyan, vbox(std::move(rows)));
|
||||
return panel("[g] GIMBAL", Color::Cyan, vbox(std::move(rows)));
|
||||
}
|
||||
|
||||
Element sensorsPanel(const SensorsView& s) {
|
||||
std::vector<Element> rows;
|
||||
rows.push_back(text("pending integration") | 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<Element> 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) {
|
||||
|
|
@ -151,6 +162,32 @@ Element logPanel(const std::vector<LogLine>& lines) {
|
|||
vbox(std::move(rows)) | focusPositionRelative(0, 1) | yframe);
|
||||
}
|
||||
|
||||
// Compact strip below the log: the running special op (live) + the last
|
||||
// calibration/diagnostics result (persists so it doesn't scroll away).
|
||||
Element activityPanel(const ActivityView& a) {
|
||||
std::vector<Element> rows;
|
||||
if (a.active) {
|
||||
rows.push_back(hbox({
|
||||
text(" \xE2\x96\xB6 ") | color(Color::Cyan) | bold,
|
||||
text(a.title + ": ") | bold,
|
||||
text(a.status) | color(Color::Cyan),
|
||||
}));
|
||||
} else if (a.has_result) {
|
||||
rows.push_back(text(" idle") | dim);
|
||||
}
|
||||
if (a.has_result) {
|
||||
rows.push_back(hbox({text("last: ") | dim,
|
||||
text(a.result_title) | color(toColor(a.result_color)) | bold}));
|
||||
for (const auto& l : a.result)
|
||||
rows.push_back(text(" " + l) | color(toColor(a.result_color)));
|
||||
}
|
||||
if (!a.prompt.empty()) {
|
||||
rows.push_back(hbox({text(" " + a.prompt + " ") | bold | color(Color::Black) |
|
||||
bgcolor(Color::Yellow)}));
|
||||
}
|
||||
return window(text(" ACTIVITY ") | bold | color(Color::Cyan), vbox(std::move(rows)));
|
||||
}
|
||||
|
||||
// Inline help pane (toggled with '?'). Lists every command section; the
|
||||
// `sel`-th section is expanded to show each entry's detail. The Diagnostics
|
||||
// section additionally renders the last captured firmware DUMP block.
|
||||
|
|
@ -281,7 +318,12 @@ Element axisDumpDetail(const DumpAxis& ax) {
|
|||
// registers. Rendered per axis so the two axes sit side by side and every row
|
||||
// (incl. RAMP_STAT, the last one) is visible without scrolling.
|
||||
Element axisColumn(const std::string& title, const AxisView& live, const DumpData& d,
|
||||
char letter) {
|
||||
char letter, bool calib_has, const CalibAxisView& cal) {
|
||||
auto fmt = [](const char* f, double v) {
|
||||
char b[32];
|
||||
std::snprintf(b, sizeof(b), f, v);
|
||||
return std::string(b);
|
||||
};
|
||||
std::vector<Element> col;
|
||||
col.push_back(axisLiveDetail(live));
|
||||
col.push_back(separator());
|
||||
|
|
@ -296,20 +338,39 @@ Element axisColumn(const std::string& title, const AxisView& live, const DumpDat
|
|||
col.push_back(text(d.valid ? "(axis absent from dump)"
|
||||
: "(awaiting dump - press 'd')") | dim);
|
||||
|
||||
// Last calibration result for this axis.
|
||||
col.push_back(separator());
|
||||
col.push_back(text("CALIBRATION") | bold | color(Color::Magenta));
|
||||
if (calib_has && cal.ok) {
|
||||
col.push_back(kvRow("cnt/deg", fmt("%.3f", cal.counts_per_deg)));
|
||||
col.push_back(kvRow("zero", std::to_string(cal.zero_count)));
|
||||
col.push_back(kvRow("R2", fmt("%.4f", cal.r2), cal.r2 >= 0.99 ? Color::Green : Color::Yellow));
|
||||
col.push_back(kvRow("points", std::to_string(cal.n)));
|
||||
} else {
|
||||
col.push_back(text(calib_has ? "fit failed" : "uncalibrated this session") | dim);
|
||||
}
|
||||
|
||||
return panel(title, Color::Cyan, vbox(std::move(col)) | yframe);
|
||||
}
|
||||
|
||||
// Full-screen gimbal view (toggled with 'g'): one column per axis, each with
|
||||
// live telemetry above its decoded firmware register dump.
|
||||
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::milliseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
calib_note = "calibrated " + formatTimeAgo(now, calib.ts_ms);
|
||||
}
|
||||
Element header = hbox({
|
||||
(g.present ? text(" link up ") | color(Color::Green)
|
||||
: text(" link down ") | color(Color::Red) | bold),
|
||||
text(" " + calib_note) | color(calib.has ? Color::Magenta : Color::GrayDark),
|
||||
filler(),
|
||||
text(d.valid ? ("build " + d.build + " up " + std::to_string(d.uptime_ms) +
|
||||
"ms reset:" + reset)
|
||||
|
|
@ -317,13 +378,64 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump) {
|
|||
});
|
||||
|
||||
std::vector<Element> cols;
|
||||
cols.push_back(axisColumn("YAW", g.yaw, d, 'Y'));
|
||||
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}));
|
||||
}
|
||||
|
||||
// Full-screen IMU view (toggled with 'i'): every MTi channel with units.
|
||||
Element imuDetailPanel(const ImuView& v) {
|
||||
auto f2 = [](float x) {
|
||||
char b[24];
|
||||
std::snprintf(b, sizeof(b), "%.2f", x);
|
||||
return std::string(b);
|
||||
};
|
||||
// One "LABEL (unit) x=.. y=.. z=.." row for a 3-vector.
|
||||
auto vecRow = [&](const std::string& label, const char* unit, const float xyz[3],
|
||||
Color c = Color::Default) {
|
||||
return hbox({
|
||||
text(label) | dim | size(WIDTH, EQUAL, 14),
|
||||
text(std::string(unit)) | dim | size(WIDTH, EQUAL, 9),
|
||||
text("x " + f2(xyz[0])) | color(c) | size(WIDTH, EQUAL, 12),
|
||||
text("y " + f2(xyz[1])) | color(c) | size(WIDTH, EQUAL, 12),
|
||||
text("z " + f2(xyz[2])) | color(c) | size(WIDTH, EQUAL, 12),
|
||||
});
|
||||
};
|
||||
const float ori[3] = {v.roll_deg, v.pitch_deg, v.yaw_deg};
|
||||
|
||||
std::vector<Element> body;
|
||||
if (!v.present) {
|
||||
body.push_back(text("(no IMU data)") | color(Color::Red) | bold);
|
||||
body.push_back(text("check [Features] enable_imu and [IMU] device, or --mock-imu") | dim);
|
||||
} else {
|
||||
body.push_back(hbox({
|
||||
text("ORIENTATION") | dim | size(WIDTH, EQUAL, 14),
|
||||
text("deg") | dim | size(WIDTH, EQUAL, 9),
|
||||
text("roll " + f2(ori[0])) | bold | size(WIDTH, EQUAL, 14),
|
||||
text("pitch " + f2(ori[1])) | bold | size(WIDTH, EQUAL, 14),
|
||||
text("yaw " + f2(ori[2])) | bold | size(WIDTH, EQUAL, 14),
|
||||
}));
|
||||
body.push_back(separator());
|
||||
body.push_back(vecRow("ACCEL", "m/s2", v.acc, Color::Cyan));
|
||||
body.push_back(vecRow("RATE OF TURN", "rad/s", v.gyr, Color::Cyan));
|
||||
body.push_back(vecRow("MAG FIELD", "a.u.", v.mag, Color::Cyan));
|
||||
body.push_back(separator());
|
||||
body.push_back(hbox({
|
||||
text("TEMP") | dim | size(WIDTH, EQUAL, 14),
|
||||
text(f2(v.temp_c) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18),
|
||||
text("sample #" + std::to_string(v.sample_counter)) | dim,
|
||||
}));
|
||||
}
|
||||
|
||||
Element status = v.present ? (text(" MTi live ") | color(Color::Green) | bold)
|
||||
: (text(" MTi offline ") | color(Color::Red) | bold);
|
||||
return window(text(" IMU (i/Esc:close) ") | bold | color(Color::Magenta),
|
||||
vbox({hbox({status, filler()}), separator(),
|
||||
vbox(std::move(body)) | flex}));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TuiUi::TuiUi() = default;
|
||||
|
|
@ -368,10 +480,11 @@ void TuiUi::refreshLoop() {
|
|||
void TuiUi::uiLoop() {
|
||||
std::string cmd_buffer;
|
||||
bool command_mode = false;
|
||||
enum class Overlay { None, Help, Gimbal };
|
||||
enum class Overlay { None, Help, Gimbal, Sensors };
|
||||
Overlay overlay = Overlay::None; // which takeover panel owns the main area
|
||||
int help_sel = 0;
|
||||
bool gimbal_dump_requested = false; // auto-pull a dump the first time
|
||||
bool calib_prompt = false; // a yes/no calib-save question is showing
|
||||
|
||||
auto input = Input(&cmd_buffer, "type a command, Enter to run, Esc to cancel");
|
||||
|
||||
|
|
@ -381,6 +494,7 @@ void TuiUi::uiLoop() {
|
|||
std::lock_guard<std::mutex> lock(log_mutex_);
|
||||
s.log.assign(log_.begin(), log_.end());
|
||||
}
|
||||
calib_prompt = !s.activity.prompt.empty();
|
||||
|
||||
std::string mode = s.header.live ? "LIVE" : "MOCK";
|
||||
Element header = hbox({
|
||||
|
|
@ -401,8 +515,8 @@ void TuiUi::uiLoop() {
|
|||
} else {
|
||||
bottom = hbox({
|
||||
keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"),
|
||||
keyHint("r", "Reset"), keyHint("g", "Gimbal"), keyHint(":", "Cmd"),
|
||||
keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
|
||||
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"),
|
||||
});
|
||||
}
|
||||
|
||||
|
|
@ -414,9 +528,19 @@ void TuiUi::uiLoop() {
|
|||
return vbox({header, separator(), helpPanel(help_sel, s.dump) | flex, bottom});
|
||||
case Overlay::Gimbal:
|
||||
return vbox({header, separator(),
|
||||
gimbalDetailPanel(s.gimbal, s.dump) | flex, bottom});
|
||||
default:
|
||||
return vbox({header, separator(), top, middle, logPanel(s.log) | flex, bottom});
|
||||
gimbalDetailPanel(s.gimbal, s.dump, s.calib) | flex, bottom});
|
||||
case Overlay::Sensors:
|
||||
return vbox({header, separator(), imuDetailPanel(s.imu) | flex, bottom});
|
||||
default: {
|
||||
// Activity strip sits between the log and the key bar; shown only
|
||||
// once a special op has run or is running, else it costs no space.
|
||||
std::vector<Element> col = {header, separator(), top, middle,
|
||||
logPanel(s.log) | flex};
|
||||
if (s.activity.active || s.activity.has_result || !s.activity.prompt.empty())
|
||||
col.push_back(activityPanel(s.activity));
|
||||
col.push_back(bottom);
|
||||
return vbox(std::move(col));
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
|
|
@ -441,8 +565,21 @@ void TuiUi::uiLoop() {
|
|||
if (e == Event::ArrowUp) { help_sel = (help_sel - 1 + n) % n; return true; }
|
||||
}
|
||||
if (overlay != Overlay::None && e == Event::Escape) { overlay = Overlay::None; return true; }
|
||||
// Arrow keys nudge the gimbal in steps (only when no overlay is open):
|
||||
// Left/Right = yaw -/+5%, Up/Down = pitch +/-10% of travel.
|
||||
if (overlay == Overlay::None && sink_) {
|
||||
if (e == Event::ArrowLeft) { sink_("gimbal nudge yaw -5"); return true; }
|
||||
if (e == Event::ArrowRight) { sink_("gimbal nudge yaw 5"); return true; }
|
||||
if (e == Event::ArrowUp) { sink_("gimbal nudge pitch 10"); return true; }
|
||||
if (e == Event::ArrowDown) { sink_("gimbal nudge pitch -10"); return true; }
|
||||
}
|
||||
if (!e.is_character()) return false;
|
||||
const std::string& c = e.character();
|
||||
// Answer the activity-strip "save calibration?" yes/no prompt.
|
||||
if (calib_prompt && overlay == Overlay::None && sink_) {
|
||||
if (c == "y" || c == "Y") { sink_("calib save"); return true; }
|
||||
if (c == "n" || c == "N") { sink_("calib discard"); return true; }
|
||||
}
|
||||
if (c == "?") {
|
||||
overlay = (overlay == Overlay::Help) ? Overlay::None : Overlay::Help;
|
||||
return true;
|
||||
|
|
@ -453,14 +590,18 @@ void TuiUi::uiLoop() {
|
|||
} else {
|
||||
overlay = Overlay::Gimbal;
|
||||
if (!gimbal_dump_requested) { // auto-pull a dump the first time
|
||||
if (sink_) sink_("dump");
|
||||
if (sink_) sink_("gimbal dump");
|
||||
gimbal_dump_requested = true;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
if (overlay == Overlay::Gimbal && c == "d") { // manual dump refresh
|
||||
if (sink_) sink_("dump");
|
||||
if (sink_) sink_("gimbal dump");
|
||||
return true;
|
||||
}
|
||||
if (c == "i") {
|
||||
overlay = (overlay == Overlay::Sensors) ? Overlay::None : Overlay::Sensors;
|
||||
return true;
|
||||
}
|
||||
if (overlay == Overlay::Help) { // vim-style section nav while help is open
|
||||
|
|
@ -470,8 +611,8 @@ void TuiUi::uiLoop() {
|
|||
if (c == "q") { if (sink_) sink_("exit"); return true; }
|
||||
if (c == "s") { if (sink_) sink_("start"); return true; }
|
||||
if (c == "x") { if (sink_) sink_("stop"); return true; }
|
||||
if (c == "h") { if (sink_) sink_("set motorctl HOME"); return true; }
|
||||
if (c == "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;
|
||||
});
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -28,6 +28,11 @@ add_executable(fgc_tests
|
|||
test_geometry.cpp
|
||||
test_scangrid.cpp
|
||||
test_uisnapshot.cpp
|
||||
test_mtiprotocol.cpp
|
||||
test_dumpparser.cpp
|
||||
test_helptext.cpp
|
||||
test_diagparser.cpp
|
||||
test_calibration.cpp
|
||||
)
|
||||
target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,73 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/Calibration.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace fgc;
|
||||
|
||||
TEST_CASE("linearFit recovers slope/intercept exactly for collinear points") {
|
||||
// counts = 983.33*deg + 500000 (a yaw-like calibration)
|
||||
std::vector<std::pair<double, double>> xy;
|
||||
for (int deg = -45; deg <= 45; deg += 10)
|
||||
xy.emplace_back(deg, 983.33 * deg + 500000.0);
|
||||
|
||||
LinearFit f = linearFit(xy);
|
||||
REQUIRE(f.ok);
|
||||
CHECK(f.slope == doctest::Approx(983.33));
|
||||
CHECK(f.intercept == doctest::Approx(500000.0));
|
||||
CHECK(f.r2 == doctest::Approx(1.0));
|
||||
}
|
||||
|
||||
TEST_CASE("linearFit is robust to noise (slope close, r2 high)") {
|
||||
std::vector<std::pair<double, double>> xy = {
|
||||
{-40, -39500}, {-20, -20100}, {0, 300}, {20, 19800}, {40, 40200}};
|
||||
LinearFit f = linearFit(xy);
|
||||
REQUIRE(f.ok);
|
||||
CHECK(f.slope == doctest::Approx(1000.0).epsilon(0.05));
|
||||
CHECK(f.r2 > 0.99);
|
||||
}
|
||||
|
||||
TEST_CASE("linearFit rejects degenerate input") {
|
||||
CHECK_FALSE(linearFit({}).ok);
|
||||
CHECK_FALSE(linearFit({{1.0, 2.0}}).ok); // single point
|
||||
CHECK_FALSE(linearFit({{5.0, 1.0}, {5.0, 9.0}}).ok); // no x spread
|
||||
}
|
||||
|
||||
TEST_CASE("circularMeanDeg handles the +/-180 wrap") {
|
||||
CHECK(circularMeanDeg({10, 20, 30}) == doctest::Approx(20.0));
|
||||
// Mean of 170 and -170 is 180 (not 0) — must not average naively to 0.
|
||||
CHECK(std::abs(circularMeanDeg({170, -170})) == doctest::Approx(180.0));
|
||||
CHECK(circularMeanDeg({}) == doctest::Approx(0.0));
|
||||
}
|
||||
|
||||
TEST_CASE("unwrapNear shifts an angle to within +/-180 of the previous sample") {
|
||||
CHECK(unwrapNear(10.0, 20.0) == doctest::Approx(20.0)); // already near
|
||||
CHECK(unwrapNear(350.0, 355.0) == doctest::Approx(355.0));
|
||||
CHECK(unwrapNear(350.0, 5.0) == doctest::Approx(365.0)); // crossed 360 going up
|
||||
CHECK(unwrapNear(10.0, 355.0) == doctest::Approx(-5.0)); // crossed 0 going down
|
||||
CHECK(unwrapNear(720.0, 10.0) == doctest::Approx(730.0)); // multiple turns up
|
||||
CHECK(unwrapNear(-360.0, 10.0) == doctest::Approx(-350.0)); // multiple turns down
|
||||
}
|
||||
|
||||
TEST_CASE("unwrapping a 0..360 sweep across the wrap yields a clean linear fit") {
|
||||
// A yaw sweep whose true (unwrapped) heading is 300..420 deg, but the IMU
|
||||
// reports it wrapped into 0..360. With per-sample unwrapNear the fit recovers
|
||||
// the straight line; without it the 360->0 jump would wreck R^2 (the bug from
|
||||
// the field calibration log).
|
||||
std::vector<std::pair<double, double>> pts; // (deg, counts), counts = 100*heading
|
||||
double prev = 0.0;
|
||||
bool have = false;
|
||||
for (int i = 0; i <= 12; ++i) {
|
||||
const double heading = 300.0 + i * 10.0; // 300..420 (true)
|
||||
const double reported = std::fmod(heading, 360.0); // 0..360 (wrapped)
|
||||
double d = have ? unwrapNear(prev, reported) : reported;
|
||||
prev = d;
|
||||
have = true;
|
||||
pts.emplace_back(d, heading * 100.0);
|
||||
}
|
||||
LinearFit f = linearFit(pts);
|
||||
CHECK(f.ok);
|
||||
CHECK(f.slope == doctest::Approx(100.0));
|
||||
CHECK(f.r2 > 0.9999);
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,81 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/DiagParser.h"
|
||||
|
||||
using namespace fgc;
|
||||
|
||||
namespace {
|
||||
// A DIAG stream as captured on the wire: DG lines interleaved with an ST line.
|
||||
const char* kDiag =
|
||||
"DG BEGIN Y\n"
|
||||
"ST Y:A,5,5,80084000,0,8,8,S P:A,5,5,80084000,0,8,8,S\n"
|
||||
"DG Y S12500 FWD ERR_PEAK 234 ERR_RMS 45 ERR_STILL 89 CS_MIN 10 CS_MAX 20 "
|
||||
"SG_MIN 456 PWM_AVG 128 FLAGS 0x00000000 PASS\n"
|
||||
"DG Y S25000 REV ERR_PEAK 300 ERR_RMS 50 ERR_STILL 100 CS_MIN 11 CS_MAX 21 "
|
||||
"SG_MIN 400 PWM_AVG 130 FLAGS 0x00000000 PASS\n"
|
||||
"DG Y RESULT PASS\n"
|
||||
"DG BEGIN P\n"
|
||||
"DG P S12500 FWD ERR_PEAK -1 ERR_RMS -1 ERR_STILL -1 CS_MIN 12 CS_MAX 22 "
|
||||
"SG_MIN 0 PWM_AVG 64 FLAGS 0x00000020 FAIL\n"
|
||||
"DG P RESULT FAIL\n"
|
||||
"DG DONE\n";
|
||||
} // namespace
|
||||
|
||||
TEST_CASE("parseDiag decodes axes, tests and results") {
|
||||
DiagResult d = parseDiag(kDiag);
|
||||
REQUIRE(d.valid);
|
||||
CHECK(d.done);
|
||||
REQUIRE(d.axes.size() == 2);
|
||||
|
||||
const DiagAxis& y = d.axes[0];
|
||||
CHECK(y.axis == 'Y');
|
||||
CHECK(y.has_result);
|
||||
CHECK(y.pass);
|
||||
REQUIRE(y.tests.size() == 2);
|
||||
CHECK(y.tests[0].speed == 12500);
|
||||
CHECK(y.tests[0].fwd);
|
||||
CHECK(y.tests[0].err_peak == 234);
|
||||
CHECK(y.tests[0].err_still == 89);
|
||||
CHECK(y.tests[0].cs_min == 10);
|
||||
CHECK(y.tests[0].cs_max == 20);
|
||||
CHECK(y.tests[0].sg_min == 456);
|
||||
CHECK(y.tests[0].pass);
|
||||
CHECK_FALSE(y.tests[1].fwd); // REV
|
||||
|
||||
const DiagAxis& p = d.axes[1];
|
||||
CHECK(p.axis == 'P');
|
||||
CHECK_FALSE(p.pass); // RESULT FAIL
|
||||
REQUIRE(p.tests.size() == 1);
|
||||
CHECK(p.tests[0].err_peak == -1); // no encoder
|
||||
CHECK(p.tests[0].flags == 0x20u);
|
||||
CHECK_FALSE(p.tests[0].pass);
|
||||
}
|
||||
|
||||
TEST_CASE("DiagResult::allPass requires every axis result to pass") {
|
||||
CHECK_FALSE(parseDiag(kDiag).allPass()); // P failed
|
||||
DiagResult ok = parseDiag(
|
||||
"DG BEGIN Y\nDG Y RESULT PASS\nDG BEGIN P\nDG P RESULT PASS\nDG DONE\n");
|
||||
CHECK(ok.allPass());
|
||||
}
|
||||
|
||||
TEST_CASE("parseDiag handles empty/garbage and incomplete streams") {
|
||||
CHECK_FALSE(parseDiag("").valid);
|
||||
CHECK_FALSE(parseDiag("ST Y:A,1,1,0,0,0,0,S\nnothing\n").valid);
|
||||
|
||||
DiagResult inc = parseDiag("DG BEGIN Y\nDG Y RESULT PASS\n"); // no DG DONE
|
||||
CHECK(inc.valid);
|
||||
CHECK_FALSE(inc.done);
|
||||
}
|
||||
|
||||
TEST_CASE("formatDiag renders a summary") {
|
||||
auto bad = formatDiag(DiagResult{});
|
||||
REQUIRE(bad.size() == 1);
|
||||
CHECK(bad[0].find("no diagnostic") != std::string::npos);
|
||||
|
||||
auto good = formatDiag(parseDiag(kDiag));
|
||||
std::string joined;
|
||||
for (const auto& l : good) joined += l + "\n";
|
||||
CHECK(joined.find("[Y]") != std::string::npos);
|
||||
CHECK(joined.find("[P]") != std::string::npos);
|
||||
CHECK(joined.find("overall: FAIL") != std::string::npos);
|
||||
}
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/DumpParser.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
|
||||
using namespace fgc;
|
||||
|
||||
namespace {
|
||||
|
||||
// A realistic two-axis firmware DUMP block (the on-wire format from
|
||||
// firmware/src/motor.cpp printDump): header, then per axis a state line and a
|
||||
// TMC register line, bracketed by DUMP BEGIN / DUMP END.
|
||||
const char* kDump =
|
||||
"DUMP BEGIN build=11dd3ce-dirty uptime=11740067 mcusr=0x01 free_ram=1852\n"
|
||||
"DUMP Y state=3 hsub=10 enabled=1 lim_neg=-82919 lim_pos=98687 hold_target=-90 "
|
||||
"speed=50000 eeprom_restored=0 has_encoder=1\n"
|
||||
"DUMP Y TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
|
||||
"RAMPMODE=0x00000000 XACTUAL=0xFFFFFFA6 VACTUAL=0x00000000 XTARGET=0xFFFFFFA6 "
|
||||
"SW_MODE=0x000008A0 RAMP_STAT=0x00001680 X_ENC=0xFFFFFF7F ENC_STATUS=0x00000002 "
|
||||
"CHOPCONF=0x00410043 DRV_STATUS=0x80084000 PWM_SCALE=0x00000011 PWM_AUTO=0x003F0039\n"
|
||||
"DUMP P state=3 hsub=10 enabled=1 lim_neg=495183 lim_pos=1069042 hold_target=500183 "
|
||||
"speed=150000 eeprom_restored=0 has_encoder=1\n"
|
||||
"DUMP P TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
|
||||
"RAMPMODE=0x00000000 XACTUAL=0x0007A1C8 VACTUAL=0x00000000 XTARGET=0x0007A1C8 "
|
||||
"SW_MODE=0x000008A3 RAMP_STAT=0x00001688 X_ENC=0x0007A231 ENC_STATUS=0x00000002 "
|
||||
"CHOPCONF=0x00410043 DRV_STATUS=0x80084000 PWM_SCALE=0x00010011 PWM_AUTO=0x0021003C\n"
|
||||
"DUMP END\n";
|
||||
|
||||
bool has(const std::vector<std::string>& v, const std::string& s) {
|
||||
return std::find(v.begin(), v.end(), s) != v.end();
|
||||
}
|
||||
|
||||
const DumpAxis* axis(const DumpData& d, char a) {
|
||||
for (const auto& ax : d.axes)
|
||||
if (ax.axis == a) return &ax;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_CASE("parseDump decodes the header") {
|
||||
DumpData d = parseDump(kDump);
|
||||
REQUIRE(d.valid);
|
||||
CHECK(d.build == "11dd3ce-dirty");
|
||||
CHECK(d.uptime_ms == 11740067);
|
||||
CHECK(d.mcusr == 0x01u);
|
||||
CHECK(d.free_ram == 1852);
|
||||
CHECK(has(d.reset_flags, "PORF (power-on)"));
|
||||
REQUIRE(d.axes.size() == 2);
|
||||
}
|
||||
|
||||
TEST_CASE("parseDump decodes the per-axis state line") {
|
||||
DumpData d = parseDump(kDump);
|
||||
const DumpAxis* y = axis(d, 'Y');
|
||||
REQUIRE(y != nullptr);
|
||||
CHECK(y->state_name == "READY"); // state=3
|
||||
CHECK(y->enabled);
|
||||
CHECK(y->has_encoder);
|
||||
CHECK_FALSE(y->eeprom_restored);
|
||||
CHECK(y->lim_neg == -82919);
|
||||
CHECK(y->lim_pos == 98687);
|
||||
CHECK(y->hold_target == -90);
|
||||
CHECK(y->speed == 50000);
|
||||
CHECK(y->hsub == 10);
|
||||
|
||||
const DumpAxis* p = axis(d, 'P');
|
||||
REQUIRE(p != nullptr);
|
||||
CHECK(p->lim_neg == 495183);
|
||||
CHECK(p->lim_pos == 1069042);
|
||||
}
|
||||
|
||||
TEST_CASE("parseDump keeps all 16 TMC registers (regression: TMC-line whitespace)") {
|
||||
// The "DUMP Y TMC ..." line was once misclassified due to a leading-space
|
||||
// slice, dropping every register. Guard that all of them survive.
|
||||
DumpData d = parseDump(kDump);
|
||||
const DumpAxis* y = axis(d, 'Y');
|
||||
REQUIRE(y != nullptr);
|
||||
for (const char* r : {"GCONF", "GSTAT", "IOIN", "TSTEP", "RAMPMODE", "XACTUAL",
|
||||
"VACTUAL", "XTARGET", "SW_MODE", "RAMP_STAT", "X_ENC",
|
||||
"ENC_STATUS", "CHOPCONF", "DRV_STATUS", "PWM_SCALE", "PWM_AUTO"}) {
|
||||
CHECK_MESSAGE(y->regs.count(r) == 1, "missing register ", r);
|
||||
}
|
||||
CHECK(y->regs.at("RAMP_STAT") == "0x00001680");
|
||||
CHECK(y->regs.at("DRV_STATUS") == "0x80084000");
|
||||
}
|
||||
|
||||
TEST_CASE("parseDump decodes status-register bit flags") {
|
||||
DumpData d = parseDump(kDump);
|
||||
const DumpAxis* y = axis(d, 'Y');
|
||||
REQUIRE(y != nullptr);
|
||||
|
||||
// DRV_STATUS = 0x80084000 -> bit31 stst, bit14 stealth; CS_ACTUAL=(>>16)&0x1F=8.
|
||||
CHECK(y->drv_status == 0x80084000u);
|
||||
CHECK(has(y->drv_flags, "stst"));
|
||||
CHECK(has(y->drv_flags, "stealth"));
|
||||
CHECK(y->cs_actual == 8);
|
||||
CHECK(y->sg_result == 0);
|
||||
|
||||
// GSTAT = 0 -> no flags (placeholder "-").
|
||||
CHECK(has(y->gstat_flags, "-"));
|
||||
|
||||
// RAMP_STAT = 0x1680 -> bit7 event_pos_reached, bit9 position_reached, bit10 vzero.
|
||||
CHECK(y->ramp_stat == 0x00001680u);
|
||||
CHECK(has(y->ramp_flags, "event_pos_reached"));
|
||||
CHECK(has(y->ramp_flags, "position_reached"));
|
||||
CHECK(has(y->ramp_flags, "vzero"));
|
||||
}
|
||||
|
||||
TEST_CASE("parseDump tolerates a corrupted/merged DUMP END (best-effort)") {
|
||||
// The host detects DUMP END anywhere in a line; a clean header still parses
|
||||
// even when later content is mangled — but a block with no BEGIN/END is invalid.
|
||||
CHECK_FALSE(parseDump("").valid);
|
||||
CHECK_FALSE(parseDump("garbage with no markers\n").valid);
|
||||
CHECK_FALSE(parseDump("DUMP Y state=3\nno end marker\n").valid);
|
||||
}
|
||||
|
||||
TEST_CASE("formatDump renders valid and invalid dumps") {
|
||||
auto bad = formatDump(DumpData{});
|
||||
REQUIRE(bad.size() == 1);
|
||||
CHECK(bad[0].find("no firmware dump") != std::string::npos);
|
||||
|
||||
auto good = formatDump(parseDump(kDump));
|
||||
std::string joined;
|
||||
for (const auto& l : good) joined += l + "\n";
|
||||
CHECK(joined.find("build=11dd3ce-dirty") != std::string::npos);
|
||||
CHECK(joined.find("[Y]") != std::string::npos);
|
||||
CHECK(joined.find("[P]") != std::string::npos);
|
||||
CHECK(joined.find("RAMP_STAT") != std::string::npos);
|
||||
}
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/HelpText.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
using namespace fgc;
|
||||
|
||||
namespace {
|
||||
std::string join(const std::vector<std::string>& v) {
|
||||
std::string s;
|
||||
for (const auto& l : v) s += l + "\n";
|
||||
return s;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST_CASE("helpCatalog is well-formed") {
|
||||
const auto& cat = helpCatalog();
|
||||
REQUIRE_FALSE(cat.empty());
|
||||
for (const auto& sec : cat) {
|
||||
CHECK_FALSE(sec.title.empty());
|
||||
CHECK_FALSE(sec.entries.empty());
|
||||
for (const auto& e : sec.entries) {
|
||||
CHECK_FALSE(e.syntax.empty());
|
||||
CHECK_FALSE(e.summary.empty());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_CASE("renderHelp() with no topic lists every section and entry") {
|
||||
std::string out = join(renderHelp(""));
|
||||
CHECK(out.find("help <topic>") != std::string::npos); // the usage hint
|
||||
// Each catalog section title and entry syntax should appear.
|
||||
for (const auto& sec : helpCatalog()) {
|
||||
CHECK(out.find(sec.title) != std::string::npos);
|
||||
for (const auto& e : sec.entries)
|
||||
CHECK(out.find(e.syntax) != std::string::npos);
|
||||
}
|
||||
// A couple of the commands.
|
||||
CHECK(out.find("gimbal move") != std::string::npos);
|
||||
CHECK(out.find("gimbal calib") != std::string::npos);
|
||||
}
|
||||
|
||||
TEST_CASE("renderHelp(<section>) expands that section with detail") {
|
||||
std::string out = join(renderHelp("positioning"));
|
||||
CHECK(out.find("gimbal move <yaw>,<pitch>") != std::string::npos);
|
||||
// Detail lines (example) are only emitted in topic mode.
|
||||
CHECK(out.find("gimbal move 30,-10") != std::string::npos);
|
||||
}
|
||||
|
||||
TEST_CASE("renderHelp(<verb>) matches the gimbal commands, case-insensitively") {
|
||||
std::string lower = join(renderHelp("gimbal"));
|
||||
std::string upper = join(renderHelp("GIMBAL"));
|
||||
CHECK(lower.find("gimbal move <yaw>,<pitch>") != std::string::npos);
|
||||
CHECK(lower.find("gimbal calib") != std::string::npos);
|
||||
CHECK(lower == upper);
|
||||
}
|
||||
|
||||
TEST_CASE("renderHelp(unknown) reports no match") {
|
||||
std::string out = join(renderHelp("definitely-not-a-command"));
|
||||
CHECK(out.find("No help topic") != std::string::npos);
|
||||
}
|
||||
|
|
@ -0,0 +1,163 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/MtiProtocol.h"
|
||||
|
||||
#include <cstring>
|
||||
#include <vector>
|
||||
|
||||
using namespace fgc;
|
||||
|
||||
namespace {
|
||||
void putBEFloat(std::vector<uint8_t>& v, float f) {
|
||||
uint32_t u;
|
||||
std::memcpy(&u, &f, 4);
|
||||
v.push_back(static_cast<uint8_t>(u >> 24));
|
||||
v.push_back(static_cast<uint8_t>(u >> 16));
|
||||
v.push_back(static_cast<uint8_t>(u >> 8));
|
||||
v.push_back(static_cast<uint8_t>(u));
|
||||
}
|
||||
|
||||
// Sum of all bytes from BID through CS must be ≡ 0 (mod 256) for a valid frame.
|
||||
bool frameChecksumOk(const std::vector<uint8_t>& m) {
|
||||
unsigned s = 0;
|
||||
for (size_t i = 1; i < m.size(); ++i) s += m[i];
|
||||
return (s & 0xFF) == 0;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST_CASE("config messages are well-formed with correct payloads") {
|
||||
auto cfg = msgGoToConfig();
|
||||
auto mode = msgSetOutputMode();
|
||||
auto set = msgSetOutputSettings();
|
||||
auto meas = msgGoToMeasurement();
|
||||
|
||||
for (const auto& m : {cfg, mode, set, meas}) {
|
||||
CHECK(m[0] == kMtiPreamble);
|
||||
CHECK(m[1] == kMtiBid);
|
||||
CHECK(frameChecksumOk(m));
|
||||
}
|
||||
// GoToConfig / GoToMeasurement: no data.
|
||||
CHECK(cfg[2] == kMidGoToConfig);
|
||||
CHECK(cfg[3] == 0);
|
||||
CHECK(meas[2] == kMidGoToMeasurement);
|
||||
CHECK(meas[3] == 0);
|
||||
// SetOutputMode = 0x0007 (Temp|Calibrated|Orientation), 2-byte big-endian.
|
||||
CHECK(mode[2] == kMidSetOutputMode);
|
||||
CHECK(mode[3] == 2);
|
||||
CHECK(mode[4] == 0x00);
|
||||
CHECK(mode[5] == 0x07);
|
||||
// SetOutputSettings = 0x00000005 (Euler + sample counter), 4-byte big-endian.
|
||||
CHECK(set[2] == kMidSetOutputSettings);
|
||||
CHECK(set[3] == 4);
|
||||
CHECK(set[4] == 0x00);
|
||||
CHECK(set[5] == 0x00);
|
||||
CHECK(set[6] == 0x00);
|
||||
CHECK(set[7] == 0x05);
|
||||
}
|
||||
|
||||
TEST_CASE("framer decodes a combined MTData frame into a full sample") {
|
||||
std::vector<uint8_t> d;
|
||||
putBEFloat(d, 24.5f); // temp
|
||||
putBEFloat(d, 0.10f); putBEFloat(d, -0.20f); putBEFloat(d, 9.81f); // acc
|
||||
putBEFloat(d, 0.01f); putBEFloat(d, 0.02f); putBEFloat(d, -0.03f); // gyr
|
||||
putBEFloat(d, 0.45f); putBEFloat(d, -0.88f); putBEFloat(d, 0.21f); // mag
|
||||
putBEFloat(d, -1.5f); putBEFloat(d, 3.25f); putBEFloat(d, 187.0f); // roll/pitch/yaw
|
||||
d.push_back(0x12); d.push_back(0x34); // sample counter
|
||||
REQUIRE(d.size() == kMTDataLen);
|
||||
|
||||
auto frame = mtiMessage(kMidMTData, d);
|
||||
|
||||
ImuSample got;
|
||||
bool fired = false;
|
||||
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
||||
if (auto s = parseMTData(mid, p, n)) { got = *s; fired = true; }
|
||||
});
|
||||
|
||||
// Leading noise must not break resync.
|
||||
const uint8_t noise[] = {0x00, 0xAB, 0xFA, 0x01};
|
||||
fr.feed(noise, sizeof(noise));
|
||||
fr.feed(frame.data(), frame.size());
|
||||
|
||||
REQUIRE(fired);
|
||||
CHECK(got.valid);
|
||||
CHECK(got.temp_c == doctest::Approx(24.5f));
|
||||
CHECK(got.acc[2] == doctest::Approx(9.81f));
|
||||
CHECK(got.gyr[0] == doctest::Approx(0.01f));
|
||||
CHECK(got.mag[1] == doctest::Approx(-0.88f));
|
||||
CHECK(got.roll_deg == doctest::Approx(-1.5f));
|
||||
CHECK(got.pitch_deg == doctest::Approx(3.25f));
|
||||
CHECK(got.yaw_deg == doctest::Approx(187.0f));
|
||||
CHECK(got.sample_counter == 0x1234);
|
||||
}
|
||||
|
||||
TEST_CASE("framer reassembles frames split across feeds and back-to-back frames") {
|
||||
// Real serial reads arrive in arbitrary chunks; the framer must not depend
|
||||
// on frame boundaries aligning with feed() calls.
|
||||
std::vector<uint8_t> d;
|
||||
putBEFloat(d, 7.5f); // temp
|
||||
d.resize(kMTDataLen, 0); // rest of the 54-byte payload = 0
|
||||
auto frame = mtiMessage(kMidMTData, d);
|
||||
|
||||
int count = 0;
|
||||
float last_temp = 0;
|
||||
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
||||
if (auto s = parseMTData(mid, p, n)) { ++count; last_temp = s->temp_c; }
|
||||
});
|
||||
|
||||
// Two frames fed one byte at a time (worst-case fragmentation).
|
||||
for (uint8_t b : frame) fr.feed(&b, 1);
|
||||
for (uint8_t b : frame) fr.feed(&b, 1);
|
||||
CHECK(count == 2);
|
||||
CHECK(last_temp == doctest::Approx(7.5f));
|
||||
|
||||
// Two frames concatenated in a single feed.
|
||||
std::vector<uint8_t> two = frame;
|
||||
two.insert(two.end(), frame.begin(), frame.end());
|
||||
count = 0;
|
||||
MtiFramer fr2([&](uint8_t mid, const uint8_t* p, size_t n) {
|
||||
if (parseMTData(mid, p, n)) ++count;
|
||||
});
|
||||
fr2.feed(two.data(), two.size());
|
||||
CHECK(count == 2);
|
||||
}
|
||||
|
||||
TEST_CASE("parseMTData reports yaw as a 0..360 heading") {
|
||||
auto frameWithYaw = [](float yaw) {
|
||||
std::vector<uint8_t> d;
|
||||
for (int i = 0; i < 10; ++i) putBEFloat(d, 0.f); // temp + acc3 + gyr3 + mag3
|
||||
putBEFloat(d, 0.f); // roll
|
||||
putBEFloat(d, 0.f); // pitch
|
||||
putBEFloat(d, yaw); // yaw
|
||||
d.push_back(0); d.push_back(0); // counter
|
||||
return mtiMessage(kMidMTData, d);
|
||||
};
|
||||
ImuSample got;
|
||||
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
||||
if (auto s = parseMTData(mid, p, n)) got = *s;
|
||||
});
|
||||
|
||||
auto fn = frameWithYaw(-90.f); fr.feed(fn.data(), fn.size());
|
||||
CHECK(got.yaw_deg == doctest::Approx(270.0f));
|
||||
auto fp = frameWithYaw(45.f); fr.feed(fp.data(), fp.size());
|
||||
CHECK(got.yaw_deg == doctest::Approx(45.0f));
|
||||
auto fb = frameWithYaw(-179.f); fr.feed(fb.data(), fb.size());
|
||||
CHECK(got.yaw_deg == doctest::Approx(181.0f));
|
||||
}
|
||||
|
||||
TEST_CASE("framer rejects a bad checksum and a wrong-length payload") {
|
||||
std::vector<uint8_t> d(kMTDataLen, 0);
|
||||
auto frame = mtiMessage(kMidMTData, d);
|
||||
|
||||
SUBCASE("corrupt checksum") {
|
||||
auto bad = frame;
|
||||
bad.back() ^= 0xFF;
|
||||
bool fired = false;
|
||||
MtiFramer fr([&](uint8_t, const uint8_t*, size_t) { fired = true; });
|
||||
fr.feed(bad.data(), bad.size());
|
||||
CHECK_FALSE(fired);
|
||||
}
|
||||
SUBCASE("wrong-length MTData parses to nullopt") {
|
||||
std::vector<uint8_t> shortData(10, 0);
|
||||
CHECK_FALSE(parseMTData(kMidMTData, shortData.data(), shortData.size()).has_value());
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue