Compare commits
No commits in common. "e721ac74c2b56bcaf21f1d62b6aa3c9dfd8a8c43" and "9a25bbcc487d8a22f4be1288cafeb6a3071b8feb" have entirely different histories.
e721ac74c2
...
9a25bbcc48
|
|
@ -2,4 +2,4 @@
|
||||||
REMOTE_HOST=ggs@10.11.12.111
|
REMOTE_HOST=ggs@10.11.12.111
|
||||||
REMOTE_DIR=/home/ggs/projects/fwt_2a/software
|
REMOTE_DIR=/home/ggs/projects/fwt_2a/software
|
||||||
CMAKE_ARGS="-DWITH_MQTT=ON -DWITH_VIMBA=ON"
|
CMAKE_ARGS="-DWITH_MQTT=ON -DWITH_VIMBA=ON"
|
||||||
RUN_ARGS="--start --trace serial"
|
RUN_ARGS="--init --start --trace serial"
|
||||||
|
|
|
||||||
|
|
@ -29,5 +29,3 @@ bin/x64/Release/NIR/
|
||||||
# Real configs hold plaintext MQTT credentials. Commit config/config.example.ini instead.
|
# Real configs hold plaintext MQTT credentials. Commit config/config.example.ini instead.
|
||||||
config.ini
|
config.ini
|
||||||
bin/x64/Release/config.ini
|
bin/x64/Release/config.ini
|
||||||
build-test/
|
|
||||||
build/
|
|
||||||
|
|
|
||||||
|
|
@ -101,9 +101,8 @@ While running, the program reads commands from stdin (one per line):
|
||||||
| `gimbal home [y\|p]` | Run the endstop-finding home sequence. |
|
| `gimbal home [y\|p]` | Run the endstop-finding home sequence. |
|
||||||
| `gimbal dump` | Request a firmware state dump (shown in the gimbal `g` view). |
|
| `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 diag [y\|p\|all]` | Run the firmware motor self-test (results in the activity strip + `logs/`). |
|
||||||
| `gimbal calib` | IMU-referenced steps↔degrees calibration (auto-homes if needed; needs the IMU; applied to the session). |
|
| `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`. |
|
| `gimbal …` | Other motor controls — `steps`/`nudge`/`stop`/`speed`/`reset`/`enable`/`disable`/`setpos`/`status`. |
|
||||||
| `gimbal raw "<command>"` | **Low-level:** send the quoted text to the firmware verbatim (newline added). Bypasses wrappers/unit conversion — use with care. |
|
|
||||||
| `debug` | Toggle debug-level logging on/off. |
|
| `debug` | Toggle debug-level logging on/off. |
|
||||||
| `set fps <n>` | Capture rate, in images per second. |
|
| `set fps <n>` | Capture rate, in images per second. |
|
||||||
| `set camera fps <n>` | Camera sensor frame rate. |
|
| `set camera fps <n>` | Camera sensor frame rate. |
|
||||||
|
|
|
||||||
|
|
@ -8,7 +8,6 @@
|
||||||
# Usage:
|
# Usage:
|
||||||
# ./deploy.sh rsync + remote configure + build
|
# ./deploy.sh rsync + remote configure + build
|
||||||
# ./deploy.sh --run ... then run the app over ssh (RUN_ARGS, ctrl-c to stop)
|
# ./deploy.sh --run ... then run the app over ssh (RUN_ARGS, ctrl-c to stop)
|
||||||
# ./deploy.sh --run --home ... and run the endstop-finding home sequence at startup
|
|
||||||
# ./deploy.sh --clean wipe the remote build dir first (fresh configure)
|
# ./deploy.sh --clean wipe the remote build dir first (fresh configure)
|
||||||
# ./deploy.sh --check-deps only check the remote build dependencies
|
# ./deploy.sh --check-deps only check the remote build dependencies
|
||||||
# ./deploy.sh --run --force skip the placeholder-config guard before running
|
# ./deploy.sh --run --force skip the placeholder-config guard before running
|
||||||
|
|
@ -39,24 +38,16 @@ CLEAN=0
|
||||||
RUN=0
|
RUN=0
|
||||||
CHECK_DEPS=0
|
CHECK_DEPS=0
|
||||||
FORCE=0
|
FORCE=0
|
||||||
HOME_INIT=0
|
|
||||||
for arg in "$@"; do
|
for arg in "$@"; do
|
||||||
case "$arg" in
|
case "$arg" in
|
||||||
--clean) CLEAN=1 ;;
|
--clean) CLEAN=1 ;;
|
||||||
--run|--start) RUN=1 ;;
|
--run|--start) RUN=1 ;;
|
||||||
--home|--init) HOME_INIT=1 ;;
|
|
||||||
--check-deps) CHECK_DEPS=1 ;;
|
--check-deps) CHECK_DEPS=1 ;;
|
||||||
--force) FORCE=1 ;;
|
--force) FORCE=1 ;;
|
||||||
*) echo "unknown option: $arg" >&2; exit 1 ;;
|
*) echo "unknown option: $arg" >&2; exit 1 ;;
|
||||||
esac
|
esac
|
||||||
done
|
done
|
||||||
|
|
||||||
# Homing is opt-in: append --init only when --home is passed, so a plain run
|
|
||||||
# brings up the TUI without driving the endstop-finding sequence.
|
|
||||||
if [[ $HOME_INIT -eq 1 && "$RUN_ARGS" != *--init* ]]; then
|
|
||||||
RUN_ARGS="--init $RUN_ARGS"
|
|
||||||
fi
|
|
||||||
|
|
||||||
# Locate cmake on the remote. Non-interactive ssh shells don't source .bashrc,
|
# Locate cmake on the remote. Non-interactive ssh shells don't source .bashrc,
|
||||||
# so allow an explicit REMOTE_CMAKE override in .deploy.env; otherwise search
|
# so allow an explicit REMOTE_CMAKE override in .deploy.env; otherwise search
|
||||||
# the usual locations.
|
# the usual locations.
|
||||||
|
|
|
||||||
|
|
@ -59,39 +59,7 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config
|
||||||
|
|
||||||
The firmware reports only **encoder counts**; `[Motor]` maps them to the heading/elevation degrees
|
The firmware reports only **encoder counts**; `[Motor]` maps them to the heading/elevation degrees
|
||||||
used by MQTT (`target_HDG`) and `CamEvent`. Calibrate `*_counts_per_deg` / `*_zero_count` against
|
used by MQTT (`target_HDG`) and `CamEvent`. Calibrate `*_counts_per_deg` / `*_zero_count` against
|
||||||
real `xenc` readings after homing (`MOVE` a known angle, read the resulting `xenc`), **or** run the
|
real `xenc` readings after homing (`MOVE` a known angle, read the resulting `xenc`).
|
||||||
automated IMU-referenced `gimbal calib` (below).
|
|
||||||
|
|
||||||
#### `gimbal calib` — automated IMU-referenced calibration
|
|
||||||
|
|
||||||
`gimbal calib` (requires the IMU) fits each axis's `counts_per_deg` / `zero_count` by sweeping it and
|
|
||||||
correlating encoder counts with the MTi's measured angle. The sequence is deliberately ordered so the
|
|
||||||
**yaw fit is not spoiled by the IMU's heading drift** (see the no-magnetometer discussion below):
|
|
||||||
|
|
||||||
1. **Home if needed** — if the gimbal is not already `READY`, it runs the endstop-finding home first.
|
|
||||||
2. **Pitch at the first yaw position** — yaw moves to the start of its travel and holds there while
|
|
||||||
pitch is swept across its soft-limit travel; at each step it dwells and records the
|
|
||||||
gravity-referenced IMU **pitch** (stable, absolute), then least-squares fits pitch.
|
|
||||||
3. **Pitch → 0°** — using the just-fitted pitch map.
|
|
||||||
4. **Switch to a no-magnetometer XKF profile** — picked from the device's own available-profiles list
|
|
||||||
(a `*nomag*` profile, else a `VRU` one), so the IMU's heading stops chasing the stepper-distorted
|
|
||||||
magnetic field. Sent in the MTi's Config state, so it **persists on the device** across power-cycles.
|
|
||||||
Best-effort: if the device reports no magnetometer-free profile, this step is skipped (logged) and
|
|
||||||
calibration continues.
|
|
||||||
5. **Drift-correct + zero the heading** — holding the gimbal still, it runs the MTi **no-rotation**
|
|
||||||
gyro-bias update (cuts yaw drift), then a **heading reset** so the current pose becomes yaw 0. This
|
|
||||||
happens right before the yaw sweep, so any drift accrued during the slow pitch sweep is discarded.
|
|
||||||
6. **Yaw sweep** — yaw is swept from that first (now zero-heading) position and fitted.
|
|
||||||
|
|
||||||
After calibration the IMU is left on the no-mag profile (this is intentional — the gimbal's homed
|
|
||||||
encoders are the absolute heading reference; the IMU only needs stable roll/pitch and short-term
|
|
||||||
yaw-rate). The `i` view's **XKF profile** row will show the new selection (press `r` to refresh if you
|
|
||||||
changed it outside calibration).
|
|
||||||
|
|
||||||
Each axis fit reports an R² (shown in the gimbal `g` view + activity strip). The result is applied to
|
|
||||||
the live session immediately; in the TUI the activity strip then offers to save it to `[Motor]` as the
|
|
||||||
new default (`y`/`n`). `gimbal stop` cancels a run. Timings (dwell, no-rotation duration, etc.) are the
|
|
||||||
`CalibParams` defaults in [CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h).
|
|
||||||
|
|
||||||
The capture **scan grid** is the ordered `(yaw,pitch)` waypoints auto-sweep visits (ping-pong). Set
|
The capture **scan grid** is the ordered `(yaw,pitch)` waypoints auto-sweep visits (ping-pong). Set
|
||||||
`[Scan] grid_file` to an editable CSV ([config/scan.csv](../config/scan.csv)) to define exact
|
`[Scan] grid_file` to an editable CSV ([config/scan.csv](../config/scan.csv)) to define exact
|
||||||
|
|
@ -111,22 +79,6 @@ was left in Config state. Yaw is reported as a **0..360 heading** (not the MTi's
|
||||||
`[Features] mock_imu = true` to use a synthetic IMU on dev machines (no hardware). Protocol/units are
|
`[Features] mock_imu = true` to use a synthetic IMU on dev machines (no hardware). Protocol/units are
|
||||||
documented in the modules reference (`MtiProtocol`).
|
documented in the modules reference (`MtiProtocol`).
|
||||||
|
|
||||||
During that same Config-state handshake the host also **reads back the device configuration**
|
|
||||||
(`ReqProductCode`, `ReqDID`, `ReqFWRev`, `ReqPeriod`, `ReqOutputMode`, `ReqOutputSettings`, and the
|
|
||||||
filter-profile / **XKF profile** via `ReqFilterProfile` + `ReqAvailableFilterProfiles`). The decoded
|
|
||||||
values — product code, firmware, device ID, output mode/format, calibration channels, sample rate, and
|
|
||||||
the list of supported **Xsens Kalman Filter (XKF) profiles** with the active one marked — are surfaced
|
|
||||||
in the **IMU CONFIG** section of the expanded Sensors view (press `i`). This is read-only: it reports
|
|
||||||
what the device is actually configured to do, which is the place to confirm the active XKF profile
|
|
||||||
(e.g. `General` vs `VRU_general`) when diagnosing yaw drift. On the legacy MTi the same concept is
|
|
||||||
called a "scenario" in the device manual (e.g. `Machine_nomagfield`); it is the same setting and shares
|
|
||||||
the wire MIDs. Older MTi firmware that does not answer the `Req*` queries simply leaves the section
|
|
||||||
absent (the host logs `no configuration acks received`).
|
|
||||||
|
|
||||||
The config is read **once at startup** and cached, so if you change the XKF profile externally the view
|
|
||||||
stays stale until you **refresh** it: press `r` (or type `refresh`). That re-queries the device (briefly
|
|
||||||
pausing the stream) and also requests a fresh firmware dump for the gimbal `g` view.
|
|
||||||
|
|
||||||
### Secrets
|
### Secrets
|
||||||
|
|
||||||
`mqtt_user` / `mqtt_pw` are read from the environment variables **`FGC_MQTT_USER` / `FGC_MQTT_PW`** first,
|
`mqtt_user` / `mqtt_pw` are read from the environment variables **`FGC_MQTT_USER` / `FGC_MQTT_PW`** first,
|
||||||
|
|
@ -205,10 +157,7 @@ one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp).
|
||||||
(`d` requests a fresh one), the homing limits, and the **last calibration** result (per-axis
|
(`d` requests a fresh one), the homing limits, and the **last calibration** result (per-axis
|
||||||
`counts_per_deg` / `zero_count` / R² / age).
|
`counts_per_deg` / `zero_count` / R² / age).
|
||||||
- **IMU** (`i`) — every MTi channel with units: orientation (°), acceleration (m/s²), rate-of-turn
|
- **IMU** (`i`) — every MTi channel with units: orientation (°), acceleration (m/s²), rate-of-turn
|
||||||
(rad/s), magnetic field (a.u.), temperature, sample counter, plus an **IMU CONFIG** section
|
(rad/s), magnetic field (a.u.), temperature, sample counter.
|
||||||
(read back from the device at startup): product code, firmware, device ID, output mode/format,
|
|
||||||
calibration channels, sample rate, and the **Xsens Kalman Filter (XKF) profile** list — every
|
|
||||||
profile the device supports, by name, with the active one marked `●` (selected).
|
|
||||||
|
|
||||||
**Activity strip** — a compact section between the log and the key bar that shows the
|
**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
|
currently-running special operation with live progress (`gimbal calib`, `gimbal diag`, homing, capture
|
||||||
|
|
@ -217,15 +166,14 @@ 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
|
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
|
yes/no prompt — **`Save this calibration to config as the new default? (y / n)`**. Press **`y`** to
|
||||||
write the full `[Motor]` per-axis map — `*_counts_per_deg`, `*_zero_count`, and `*_min_deg`/`*_max_deg`
|
write the fitted `[Motor]` `*_counts_per_deg` / `*_zero_count` back into the `config.ini` the program
|
||||||
— back into the `config.ini` the program was launched with (replacing those keys in place, preserving
|
was launched with (replacing those keys in place, preserving everything else) so it persists across
|
||||||
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
|
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
|
showing. (Until you answer `y`, calibration remains session-only — see
|
||||||
[known-issues.md](known-issues.md).)
|
[known-issues.md](known-issues.md).)
|
||||||
|
|
||||||
Keys (bottom bar): `s` start · `x` stop · `h` home · `g` gimbal view · `i` IMU view · `r` refresh
|
Keys (bottom bar): `s` start · `x` stop · `h` home · `r` reset · `g` gimbal view · `i` IMU view ·
|
||||||
(re-read IMU config + firmware dump) · arrow keys nudge the gimbal (yaw ±5 % / pitch ±10 %) · `:` open
|
arrow keys nudge the gimbal (yaw ±5 % / pitch ±10 %) · `:` open a command line (any console/`gimbal …`
|
||||||
a command line (any console/`gimbal …`
|
|
||||||
command) · `?` help · `q` quit. Plain letters are used rather than Ctrl chords so terminal
|
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
|
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.
|
from stdout into the on-screen log pane via a `Logger` sink, so the screen is never corrupted.
|
||||||
|
|
|
||||||
|
|
@ -53,13 +53,7 @@ doctest unit-test suite (`ctest`).
|
||||||
- **`gimbal calib` persistence**: the fitted `counts_per_deg`/`zero_count` are always applied to the live
|
- **`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
|
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)`
|
`logs/calib_*.log`. In the **TUI**, the activity strip then prompts `Save … as the new default? (y/n)`
|
||||||
— `y` writes the full per-axis `[Motor]` map (`*_counts_per_deg`, `*_zero_count`,
|
— `y` writes them into `[Motor]` in `config.ini` (persists across restarts), `n` keeps them
|
||||||
`*_min_deg`/`*_max_deg`) into `config.ini` (persists across restarts), `n` keeps them
|
|
||||||
session-only. In the **headless** console there is no prompt, so calibration stays session-only there;
|
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.
|
copy the logged values into `[Motor]` by hand to keep them.
|
||||||
- **`gimbal nudge`** moves a fixed fraction of the **homed** endstop-to-endstop travel (from the
|
|
||||||
firmware dump), so it is independent of the degrees↔counts calibration. If no dump has been captured
|
|
||||||
yet (not homed / no `gimbal dump`), nudge requests one and does nothing that press — it never falls
|
|
||||||
back to the configured degree clamps (which, if `*_min_deg`/`*_max_deg` were unset, produced absurd
|
|
||||||
±100000° steps).
|
|
||||||
- DHT11 temperature/humidity is still a Sensors-panel placeholder (the IMU half is integrated).
|
- DHT11 temperature/humidity is still a Sensors-panel placeholder (the IMU half is integrated).
|
||||||
|
|
|
||||||
|
|
@ -17,8 +17,8 @@ Per-file reference for the refactored tree, plus the shared data structures.
|
||||||
| [include/fgc/DumpParser.h](../include/fgc/DumpParser.h), [src/core/DumpParser.cpp](../src/core/DumpParser.cpp) | `parseDump` firmware `DUMP` block → `DumpData` (per-axis state + decoded TMC registers/flags); `formatDump` |
|
| [include/fgc/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/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/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: home-if-needed → pitch sweep at the first yaw position → pitch 0° → switch IMU to a no-mag XKF profile → IMU no-rotation + heading reset → yaw sweep, fitting degrees↔counts; tunable via `CalibParams`; exposes `progress()`/`report()`/`takeResult()` |
|
| [include/fgc/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), config-readback query builders + `applyImuConfigAck`/`finalizeImuConfig` → `ImuDeviceConfig` (product/firmware/device-id/output mode+settings/sample rate/**XKF scenario**); orientation-control builders `msgSetNoRotation` (gyro-bias update) + `msgResetOrientation` (heading reset / store) + `msgSetFilterProfile` (select XKF profile) + `pickNoMagProfile` (choose a magnetometer-free profile from the available list) |
|
| [include/fgc/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/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/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/UiSnapshot.h](../include/fgc/ui/UiSnapshot.h), [src/ui/UiSnapshot.cpp](../src/ui/UiSnapshot.cpp) | Plain-data view model (incl. `ImuView`, `ActivityView`, `CalibResultView`, `DumpView`) + pure formatting helpers (state label/colour, degrees, time-ago, pending-sensors) |
|
||||||
|
|
@ -32,7 +32,7 @@ Per-file reference for the refactored tree, plus the shared data structures.
|
||||||
| [include/fgc/IMotorController.h](../include/fgc/IMotorController.h) | `IMotorController` | `MotorTelemetry` |
|
| [include/fgc/IMotorController.h](../include/fgc/IMotorController.h) | `IMotorController` | `MotorTelemetry` |
|
||||||
| [include/fgc/IControlChannel.h](../include/fgc/IControlChannel.h) | `IControlChannel` | `ControlCommand`, `CamEvent` |
|
| [include/fgc/IControlChannel.h](../include/fgc/IControlChannel.h) | `IControlChannel` | `ControlCommand`, `CamEvent` |
|
||||||
| [include/fgc/ICameraSource.h](../include/fgc/ICameraSource.h) | `ICameraSource` | `Frame` |
|
| [include/fgc/ICameraSource.h](../include/fgc/ICameraSource.h) | `ICameraSource` | `Frame` |
|
||||||
| [include/fgc/IImuSource.h](../include/fgc/IImuSource.h) | `IImuSource` (incl. `config()`, `refreshConfig()`, `noRotation()`, `headingReset()`, `setFilterProfile()`) | `ImuSample` (from `MtiProtocol.h`) |
|
| [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` |
|
| [include/fgc/ui/IUserInterface.h](../include/fgc/ui/IUserInterface.h) | `IUserInterface` | `UiSnapshot` |
|
||||||
|
|
||||||
## Real implementations (SDK-gated)
|
## Real implementations (SDK-gated)
|
||||||
|
|
@ -40,7 +40,7 @@ Per-file reference for the refactored tree, plus the shared data structures.
|
||||||
| File | Implements | Built when |
|
| File | Implements | Built when |
|
||||||
|------|-----------|-----------|
|
|------|-----------|-----------|
|
||||||
| [src/serial/SerialMotorController.cpp](../src/serial/SerialMotorController.cpp) | `IMotorController` over Boost.Asio serial (pImpl) | always |
|
| [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, reads back its config (`config()` → `ImuDeviceConfig`), then frames the MTData stream | always |
|
| [src/serial/MtiImuSource.cpp](../src/serial/MtiImuSource.cpp) | `IImuSource` over Boost.Asio serial: configures the MTi to Euler+calibrated, then frames the MTData stream | always |
|
||||||
| [src/mqtt/MqttControlChannel.cpp](../src/mqtt/MqttControlChannel.cpp) | `IControlChannel` over Eclipse Paho | `WITH_MQTT` |
|
| [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/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` |
|
| [src/camera/VimbaCameraSource.cpp](../src/camera/VimbaCameraSource.cpp) | `ICameraSource` over Vimba X (pImpl) | `WITH_VIMBA` |
|
||||||
|
|
@ -90,16 +90,6 @@ One decoded Xsens MTi reading: `temp_c` (°C), `acc[3]` (m/s², incl. gravity),
|
||||||
`gimbal calib` phase-unwraps the swept yaw (`unwrapNear`) so a sweep crossing 0/360 still fits a clean
|
`gimbal calib` phase-unwraps the swept yaw (`unwrapNear`) so a sweep crossing 0/360 still fits a clean
|
||||||
line.
|
line.
|
||||||
|
|
||||||
### `ImuDeviceConfig` ([MtiProtocol.h](../include/fgc/MtiProtocol.h))
|
|
||||||
The device configuration read back during the Config-state handshake (each `has_*` flag marks whether
|
|
||||||
the device actually answered): `product_code`, `device_id`, `firmware`, output mode flags
|
|
||||||
(temp/calibrated/orientation/aux/status), output settings (`orientation_mode`, `timestamp_mode`,
|
|
||||||
per-channel `acc/gyr/mag_enabled`, `data_format`), `period`/`sample_rate_hz`, and the **XKF scenario**
|
|
||||||
(`scenario_type`/`version`, resolved to `scenario_label` against `available_profiles`). Built by
|
|
||||||
`applyImuConfigAck` (one ack frame at a time) + `finalizeImuConfig` (derive rate, resolve label).
|
|
||||||
Surfaced read-only in the TUI's `i` view (IMU CONFIG section); the legacy "scenario" is the modern
|
|
||||||
"filter profile" — same MIDs (`0x62/0x64/0x65`).
|
|
||||||
|
|
||||||
## On-disk artifacts
|
## On-disk artifacts
|
||||||
|
|
||||||
| Artifact | Path | Format |
|
| Artifact | Path | Format |
|
||||||
|
|
|
||||||
|
|
@ -39,38 +39,16 @@ struct CalibReport {
|
||||||
std::vector<Axis> axes;
|
std::vector<Axis> axes;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Tunable timings/sizes for a calibration run. Defaults are the production
|
|
||||||
// values; tests inject tiny ones so a full run completes in milliseconds.
|
|
||||||
struct CalibParams {
|
|
||||||
int positions = 10; // sweep steps per axis
|
|
||||||
int dwell_ms = 5000; // hold at each position while sampling
|
|
||||||
int sample_ms = 100; // IMU sampling cadence during a dwell
|
|
||||||
int settle_timeout_ms = 15000; // max wait for a single MOVE to settle
|
|
||||||
int home_timeout_ms = 90000; // max wait for HOME to reach READY
|
|
||||||
int norotation_s = 3; // no-rotation gyro-bias update duration
|
|
||||||
int reset_settle_ms = 1500; // let the filter apply the heading reset
|
|
||||||
double inset_frac = 0.05; // keep targets off the hard endstops
|
|
||||||
};
|
|
||||||
|
|
||||||
// `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own
|
// `gimbal calib`: an IMU-referenced steps<->degrees calibration. Runs on its own
|
||||||
// thread (the motor/imu/Logger interfaces are thread-safe). Sequence:
|
// thread (the motor/imu/Logger interfaces are thread-safe), sweeping each axis
|
||||||
// 1. home the gimbal if it is not already READY;
|
// across its homed soft-limit travel in equal step intervals, dwelling at each to
|
||||||
// 2. move yaw to its first sweep position and calibrate PITCH there (sweep its
|
// record the IMU orientation, then least-squares fitting counts vs degrees. The
|
||||||
// soft-limit travel, dwelling to record the gravity-referenced IMU pitch);
|
// resulting Geometry is published via takeResult() for the main thread to apply
|
||||||
// 3. move pitch to 0 deg;
|
// to the live session; the raw samples + fit are written to a logfile. Progress
|
||||||
// 4. switch the IMU to a no-magnetometer XKF profile (persists on the device) so
|
// is streamed to the LOG pane via LOG_INFO. Cancellable and one-at-a-time.
|
||||||
// its heading stops chasing the stepper-distorted magnetic field;
|
|
||||||
// 5. with the gimbal held still, run the IMU no-rotation update (cuts yaw drift)
|
|
||||||
// then reset the IMU heading so the current pose is yaw 0 — this removes the
|
|
||||||
// drift accumulated during the slow pitch sweep right before it matters;
|
|
||||||
// 6. sweep YAW from that first position and calibrate it.
|
|
||||||
// Each axis fit is least-squares (counts vs degrees). The resulting Geometry is
|
|
||||||
// published via takeResult() for the main thread to apply; raw samples + fits are
|
|
||||||
// written to a logfile. Progress streams to the LOG pane. Cancellable, one-at-a-time.
|
|
||||||
class CalibrationRoutine {
|
class CalibrationRoutine {
|
||||||
public:
|
public:
|
||||||
CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial);
|
||||||
CalibParams params = {});
|
|
||||||
~CalibrationRoutine();
|
~CalibrationRoutine();
|
||||||
|
|
||||||
// Begin on a worker thread. Logs a reason and returns false if already
|
// Begin on a worker thread. Logs a reason and returns false if already
|
||||||
|
|
@ -93,7 +71,6 @@ private:
|
||||||
IMotorController& motor_;
|
IMotorController& motor_;
|
||||||
IImuSource& imu_;
|
IImuSource& imu_;
|
||||||
Geometry initial_;
|
Geometry initial_;
|
||||||
CalibParams params_;
|
|
||||||
|
|
||||||
std::thread thread_;
|
std::thread thread_;
|
||||||
std::atomic<bool> running_{false};
|
std::atomic<bool> running_{false};
|
||||||
|
|
|
||||||
|
|
@ -21,32 +21,6 @@ public:
|
||||||
|
|
||||||
// Latest reading, or nullopt if none/stale.
|
// Latest reading, or nullopt if none/stale.
|
||||||
virtual std::optional<ImuSample> sample() = 0;
|
virtual std::optional<ImuSample> sample() = 0;
|
||||||
|
|
||||||
// Device configuration read back during start-up (output mode/settings,
|
|
||||||
// sample rate, identity, XKF scenario). nullopt if not yet known or the
|
|
||||||
// backend cannot report it.
|
|
||||||
virtual std::optional<ImuDeviceConfig> config() const { return std::nullopt; }
|
|
||||||
|
|
||||||
// Re-read the device configuration (e.g. after the XKF profile was changed
|
|
||||||
// externally). May briefly pause the measurement stream. Blocking; after it
|
|
||||||
// returns config() reflects the device's current settings.
|
|
||||||
virtual void refreshConfig() {}
|
|
||||||
|
|
||||||
// Run the "no rotation" gyro-bias update for `seconds`: the device must be
|
|
||||||
// held perfectly still while it estimates and cancels gyro bias, which cuts
|
|
||||||
// heading drift (important in no-magnetometer use). Non-blocking: it sends the
|
|
||||||
// command; the caller is responsible for keeping the unit still that long.
|
|
||||||
virtual void noRotation(int seconds) { (void)seconds; }
|
|
||||||
|
|
||||||
// Redefine the *current* heading as yaw = 0 (bore-sighting / heading reset).
|
|
||||||
// Subsequent yaw is measured relative to this pose. Non-blocking.
|
|
||||||
virtual void headingReset() {}
|
|
||||||
|
|
||||||
// Select the XKF profile/scenario by its `type` number (from config()'s
|
|
||||||
// available_profiles). Applied in the device's Config state, so it persists to
|
|
||||||
// non-volatile memory. Briefly pauses the stream; blocking. Returns true if the
|
|
||||||
// device reports the new profile afterwards. Default: not supported.
|
|
||||||
virtual bool setFilterProfile(int type) { (void)type; return false; }
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace fgc
|
} // namespace fgc
|
||||||
|
|
|
||||||
|
|
@ -20,11 +20,6 @@ public:
|
||||||
void stop() override;
|
void stop() override;
|
||||||
bool connected() const override;
|
bool connected() const override;
|
||||||
std::optional<ImuSample> sample() override;
|
std::optional<ImuSample> sample() override;
|
||||||
std::optional<ImuDeviceConfig> config() const override;
|
|
||||||
void refreshConfig() override;
|
|
||||||
bool setFilterProfile(int type) override;
|
|
||||||
void noRotation(int seconds) override;
|
|
||||||
void headingReset() override;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
struct Impl;
|
struct Impl;
|
||||||
|
|
|
||||||
|
|
@ -3,7 +3,6 @@
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace fgc {
|
namespace fgc {
|
||||||
|
|
@ -32,31 +31,6 @@ inline constexpr uint8_t kMidSetOutputSettingsAck = 0xD3;
|
||||||
inline constexpr uint8_t kMidMTData = 0x32;
|
inline constexpr uint8_t kMidMTData = 0x32;
|
||||||
inline constexpr uint8_t kMidError = 0x42;
|
inline constexpr uint8_t kMidError = 0x42;
|
||||||
|
|
||||||
// Config-readback queries (sent with an empty data field in Config State; the
|
|
||||||
// device replies with the matching ack MID = request MID + 1). ReqOutputMode /
|
|
||||||
// ReqOutputSettings reuse the Set MIDs above (len 0 => request, not set).
|
|
||||||
inline constexpr uint8_t kMidReqDID = 0x00;
|
|
||||||
inline constexpr uint8_t kMidDeviceID = 0x01;
|
|
||||||
inline constexpr uint8_t kMidReqPeriod = 0x04;
|
|
||||||
inline constexpr uint8_t kMidReqPeriodAck = 0x05;
|
|
||||||
inline constexpr uint8_t kMidReqFWRev = 0x12;
|
|
||||||
inline constexpr uint8_t kMidFirmwareRev = 0x13;
|
|
||||||
inline constexpr uint8_t kMidReqProductCode = 0x1C;
|
|
||||||
inline constexpr uint8_t kMidProductCode = 0x1D;
|
|
||||||
inline constexpr uint8_t kMidReqAvailFilterProf = 0x62;
|
|
||||||
inline constexpr uint8_t kMidAvailFilterProf = 0x63;
|
|
||||||
inline constexpr uint8_t kMidReqFilterProfile = 0x64; // SetScenario shares this MID
|
|
||||||
inline constexpr uint8_t kMidReqFilterProfileAck = 0x65;
|
|
||||||
|
|
||||||
// Orientation-control commands (valid in Measurement State).
|
|
||||||
inline constexpr uint8_t kMidSetNoRotation = 0x22; // 2-byte duration (seconds)
|
|
||||||
inline constexpr uint8_t kMidSetNoRotationAck = 0x23;
|
|
||||||
inline constexpr uint8_t kMidResetOrientation = 0xA4; // 2-byte CODE (Table 33)
|
|
||||||
inline constexpr uint8_t kMidResetOrientationAck = 0xA5;
|
|
||||||
// ResetOrientation CODE values we use.
|
|
||||||
inline constexpr uint16_t kResetHeading = 0x0001; // current heading becomes yaw 0
|
|
||||||
inline constexpr uint16_t kResetStore = 0x0000; // persist current reset (Config state)
|
|
||||||
|
|
||||||
// OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04).
|
// OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04).
|
||||||
inline constexpr uint16_t kOutputMode = 0x0007;
|
inline constexpr uint16_t kOutputMode = 0x0007;
|
||||||
// OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp
|
// OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp
|
||||||
|
|
@ -79,91 +53,6 @@ struct ImuSample {
|
||||||
uint16_t sample_counter = 0;
|
uint16_t sample_counter = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// One available filter profile (a.k.a. XKF "scenario") as reported by the device
|
|
||||||
// in the AvailableFilterProfiles message.
|
|
||||||
struct ImuFilterProfile {
|
|
||||||
uint8_t type = 0;
|
|
||||||
uint8_t version = 0;
|
|
||||||
std::string label; // human name, e.g. "General" / "VRU_general"
|
|
||||||
};
|
|
||||||
|
|
||||||
// Device configuration read back during the Config-state handshake. Each `has_*`
|
|
||||||
// flag marks whether the corresponding ack was actually received and decoded, so
|
|
||||||
// the UI can show "—" for anything the device did not answer.
|
|
||||||
struct ImuDeviceConfig {
|
|
||||||
bool valid = false; // at least one field was populated
|
|
||||||
|
|
||||||
// Identity.
|
|
||||||
std::string product_code; // e.g. "MTi-28A33G85"
|
|
||||||
bool has_device_id = false;
|
|
||||||
uint32_t device_id = 0; // serial / device ID
|
|
||||||
std::string firmware; // "2.3.1 build 25" ("" if unknown)
|
|
||||||
|
|
||||||
// Output mode (which data the device streams).
|
|
||||||
bool has_output_mode = false;
|
|
||||||
uint16_t output_mode = 0;
|
|
||||||
bool out_temperature = false, out_calibrated = false;
|
|
||||||
bool out_orientation = false, out_auxiliary = false, out_status = false;
|
|
||||||
|
|
||||||
// Output settings (how the data is formatted).
|
|
||||||
bool has_output_settings = false;
|
|
||||||
uint32_t output_settings = 0;
|
|
||||||
std::string orientation_mode; // "Quaternion" / "Euler" / "Matrix"
|
|
||||||
std::string timestamp_mode; // "Sample counter" / "None"
|
|
||||||
bool acc_enabled = true, gyr_enabled = true, mag_enabled = true;
|
|
||||||
std::string data_format; // "Float" / "Fixed 12.20"
|
|
||||||
|
|
||||||
// Sample rate.
|
|
||||||
bool has_period = false;
|
|
||||||
uint16_t period = 0; // raw, resolution 1/115200 s
|
|
||||||
float sample_rate_hz = 0.f;
|
|
||||||
|
|
||||||
// Filter profile / XKF scenario.
|
|
||||||
bool has_scenario = false;
|
|
||||||
uint8_t scenario_type = 0, scenario_version = 0;
|
|
||||||
std::string scenario_label; // resolved from available_profiles, else ""
|
|
||||||
std::vector<ImuFilterProfile> available_profiles;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Orientation-control command builders.
|
|
||||||
// SetNoRotation: run the "no rotation" gyro-bias update for `seconds` (the device
|
|
||||||
// must be held still during it) to cut heading drift, esp. in no-magnetometer use.
|
|
||||||
std::vector<uint8_t> msgSetNoRotation(uint16_t seconds);
|
|
||||||
// ResetOrientation: CODE 0x0001 redefines the *current* heading as yaw 0
|
|
||||||
// (bore-sighting); CODE 0x0000 (in Config state) stores it across power cycles.
|
|
||||||
std::vector<uint8_t> msgResetOrientation(uint16_t code);
|
|
||||||
|
|
||||||
// SetFilterProfile (classic): select the XKF profile/scenario by type number.
|
|
||||||
// Valid in Config state; settings changed in Config state persist to the device's
|
|
||||||
// non-volatile memory automatically.
|
|
||||||
std::vector<uint8_t> msgSetFilterProfile(uint16_t profile);
|
|
||||||
|
|
||||||
// Pick the no-magnetometer XKF profile from the device's reported list: prefers a
|
|
||||||
// label containing "nomag"/"no_mag", else a "VRU" (gyro-tracked heading) profile.
|
|
||||||
// Returns the profile `type` number, or -1 if none looks magnetometer-free.
|
|
||||||
int pickNoMagProfile(const std::vector<ImuFilterProfile>& profiles);
|
|
||||||
|
|
||||||
// Config-readback query builders (empty data field => "request", not "set").
|
|
||||||
std::vector<uint8_t> msgReqProductCode();
|
|
||||||
std::vector<uint8_t> msgReqDID();
|
|
||||||
std::vector<uint8_t> msgReqFWRev();
|
|
||||||
std::vector<uint8_t> msgReqPeriod();
|
|
||||||
std::vector<uint8_t> msgReqOutputMode(); // kMidSetOutputMode, len 0
|
|
||||||
std::vector<uint8_t> msgReqOutputSettings(); // kMidSetOutputSettings, len 0
|
|
||||||
std::vector<uint8_t> msgReqFilterProfile();
|
|
||||||
std::vector<uint8_t> msgReqAvailFilterProfiles();
|
|
||||||
|
|
||||||
// Decode a single config-ack frame into `c`. Recognizes DeviceID(0x01),
|
|
||||||
// ProductCode(0x1D), FirmwareRev(0x13), ReqPeriodAck(0x05), output-mode ack
|
|
||||||
// (0xD1), output-settings ack (0xD3), filter-profile ack (0x65), and the
|
|
||||||
// available-profiles list (0x63). Unknown MIDs are ignored. Returns true if the
|
|
||||||
// frame was recognized and applied.
|
|
||||||
bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n);
|
|
||||||
|
|
||||||
// Resolve derived fields once all acks are applied: the scenario label (matched
|
|
||||||
// against available_profiles by type) and sample_rate_hz from the period.
|
|
||||||
void finalizeImuConfig(ImuDeviceConfig& c);
|
|
||||||
|
|
||||||
// Lower byte of the sum of all bytes from BID through the end of DATA. The CS
|
// 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.
|
// 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);
|
uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len);
|
||||||
|
|
|
||||||
|
|
@ -44,55 +44,9 @@ public:
|
||||||
return s;
|
return s;
|
||||||
}
|
}
|
||||||
|
|
||||||
void refreshConfig() override { LOG_INFO << "[mock] IMU config refreshed"; }
|
|
||||||
void noRotation(int seconds) override {
|
|
||||||
LOG_INFO << "[mock] IMU no-rotation update for " << seconds << " s";
|
|
||||||
}
|
|
||||||
void headingReset() override {
|
|
||||||
LOG_INFO << "[mock] IMU heading reset (current direction is now yaw 0)";
|
|
||||||
}
|
|
||||||
bool setFilterProfile(int type) override {
|
|
||||||
scenario_type_ = type; // observable in the next config()
|
|
||||||
LOG_INFO << "[mock] IMU XKF profile set to type " << type;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Synthetic config mirroring the real handshake (Euler + calibrated, 100 Hz),
|
|
||||||
// so the expanded view's IMU CONFIG section renders without hardware. The
|
|
||||||
// available list includes a no-mag profile so the calibration's profile switch
|
|
||||||
// has something to pick.
|
|
||||||
std::optional<ImuDeviceConfig> config() const override {
|
|
||||||
ImuDeviceConfig c;
|
|
||||||
c.valid = true;
|
|
||||||
c.product_code = "MTi-28A53G35 (mock)";
|
|
||||||
c.has_device_id = true;
|
|
||||||
c.device_id = 0x00990ABC;
|
|
||||||
c.firmware = "2.8.1 build 0";
|
|
||||||
c.has_output_mode = true;
|
|
||||||
c.output_mode = kOutputMode;
|
|
||||||
c.out_temperature = c.out_calibrated = c.out_orientation = true;
|
|
||||||
c.has_output_settings = true;
|
|
||||||
c.output_settings = kOutputSettings;
|
|
||||||
c.orientation_mode = "Euler";
|
|
||||||
c.timestamp_mode = "Sample counter";
|
|
||||||
c.data_format = "Float";
|
|
||||||
c.has_period = true;
|
|
||||||
c.period = 1152; // 100 Hz
|
|
||||||
c.sample_rate_hz = 100.0f;
|
|
||||||
c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
|
|
||||||
{41, 11, "Dynamic"}, {53, 11, "VRU_general"}};
|
|
||||||
c.has_scenario = true;
|
|
||||||
c.scenario_type = static_cast<uint8_t>(scenario_type_);
|
|
||||||
c.scenario_version = 11;
|
|
||||||
for (const auto& p : c.available_profiles)
|
|
||||||
if (p.type == c.scenario_type) c.scenario_label = p.label;
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
using clock = std::chrono::steady_clock;
|
using clock = std::chrono::steady_clock;
|
||||||
clock::time_point start_ = clock::now();
|
clock::time_point start_ = clock::now();
|
||||||
int scenario_type_ = 39; // current XKF profile (mutable via setFilterProfile)
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace fgc
|
} // namespace fgc
|
||||||
|
|
|
||||||
|
|
@ -118,30 +118,6 @@ struct DumpView {
|
||||||
std::string text;
|
std::string text;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Device configuration read back from the MTi at startup, formatted for the
|
|
||||||
// expanded Sensors view. `present` is false until the handshake reports it (or
|
|
||||||
// for backends that cannot report it).
|
|
||||||
// One XKF (Xsens Kalman Filter) profile the device supports, with whether it is
|
|
||||||
// the active one.
|
|
||||||
struct ImuProfileView {
|
|
||||||
std::string name; // human label, no numeric IDs ("General")
|
|
||||||
bool selected = false; // the profile currently in use
|
|
||||||
};
|
|
||||||
|
|
||||||
struct ImuConfigView {
|
|
||||||
bool present = false;
|
|
||||||
std::string product_code; // "MTi-28A53G35"
|
|
||||||
std::string device_id; // "0x00990ABC"
|
|
||||||
std::string firmware; // "2.8.1 build 0"
|
|
||||||
std::string output_mode; // "Temp · Calibrated · Orientation"
|
|
||||||
std::string output_settings; // "Euler · Sample counter · Float"
|
|
||||||
std::string channels; // "acc gyr mag"
|
|
||||||
std::string sample_rate; // "100 Hz"
|
|
||||||
// Available XKF profiles with the active one flagged. Empty if the device
|
|
||||||
// did not report them.
|
|
||||||
std::vector<ImuProfileView> xkf_profiles;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Full Xsens MTi reading for the expanded Sensors view (units: acc m/s^2,
|
// Full Xsens MTi reading for the expanded Sensors view (units: acc m/s^2,
|
||||||
// gyr rad/s, mag a.u., angles deg, temp °C).
|
// gyr rad/s, mag a.u., angles deg, temp °C).
|
||||||
struct ImuView {
|
struct ImuView {
|
||||||
|
|
@ -152,7 +128,6 @@ struct ImuView {
|
||||||
float mag[3] = {0, 0, 0};
|
float mag[3] = {0, 0, 0};
|
||||||
float temp_c = 0;
|
float temp_c = 0;
|
||||||
unsigned sample_counter = 0;
|
unsigned sample_counter = 0;
|
||||||
ImuConfigView config;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
// Status of the currently-running special operation (calibration, diagnostics,
|
// Status of the currently-running special operation (calibration, diagnostics,
|
||||||
|
|
|
||||||
|
|
@ -73,70 +73,6 @@ std::string traceNames(unsigned mask) {
|
||||||
return out;
|
return out;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Build the expanded-view ImuConfigView from the device's decoded config.
|
|
||||||
ImuConfigView formatImuConfig(const ImuDeviceConfig& c) {
|
|
||||||
ImuConfigView v;
|
|
||||||
v.present = true;
|
|
||||||
v.product_code = c.product_code.empty() ? "—" : c.product_code;
|
|
||||||
if (c.has_device_id) {
|
|
||||||
char b[16];
|
|
||||||
std::snprintf(b, sizeof(b), "0x%08X", c.device_id);
|
|
||||||
v.device_id = b;
|
|
||||||
} else {
|
|
||||||
v.device_id = "—";
|
|
||||||
}
|
|
||||||
v.firmware = c.firmware.empty() ? "—" : c.firmware;
|
|
||||||
|
|
||||||
if (c.has_output_mode) {
|
|
||||||
std::string m;
|
|
||||||
auto add = [&](bool on, const char* name) {
|
|
||||||
if (on) { if (!m.empty()) m += " · "; m += name; }
|
|
||||||
};
|
|
||||||
add(c.out_temperature, "Temp");
|
|
||||||
add(c.out_calibrated, "Calibrated");
|
|
||||||
add(c.out_orientation, "Orientation");
|
|
||||||
add(c.out_auxiliary, "Auxiliary");
|
|
||||||
add(c.out_status, "Status");
|
|
||||||
v.output_mode = m.empty() ? "—" : m;
|
|
||||||
} else {
|
|
||||||
v.output_mode = "—";
|
|
||||||
}
|
|
||||||
|
|
||||||
if (c.has_output_settings) {
|
|
||||||
v.output_settings = c.orientation_mode + " · " + c.timestamp_mode + " · " + c.data_format;
|
|
||||||
std::string ch;
|
|
||||||
if (c.acc_enabled) ch += "acc ";
|
|
||||||
if (c.gyr_enabled) ch += "gyr ";
|
|
||||||
if (c.mag_enabled) ch += "mag";
|
|
||||||
v.channels = ch.empty() ? "none" : ch;
|
|
||||||
} else {
|
|
||||||
v.output_settings = "—";
|
|
||||||
v.channels = "—";
|
|
||||||
}
|
|
||||||
|
|
||||||
if (c.has_period && c.sample_rate_hz > 0) {
|
|
||||||
char b[24];
|
|
||||||
std::snprintf(b, sizeof(b), "%.0f Hz", c.sample_rate_hz);
|
|
||||||
v.sample_rate = b;
|
|
||||||
} else {
|
|
||||||
v.sample_rate = "—";
|
|
||||||
}
|
|
||||||
|
|
||||||
// XKF profiles: list every available profile by name (no numeric IDs) and
|
|
||||||
// flag the active one (matched by profile type).
|
|
||||||
for (const auto& p : c.available_profiles) {
|
|
||||||
ImuProfileView pv;
|
|
||||||
pv.name = p.label.empty() ? "(unnamed)" : p.label;
|
|
||||||
pv.selected = c.has_scenario && p.type == c.scenario_type;
|
|
||||||
v.xkf_profiles.push_back(std::move(pv));
|
|
||||||
}
|
|
||||||
// If the device reported the active profile but not the full list, still show
|
|
||||||
// the one in use (by its resolved label, when known).
|
|
||||||
if (v.xkf_profiles.empty() && c.has_scenario && !c.scenario_label.empty())
|
|
||||||
v.xkf_profiles.push_back({c.scenario_label, true});
|
|
||||||
return v;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
struct Application::Impl {
|
struct Application::Impl {
|
||||||
|
|
@ -166,8 +102,6 @@ struct Application::Impl {
|
||||||
std::vector<std::string> last_calib_summary;
|
std::vector<std::string> last_calib_summary;
|
||||||
long long last_calib_ts = 0; // epoch ms
|
long long last_calib_ts = 0; // epoch ms
|
||||||
bool calib_save_pending_ = false; // awaiting y/n to persist to config
|
bool calib_save_pending_ = false; // awaiting y/n to persist to config
|
||||||
mutable ImuConfigView imu_config_view_; // formatted MTi config (read once)
|
|
||||||
mutable bool imu_config_done_ = false;
|
|
||||||
|
|
||||||
std::atomic<bool> running{true};
|
std::atomic<bool> running{true};
|
||||||
std::mutex cmd_mutex;
|
std::mutex cmd_mutex;
|
||||||
|
|
@ -309,14 +243,6 @@ struct Application::Impl {
|
||||||
f.value = v;
|
f.value = v;
|
||||||
f.present = true;
|
f.present = true;
|
||||||
};
|
};
|
||||||
// Device configuration (read once at startup): format and cache.
|
|
||||||
if (!imu_config_done_) {
|
|
||||||
if (auto c = imu->config()) {
|
|
||||||
imu_config_view_ = formatImuConfig(*c);
|
|
||||||
imu_config_done_ = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
s.imu.config = imu_config_view_;
|
|
||||||
if (auto m = imu->sample()) {
|
if (auto m = imu->sample()) {
|
||||||
// Full reading for the expanded view.
|
// Full reading for the expanded view.
|
||||||
s.imu.present = true;
|
s.imu.present = true;
|
||||||
|
|
@ -530,17 +456,15 @@ struct Application::Impl {
|
||||||
return static_cast<bool>(iss >> a >> b);
|
return static_cast<bool>(iss >> a >> b);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Homed physical travel (counts) for an axis: the endstop-to-endstop span from
|
// Soft-limit travel (counts) for an axis from the last dump, else the
|
||||||
// the last firmware dump. Returns 0 if no dump has been captured yet (caller
|
// configured degree range converted to counts. Returns 0 if unknown.
|
||||||
// should request one). Deliberately does NOT fall back to the configured degree
|
long axisRangeCounts(char axis) {
|
||||||
// range × counts_per_deg — that conflates calibration with physical travel and,
|
|
||||||
// when min/max are unset, yields an absurd span.
|
|
||||||
long homedTravelCounts(char axis) {
|
|
||||||
DumpData d = parseDump(motor->lastDump());
|
DumpData d = parseDump(motor->lastDump());
|
||||||
if (d.valid)
|
if (d.valid)
|
||||||
for (const auto& ax : d.axes)
|
for (const auto& ax : d.axes)
|
||||||
if (ax.axis == axis) return std::labs(ax.lim_pos - ax.lim_neg);
|
if (ax.axis == axis) return std::labs(ax.lim_pos - ax.lim_neg);
|
||||||
return 0;
|
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
|
// `gimbal <subcommand>` — unified, lowercase motor control. `move` is degrees
|
||||||
|
|
@ -552,7 +476,7 @@ struct Application::Impl {
|
||||||
while (iss >> t) tok.push_back(t); // tok[0] == "gimbal"
|
while (iss >> t) tok.push_back(t); // tok[0] == "gimbal"
|
||||||
if (tok.size() < 2) {
|
if (tok.size() < 2) {
|
||||||
LOG_WARN << "usage: gimbal <move|steps|nudge|home|stop|reset|enable|disable|"
|
LOG_WARN << "usage: gimbal <move|steps|nudge|home|stop|reset|enable|disable|"
|
||||||
"speed|setpos|status|dump|diag|calib|raw>";
|
"speed|setpos|status|dump|diag|calib>";
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
std::string sub = tok[1];
|
std::string sub = tok[1];
|
||||||
|
|
@ -581,16 +505,8 @@ struct Application::Impl {
|
||||||
char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
|
char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
|
||||||
double pct = 0;
|
double pct = 0;
|
||||||
try { pct = std::stod(tok[3]); } catch (...) { LOG_WARN << "gimbal nudge: bad percent"; return; }
|
try { pct = std::stod(tok[3]); } catch (...) { LOG_WARN << "gimbal nudge: bad percent"; return; }
|
||||||
// Nudge is a fraction of the *homed* physical travel (endstop-to-endstop
|
long range = axisRangeCounts(axis);
|
||||||
// from the firmware dump), so it is independent of calibration and never
|
if (range <= 0) { LOG_WARN << "gimbal nudge: travel unknown; run gimbal home/dump first"; motor->sendCommand("DUMP"); return; }
|
||||||
// a fraction of the configured degree clamps. If no dump is available
|
|
||||||
// yet, request one and bail rather than guessing.
|
|
||||||
long range = homedTravelCounts(axis);
|
|
||||||
if (range <= 0) {
|
|
||||||
LOG_WARN << "gimbal nudge: homed travel unknown; home or 'gimbal dump' first";
|
|
||||||
motor->sendCommand("DUMP");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
MotorTelemetry tel = motor->telemetry();
|
MotorTelemetry tel = motor->telemetry();
|
||||||
long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
|
long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
|
||||||
long target = cur + static_cast<long>(pct / 100.0 * range);
|
long target = cur + static_cast<long>(pct / 100.0 * range);
|
||||||
|
|
@ -613,25 +529,6 @@ struct Application::Impl {
|
||||||
} else if (sub == "dump") {
|
} else if (sub == "dump") {
|
||||||
LOG_INFO << "requesting firmware dump...";
|
LOG_INFO << "requesting firmware dump...";
|
||||||
motor->sendCommand("DUMP");
|
motor->sendCommand("DUMP");
|
||||||
} else if (sub == "raw") {
|
|
||||||
// Low-level escape hatch: send the text inside the quotation marks to
|
|
||||||
// the serial port verbatim (the transport appends the newline). This
|
|
||||||
// exposes any firmware verb directly, including ones with no wrapper.
|
|
||||||
std::string raw;
|
|
||||||
auto q1 = line.find('"');
|
|
||||||
auto q2 = line.rfind('"');
|
|
||||||
if (q1 != std::string::npos && q2 != std::string::npos && q2 > q1) {
|
|
||||||
raw = line.substr(q1 + 1, q2 - q1 - 1); // exact text between quotes
|
|
||||||
} else {
|
|
||||||
for (size_t i = 2; i < tok.size(); ++i) // unquoted: join remaining tokens
|
|
||||||
raw += (raw.empty() ? "" : " ") + tok[i];
|
|
||||||
}
|
|
||||||
if (raw.empty()) {
|
|
||||||
LOG_WARN << "usage: gimbal raw \"<verbatim firmware command>\"";
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
LOG_INFO << "gimbal raw -> " << raw;
|
|
||||||
motor->sendCommand(raw);
|
|
||||||
} else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" ||
|
} else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" ||
|
||||||
sub == "disable" || sub == "speed" || sub == "setpos" || sub == "status") {
|
sub == "disable" || sub == "speed" || sub == "setpos" || sub == "status") {
|
||||||
// Passthrough to the (case-insensitive) firmware verb + args.
|
// Passthrough to the (case-insensitive) firmware verb + args.
|
||||||
|
|
@ -759,16 +656,6 @@ struct Application::Impl {
|
||||||
bool on = Logger::level() != LogLevel::Debug;
|
bool on = Logger::level() != LogLevel::Debug;
|
||||||
Logger::setLevel(on ? LogLevel::Debug : LogLevel::Info);
|
Logger::setLevel(on ? LogLevel::Debug : LogLevel::Info);
|
||||||
LOG_INFO << "debug logging " << (on ? "on" : "off");
|
LOG_INFO << "debug logging " << (on ? "on" : "off");
|
||||||
} else if (c.verb == "refresh") {
|
|
||||||
// Re-read live device state surfaced in the expanded views: the IMU
|
|
||||||
// configuration (e.g. after the XKF profile was changed externally)
|
|
||||||
// and the firmware register dump shown in the gimbal 'g' view.
|
|
||||||
LOG_INFO << "refresh: re-reading IMU configuration + firmware dump";
|
|
||||||
if (imu) {
|
|
||||||
imu->refreshConfig();
|
|
||||||
imu_config_done_ = false; // force buildSnapshot to re-read config()
|
|
||||||
}
|
|
||||||
motor->sendCommand("DUMP");
|
|
||||||
} else if (c.verb == "trace") {
|
} else if (c.verb == "trace") {
|
||||||
handleTrace(c);
|
handleTrace(c);
|
||||||
} else if (c.verb == "set") {
|
} else if (c.verb == "set") {
|
||||||
|
|
|
||||||
|
|
@ -19,13 +19,17 @@ namespace fgc {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
using clock = std::chrono::steady_clock;
|
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)); }
|
void msleep(int ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); }
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial,
|
CalibrationRoutine::CalibrationRoutine(IMotorController& motor, IImuSource& imu, Geometry initial)
|
||||||
CalibParams params)
|
: motor_(motor), imu_(imu), initial_(std::move(initial)) {}
|
||||||
: motor_(motor), imu_(imu), initial_(std::move(initial)), params_(params) {}
|
|
||||||
|
|
||||||
CalibrationRoutine::~CalibrationRoutine() {
|
CalibrationRoutine::~CalibrationRoutine() {
|
||||||
cancel();
|
cancel();
|
||||||
|
|
@ -82,9 +86,9 @@ long long nowMs() {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
// Wait until the axis is at `target` (standstill + within tol), or timeout/cancel.
|
// Wait until the axis is at `target` (standstill + within tol), or timeout/cancel.
|
||||||
bool waitSettle(IMotorController& motor, char axis, long target, long tol, int timeout_ms,
|
bool waitSettle(IMotorController& motor, char axis, long target, long tol,
|
||||||
const std::atomic<bool>& cancel) {
|
const std::atomic<bool>& cancel) {
|
||||||
const auto deadline = clock::now() + std::chrono::milliseconds(timeout_ms);
|
const auto deadline = clock::now() + std::chrono::milliseconds(kSettleTimeoutMs);
|
||||||
while (clock::now() < deadline) {
|
while (clock::now() < deadline) {
|
||||||
if (cancel) return false;
|
if (cancel) return false;
|
||||||
MotorTelemetry t = motor.telemetry();
|
MotorTelemetry t = motor.telemetry();
|
||||||
|
|
@ -95,29 +99,16 @@ bool waitSettle(IMotorController& motor, char axis, long target, long tol, int t
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Wait until both present axes report READY (homing complete), or timeout/cancel.
|
// Dwell `kDwellMs` sampling the IMU; return the (circular for yaw) mean of the
|
||||||
bool waitHomed(IMotorController& motor, int timeout_ms, const std::atomic<bool>& cancel) {
|
|
||||||
const auto deadline = clock::now() + std::chrono::milliseconds(timeout_ms);
|
|
||||||
while (clock::now() < deadline) {
|
|
||||||
if (cancel) return false;
|
|
||||||
MotorTelemetry t = motor.telemetry();
|
|
||||||
if (t.yaw.ready() && (!t.pitch_present || t.pitch.ready())) return true;
|
|
||||||
msleep(200);
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Dwell `dwell_ms` sampling the IMU; return the (circular for yaw) mean of the
|
|
||||||
// requested Euler angle in degrees.
|
// requested Euler angle in degrees.
|
||||||
double dwellAndMeasure(IImuSource& imu, char axis, int dwell_ms, int sample_ms,
|
double dwellAndMeasure(IImuSource& imu, char axis, const std::atomic<bool>& cancel) {
|
||||||
const std::atomic<bool>& cancel) {
|
|
||||||
std::vector<double> vals;
|
std::vector<double> vals;
|
||||||
const auto end = clock::now() + std::chrono::milliseconds(dwell_ms);
|
const auto end = clock::now() + std::chrono::milliseconds(kDwellMs);
|
||||||
while (clock::now() < end) {
|
while (clock::now() < end) {
|
||||||
if (cancel) break;
|
if (cancel) break;
|
||||||
if (auto s = imu.sample())
|
if (auto s = imu.sample())
|
||||||
vals.push_back(axis == 'Y' ? s->yaw_deg : s->pitch_deg);
|
vals.push_back(axis == 'Y' ? s->yaw_deg : s->pitch_deg);
|
||||||
msleep(sample_ms);
|
msleep(kSampleMs);
|
||||||
}
|
}
|
||||||
if (vals.empty()) return 0.0;
|
if (vals.empty()) return 0.0;
|
||||||
if (axis == 'Y') {
|
if (axis == 'Y') {
|
||||||
|
|
@ -142,9 +133,8 @@ void CalibrationRoutine::run() {
|
||||||
}
|
}
|
||||||
} done{this};
|
} done{this};
|
||||||
|
|
||||||
const int N = params_.positions;
|
|
||||||
LOG_INFO << "=== gimbal calibration starting ===";
|
LOG_INFO << "=== gimbal calibration starting ===";
|
||||||
setProgress('?', 0, N, "starting");
|
setProgress('?', 0, kPositions, "starting");
|
||||||
CalibReport report;
|
CalibReport report;
|
||||||
|
|
||||||
// 1. Preconditions.
|
// 1. Preconditions.
|
||||||
|
|
@ -152,22 +142,11 @@ void CalibrationRoutine::run() {
|
||||||
LOG_WARN << "calibration aborted: IMU not connected";
|
LOG_WARN << "calibration aborted: IMU not connected";
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 2. Home first if the gimbal is not already READY.
|
|
||||||
MotorTelemetry t0 = motor_.telemetry();
|
MotorTelemetry t0 = motor_.telemetry();
|
||||||
bool homed = t0.yaw.ready() && (!t0.pitch_present || t0.pitch.ready());
|
if (!t0.yaw.ready() || (t0.pitch_present && !t0.pitch.ready())) {
|
||||||
if (!homed) {
|
LOG_WARN << "calibration aborted: axes must be homed (READY) first";
|
||||||
LOG_INFO << "calibration: gimbal not homed; homing first...";
|
|
||||||
setProgress('?', 0, N, "homing");
|
|
||||||
motor_.sendCommand("HOME");
|
|
||||||
if (!waitHomed(motor_, params_.home_timeout_ms, cancel_)) {
|
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
|
||||||
LOG_WARN << "calibration aborted: homing did not reach READY";
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
t0 = motor_.telemetry();
|
|
||||||
}
|
|
||||||
const bool pitch_present = t0.pitch_present;
|
|
||||||
|
|
||||||
// Fetch soft limits from a fresh dump.
|
// Fetch soft limits from a fresh dump.
|
||||||
LOG_INFO << "calibration: requesting soft limits (dump)...";
|
LOG_INFO << "calibration: requesting soft limits (dump)...";
|
||||||
|
|
@ -187,143 +166,73 @@ void CalibrationRoutine::run() {
|
||||||
std::ostringstream log;
|
std::ostringstream log;
|
||||||
log << "gimbal calibration\n==================\n";
|
log << "gimbal calibration\n==================\n";
|
||||||
|
|
||||||
// Per-axis sweep span [a..b] (inset off the hard endstops) + a settle tolerance.
|
struct AxisJob { char axis; AxisMap* map; };
|
||||||
auto axisSpan = [&](char axis, long& a, long& b, long& tol) -> bool {
|
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;
|
const DumpAxis* da = nullptr;
|
||||||
for (const auto& ax : dump.axes) if (ax.axis == axis) da = &ax;
|
for (const auto& ax : dump.axes) if (ax.axis == job.axis) da = &ax;
|
||||||
if (!da) return false;
|
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 lo = da->lim_neg, hi = da->lim_pos;
|
||||||
const long travel = std::labs(hi - lo);
|
const long travel = std::labs(hi - lo);
|
||||||
const long inset = static_cast<long>(params_.inset_frac * travel);
|
const long inset = static_cast<long>(kInsetFrac * travel);
|
||||||
a = lo + inset;
|
const long a = lo + inset, b = hi - inset;
|
||||||
b = hi - inset;
|
const long tol = std::max<long>(500, travel / 200);
|
||||||
tol = std::max<long>(500, travel / 200);
|
|
||||||
return true;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Sweep one axis across [a..b] in N steps, dwelling to measure the IMU angle,
|
LOG_INFO << "calibrating " << (job.axis == 'Y' ? "YAW" : "PITCH")
|
||||||
// then least-squares fit counts vs degrees and record the result. Returns
|
<< " over [" << a << ".." << b << "] counts in " << kPositions << " steps";
|
||||||
// false on cancel; on a degenerate fit it records a failed axis and returns true.
|
log << "\n[" << job.axis << "] travel=[" << lo << ".." << hi << "] tol=" << tol
|
||||||
auto sweepAxis = [&](char axis, AxisMap& map, long a, long b, long tol) -> bool {
|
|
||||||
LOG_INFO << "calibrating " << (axis == 'Y' ? "YAW" : "PITCH")
|
|
||||||
<< " over [" << a << ".." << b << "] counts in " << N << " steps";
|
|
||||||
log << "\n[" << axis << "] sweep=[" << a << ".." << b << "] tol=" << tol
|
|
||||||
<< "\n target_counts, imu_deg\n";
|
<< "\n target_counts, imu_deg\n";
|
||||||
|
|
||||||
std::vector<std::pair<double, double>> pts; // (deg, counts)
|
std::vector<std::pair<double, double>> pts; // (deg, counts)
|
||||||
double prev_deg = 0.0;
|
double prev_deg = 0.0;
|
||||||
bool have_prev = false;
|
bool have_prev = false;
|
||||||
for (int i = 0; i < N; ++i) {
|
for (int i = 0; i < kPositions; ++i) {
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return false; }
|
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||||
long target = a + (b - a) * i / (N - 1);
|
long target = a + (b - a) * i / (kPositions - 1);
|
||||||
setProgress(axis, i + 1, N, "moving");
|
setProgress(job.axis, i + 1, kPositions, "moving");
|
||||||
motor_.sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
|
motor_.sendCommand(std::string("MOVE ") + job.axis + " " + std::to_string(target));
|
||||||
if (!waitSettle(motor_, axis, target, tol, params_.settle_timeout_ms, cancel_)) {
|
if (!waitSettle(motor_, job.axis, target, tol, cancel_)) {
|
||||||
if (cancel_) return false;
|
if (cancel_) return;
|
||||||
LOG_WARN << " position " << (i + 1) << "/" << N
|
LOG_WARN << " position " << (i + 1) << "/" << kPositions
|
||||||
<< " did not settle near " << target << " (continuing)";
|
<< " did not settle near " << target << " (continuing)";
|
||||||
}
|
}
|
||||||
setProgress(axis, i + 1, N, "dwelling");
|
setProgress(job.axis, i + 1, kPositions, "dwelling");
|
||||||
double deg = dwellAndMeasure(imu_, axis, params_.dwell_ms, params_.sample_ms, cancel_);
|
double deg = dwellAndMeasure(imu_, job.axis, cancel_);
|
||||||
// Unwrap against the previous sample so a sweep crossing 0/360 stays a
|
// Unwrap against the previous sample so a sweep crossing 0/360 stays a
|
||||||
// continuous line for the fit (no ±360 jump).
|
// continuous line for the fit (no ±360 jump).
|
||||||
if (have_prev) deg = unwrapNear(prev_deg, deg);
|
if (have_prev) deg = unwrapNear(prev_deg, deg);
|
||||||
prev_deg = deg;
|
prev_deg = deg;
|
||||||
have_prev = true;
|
have_prev = true;
|
||||||
pts.emplace_back(deg, static_cast<double>(target));
|
pts.emplace_back(deg, static_cast<double>(target));
|
||||||
LOG_INFO << " " << (i + 1) << "/" << N << " counts=" << target
|
LOG_INFO << " " << (i + 1) << "/" << kPositions << " counts=" << target
|
||||||
<< " imu=" << deg << " deg";
|
<< " imu=" << deg << " deg";
|
||||||
log << " " << target << ", " << deg << "\n";
|
log << " " << target << ", " << deg << "\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
setProgress(axis, N, N, "fitting");
|
setProgress(job.axis, kPositions, kPositions, "fitting");
|
||||||
LinearFit fit = linearFit(pts);
|
LinearFit fit = linearFit(pts);
|
||||||
if (!fit.ok) {
|
if (!fit.ok) {
|
||||||
LOG_WARN << " fit failed for axis " << axis << " (degenerate data)";
|
LOG_WARN << " fit failed for axis " << job.axis << " (degenerate data)";
|
||||||
log << " FIT FAILED\n";
|
log << " FIT FAILED\n";
|
||||||
report.axes.push_back({axis, false, 0, 0, 0, static_cast<int>(pts.size())});
|
report.axes.push_back({job.axis, false, 0, 0, 0, static_cast<int>(pts.size())});
|
||||||
return true;
|
continue;
|
||||||
}
|
}
|
||||||
const double old_cpd = map.counts_per_deg;
|
const double old_cpd = job.map->counts_per_deg;
|
||||||
const long old_zc = map.zero_count;
|
const long old_zc = job.map->zero_count;
|
||||||
map.counts_per_deg = fit.slope;
|
job.map->counts_per_deg = fit.slope;
|
||||||
map.zero_count = static_cast<long>(std::lround(fit.intercept));
|
job.map->zero_count = static_cast<long>(std::lround(fit.intercept));
|
||||||
LOG_INFO << " fit: counts_per_deg " << old_cpd << " -> " << fit.slope
|
LOG_INFO << " fit: counts_per_deg " << old_cpd << " -> " << fit.slope
|
||||||
<< ", zero_count " << old_zc << " -> " << map.zero_count
|
<< ", zero_count " << old_zc << " -> " << job.map->zero_count
|
||||||
<< " (R^2=" << fit.r2 << ")";
|
<< " (R^2=" << fit.r2 << ")";
|
||||||
log << " fit: counts_per_deg=" << fit.slope << " zero_count=" << map.zero_count
|
log << " fit: counts_per_deg=" << fit.slope << " zero_count=" << job.map->zero_count
|
||||||
<< " R2=" << fit.r2 << " (was cpd=" << old_cpd << " zc=" << old_zc << ")\n";
|
<< " R2=" << fit.r2 << " (was cpd=" << old_cpd << " zc=" << old_zc << ")\n";
|
||||||
report.axes.push_back({axis, true, fit.slope, map.zero_count, fit.r2, fit.n});
|
report.axes.push_back({job.axis, true, fit.slope, job.map->zero_count, fit.r2, fit.n});
|
||||||
return true;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Resolve the yaw sweep span up front — we move to its first position before
|
|
||||||
// the pitch sweep so the whole pitch calibration happens at that fixed yaw.
|
|
||||||
long ya = 0, yb = 0, ytol = 0;
|
|
||||||
if (!axisSpan('Y', ya, yb, ytol)) {
|
|
||||||
LOG_WARN << "calibration aborted: no yaw soft limits in dump";
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 3. Move yaw to its first sweep position and hold it there.
|
|
||||||
LOG_INFO << "calibration: moving yaw to first position (counts=" << ya << ")";
|
|
||||||
setProgress('Y', 1, N, "moving");
|
|
||||||
motor_.sendCommand(std::string("MOVE Y ") + std::to_string(ya));
|
|
||||||
waitSettle(motor_, 'Y', ya, ytol, params_.settle_timeout_ms, cancel_);
|
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
|
||||||
|
|
||||||
// 4. Calibrate PITCH at this yaw position, then 5. move pitch to 0 deg.
|
|
||||||
if (pitch_present) {
|
|
||||||
long pa = 0, pb = 0, ptol = 0;
|
|
||||||
if (axisSpan('P', pa, pb, ptol)) {
|
|
||||||
if (!sweepAxis('P', result.pitch, pa, pb, ptol)) { return; } // cancelled
|
|
||||||
const long pzero = result.pitch.toCounts(0.0);
|
|
||||||
LOG_INFO << "calibration: moving pitch to 0 deg (counts=" << pzero << ")";
|
|
||||||
setProgress('P', N, N, "pitch->0");
|
|
||||||
motor_.sendCommand(std::string("MOVE P ") + std::to_string(pzero));
|
|
||||||
waitSettle(motor_, 'P', pzero, ptol, params_.settle_timeout_ms, cancel_);
|
|
||||||
} else {
|
|
||||||
LOG_WARN << "calibration: no pitch soft limits in dump; skipping pitch";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
|
||||||
|
|
||||||
// 6. Switch the IMU to a no-magnetometer XKF profile so its heading stops
|
|
||||||
// chasing the (stepper-distorted) magnetic field. Selected from the device's
|
|
||||||
// own available list; persists on the device. Best-effort — if none is
|
|
||||||
// available we log and continue with the drift correction below.
|
|
||||||
if (auto cfg = imu_.config()) {
|
|
||||||
int nomag = pickNoMagProfile(cfg->available_profiles);
|
|
||||||
if (nomag >= 0 && (!cfg->has_scenario || cfg->scenario_type != nomag)) {
|
|
||||||
LOG_INFO << "calibration: switching IMU to no-mag XKF profile (type " << nomag << ")";
|
|
||||||
setProgress('Y', 0, N, "no-mag profile");
|
|
||||||
if (!imu_.setFilterProfile(nomag))
|
|
||||||
LOG_WARN << "calibration: IMU did not confirm the no-mag profile (continuing)";
|
|
||||||
} else if (nomag < 0) {
|
|
||||||
LOG_WARN << "calibration: no no-mag XKF profile available on this device (continuing)";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
|
||||||
|
|
||||||
// 7. Hold still: correct yaw drift (no-rotation gyro-bias update) then reset
|
|
||||||
// the IMU heading so this pose is yaw 0 — done right before the yaw sweep so
|
|
||||||
// the drift accumulated during the long pitch sweep doesn't bias the fit.
|
|
||||||
LOG_INFO << "calibration: correcting yaw drift (no-rotation " << params_.norotation_s
|
|
||||||
<< " s, holding still)...";
|
|
||||||
setProgress('Y', 0, N, "yaw drift correction");
|
|
||||||
imu_.noRotation(params_.norotation_s);
|
|
||||||
for (int waited = 0; waited < params_.norotation_s * 1000 + 500 && !cancel_; waited += 100)
|
|
||||||
msleep(100);
|
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
|
||||||
LOG_INFO << "calibration: resetting IMU heading (this pose is now yaw 0)";
|
|
||||||
setProgress('Y', 0, N, "yaw heading reset");
|
|
||||||
imu_.headingReset();
|
|
||||||
msleep(params_.reset_settle_ms);
|
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
|
||||||
|
|
||||||
// 8. Calibrate YAW from this (now zero-heading) first position.
|
|
||||||
if (!sweepAxis('Y', result.yaw, ya, yb, ytol)) { return; } // cancelled
|
|
||||||
|
|
||||||
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
if (cancel_) { LOG_WARN << "calibration cancelled"; return; }
|
||||||
|
|
||||||
std::string path = paths::writeLogFile(paths::timestampedLogName("calib"), log.str());
|
std::string path = paths::writeLogFile(paths::timestampedLogName("calib"), log.str());
|
||||||
|
|
|
||||||
|
|
@ -219,19 +219,11 @@ bool saveMotorCalibration(const std::string& path, const Geometry& geo) {
|
||||||
std::snprintf(b, sizeof(b), "%.10g", v);
|
std::snprintf(b, sizeof(b), "%.10g", v);
|
||||||
return std::string(b);
|
return std::string(b);
|
||||||
};
|
};
|
||||||
// Persist the full per-axis map, not just the fitted counts_per_deg/zero_count:
|
|
||||||
// min_deg/max_deg are part of the geometry and consumers (e.g. travel-range
|
|
||||||
// estimates) rely on them, so writing them keeps the [Motor] block complete and
|
|
||||||
// self-consistent rather than depending on pre-existing lines.
|
|
||||||
const std::vector<std::pair<std::string, std::string>> kv = {
|
const std::vector<std::pair<std::string, std::string>> kv = {
|
||||||
{"yaw_counts_per_deg", num(geo.yaw.counts_per_deg)},
|
{"yaw_counts_per_deg", num(geo.yaw.counts_per_deg)},
|
||||||
{"yaw_zero_count", std::to_string(geo.yaw.zero_count)},
|
{"yaw_zero_count", std::to_string(geo.yaw.zero_count)},
|
||||||
{"yaw_min_deg", num(geo.yaw.min_deg)},
|
|
||||||
{"yaw_max_deg", num(geo.yaw.max_deg)},
|
|
||||||
{"pitch_counts_per_deg", num(geo.pitch.counts_per_deg)},
|
{"pitch_counts_per_deg", num(geo.pitch.counts_per_deg)},
|
||||||
{"pitch_zero_count", std::to_string(geo.pitch.zero_count)},
|
{"pitch_zero_count", std::to_string(geo.pitch.zero_count)},
|
||||||
{"pitch_min_deg", num(geo.pitch.min_deg)},
|
|
||||||
{"pitch_max_deg", num(geo.pitch.max_deg)},
|
|
||||||
};
|
};
|
||||||
const std::string updated = updateIniSectionKeys(buf.str(), "Motor", kv);
|
const std::string updated = updateIniSectionKeys(buf.str(), "Motor", kv);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -58,19 +58,12 @@ const std::vector<HelpSection>& helpCatalog() {
|
||||||
"Each axis is swept at several speeds/directions (DG lines). A PASS/FAIL",
|
"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."}},
|
"summary is logged and saved to logs/diag_*.log. Axes must be homed first."}},
|
||||||
{"gimbal calib",
|
{"gimbal calib",
|
||||||
"Calibrate steps<->degrees using the IMU (auto-homes; ~minutes).", {
|
"Calibrate steps<->degrees using the IMU (sweeps each axis, ~minutes).", {
|
||||||
"Needs the IMU. Homes first if needed, calibrates PITCH at the first yaw",
|
"Homes-limits required + IMU enabled. Sweeps yaw then pitch in 10 steps,",
|
||||||
"position, moves pitch to 0, switches the IMU to a no-mag XKF profile",
|
"dwells 5 s recording IMU orientation, fits the conversion, applies it to",
|
||||||
"(persists on the device), runs no-rotation + heading reset to zero yaw",
|
"the session and writes logs/calib_*.log. 'gimbal stop' cancels."}},
|
||||||
"drift, then calibrates YAW. Applies the fit to the session and writes",
|
|
||||||
"logs/calib_*.log. 'gimbal stop' cancels."}},
|
|
||||||
{"gimbal status",
|
{"gimbal status",
|
||||||
"Ask the firmware to emit one telemetry (ST) line now.", {}},
|
"Ask the firmware to emit one telemetry (ST) line now.", {}},
|
||||||
{"gimbal raw \"<command>\"",
|
|
||||||
"Low-level: send the quoted text to the firmware verbatim (a newline is", {
|
|
||||||
"added automatically). Bypasses all wrappers/unit conversion for direct",
|
|
||||||
"firmware control — use with care; sends exactly what you type.",
|
|
||||||
"Example: gimbal raw \"MOVE 100000,250000\" / gimbal raw \"DUMP\""}},
|
|
||||||
}},
|
}},
|
||||||
{"Capture", "Control image capture and encoding.", {
|
{"Capture", "Control image capture and encoding.", {
|
||||||
{"start", "Begin the capture scan.", {}},
|
{"start", "Begin the capture scan.", {}},
|
||||||
|
|
@ -88,12 +81,6 @@ const std::vector<HelpSection>& helpCatalog() {
|
||||||
"Example: trace serial on / trace off"}},
|
"Example: trace serial on / trace off"}},
|
||||||
}},
|
}},
|
||||||
{"Session", "Help and exit.", {
|
{"Session", "Help and exit.", {
|
||||||
{"refresh",
|
|
||||||
"Re-read live device state (TUI: press 'r').", {
|
|
||||||
"Re-queries the IMU configuration (XKF profile, output settings) and",
|
|
||||||
"requests a fresh firmware dump, updating the 'i' and 'g' expanded views.",
|
|
||||||
"Use after changing the XKF profile externally. Briefly pauses the IMU",
|
|
||||||
"stream while it re-enters the device's Config state."}},
|
|
||||||
{"help [topic]",
|
{"help [topic]",
|
||||||
"Show this reference; 'help <topic>' expands one section.", {
|
"Show this reference; 'help <topic>' expands one section.", {
|
||||||
"Example: help positioning / help dump"}},
|
"Example: help positioning / help dump"}},
|
||||||
|
|
|
||||||
|
|
@ -1,10 +1,6 @@
|
||||||
#include "fgc/MtiProtocol.h"
|
#include "fgc/MtiProtocol.h"
|
||||||
|
|
||||||
#include <cctype>
|
|
||||||
#include <cstdio>
|
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <string>
|
|
||||||
#include <utility>
|
|
||||||
|
|
||||||
namespace fgc {
|
namespace fgc {
|
||||||
|
|
||||||
|
|
@ -23,11 +19,6 @@ uint16_t beU16(const uint8_t* p) {
|
||||||
return static_cast<uint16_t>((uint16_t(p[0]) << 8) | uint16_t(p[1]));
|
return static_cast<uint16_t>((uint16_t(p[0]) << 8) | uint16_t(p[1]));
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t beU32(const uint8_t* p) {
|
|
||||||
return (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) | (uint32_t(p[2]) << 8) |
|
|
||||||
uint32_t(p[3]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Append a big-endian value to a byte vector.
|
// Append a big-endian value to a byte vector.
|
||||||
void putBE(std::vector<uint8_t>& v, uint16_t x) {
|
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 >> 8));
|
||||||
|
|
@ -77,168 +68,6 @@ std::vector<uint8_t> msgSetOutputSettings() {
|
||||||
return mtiMessage(kMidSetOutputSettings, d);
|
return mtiMessage(kMidSetOutputSettings, d);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<uint8_t> msgSetNoRotation(uint16_t seconds) {
|
|
||||||
std::vector<uint8_t> d;
|
|
||||||
putBE(d, seconds);
|
|
||||||
return mtiMessage(kMidSetNoRotation, d);
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<uint8_t> msgResetOrientation(uint16_t code) {
|
|
||||||
std::vector<uint8_t> d;
|
|
||||||
putBE(d, code);
|
|
||||||
return mtiMessage(kMidResetOrientation, d);
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<uint8_t> msgSetFilterProfile(uint16_t profile) {
|
|
||||||
std::vector<uint8_t> d;
|
|
||||||
putBE(d, profile);
|
|
||||||
return mtiMessage(kMidReqFilterProfile, d); // same MID; non-empty data => "set"
|
|
||||||
}
|
|
||||||
|
|
||||||
int pickNoMagProfile(const std::vector<ImuFilterProfile>& profiles) {
|
|
||||||
auto lower = [](std::string s) {
|
|
||||||
for (char& c : s) c = static_cast<char>(std::tolower((unsigned char)c));
|
|
||||||
return s;
|
|
||||||
};
|
|
||||||
int vru = -1;
|
|
||||||
for (const auto& p : profiles) {
|
|
||||||
const std::string l = lower(p.label);
|
|
||||||
if (l.find("nomag") != std::string::npos || l.find("no_mag") != std::string::npos ||
|
|
||||||
l.find("no mag") != std::string::npos)
|
|
||||||
return p.type; // explicit no-magnetometer
|
|
||||||
if (vru < 0 && l.find("vru") != std::string::npos) // gyro-tracked heading
|
|
||||||
vru = p.type;
|
|
||||||
}
|
|
||||||
return vru;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<uint8_t> msgReqProductCode() { return mtiMessage(kMidReqProductCode); }
|
|
||||||
std::vector<uint8_t> msgReqDID() { return mtiMessage(kMidReqDID); }
|
|
||||||
std::vector<uint8_t> msgReqFWRev() { return mtiMessage(kMidReqFWRev); }
|
|
||||||
std::vector<uint8_t> msgReqPeriod() { return mtiMessage(kMidReqPeriod); }
|
|
||||||
std::vector<uint8_t> msgReqOutputMode() { return mtiMessage(kMidSetOutputMode); }
|
|
||||||
std::vector<uint8_t> msgReqOutputSettings() { return mtiMessage(kMidSetOutputSettings); }
|
|
||||||
std::vector<uint8_t> msgReqFilterProfile() { return mtiMessage(kMidReqFilterProfile); }
|
|
||||||
std::vector<uint8_t> msgReqAvailFilterProfiles(){ return mtiMessage(kMidReqAvailFilterProf); }
|
|
||||||
|
|
||||||
bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n) {
|
|
||||||
switch (mid) {
|
|
||||||
case kMidDeviceID:
|
|
||||||
if (n < 4) return false;
|
|
||||||
c.device_id = beU32(d);
|
|
||||||
c.has_device_id = true;
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
|
|
||||||
case kMidProductCode: {
|
|
||||||
// ASCII string, possibly space-padded; trim trailing spaces/NULs.
|
|
||||||
std::size_t end = n;
|
|
||||||
while (end > 0 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
|
|
||||||
c.product_code.assign(reinterpret_cast<const char*>(d), end);
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
case kMidFirmwareRev: {
|
|
||||||
// MAJOR MINOR REV [BUILDNR(4) SCMREF(4)] — older firmware sends only 3.
|
|
||||||
if (n < 3) return false;
|
|
||||||
char buf[48];
|
|
||||||
if (n >= 7) {
|
|
||||||
uint32_t build = beU32(d + 3);
|
|
||||||
std::snprintf(buf, sizeof(buf), "%u.%u.%u build %u", d[0], d[1], d[2], build);
|
|
||||||
} else {
|
|
||||||
std::snprintf(buf, sizeof(buf), "%u.%u.%u", d[0], d[1], d[2]);
|
|
||||||
}
|
|
||||||
c.firmware = buf;
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
case kMidReqPeriodAck:
|
|
||||||
if (n < 2) return false;
|
|
||||||
c.period = beU16(d);
|
|
||||||
c.has_period = true;
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
|
|
||||||
case kMidSetOutputModeAck: { // 0xD1, ack to ReqOutputMode
|
|
||||||
if (n < 2) return false;
|
|
||||||
uint16_t m = beU16(d);
|
|
||||||
c.output_mode = m;
|
|
||||||
c.out_temperature = m & 0x0001;
|
|
||||||
c.out_calibrated = m & 0x0002;
|
|
||||||
c.out_orientation = m & 0x0004;
|
|
||||||
c.out_auxiliary = m & 0x0008;
|
|
||||||
c.out_status = m & 0x0800;
|
|
||||||
c.has_output_mode = true;
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
case kMidSetOutputSettingsAck: { // 0xD3, ack to ReqOutputSettings
|
|
||||||
if (n < 4) return false;
|
|
||||||
uint32_t s = beU32(d);
|
|
||||||
c.output_settings = s;
|
|
||||||
switch (s & 0x0003) {
|
|
||||||
case 0x1: c.timestamp_mode = "Sample counter"; break;
|
|
||||||
default: c.timestamp_mode = "None"; break;
|
|
||||||
}
|
|
||||||
switch ((s >> 2) & 0x0003) {
|
|
||||||
case 0x0: c.orientation_mode = "Quaternion"; break;
|
|
||||||
case 0x1: c.orientation_mode = "Euler"; break;
|
|
||||||
case 0x2: c.orientation_mode = "Matrix"; break;
|
|
||||||
default: c.orientation_mode = "?"; break;
|
|
||||||
}
|
|
||||||
// Bits 4/5/6: 1 = output DISABLED.
|
|
||||||
c.acc_enabled = !(s & 0x0010);
|
|
||||||
c.gyr_enabled = !(s & 0x0020);
|
|
||||||
c.mag_enabled = !(s & 0x0040);
|
|
||||||
c.data_format = ((s >> 8) & 0x0003) == 0x1 ? "Fixed 12.20" : "Float";
|
|
||||||
c.has_output_settings = true;
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
case kMidReqFilterProfileAck: // VERSION, FILTERPROFILE(type)
|
|
||||||
if (n < 2) return false;
|
|
||||||
c.scenario_version = d[0];
|
|
||||||
c.scenario_type = d[1];
|
|
||||||
c.has_scenario = true;
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
|
|
||||||
case kMidAvailFilterProf: {
|
|
||||||
// Repeating 22-byte records: TYPE(1) VERSION(1) LABEL(20, space-padded).
|
|
||||||
c.available_profiles.clear();
|
|
||||||
for (std::size_t o = 0; o + 22 <= n; o += 22) {
|
|
||||||
ImuFilterProfile p;
|
|
||||||
p.type = d[o];
|
|
||||||
p.version = d[o + 1];
|
|
||||||
if (p.type == 0) continue; // empty slot
|
|
||||||
std::size_t end = o + 22;
|
|
||||||
while (end > o + 2 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
|
|
||||||
p.label.assign(reinterpret_cast<const char*>(d + o + 2), end - (o + 2));
|
|
||||||
c.available_profiles.push_back(std::move(p));
|
|
||||||
}
|
|
||||||
c.valid = true;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
default:
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void finalizeImuConfig(ImuDeviceConfig& c) {
|
|
||||||
if (c.has_period && c.period > 0)
|
|
||||||
c.sample_rate_hz = 115200.0f / static_cast<float>(c.period);
|
|
||||||
if (c.has_scenario) {
|
|
||||||
for (const auto& p : c.available_profiles) {
|
|
||||||
if (p.type == c.scenario_type) { c.scenario_label = p.label; break; }
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) {
|
std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) {
|
||||||
if (mid != kMidMTData || len != kMTDataLen) return std::nullopt;
|
if (mid != kMidMTData || len != kMTDataLen) return std::nullopt;
|
||||||
ImuSample s;
|
ImuSample s;
|
||||||
|
|
|
||||||
|
|
@ -6,7 +6,6 @@
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <functional>
|
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <thread>
|
#include <thread>
|
||||||
|
|
||||||
|
|
@ -35,9 +34,6 @@ struct MtiImuSource::Impl {
|
||||||
clock::time_point last_rx{};
|
clock::time_point last_rx{};
|
||||||
std::atomic<bool> open{false};
|
std::atomic<bool> open{false};
|
||||||
|
|
||||||
ImuDeviceConfig cfg; // device config read back at startup
|
|
||||||
bool have_cfg = false;
|
|
||||||
|
|
||||||
MtiFramer framer;
|
MtiFramer framer;
|
||||||
unsigned bad_len_warned = 0;
|
unsigned bad_len_warned = 0;
|
||||||
|
|
||||||
|
|
@ -72,9 +68,7 @@ struct MtiImuSource::Impl {
|
||||||
// Synchronous, best-effort config handshake (no ack parsing): runs on the
|
// Synchronous, best-effort config handshake (no ack parsing): runs on the
|
||||||
// calling thread BEFORE the io_thread starts, so there is no concurrent
|
// calling thread BEFORE the io_thread starts, so there is no concurrent
|
||||||
// access to the serial port. Small delays let the device switch states.
|
// access to the serial port. Small delays let the device switch states.
|
||||||
// If set_profile >= 0, also selects that XKF profile while in Config state
|
void configure() {
|
||||||
// (persists to non-volatile memory).
|
|
||||||
void configure(int set_profile = -1) {
|
|
||||||
using namespace std::chrono_literals;
|
using namespace std::chrono_literals;
|
||||||
auto write = [this](const std::vector<uint8_t>& m) {
|
auto write = [this](const std::vector<uint8_t>& m) {
|
||||||
boost::system::error_code ec;
|
boost::system::error_code ec;
|
||||||
|
|
@ -82,96 +76,12 @@ struct MtiImuSource::Impl {
|
||||||
if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
|
if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
|
||||||
};
|
};
|
||||||
write(msgGoToConfig()); std::this_thread::sleep_for(60ms);
|
write(msgGoToConfig()); std::this_thread::sleep_for(60ms);
|
||||||
if (set_profile >= 0) {
|
|
||||||
write(msgSetFilterProfile(static_cast<uint16_t>(set_profile)));
|
|
||||||
std::this_thread::sleep_for(60ms);
|
|
||||||
}
|
|
||||||
write(msgSetOutputMode()); std::this_thread::sleep_for(60ms);
|
write(msgSetOutputMode()); std::this_thread::sleep_for(60ms);
|
||||||
write(msgSetOutputSettings()); std::this_thread::sleep_for(60ms);
|
write(msgSetOutputSettings()); std::this_thread::sleep_for(60ms);
|
||||||
// Read back the device configuration while still in Config State.
|
|
||||||
queryConfig(write);
|
|
||||||
write(msgGoToMeasurement()); std::this_thread::sleep_for(60ms);
|
write(msgGoToMeasurement()); std::this_thread::sleep_for(60ms);
|
||||||
// Drop any pre-config (old-format) bytes so the framer starts clean.
|
// Drop any pre-config (old-format) bytes so the framer starts clean.
|
||||||
::tcflush(serial.native_handle(), TCIFLUSH);
|
::tcflush(serial.native_handle(), TCIFLUSH);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Pause the streaming io_thread, re-run the handshake (optionally setting an
|
|
||||||
// XKF profile), then resume streaming. Cancels + drains the outstanding read
|
|
||||||
// first so it can't steal the config-ack bytes during the re-query. Called
|
|
||||||
// from the control thread (no concurrent serial access). Used by refreshConfig
|
|
||||||
// and setFilterProfile.
|
|
||||||
void restartWithConfig(int set_profile) {
|
|
||||||
io.stop();
|
|
||||||
if (io_thread.joinable()) io_thread.join();
|
|
||||||
io.restart();
|
|
||||||
boost::system::error_code ec;
|
|
||||||
serial.cancel(ec);
|
|
||||||
static_cast<void>(io.run()); // run the aborted read handler (no reschedule)
|
|
||||||
io.restart();
|
|
||||||
configure(set_profile);
|
|
||||||
doRead();
|
|
||||||
io_thread = std::thread([this] { io.run(); });
|
|
||||||
}
|
|
||||||
|
|
||||||
// Query the device configuration in Config State and decode the acks. Runs on
|
|
||||||
// the calling thread before the streaming io_thread starts; it drives the
|
|
||||||
// io_context itself with run_for() and leaves it clean (restarted, all
|
|
||||||
// pending ops cancelled) so the later io.run() streams normally.
|
|
||||||
template <class WriteFn>
|
|
||||||
void queryConfig(WriteFn&& write) {
|
|
||||||
using namespace std::chrono_literals;
|
|
||||||
ImuDeviceConfig local;
|
|
||||||
MtiFramer cf([&](uint8_t mid, const uint8_t* d, std::size_t n) {
|
|
||||||
applyImuConfigAck(local, mid, d, n);
|
|
||||||
});
|
|
||||||
|
|
||||||
// Drop any buffered pre-config stream (e.g. MTData from before GoToConfig
|
|
||||||
// took effect) so the read window below is dominated by the query acks.
|
|
||||||
::tcflush(serial.native_handle(), TCIFLUSH);
|
|
||||||
|
|
||||||
write(msgReqProductCode());
|
|
||||||
write(msgReqDID());
|
|
||||||
write(msgReqFWRev());
|
|
||||||
write(msgReqPeriod());
|
|
||||||
write(msgReqOutputMode());
|
|
||||||
write(msgReqOutputSettings());
|
|
||||||
write(msgReqFilterProfile());
|
|
||||||
write(msgReqAvailFilterProfiles());
|
|
||||||
|
|
||||||
std::array<uint8_t, 256> buf{};
|
|
||||||
std::function<void()> rd = [&]() {
|
|
||||||
serial.async_read_some(boost::asio::buffer(buf),
|
|
||||||
[&](const boost::system::error_code& ec, std::size_t n) {
|
|
||||||
if (ec) return; // cancelled / error: stop
|
|
||||||
cf.feed(buf.data(), n);
|
|
||||||
rd();
|
|
||||||
});
|
|
||||||
};
|
|
||||||
io.restart();
|
|
||||||
rd();
|
|
||||||
static_cast<void>(io.run_for(500ms)); // collect acks for up to half a second
|
|
||||||
// Drain the pending read so its by-ref handler can't fire later with the
|
|
||||||
// local buffer/framer already destroyed.
|
|
||||||
boost::system::error_code ec;
|
|
||||||
serial.cancel(ec);
|
|
||||||
io.restart();
|
|
||||||
static_cast<void>(io.run());
|
|
||||||
io.restart();
|
|
||||||
|
|
||||||
finalizeImuConfig(local);
|
|
||||||
if (local.valid) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex);
|
|
||||||
cfg = local;
|
|
||||||
have_cfg = true;
|
|
||||||
LOG_INFO << "MTi config: " << (local.product_code.empty() ? "?" : local.product_code)
|
|
||||||
<< " fw " << (local.firmware.empty() ? "?" : local.firmware)
|
|
||||||
<< " scenario " << int(local.scenario_type)
|
|
||||||
<< (local.scenario_label.empty() ? "" : " (" + local.scenario_label + ")")
|
|
||||||
<< " @ " << local.sample_rate_hz << " Hz";
|
|
||||||
} else {
|
|
||||||
LOG_WARN << "MTi: no configuration acks received (device may not answer Req* in this firmware)";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
|
MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
|
||||||
|
|
@ -226,59 +136,4 @@ std::optional<ImuSample> MtiImuSource::sample() {
|
||||||
return impl_->latest;
|
return impl_->latest;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::optional<ImuDeviceConfig> MtiImuSource::config() const {
|
|
||||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
||||||
if (!impl_->have_cfg) return std::nullopt;
|
|
||||||
return impl_->cfg;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Re-read the device configuration: briefly pause the streaming io_thread,
|
|
||||||
// re-run the Config-state handshake (which re-queries product/firmware/output/
|
|
||||||
// XKF profile and re-applies our output settings), then resume streaming. Mirrors
|
|
||||||
// start() minus opening the port. Called from the control thread; sample()/config()
|
|
||||||
// are called from that same thread when building the snapshot, so there is no
|
|
||||||
// concurrent serial access during the refresh.
|
|
||||||
void MtiImuSource::refreshConfig() {
|
|
||||||
if (!impl_->open) { LOG_WARN << "MTi: refresh ignored (not started)"; return; }
|
|
||||||
LOG_INFO << "MTi: refreshing configuration (briefly pausing the stream)...";
|
|
||||||
impl_->restartWithConfig(-1); // re-query only, no profile change
|
|
||||||
}
|
|
||||||
|
|
||||||
bool MtiImuSource::setFilterProfile(int type) {
|
|
||||||
if (!impl_->open) { LOG_WARN << "MTi: setFilterProfile ignored (not started)"; return false; }
|
|
||||||
if (type < 0) return false;
|
|
||||||
LOG_INFO << "MTi: setting XKF profile to type " << type << " (persists on device)...";
|
|
||||||
impl_->restartWithConfig(type);
|
|
||||||
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
||||||
return impl_->have_cfg && impl_->cfg.has_scenario && impl_->cfg.scenario_type == type;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Send a command while the streaming io_thread is running. The write is posted
|
|
||||||
// onto the io_context so it runs on the io_thread, never concurrently with the
|
|
||||||
// in-flight async_read (concurrent ops on one serial_port are unsafe — same rule
|
|
||||||
// as SerialMotorController).
|
|
||||||
void MtiImuSource::noRotation(int seconds) {
|
|
||||||
if (!impl_->open) { LOG_WARN << "MTi: noRotation ignored (not started)"; return; }
|
|
||||||
if (seconds < 1) seconds = 1;
|
|
||||||
auto msg = std::make_shared<std::vector<uint8_t>>(
|
|
||||||
msgSetNoRotation(static_cast<uint16_t>(seconds)));
|
|
||||||
boost::asio::post(impl_->io, [this, msg] {
|
|
||||||
boost::system::error_code ec;
|
|
||||||
boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
|
|
||||||
if (ec) LOG_WARN << "MTi noRotation write failed: " << ec.message();
|
|
||||||
});
|
|
||||||
LOG_INFO << "MTi: no-rotation update for " << seconds << " s (hold still)";
|
|
||||||
}
|
|
||||||
|
|
||||||
void MtiImuSource::headingReset() {
|
|
||||||
if (!impl_->open) { LOG_WARN << "MTi: headingReset ignored (not started)"; return; }
|
|
||||||
auto msg = std::make_shared<std::vector<uint8_t>>(msgResetOrientation(kResetHeading));
|
|
||||||
boost::asio::post(impl_->io, [this, msg] {
|
|
||||||
boost::system::error_code ec;
|
|
||||||
boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
|
|
||||||
if (ec) LOG_WARN << "MTi headingReset write failed: " << ec.message();
|
|
||||||
});
|
|
||||||
LOG_INFO << "MTi: heading reset (current direction is now yaw 0)";
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace fgc
|
} // namespace fgc
|
||||||
|
|
|
||||||
|
|
@ -387,30 +387,20 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump, const Calib
|
||||||
|
|
||||||
// Full-screen IMU view (toggled with 'i'): every MTi channel with units.
|
// Full-screen IMU view (toggled with 'i'): every MTi channel with units.
|
||||||
Element imuDetailPanel(const ImuView& v) {
|
Element imuDetailPanel(const ImuView& v) {
|
||||||
// Fixed-width, right-aligned to 2 decimals. The constant width keeps the sign
|
auto f2 = [](float x) {
|
||||||
// column and decimal point from jumping as values cross zero or change digit
|
|
||||||
// count, so the readout stays steady instead of flickering. The width is kept
|
|
||||||
// just wide enough for the field's range so the number sits close to its
|
|
||||||
// x/y/z label: 6 for the vectors (accel/gyro/mag stay well under ±100), 7 for
|
|
||||||
// orientation (so a 3-digit "-180.00" still fits without widening).
|
|
||||||
auto f2 = [](float x, int w) {
|
|
||||||
char b[24];
|
char b[24];
|
||||||
std::snprintf(b, sizeof(b), "%*.2f", w, x);
|
std::snprintf(b, sizeof(b), "%.2f", x);
|
||||||
return std::string(b);
|
return std::string(b);
|
||||||
};
|
};
|
||||||
// A dim " │ " divider between value columns.
|
// One "LABEL (unit) x=.. y=.. z=.." row for a 3-vector.
|
||||||
auto vsep = [] { return text(" \xE2\x94\x82 ") | dim; };
|
|
||||||
// One "LABEL (unit) x:.. │ y:.. │ z:.." row for a 3-vector.
|
|
||||||
auto vecRow = [&](const std::string& label, const char* unit, const float xyz[3],
|
auto vecRow = [&](const std::string& label, const char* unit, const float xyz[3],
|
||||||
Color c = Color::Default) {
|
Color c = Color::Default) {
|
||||||
return hbox({
|
return hbox({
|
||||||
text(label) | dim | size(WIDTH, EQUAL, 14),
|
text(label) | dim | size(WIDTH, EQUAL, 14),
|
||||||
text(std::string(unit)) | dim | size(WIDTH, EQUAL, 9),
|
text(std::string(unit)) | dim | size(WIDTH, EQUAL, 9),
|
||||||
text("x:" + f2(xyz[0], 6)) | color(c) | size(WIDTH, EQUAL, 8),
|
text("x " + f2(xyz[0])) | color(c) | size(WIDTH, EQUAL, 12),
|
||||||
vsep(),
|
text("y " + f2(xyz[1])) | color(c) | size(WIDTH, EQUAL, 12),
|
||||||
text("y:" + f2(xyz[1], 6)) | color(c) | size(WIDTH, EQUAL, 8),
|
text("z " + f2(xyz[2])) | color(c) | size(WIDTH, EQUAL, 12),
|
||||||
vsep(),
|
|
||||||
text("z:" + f2(xyz[2], 6)) | color(c) | size(WIDTH, EQUAL, 8),
|
|
||||||
});
|
});
|
||||||
};
|
};
|
||||||
const float ori[3] = {v.roll_deg, v.pitch_deg, v.yaw_deg};
|
const float ori[3] = {v.roll_deg, v.pitch_deg, v.yaw_deg};
|
||||||
|
|
@ -423,11 +413,9 @@ Element imuDetailPanel(const ImuView& v) {
|
||||||
body.push_back(hbox({
|
body.push_back(hbox({
|
||||||
text("ORIENTATION") | dim | size(WIDTH, EQUAL, 14),
|
text("ORIENTATION") | dim | size(WIDTH, EQUAL, 14),
|
||||||
text("deg") | dim | size(WIDTH, EQUAL, 9),
|
text("deg") | dim | size(WIDTH, EQUAL, 9),
|
||||||
text("roll:" + f2(ori[0], 6)) | bold | size(WIDTH, EQUAL, 11),
|
text("roll " + f2(ori[0])) | bold | size(WIDTH, EQUAL, 14),
|
||||||
vsep(),
|
text("pitch " + f2(ori[1])) | bold | size(WIDTH, EQUAL, 14),
|
||||||
text("pitch:" + f2(ori[1], 6)) | bold | size(WIDTH, EQUAL, 12),
|
text("yaw " + f2(ori[2])) | bold | size(WIDTH, EQUAL, 14),
|
||||||
vsep(),
|
|
||||||
text("yaw:" + f2(ori[2], 6)) | bold | size(WIDTH, EQUAL, 10),
|
|
||||||
}));
|
}));
|
||||||
body.push_back(separator());
|
body.push_back(separator());
|
||||||
body.push_back(vecRow("ACCEL", "m/s2", v.acc, Color::Cyan));
|
body.push_back(vecRow("ACCEL", "m/s2", v.acc, Color::Cyan));
|
||||||
|
|
@ -436,50 +424,14 @@ Element imuDetailPanel(const ImuView& v) {
|
||||||
body.push_back(separator());
|
body.push_back(separator());
|
||||||
body.push_back(hbox({
|
body.push_back(hbox({
|
||||||
text("TEMP") | dim | size(WIDTH, EQUAL, 14),
|
text("TEMP") | dim | size(WIDTH, EQUAL, 14),
|
||||||
text(f2(v.temp_c, 6) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18),
|
text(f2(v.temp_c) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18),
|
||||||
text("sample #" + std::to_string(v.sample_counter)) | dim,
|
text("sample #" + std::to_string(v.sample_counter)) | dim,
|
||||||
}));
|
}));
|
||||||
}
|
}
|
||||||
|
|
||||||
// IMU CONFIG section (device configuration read back at startup). Shown
|
|
||||||
// whenever it is known, even if the live stream is offline.
|
|
||||||
const auto& c = v.config;
|
|
||||||
if (c.present) {
|
|
||||||
auto cfgRow = [](const std::string& k, const std::string& val,
|
|
||||||
Color vc = Color::Default) {
|
|
||||||
return hbox({text(k) | dim | size(WIDTH, EQUAL, 16),
|
|
||||||
text(val) | color(vc)});
|
|
||||||
};
|
|
||||||
body.push_back(separator());
|
|
||||||
body.push_back(text("IMU CONFIG") | bold | color(Color::Magenta));
|
|
||||||
body.push_back(cfgRow("Product", c.product_code));
|
|
||||||
body.push_back(cfgRow("Firmware", c.firmware));
|
|
||||||
body.push_back(cfgRow("Device ID", c.device_id));
|
|
||||||
body.push_back(cfgRow("Output mode", c.output_mode));
|
|
||||||
body.push_back(cfgRow("Output fmt", c.output_settings));
|
|
||||||
body.push_back(cfgRow("Calib channels", c.channels));
|
|
||||||
body.push_back(cfgRow("Sample rate", c.sample_rate));
|
|
||||||
// XKF profile list: the active one is marked "●" and highlighted; the
|
|
||||||
// rest are dim "○". (Numeric profile IDs are intentionally hidden.)
|
|
||||||
body.push_back(text("Xsens Kalman Filter (XKF) profile") | dim);
|
|
||||||
if (c.xkf_profiles.empty()) {
|
|
||||||
body.push_back(hbox({text(" "), text("(not reported by device)") | dim}));
|
|
||||||
} else {
|
|
||||||
for (const auto& p : c.xkf_profiles) {
|
|
||||||
if (p.selected)
|
|
||||||
body.push_back(hbox({text(" \xE2\x97\x8F ") | color(Color::Yellow),
|
|
||||||
text(p.name) | color(Color::Yellow) | bold,
|
|
||||||
text(" (selected)") | dim}));
|
|
||||||
else
|
|
||||||
body.push_back(hbox({text(" \xE2\x97\x8B ") | dim,
|
|
||||||
text(p.name) | dim}));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Element status = v.present ? (text(" MTi live ") | color(Color::Green) | bold)
|
Element status = v.present ? (text(" MTi live ") | color(Color::Green) | bold)
|
||||||
: (text(" MTi offline ") | color(Color::Red) | bold);
|
: (text(" MTi offline ") | color(Color::Red) | bold);
|
||||||
return window(text(" IMU (i/Esc:close r:refresh config) ") | bold | color(Color::Magenta),
|
return window(text(" IMU (i/Esc:close) ") | bold | color(Color::Magenta),
|
||||||
vbox({hbox({status, filler()}), separator(),
|
vbox({hbox({status, filler()}), separator(),
|
||||||
vbox(std::move(body)) | flex}));
|
vbox(std::move(body)) | flex}));
|
||||||
}
|
}
|
||||||
|
|
@ -563,8 +515,7 @@ void TuiUi::uiLoop() {
|
||||||
} else {
|
} else {
|
||||||
bottom = hbox({
|
bottom = hbox({
|
||||||
keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"),
|
keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"),
|
||||||
keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("r", "Refresh"),
|
keyHint("g", "Gimbal"), keyHint("i", "IMU"), keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"),
|
||||||
keyHint("\xE2\x86\x90\xE2\x86\x92\xE2\x86\x91\xE2\x86\x93", "Nudge"),
|
|
||||||
keyHint(":", "Cmd"), keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
|
keyHint(":", "Cmd"), keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
@ -615,12 +566,12 @@ void TuiUi::uiLoop() {
|
||||||
}
|
}
|
||||||
if (overlay != Overlay::None && e == Event::Escape) { overlay = Overlay::None; 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):
|
// Arrow keys nudge the gimbal in steps (only when no overlay is open):
|
||||||
// Left/Right = yaw -/+5%, Up/Down = pitch -/+10% of travel.
|
// Left/Right = yaw -/+5%, Up/Down = pitch +/-10% of travel.
|
||||||
if (overlay == Overlay::None && sink_) {
|
if (overlay == Overlay::None && sink_) {
|
||||||
if (e == Event::ArrowLeft) { sink_("gimbal nudge yaw -5"); return true; }
|
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::ArrowRight) { sink_("gimbal nudge yaw 5"); return true; }
|
||||||
if (e == Event::ArrowUp) { sink_("gimbal nudge pitch -10"); return true; }
|
if (e == Event::ArrowUp) { sink_("gimbal nudge pitch 10"); return true; }
|
||||||
if (e == Event::ArrowDown) { sink_("gimbal nudge pitch 10"); return true; }
|
if (e == Event::ArrowDown) { sink_("gimbal nudge pitch -10"); return true; }
|
||||||
}
|
}
|
||||||
if (!e.is_character()) return false;
|
if (!e.is_character()) return false;
|
||||||
const std::string& c = e.character();
|
const std::string& c = e.character();
|
||||||
|
|
@ -661,7 +612,7 @@ void TuiUi::uiLoop() {
|
||||||
if (c == "s") { if (sink_) sink_("start"); return true; }
|
if (c == "s") { if (sink_) sink_("start"); return true; }
|
||||||
if (c == "x") { if (sink_) sink_("stop"); return true; }
|
if (c == "x") { if (sink_) sink_("stop"); return true; }
|
||||||
if (c == "h") { if (sink_) sink_("gimbal home"); return true; }
|
if (c == "h") { if (sink_) sink_("gimbal home"); return true; }
|
||||||
if (c == "r") { if (sink_) sink_("refresh"); return true; } // IMU config + dump
|
if (c == "r") { if (sink_) sink_("gimbal reset"); return true; }
|
||||||
if (c == ":") { command_mode = true; return true; }
|
if (c == ":") { command_mode = true; return true; }
|
||||||
return false;
|
return false;
|
||||||
});
|
});
|
||||||
|
|
|
||||||
|
|
@ -33,7 +33,6 @@ add_executable(fgc_tests
|
||||||
test_helptext.cpp
|
test_helptext.cpp
|
||||||
test_diagparser.cpp
|
test_diagparser.cpp
|
||||||
test_calibration.cpp
|
test_calibration.cpp
|
||||||
test_calibroutine.cpp
|
|
||||||
)
|
)
|
||||||
target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest)
|
target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest)
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,305 +0,0 @@
|
||||||
#include <doctest/doctest.h>
|
|
||||||
|
|
||||||
#include "fgc/CalibrationRoutine.h"
|
|
||||||
#include "fgc/Geometry.h"
|
|
||||||
#include "fgc/IImuSource.h"
|
|
||||||
#include "fgc/IMotorController.h"
|
|
||||||
|
|
||||||
#include <chrono>
|
|
||||||
#include <mutex>
|
|
||||||
#include <sstream>
|
|
||||||
#include <string>
|
|
||||||
#include <thread>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
using namespace fgc;
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
// A two-axis firmware DUMP with soft limits (subset of the real format; enough
|
|
||||||
// for parseDump to expose lim_neg/lim_pos per axis).
|
|
||||||
const char* kDump =
|
|
||||||
"DUMP BEGIN build=test uptime=1000 mcusr=0x01 free_ram=1852\n"
|
|
||||||
"DUMP Y state=3 hsub=10 enabled=1 lim_neg=-80000 lim_pos=80000 hold_target=0 "
|
|
||||||
"speed=50000 eeprom_restored=0 has_encoder=1\n"
|
|
||||||
"DUMP Y TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
|
|
||||||
"DUMP P state=3 hsub=10 enabled=1 lim_neg=0 lim_pos=600000 hold_target=0 "
|
|
||||||
"speed=150000 eeprom_restored=0 has_encoder=1\n"
|
|
||||||
"DUMP P TMC GCONF=0x0000000C DRV_STATUS=0x80084000\n"
|
|
||||||
"DUMP END\n";
|
|
||||||
|
|
||||||
// Counts-per-degree the fake IMU reports, so a clean sweep recovers this slope.
|
|
||||||
constexpr double kFakeCpd = 10000.0;
|
|
||||||
|
|
||||||
// Fake motor: records every command in order, and "executes" HOME/MOVE instantly
|
|
||||||
// (axis jumps to target, standstill + READY) so waitSettle/waitHomed return at
|
|
||||||
// once. IMU angle is derived from the live encoder position (see FakeImu).
|
|
||||||
struct FakeImu;
|
|
||||||
struct FakeMotor : IMotorController {
|
|
||||||
mutable std::mutex mtx;
|
|
||||||
MotorTelemetry tel;
|
|
||||||
std::vector<std::string> events; // unified, ordered log (motor + imu)
|
|
||||||
bool pitch_present = true;
|
|
||||||
bool have_dump = true;
|
|
||||||
|
|
||||||
void start() override {}
|
|
||||||
void stop() override {}
|
|
||||||
|
|
||||||
void record(const std::string& s) {
|
|
||||||
std::lock_guard<std::mutex> lk(mtx);
|
|
||||||
events.push_back(s);
|
|
||||||
}
|
|
||||||
|
|
||||||
void sendCommand(const std::string& c) override {
|
|
||||||
record(c);
|
|
||||||
std::lock_guard<std::mutex> lk(mtx);
|
|
||||||
std::istringstream iss(c);
|
|
||||||
std::string verb;
|
|
||||||
iss >> verb;
|
|
||||||
if (verb == "HOME") {
|
|
||||||
tel.yaw.state = AxisState::Ready; tel.yaw.standstill = true;
|
|
||||||
tel.pitch.state = AxisState::Ready; tel.pitch.standstill = true;
|
|
||||||
tel.pitch_present = pitch_present;
|
|
||||||
} else if (verb == "MOVE") {
|
|
||||||
std::string axis;
|
|
||||||
long target = 0;
|
|
||||||
iss >> axis >> target;
|
|
||||||
AxisTelemetry& a = (axis == "Y") ? tel.yaw : tel.pitch;
|
|
||||||
a.xenc = target;
|
|
||||||
a.standstill = true;
|
|
||||||
a.state = AxisState::Ready;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
MotorTelemetry telemetry() override {
|
|
||||||
std::lock_guard<std::mutex> lk(mtx);
|
|
||||||
return tel;
|
|
||||||
}
|
|
||||||
std::string lastDump() override { return have_dump ? kDump : ""; }
|
|
||||||
std::string lastDiag() override { return ""; }
|
|
||||||
unsigned diagSeq() const override { return 0; }
|
|
||||||
bool connected() const override { return true; }
|
|
||||||
};
|
|
||||||
|
|
||||||
// Fake IMU: pitch is the live pitch encoder / kFakeCpd; yaw is the live yaw
|
|
||||||
// encoder offset by the heading-reset zero, / kFakeCpd. Records noRotation /
|
|
||||||
// headingReset into the shared motor event log so ordering can be asserted.
|
|
||||||
struct FakeImu : IImuSource {
|
|
||||||
FakeMotor& motor;
|
|
||||||
long yaw_zero = 0;
|
|
||||||
int scenario_type = 39; // starts on a mag-using profile
|
|
||||||
bool has_profiles = true; // expose an available list (incl. a no-mag one)
|
|
||||||
explicit FakeImu(FakeMotor& m) : motor(m) {}
|
|
||||||
|
|
||||||
void start() override {}
|
|
||||||
void stop() override {}
|
|
||||||
bool connected() const override { return true; }
|
|
||||||
|
|
||||||
std::optional<ImuSample> sample() override {
|
|
||||||
MotorTelemetry t = motor.telemetry();
|
|
||||||
ImuSample s;
|
|
||||||
s.valid = true;
|
|
||||||
s.pitch_deg = static_cast<float>(t.pitch.xenc / kFakeCpd);
|
|
||||||
double y = (t.yaw.xenc - yaw_zero) / kFakeCpd;
|
|
||||||
s.yaw_deg = static_cast<float>(y);
|
|
||||||
return s;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::optional<ImuDeviceConfig> config() const override {
|
|
||||||
ImuDeviceConfig c;
|
|
||||||
c.valid = true;
|
|
||||||
c.has_scenario = true;
|
|
||||||
c.scenario_type = static_cast<uint8_t>(scenario_type);
|
|
||||||
if (has_profiles)
|
|
||||||
c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
|
|
||||||
{53, 11, "VRU_general"}};
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
void noRotation(int seconds) override {
|
|
||||||
motor.record("noRotation:" + std::to_string(seconds));
|
|
||||||
}
|
|
||||||
void headingReset() override {
|
|
||||||
motor.record("headingReset");
|
|
||||||
std::lock_guard<std::mutex> lk(motor.mtx);
|
|
||||||
yaw_zero = motor.tel.yaw.xenc; // current pose becomes yaw 0
|
|
||||||
}
|
|
||||||
bool setFilterProfile(int type) override {
|
|
||||||
motor.record("setProfile:" + std::to_string(type));
|
|
||||||
scenario_type = type;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
Geometry uncalibrated() {
|
|
||||||
Geometry g;
|
|
||||||
g.yaw = {1.0, 0, -100000.0, 100000.0};
|
|
||||||
g.pitch = {1.0, 0, -100000.0, 100000.0};
|
|
||||||
return g;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Fast timings so a full run completes in well under a second.
|
|
||||||
CalibParams fastParams() {
|
|
||||||
CalibParams p;
|
|
||||||
p.positions = 4;
|
|
||||||
p.dwell_ms = 4;
|
|
||||||
p.sample_ms = 1;
|
|
||||||
p.settle_timeout_ms = 200;
|
|
||||||
p.home_timeout_ms = 500;
|
|
||||||
p.norotation_s = 0;
|
|
||||||
p.reset_settle_ms = 2;
|
|
||||||
return p;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Run to completion (or fail the test on timeout).
|
|
||||||
void runToCompletion(CalibrationRoutine& cal) {
|
|
||||||
REQUIRE(cal.start());
|
|
||||||
auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
|
|
||||||
while (cal.running()) {
|
|
||||||
if (std::chrono::steady_clock::now() > deadline) { FAIL("calibration did not finish"); }
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int indexOf(const std::vector<std::string>& v, const std::string& needle) {
|
|
||||||
for (int i = 0; i < static_cast<int>(v.size()); ++i)
|
|
||||||
if (v[i].find(needle) != std::string::npos) return i;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
int lastIndexOf(const std::vector<std::string>& v, const std::string& needle) {
|
|
||||||
int found = -1;
|
|
||||||
for (int i = 0; i < static_cast<int>(v.size()); ++i)
|
|
||||||
if (v[i].find(needle) != std::string::npos) found = i;
|
|
||||||
return found;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
TEST_CASE("calibration homes first when the gimbal is not READY") {
|
|
||||||
FakeMotor motor; // starts in Boot (not ready)
|
|
||||||
motor.tel.pitch_present = true;
|
|
||||||
FakeImu imu(motor);
|
|
||||||
|
|
||||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
||||||
runToCompletion(cal);
|
|
||||||
|
|
||||||
const auto& ev = motor.events;
|
|
||||||
REQUIRE_FALSE(ev.empty());
|
|
||||||
CHECK(ev.front() == "HOME"); // homing is the first action
|
|
||||||
CHECK(indexOf(ev, "HOME") < indexOf(ev, "MOVE P")); // home before any pitch move
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("calibration does NOT home when already READY") {
|
|
||||||
FakeMotor motor;
|
|
||||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
||||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
||||||
motor.tel.pitch_present = true;
|
|
||||||
FakeImu imu(motor);
|
|
||||||
|
|
||||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
||||||
runToCompletion(cal);
|
|
||||||
|
|
||||||
CHECK(indexOf(motor.events, "HOME") == -1); // no homing issued
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("calibration runs pitch-at-first-yaw, then drift-correct+reset, then yaw") {
|
|
||||||
FakeMotor motor;
|
|
||||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
||||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
||||||
motor.tel.pitch_present = true;
|
|
||||||
FakeImu imu(motor);
|
|
||||||
|
|
||||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
||||||
runToCompletion(cal);
|
|
||||||
|
|
||||||
const auto& ev = motor.events;
|
|
||||||
// Yaw is parked at its first position before the pitch sweep begins.
|
|
||||||
const int first_yaw_move = indexOf(ev, "MOVE Y");
|
|
||||||
const int first_pitch_move = indexOf(ev, "MOVE P");
|
|
||||||
REQUIRE(first_yaw_move >= 0);
|
|
||||||
REQUIRE(first_pitch_move >= 0);
|
|
||||||
CHECK(first_yaw_move < first_pitch_move);
|
|
||||||
|
|
||||||
// No-mag profile switch, drift correction, then heading reset happen AFTER the
|
|
||||||
// pitch sweep and BEFORE the yaw sweep reaches its far end.
|
|
||||||
const int prof = indexOf(ev, "setProfile:53"); // VRU_general from the fake list
|
|
||||||
const int norot = indexOf(ev, "noRotation");
|
|
||||||
const int reset = indexOf(ev, "headingReset");
|
|
||||||
REQUIRE(prof >= 0);
|
|
||||||
REQUIRE(norot >= 0);
|
|
||||||
REQUIRE(reset >= 0);
|
|
||||||
CHECK(first_pitch_move < prof); // pitch calibrated before the profile switch
|
|
||||||
CHECK(prof < norot); // switch to no-mag, then correct drift
|
|
||||||
CHECK(norot < reset); // correct drift, then reset
|
|
||||||
CHECK(reset < lastIndexOf(ev, "MOVE Y")); // yaw sweep continues after the reset
|
|
||||||
|
|
||||||
// Pitch is commanded to 0 deg before the heading reset.
|
|
||||||
CHECK(indexOf(ev, "MOVE P 0") >= 0);
|
|
||||||
CHECK(indexOf(ev, "MOVE P 0") < reset);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("calibration skips the profile switch when no no-mag profile exists") {
|
|
||||||
FakeMotor motor;
|
|
||||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
||||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
||||||
motor.tel.pitch_present = true;
|
|
||||||
FakeImu imu(motor);
|
|
||||||
imu.has_profiles = false; // device reports no available profiles
|
|
||||||
|
|
||||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
||||||
runToCompletion(cal);
|
|
||||||
|
|
||||||
const auto& ev = motor.events;
|
|
||||||
CHECK(indexOf(ev, "setProfile") == -1); // nothing to switch to
|
|
||||||
CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw
|
|
||||||
CHECK(cal.report().valid);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("calibration recovers the fake IMU slope for both axes") {
|
|
||||||
FakeMotor motor;
|
|
||||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
||||||
motor.tel.pitch.state = AxisState::Ready; motor.tel.pitch.standstill = true;
|
|
||||||
motor.tel.pitch_present = true;
|
|
||||||
FakeImu imu(motor);
|
|
||||||
|
|
||||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
||||||
runToCompletion(cal);
|
|
||||||
|
|
||||||
CalibReport rep = cal.report();
|
|
||||||
REQUIRE(rep.valid);
|
|
||||||
CHECK(rep.all_ok);
|
|
||||||
REQUIRE(rep.axes.size() == 2);
|
|
||||||
for (const auto& ax : rep.axes) {
|
|
||||||
CHECK(ax.ok);
|
|
||||||
CHECK(ax.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
||||||
CHECK(ax.r2 == doctest::Approx(1.0).epsilon(0.001));
|
|
||||||
}
|
|
||||||
|
|
||||||
// The new geometry is published for the session to apply.
|
|
||||||
auto geo = cal.takeResult();
|
|
||||||
REQUIRE(geo.has_value());
|
|
||||||
CHECK(geo->yaw.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
||||||
CHECK(geo->pitch.counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("calibration handles a yaw-only gimbal (no pitch axis)") {
|
|
||||||
FakeMotor motor;
|
|
||||||
motor.pitch_present = false;
|
|
||||||
motor.tel.yaw.state = AxisState::Ready; motor.tel.yaw.standstill = true;
|
|
||||||
motor.tel.pitch_present = false;
|
|
||||||
FakeImu imu(motor);
|
|
||||||
|
|
||||||
CalibrationRoutine cal(motor, imu, uncalibrated(), fastParams());
|
|
||||||
runToCompletion(cal);
|
|
||||||
|
|
||||||
const auto& ev = motor.events;
|
|
||||||
CHECK(indexOf(ev, "MOVE P") == -1); // no pitch moves
|
|
||||||
CHECK(indexOf(ev, "headingReset") >= 0); // still resets + sweeps yaw
|
|
||||||
CHECK(indexOf(ev, "MOVE Y") >= 0);
|
|
||||||
|
|
||||||
CalibReport rep = cal.report();
|
|
||||||
REQUIRE(rep.valid);
|
|
||||||
REQUIRE(rep.axes.size() == 1);
|
|
||||||
CHECK(rep.axes[0].axis == 'Y');
|
|
||||||
CHECK(rep.axes[0].counts_per_deg == doctest::Approx(kFakeCpd).epsilon(0.001));
|
|
||||||
}
|
|
||||||
|
|
@ -3,9 +3,7 @@
|
||||||
#include "fgc/Config.h"
|
#include "fgc/Config.h"
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <cstdio>
|
|
||||||
#include <cstdlib>
|
#include <cstdlib>
|
||||||
#include <fstream>
|
|
||||||
|
|
||||||
using namespace fgc;
|
using namespace fgc;
|
||||||
|
|
||||||
|
|
@ -80,41 +78,3 @@ TEST_CASE("updateIniSectionKeys appends keys missing from the section") {
|
||||||
// re-parsing must see the appended key under [Motor]
|
// re-parsing must see the appended key under [Motor]
|
||||||
CHECK(out.find("pitch_zero_count = 7") != std::string::npos);
|
CHECK(out.find("pitch_zero_count = 7") != std::string::npos);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_CASE("saveMotorCalibration round-trips the full per-axis map") {
|
|
||||||
// Write a starting config, save a calibrated Geometry into it, reload, and
|
|
||||||
// confirm every [Motor] parameter (incl. min/max_deg) survives, while another
|
|
||||||
// section is left untouched.
|
|
||||||
const std::string path = "test_calib_roundtrip.ini";
|
|
||||||
{
|
|
||||||
std::ofstream f(path, std::ios::trunc);
|
|
||||||
f << "[General]\ntower_name = rig7\n\n"
|
|
||||||
"[Motor]\n"
|
|
||||||
"yaw_counts_per_deg = 1.0\n"
|
|
||||||
"yaw_zero_count = 0\n"
|
|
||||||
"yaw_min_deg = -90\n"
|
|
||||||
"yaw_max_deg = 90\n"
|
|
||||||
"pitch_counts_per_deg = 1.0\n"
|
|
||||||
"pitch_zero_count = 0\n"
|
|
||||||
"pitch_min_deg = 0\n"
|
|
||||||
"pitch_max_deg = 60\n";
|
|
||||||
}
|
|
||||||
|
|
||||||
Geometry geo;
|
|
||||||
geo.yaw = {983.33, 500123, -88.0, 92.0};
|
|
||||||
geo.pitch = {8333.33, -4200, -2.0, 61.5};
|
|
||||||
REQUIRE(saveMotorCalibration(path, geo));
|
|
||||||
|
|
||||||
AppConfig cfg = ConfigLoader::loadFromFile(path);
|
|
||||||
CHECK(cfg.general.tower_name == "rig7"); // unrelated section preserved
|
|
||||||
CHECK(cfg.geometry.yaw.counts_per_deg == doctest::Approx(983.33));
|
|
||||||
CHECK(cfg.geometry.yaw.zero_count == 500123);
|
|
||||||
CHECK(cfg.geometry.yaw.min_deg == doctest::Approx(-88.0));
|
|
||||||
CHECK(cfg.geometry.yaw.max_deg == doctest::Approx(92.0));
|
|
||||||
CHECK(cfg.geometry.pitch.counts_per_deg == doctest::Approx(8333.33));
|
|
||||||
CHECK(cfg.geometry.pitch.zero_count == -4200);
|
|
||||||
CHECK(cfg.geometry.pitch.min_deg == doctest::Approx(-2.0));
|
|
||||||
CHECK(cfg.geometry.pitch.max_deg == doctest::Approx(61.5));
|
|
||||||
|
|
||||||
std::remove(path.c_str());
|
|
||||||
}
|
|
||||||
|
|
|
||||||
|
|
@ -144,209 +144,6 @@ TEST_CASE("parseMTData reports yaw as a 0..360 heading") {
|
||||||
CHECK(got.yaw_deg == doctest::Approx(181.0f));
|
CHECK(got.yaw_deg == doctest::Approx(181.0f));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_CASE("orientation-control command builders are well-formed") {
|
|
||||||
SUBCASE("SetNoRotation carries the duration big-endian") {
|
|
||||||
auto m = msgSetNoRotation(3);
|
|
||||||
CHECK(m[0] == kMtiPreamble);
|
|
||||||
CHECK(m[1] == kMtiBid);
|
|
||||||
CHECK(m[2] == kMidSetNoRotation);
|
|
||||||
CHECK(m[3] == 2);
|
|
||||||
CHECK(m[4] == 0x00);
|
|
||||||
CHECK(m[5] == 0x03);
|
|
||||||
CHECK(frameChecksumOk(m));
|
|
||||||
}
|
|
||||||
SUBCASE("ResetOrientation heading reset = CODE 0x0001") {
|
|
||||||
auto m = msgResetOrientation(kResetHeading);
|
|
||||||
CHECK(m[2] == kMidResetOrientation);
|
|
||||||
CHECK(m[3] == 2);
|
|
||||||
CHECK(m[4] == 0x00);
|
|
||||||
CHECK(m[5] == 0x01);
|
|
||||||
CHECK(frameChecksumOk(m));
|
|
||||||
}
|
|
||||||
SUBCASE("ResetOrientation store = CODE 0x0000") {
|
|
||||||
auto m = msgResetOrientation(kResetStore);
|
|
||||||
CHECK(m[4] == 0x00);
|
|
||||||
CHECK(m[5] == 0x00);
|
|
||||||
CHECK(frameChecksumOk(m));
|
|
||||||
}
|
|
||||||
SUBCASE("SetFilterProfile carries the profile number (set = non-empty data)") {
|
|
||||||
auto m = msgSetFilterProfile(53);
|
|
||||||
CHECK(m[2] == kMidReqFilterProfile); // same MID as Req; data makes it a Set
|
|
||||||
CHECK(m[3] == 2);
|
|
||||||
CHECK(m[4] == 0x00);
|
|
||||||
CHECK(m[5] == 53);
|
|
||||||
CHECK(frameChecksumOk(m));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("pickNoMagProfile chooses a magnetometer-free profile") {
|
|
||||||
SUBCASE("prefers an explicit no-mag label") {
|
|
||||||
std::vector<ImuFilterProfile> ps = {
|
|
||||||
{39, 11, "General"}, {40, 11, "High_mag_dep"}, {7, 1, "Machine_nomagfield"}};
|
|
||||||
CHECK(pickNoMagProfile(ps) == 7);
|
|
||||||
}
|
|
||||||
SUBCASE("falls back to a VRU profile") {
|
|
||||||
std::vector<ImuFilterProfile> ps = {
|
|
||||||
{39, 11, "General"}, {53, 11, "VRU_general"}, {40, 11, "High_mag_dep"}};
|
|
||||||
CHECK(pickNoMagProfile(ps) == 53);
|
|
||||||
}
|
|
||||||
SUBCASE("returns -1 when every profile uses the magnetometer") {
|
|
||||||
std::vector<ImuFilterProfile> ps = {
|
|
||||||
{39, 11, "General"}, {40, 11, "High_mag_dep"}, {41, 11, "Dynamic"}};
|
|
||||||
CHECK(pickNoMagProfile(ps) == -1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("config-readback query builders are well-formed requests (empty data)") {
|
|
||||||
struct Q { std::vector<uint8_t> m; uint8_t mid; };
|
|
||||||
Q qs[] = {
|
|
||||||
{msgReqProductCode(), kMidReqProductCode},
|
|
||||||
{msgReqDID(), kMidReqDID},
|
|
||||||
{msgReqFWRev(), kMidReqFWRev},
|
|
||||||
{msgReqPeriod(), kMidReqPeriod},
|
|
||||||
{msgReqOutputMode(), kMidSetOutputMode},
|
|
||||||
{msgReqOutputSettings(), kMidSetOutputSettings},
|
|
||||||
{msgReqFilterProfile(), kMidReqFilterProfile},
|
|
||||||
{msgReqAvailFilterProfiles(), kMidReqAvailFilterProf},
|
|
||||||
};
|
|
||||||
for (const auto& q : qs) {
|
|
||||||
CHECK(q.m[0] == kMtiPreamble);
|
|
||||||
CHECK(q.m[1] == kMtiBid);
|
|
||||||
CHECK(q.m[2] == q.mid);
|
|
||||||
CHECK(q.m[3] == 0); // request => empty data field
|
|
||||||
CHECK(frameChecksumOk(q.m));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("applyImuConfigAck decodes each ack type") {
|
|
||||||
ImuDeviceConfig c;
|
|
||||||
|
|
||||||
SUBCASE("DeviceID is a 32-bit big-endian serial") {
|
|
||||||
uint8_t d[] = {0x00, 0x99, 0x0A, 0xBC};
|
|
||||||
CHECK(applyImuConfigAck(c, kMidDeviceID, d, sizeof(d)));
|
|
||||||
CHECK(c.has_device_id);
|
|
||||||
CHECK(c.device_id == 0x00990ABCu);
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("ProductCode trims trailing spaces") {
|
|
||||||
const char* s = "MTi-28A53G35 ";
|
|
||||||
CHECK(applyImuConfigAck(c, kMidProductCode,
|
|
||||||
reinterpret_cast<const uint8_t*>(s), 15));
|
|
||||||
CHECK(c.product_code == "MTi-28A53G35");
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("FirmwareRev with build number") {
|
|
||||||
uint8_t d[] = {2, 8, 1, 0, 0, 0, 25}; // 2.8.1 build 25
|
|
||||||
CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d)));
|
|
||||||
CHECK(c.firmware == "2.8.1 build 25");
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("FirmwareRev short form (major.minor.rev only)") {
|
|
||||||
uint8_t d[] = {1, 2, 3};
|
|
||||||
CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d)));
|
|
||||||
CHECK(c.firmware == "1.2.3");
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("Period yields 100 Hz from 0x0480") {
|
|
||||||
uint8_t d[] = {0x04, 0x80}; // 1152 => 115200/1152 = 100 Hz
|
|
||||||
CHECK(applyImuConfigAck(c, kMidReqPeriodAck, d, sizeof(d)));
|
|
||||||
finalizeImuConfig(c);
|
|
||||||
CHECK(c.has_period);
|
|
||||||
CHECK(c.sample_rate_hz == doctest::Approx(100.0f));
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("OutputMode 0x0007 = Temp + Calibrated + Orientation") {
|
|
||||||
uint8_t d[] = {0x00, 0x07};
|
|
||||||
CHECK(applyImuConfigAck(c, kMidSetOutputModeAck, d, sizeof(d)));
|
|
||||||
CHECK(c.out_temperature);
|
|
||||||
CHECK(c.out_calibrated);
|
|
||||||
CHECK(c.out_orientation);
|
|
||||||
CHECK_FALSE(c.out_auxiliary);
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("OutputSettings 0x00000005 = Euler + sample counter + float, all channels") {
|
|
||||||
uint8_t d[] = {0x00, 0x00, 0x00, 0x05};
|
|
||||||
CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d)));
|
|
||||||
CHECK(c.orientation_mode == "Euler");
|
|
||||||
CHECK(c.timestamp_mode == "Sample counter");
|
|
||||||
CHECK(c.data_format == "Float");
|
|
||||||
CHECK(c.acc_enabled);
|
|
||||||
CHECK(c.gyr_enabled);
|
|
||||||
CHECK(c.mag_enabled);
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("OutputSettings with disabled mag and fixed-point format") {
|
|
||||||
// bit6 set (disable mag), output format bits 9:8 = 01 (Fixed 12.20).
|
|
||||||
uint32_t s = 0x05 | 0x40 | 0x100;
|
|
||||||
uint8_t d[] = {uint8_t(s >> 24), uint8_t(s >> 16), uint8_t(s >> 8), uint8_t(s)};
|
|
||||||
CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d)));
|
|
||||||
CHECK(c.acc_enabled);
|
|
||||||
CHECK(c.gyr_enabled);
|
|
||||||
CHECK_FALSE(c.mag_enabled);
|
|
||||||
CHECK(c.data_format == "Fixed 12.20");
|
|
||||||
}
|
|
||||||
|
|
||||||
SUBCASE("unknown MID is ignored") {
|
|
||||||
uint8_t d[] = {0};
|
|
||||||
CHECK_FALSE(applyImuConfigAck(c, 0x99, d, sizeof(d)));
|
|
||||||
CHECK_FALSE(c.valid);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("scenario label resolves against the available-profiles list") {
|
|
||||||
ImuDeviceConfig c;
|
|
||||||
// AvailableFilterProfiles: two 22-byte records (type, version, 20-byte label).
|
|
||||||
std::vector<uint8_t> d;
|
|
||||||
auto addProfile = [&](uint8_t type, uint8_t ver, const std::string& label) {
|
|
||||||
d.push_back(type);
|
|
||||||
d.push_back(ver);
|
|
||||||
std::string padded = label;
|
|
||||||
padded.resize(20, ' ');
|
|
||||||
d.insert(d.end(), padded.begin(), padded.end());
|
|
||||||
};
|
|
||||||
addProfile(39, 11, "General");
|
|
||||||
addProfile(40, 11, "High_mag_dep");
|
|
||||||
CHECK(applyImuConfigAck(c, kMidAvailFilterProf, d.data(), d.size()));
|
|
||||||
REQUIRE(c.available_profiles.size() == 2);
|
|
||||||
CHECK(c.available_profiles[0].label == "General");
|
|
||||||
CHECK(c.available_profiles[1].label == "High_mag_dep");
|
|
||||||
|
|
||||||
// Current filter profile ack: VERSION, FILTERPROFILE(type).
|
|
||||||
uint8_t fp[] = {11, 40};
|
|
||||||
CHECK(applyImuConfigAck(c, kMidReqFilterProfileAck, fp, sizeof(fp)));
|
|
||||||
finalizeImuConfig(c);
|
|
||||||
CHECK(c.has_scenario);
|
|
||||||
CHECK(c.scenario_type == 40);
|
|
||||||
CHECK(c.scenario_label == "High_mag_dep");
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("a full ack stream decodes through the framer") {
|
|
||||||
// Concatenate realistic acks and run them through MtiFramer, mirroring how
|
|
||||||
// the live handshake collects them.
|
|
||||||
ImuDeviceConfig c;
|
|
||||||
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
|
||||||
applyImuConfigAck(c, mid, p, n);
|
|
||||||
});
|
|
||||||
auto feed = [&](const std::vector<uint8_t>& m) { fr.feed(m.data(), m.size()); };
|
|
||||||
|
|
||||||
feed(mtiMessage(kMidProductCode,
|
|
||||||
{'M','T','i','-','2','8'}));
|
|
||||||
feed(mtiMessage(kMidDeviceID, {0x00, 0x99, 0x0A, 0xBC}));
|
|
||||||
feed(mtiMessage(kMidSetOutputModeAck, {0x00, 0x07}));
|
|
||||||
feed(mtiMessage(kMidSetOutputSettingsAck, {0x00, 0x00, 0x00, 0x05}));
|
|
||||||
feed(mtiMessage(kMidReqPeriodAck, {0x04, 0x80}));
|
|
||||||
feed(mtiMessage(kMidReqFilterProfileAck, {11, 39}));
|
|
||||||
finalizeImuConfig(c);
|
|
||||||
|
|
||||||
CHECK(c.valid);
|
|
||||||
CHECK(c.product_code == "MTi-28");
|
|
||||||
CHECK(c.device_id == 0x00990ABCu);
|
|
||||||
CHECK(c.out_orientation);
|
|
||||||
CHECK(c.orientation_mode == "Euler");
|
|
||||||
CHECK(c.sample_rate_hz == doctest::Approx(100.0f));
|
|
||||||
CHECK(c.scenario_type == 39);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_CASE("framer rejects a bad checksum and a wrong-length payload") {
|
TEST_CASE("framer rejects a bad checksum and a wrong-length payload") {
|
||||||
std::vector<uint8_t> d(kMTDataLen, 0);
|
std::vector<uint8_t> d(kMTDataLen, 0);
|
||||||
auto frame = mtiMessage(kMidMTData, d);
|
auto frame = mtiMessage(kMidMTData, d);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue