diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d2e62a..077f52d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,7 @@ add_library(fgc_core STATIC src/core/Calibration.cpp src/core/CalibrationRoutine.cpp src/core/MtiProtocol.cpp + src/core/ImuMount.cpp src/core/CameraProfile.cpp src/core/CameraSettingsXml.cpp src/core/Hash.cpp diff --git a/config/config.example.ini b/config/config.example.ini index d3608eb..bb14a71 100644 --- a/config/config.example.ini +++ b/config/config.example.ini @@ -79,6 +79,63 @@ baud = 115200 device = baud = 115200 +; The host puts the MTi into a fixed output configuration at every startup +; (Euler + calibrated at 100 Hz) and then READS IT BACK to check the device +; actually took it. There is deliberately no setting for that configuration: it +; is a wire-format contract the parser depends on, not a preference. A mismatch +; is logged as an error and shown as a red MISCONFIGURED row in the Sensors +; view (press i) - the program keeps running so you can diagnose it. +; +; The MTi measures orientation against its OWN earth frame, and the classic +; unit's is NWU (X North, Y West, Z UP) - which reports pitch FALLING as the nose +; rises. The host re-references it to NED. That correction is not settable here: +; it is a fact about the device, stated once in kMtiReference (MtiProtocol.h), +; and shown as the Reference row in the Sensors view (press i). +; +; It is also NOT the same thing as the mount below, and one cannot substitute for +; the other: the earth reference multiplies from the left, the mount from the +; right. No x/y/z value can correct an inverted pitch. +; +; --- Mounting orientation -------------------------------------------------- +; How the IMU is bolted to the gimbal. Both the gimbal and the IMU use a NED +; body frame: +; +; X = forward Y = right Z = down +; roll about X pitch about Y yaw about Z +; (positive follows the right-hand rule about that axis) +; +; x, y and z are DEGREES, and they are the rotation you apply TO THE IMU to +; bring it into alignment with the gimbal - the turns needed to get from the +; IMU's orientation to the gimbal's. So an IMU that physically sits turned 90 +; deg to the right is corrected with z = -90. +; +; Applied in Z-X-Y order, each turn about the axes as they stand after the +; previous one: first z about down, then x about the new forward, then y about +; the new right. +; +; Worked examples: +; dead level, facing the same way as the gimbal ..... x=0 y=0 z=0 +; upside down, still facing forward ................. x=180 y=0 z=0 +; turned 90 deg clockwise seen from above ........... x=0 y=0 z=-90 +; lying on its right side, facing forward ........... x=-90 y=0 z=0 +; upside down AND turned to face backwards .......... x=180 y=0 z=180 +; +; Every reading the program produces - orientation, acceleration, rate of turn +; and magnetic field - is rotated into the gimbal frame by this. Leaving all +; three at 0 changes nothing. +; +; Check it on the rig: press `i` for the Sensors view. Every reading there is +; already corrected, and IMU CONFIG shows the mount being applied. Level the +; gimbal - roll and pitch should read ~0. Tip the nose up - pitch goes +; positive. Roll to the right - roll goes positive. Slew right - yaw increases. +; +; NOTE: `gimbal calib` fits IMU yaw against encoder counts, so a calibration +; made before this was set has the mount's error baked in. Re-run it after +; changing these. +x = 0 +y = 0 +z = 0 + [Env] ; Ambient temperature/humidity sensor (Adafruit SHT41), wired to the ; LattePanda's OWN native I2C bus - NOT the Arduino/motor serial link. Bus diff --git a/docs/configuration.md b/docs/configuration.md index b2a2319..d2ea13e 100644 --- a/docs/configuration.md +++ b/docs/configuration.md @@ -57,6 +57,7 @@ Parsed and validated by `ConfigLoader` ([src/core/Config.cpp](../src/core/Config | `Features` | `mock_env` | bool | `false` | Use the simulated env sensor instead of the SHT41 | | `IMU` | `device` | string | — | MTi serial device (see `[IMU]` note); required when `enable_imu` | | `IMU` | `baud` | int | `115200` | MTi serial baud rate | +| `IMU` | `x` / `y` / `z` | float ±360 | `0` | Mounting rotation in degrees about forward/right/down, applied to every IMU reading (see below). All zero = the IMU is aligned with the gimbal | | `Env` | `i2c_device` | string | `/dev/i2c-1` | SHT41 I2C bus device (see `[Env]` note); required when `enable_env` | | `Env` | `i2c_addr` | int | `68` (`0x44`) | SHT41 I2C slave address; decimal or `0x`-prefixed hex | | `Env` | `period_ms` | int | `2000` | Sample interval | @@ -124,7 +125,7 @@ LattePanda the MTi is wired to the **RS-232 header**, i.e. an onboard hardware U modem). Find it with `ls /dev/ttyS*` / `dmesg | grep -iE 'ttyS|LPSS'`, or probe for the `0xFA`-framed stream. At startup the host reconfigures the MTi (`GoToConfig → SetOutputMode → SetOutputSettings → GoToMeasurement`) to a 100 Hz Euler + calibrated stream — so the device will start streaming even if it -was left in Config state. Yaw is reported as a **0..360 heading** (not the MTi's native −180..180). Set +was left in Config state. Set `[Features] mock_imu = true` to use a synthetic IMU on dev machines (no hardware). Protocol/units are documented in the modules reference (`MtiProtocol`). @@ -144,6 +145,135 @@ The config is read **once at startup** and cached, so if you change the XKF prof 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. +#### The device configuration is verified, not assumed + +What the host asks the MTi for — `OutputMode` and `OutputSettings` — is a **wire-format +contract, not a preference**, and is deliberately **not exposed in `config.ini`**. +`parseMTData` decodes a fixed field order that follows from exactly those bits, so a +settable value would only give you ways to break the parser. The payload length is +therefore *derived* from the settings at compile time (`mtDataLen`), guarded by a +`static_assert`: change a bit and the length follows, so the parser must be updated with +it rather than silently mismatching at runtime. + +Since the handshake's writes were previously blind, the host now **compares the readback +against what it asked for** (`checkImuDeviceConfig`) and reports every difference at +`ERROR`. This matters because the length gate cannot see the two most damaging +misconfigurations — a fixed-point instead of float data format, and the wrong coordinate +system — since neither changes the payload length. A device that never answered the +`Req*` queries is reported as **unverifiable**, never as a match. An `Error` message +(MID `0x42`), i.e. the device rejecting a message outright, is also decoded and logged +instead of being dropped. + +A failed check never stops the program: it logs at `ERROR`, shows a red **MISCONFIGURED** +row in the **IMU CONFIG** section of the expanded Sensors view (`i`), a red ` IMU CFG ` +badge on the compact Sensors panel, and repeats in `imu config`. The persistent surfaces +matter because the handshake runs *before* the TUI installs its log sink, so the log line +alone is wiped by the alt-screen. + +#### Earth reference — why pitch fell as the nose rose + +The MTi measures orientation against **its own local earth frame**, and the classic unit's +is **NWU** (X North, Y West, Z **Up**). The gimbal frame is NED. A Z-up reference reports +pitch with the opposite sign: with the body Y axis pointing left, a right-hand rotation +about it tips the nose *down*, so "nose up" comes back negative. + +**No `[IMU] x/y/z` value can fix this.** Re-referencing the earth frame +*left*-multiplies the reported orientation; a mount *right*-multiplies it, and no fixed +right-multiplication reproduces a left-multiplication across all attitudes. With an +upside-down IMU the mount alone does nothing but add 180° to roll — pitch passes straight +through, which is exactly the symptom. + +Both corrections are applied, in `applyFrames` +([ImuMount.h](../include/fgc/ImuMount.h)): + +``` +R_ned←gimbal = nedFromReference(ref) · R_local←sensor · mountRotationZXY(mount) + \______ earth ______/ \______ body ______/ +``` + +Only the **orientation** is re-referenced. `acc`, `gyr` and `mag` are body-frame +directions — which way the sensor felt something, not where it points on the earth — so +the earth frame leaves them alone and only the mount rotates them. + +The device is deliberately **not** asked to change its own coordinate system +(`mtsettings::kCoordSysNed` stays `0`). Correcting on the host is deterministic, +reversible and unit-testable without hardware; writing an unverified bit to the unit's +non-volatile memory would be none of those. And because the host rewrites `OutputSettings` +on every startup, the device's reference is pinned to what that word selects rather than +to whatever was left in the unit — which is what lets `kMtiReference` +([MtiProtocol.h](../include/fgc/MtiProtocol.h)) state it with certainty. + +The active reference is stated on the startup log line (`IMU frames: reference NWU -> NED; +mount ...`). It is not shown in the Sensors view: unlike the mount it is a fixed property of +the device rather than something the operator sets, so it would be a permanent row nobody +can act on. + +> **One thing to check on the rig.** NWU and ENU produce *identical* roll and pitch and +> differ only in yaw, by exactly 90°. If the corrected yaw is 90° off a compass, the device +> is ENU: change `kMtiReference` to `ImuReference::Enu`. Nothing else needs to move. + +#### Raw readings — nothing is flipped + +Roll, pitch, yaw, acceleration, rate of turn and magnetic field are reported **exactly as the device +sends them**, including yaw's native **−180..180** (it used to be shifted to a 0..360 heading; that +shift is gone). No axis is negated and no range is rewritten anywhere between the wire and the screen. + +**The parser applies no transformation at all**: no axis is negated and no range is rewritten +between the wire and `ImuSample`. The two corrections that do apply are declared above and +applied together in one place (`applyFrames`, via `MountedImuSource`) — the earth reference +NWU → NED, and the `[IMU] x/y/z` mount. With a NED-referenced device and `x`/`y`/`z` all zero +there is no transformation at all and you see the sensor's own numbers. + +Consumers that do arithmetic across yaw handle the ±180 wrap themselves with `unwrapNear` +([Calibration.h](../include/fgc/Calibration.h)): the calibration sweep unwraps per waypoint before +fitting, and the IMU drift test unwraps before its slope and spread. That makes both independent of +which heading the tower happens to face, rather than merely moving the problem to a different bearing. + +#### Mounting orientation (`x` / `y` / `z`) + +The MTi reports in **its own sensor frame**. `[IMU] x`, `y` and `z` say how it is bolted on, and every +reading the program produces — orientation, acceleration, rate of turn and magnetic field — is rotated +into the gimbal's frame accordingly. Nothing downstream sees the sensor frame: the rotation is applied +at the source (`MountedImuSource`), so the calibration routine, the self-test suite, the Sensors view +and `imu status` all speak gimbal coordinates. + +Both frames are NED body frames: + +``` +X = forward Y = right Z = down +roll about X pitch about Y yaw about Z (right-hand rule) +``` + +`x`, `y` and `z` are degrees, and they are **the rotation you apply to the IMU to bring it into +alignment with the gimbal** — the turns needed to get *from* the IMU's orientation *to* the gimbal's. +An IMU that physically sits turned 90° to the right is therefore corrected with `z = -90`. + +They compose in **Z-X-Y order, intrinsically**: first `z` about down, then `x` about the forward axis +*as it now stands*, then `y` about the new right — i.e. `R = Rz(z)·Rx(x)·Ry(y)`. Order matters as soon +as two of them are non-zero. + +| The IMU is… | `x` | `y` | `z` | +|---|---|---|---| +| level, facing the same way as the gimbal | 0 | 0 | 0 | +| upside down, still facing forward | 180 | 0 | 0 | +| turned 90° clockwise seen from above | 0 | 0 | -90 | +| lying on its right side, facing forward | -90 | 0 | 0 | +| upside down and facing backwards | 180 | 0 | 180 | + +Values outside ±360° are rejected at startup, and a non-zero mount is logged at INFO so the session's +log records which frame its readings are in. The expanded Sensors view (`i`) shows the mount in force +on the `Mount (cfg)` row of **IMU CONFIG** — labelled `(cfg)` because, unlike everything else in that +section, it comes from `config.ini` rather than from the device. + +**To check it on the rig:** level the gimbal — roll and pitch should read ≈0. Tip the nose up — pitch +goes positive. Roll to the right — roll goes positive. Slew right — yaw increases. + +**Re-run `gimbal calib` after changing the mount.** Calibration fits IMU yaw against encoder counts, so +a calibration made under a different (or absent) mount has that error baked into `yaw_zero_count`. + +`imu zero` is unaffected: it acts on the device, before this rotation, and the two compose correctly — +the mount is what makes "zero" mean zero *in the gimbal frame*. + ### `[Camera]` — cameras and profiles Each camera gets its own `[Camera.]` block. Nothing is implied by position: diff --git a/docs/known-issues.md b/docs/known-issues.md index f002443..649b710 100644 --- a/docs/known-issues.md +++ b/docs/known-issues.md @@ -107,17 +107,43 @@ scripts for this are worth keeping; note that wrapping them in `timeout` SIGTERM ## Possible follow-ups (not done) +- **NWU vs ENU is decided by a constant, not by the device.** `kMtiReference` + ([MtiProtocol.h](../include/fgc/MtiProtocol.h)) says the MTi reports against NWU, and the + host re-references it to NED. The two candidates give *identical* roll and pitch and + differ only in yaw, by exactly 90°, so nothing in the software can tell them apart — + only a compass check on the rig can. If corrected yaw is 90° off, change that constant to + `ImuReference::Enu`. Reading the device's actual coordinate-system bits would settle it, + but their position in `OutputSettings` is not established for this firmware. +- **`[IMU] x` should be re-confirmed now that the reference is corrected.** `x = 180` was + set while pitch was inverted. Level the gimbal and confirm roll and pitch ≈ 0, tip the + nose up and confirm pitch goes **positive**, roll right and confirm roll goes positive. + Then re-run `gimbal calib` — its yaw reference moved with the earth-frame correction. +- **The handshake still does not await acks for its state transitions.** + `GoToConfig`/`GoToMeasurement` are blind writes separated by 60 ms sleeps + ([MtiImuSource.cpp](../src/serial/MtiImuSource.cpp)). The readback verification makes + this harmless for correctness — if `GoToConfig` is missed the Sets do nothing and the + mismatch is reported — but a missed transition is diagnosed one step later than it + happened. +- **A saved calibration silently predates the IMU mount.** `gimbal calib` fits IMU yaw against encoder + counts, so a calibration made before `[IMU] x/y/z` were set — or under different values — has that + mount error baked into `yaw_zero_count`. Nothing detects this; re-run `gimbal calib` after changing + the mount. Stamping the mount into the saved `[Motor]` block and warning on a mismatch would close it. +- **The mount is host-side only.** The rotation is applied to the MTi's output, so the XKF filter itself + still runs in the sensor frame. Xsens `SetAlignmentRotation` (MID 0xEC) would push the alignment into + the device so the filter's own gravity/magnetic corrections are made in the gimbal frame — better + filtering, at the price of writing to the unit's non-volatile memory, which `config.ini` cannot undo. + - **A profile cannot be reloaded without a restart.** `set camera profile ` re-applies a profile from the copy loaded at start-up, so re-exporting the XML while the program runs has no effect. Tuning on the rig therefore costs a restart per export. Re-reading on switch would fix it, at the price of a failure mode (a mid-session unreadable export) that start-up loading currently rules out. -- **Nothing checks that a mode actually took.** Features are written and warned about individually, but the - camera is never read back to confirm the resulting state, and a mode whose every write failed still logs - as applied. A post-apply readback of a handful of key features (`AcquisitionFrameRate`, `Width`, - `PixelFormat`) would turn a silent misconfiguration into an error. +- **Nothing checks that a profile actually took.** `LoadSettings` reports success for the file as a whole, + but the camera is never read back to confirm the resulting state. A post-apply readback of a handful of + key features (`AcquisitionFrameRate`, `Width`, `PixelFormat`) would turn a silent misconfiguration into + an error. - Graceful shutdown on SIGINT (currently exit via `exit`/Ctrl-D; a pending `getline` can delay shutdown). -- Make the camera index→label map fully config-driven. (JPEG XL distance/effort live under `[Camera]`, and - every on-camera setting lives in a mode file — see [configuration.md](configuration.md).) +- Support a second camera backend. `driver` is parsed and validated but only `vimba` is accepted, so a + thermal or NIR unit needs a new `ICameraSource` implementation behind that key. - Reintroduce optional image upload to the ground station, config-driven (the old hardcoded NFS/SMB upload was removed). - **`gimbal calib` persistence**: the fitted `counts_per_deg`/`zero_count` are always applied to the live diff --git a/docs/modules-reference.md b/docs/modules-reference.md index 0183a61..80bb7eb 100644 --- a/docs/modules-reference.md +++ b/docs/modules-reference.md @@ -18,7 +18,9 @@ Per-file reference for the refactored tree, plus the shared data structures. | [include/fgc/DiagParser.h](../include/fgc/DiagParser.h), [src/core/DiagParser.cpp](../src/core/DiagParser.cpp) | `parseDiag` firmware `DG` self-test stream → `DiagResult` (per-axis tests, PASS/FAIL); `formatDiag` | | [include/fgc/Calibration.h](../include/fgc/Calibration.h), [src/core/Calibration.cpp](../src/core/Calibration.cpp) | `linearFit` (least-squares, R²) + `circularMeanDeg` for the IMU-referenced calibration | | [include/fgc/CalibrationRoutine.h](../include/fgc/CalibrationRoutine.h), [src/core/CalibrationRoutine.cpp](../src/core/CalibrationRoutine.cpp) | `gimbal calib` worker thread: home-if-needed → pitch sweep at the first yaw position → pitch 0° → switch IMU to a no-mag XKF profile → IMU no-rotation + heading reset → yaw sweep, fitting degrees↔counts; tunable via `CalibParams`; exposes `progress()`/`report()`/`takeResult()` | -| [include/fgc/MtiProtocol.h](../include/fgc/MtiProtocol.h), [src/core/MtiProtocol.cpp](../src/core/MtiProtocol.cpp) | Xsens MTi binary protocol: `MtiFramer` (checksum framing), config-message builders, `parseMTData` → `ImuSample` (temp/acc/gyr/mag/euler), 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), 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); named `mtmode`/`mtsettings` bitfields with `mtDataLen` deriving the payload length from them (so `kMTDataLen` cannot drift from what the parser decodes), plus `checkImuDeviceConfig` → `ImuConfigMismatch` (does the device report what the host asked for?) and `mtiErrorText` for MID `0x42` rejections | +| [include/fgc/ImuMount.h](../include/fgc/ImuMount.h), [src/core/ImuMount.cpp](../src/core/ImuMount.cpp) | `ImuMount` (the `[IMU] x/y/z` mounting rotation) and the pure math around it: `mountRotationZXY` (intrinsic Z-X-Y), the `rotationFromEulerZYX`/`eulerZYXFromRotation` pair for the MTi's own yaw-pitch-roll output, `applyFrames`, which moves a whole `ImuSample` into the gimbal's NED frame by re-referencing the earth frame (`ImuReference`/`nedFromReference`, left-multiplied) and then applying the mount (right-multiplied) — the two are not interchangeable, which is why a mount alone cannot correct a Z-up device; `applyMount` is the NED-already shorthand. Body-frame vectors are rotated by the mount only. Returns the sample untouched when there is nothing to do | +| [include/fgc/MountedImuSource.h](../include/fgc/MountedImuSource.h) | Decorator over any `IImuSource` applying that rotation in `sample()`. Wrapped around the factory's output in `Application::makeImu()`, so every consumer reads gimbal-frame values without knowing a mount exists; skipped entirely when the mount is the identity | | [include/fgc/sensors/Sht41Protocol.h](../include/fgc/sensors/Sht41Protocol.h), [src/sensors/Sht41Protocol.cpp](../src/sensors/Sht41Protocol.cpp) | Sensirion SHT4x wire protocol, pure: command bytes, `sht41Crc8` (poly 0x31/init 0xFF), `decodeSht41Measurement` (6-byte response → `Sht41Reading`, CRC-checked, raw→°C/%RH with RH clamped 0..100) | | [include/fgc/CameraProfile.h](../include/fgc/CameraProfile.h), [src/core/CameraProfile.cpp](../src/core/CameraProfile.cpp) | `loadCameraProfile` / `loadCameraProfiles`: one VimbaX settings XML per profile. Hashes the XML, regenerates the `.txt` record when it changes, and reports which settings moved. A missing XML throws — the record is never a fallback | | [include/fgc/CameraSettingsXml.h](../include/fgc/CameraSettingsXml.h), [src/core/CameraSettingsXml.cpp](../src/core/CameraSettingsXml.cpp) | Parses the Viewer export's `` features (ordered, selector-aware), renders the human-readable `.txt` record, reads one back, and diffs two settings lists. Used for display and change detection only — never to configure a camera | @@ -38,7 +40,7 @@ Per-file reference for the refactored tree, plus the shared data structures. | [include/fgc/IMotorController.h](../include/fgc/IMotorController.h) | `IMotorController` | `MotorTelemetry` | | [include/fgc/IControlChannel.h](../include/fgc/IControlChannel.h) | `IControlChannel` | `ControlCommand`, `CamEvent`, `EnvEvent` | | [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` (incl. `config()`, `configProblems()`, `refreshConfig()`, `noRotation()`, `headingReset()`, `setFilterProfile()`) | `ImuSample` (from `MtiProtocol.h`) | | [include/fgc/IEnvSensor.h](../include/fgc/IEnvSensor.h) | `IEnvSensor` (`connected()`, `sample()`, `name()`) | `EnvSample` | | [include/fgc/ui/IUserInterface.h](../include/fgc/ui/IUserInterface.h) | `IUserInterface` | `UiSnapshot` | @@ -112,7 +114,7 @@ Owned pixel buffer + `width`, `height`, `channels` (1 or 3), `timestamp_ms`, `ca ### `ImuSample` ([MtiProtocol.h](../include/fgc/MtiProtocol.h)) One decoded Xsens MTi reading: `temp_c` (°C), `acc[3]` (m/s², incl. gravity), `gyr[3]` (rad/s), `mag[3]` (a.u., earth-normalized), `roll_deg` (−180..180), `pitch_deg` (−90..90), `yaw_deg` -(**0..360 heading** — the MTi's native −180..180 is shifted by `parseMTData`), `sample_counter`. +(**−180..180**, exactly as the device reports it), `sample_counter`. `parseMTData` applies no sign flip and no re-ranging to any axis. `gimbal calib` phase-unwraps the swept yaw (`unwrapNear`) so a sweep crossing 0/360 still fits a clean line. diff --git a/include/fgc/Config.h b/include/fgc/Config.h index a1944fd..44b03b7 100644 --- a/include/fgc/Config.h +++ b/include/fgc/Config.h @@ -1,5 +1,6 @@ #pragma once +#include "fgc/ImuMount.h" #include "fgc/Geometry.h" #include @@ -109,6 +110,12 @@ struct FeaturesConfig { struct ImuConfig { std::string device = ""; // e.g. /dev/ttyS4; empty => required when enabled unsigned int baud = 115200; + + // How the unit is bolted to the gimbal, as [IMU] x/y/z. Every reading the + // program produces is rotated into the gimbal's NED body frame by this, so + // nothing downstream ever sees the sensor frame. See ImuMount.h for the + // convention; the default is the identity, which changes nothing. + ImuMount mount; }; // [Env]: ambient temperature/humidity sensor (SHT41) on the LattePanda's own diff --git a/include/fgc/IImuSource.h b/include/fgc/IImuSource.h index d5feaf6..e28f930 100644 --- a/include/fgc/IImuSource.h +++ b/include/fgc/IImuSource.h @@ -3,6 +3,8 @@ #include "fgc/MtiProtocol.h" // ImuSample #include +#include +#include namespace fgc { @@ -27,6 +29,15 @@ public: // backend cannot report it. virtual std::optional config() const { return std::nullopt; } + // How the device disagreed with what the host asked it for during the last + // handshake, already formatted one finding per entry. Empty means it agreed - + // or that the backend does not configure a device at all. + // + // This exists because the handshake runs before the TUI installs its log sink, + // so an error logged there scrolls away with the alt-screen. The operator has + // to be able to see it later, from the Sensors view and from `imu config`. + virtual std::vector configProblems() const { return {}; } + // 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. diff --git a/include/fgc/ImuMount.h b/include/fgc/ImuMount.h new file mode 100644 index 0000000..af56271 --- /dev/null +++ b/include/fgc/ImuMount.h @@ -0,0 +1,85 @@ +#pragma once + +#include "fgc/MtiProtocol.h" // ImuSample + +#include + +namespace fgc { + +// How the IMU is physically bolted to the gimbal. +// +// Both frames are NED body frames: +// +// X = forward Y = right Z = down +// roll about X pitch about Y yaw about Z (right-hand rule) +// +// x/y/z are degrees, and they are the rotation applied TO THE IMU to bring it +// into alignment with the gimbal - the direction the operator reads off the +// installation "to get from the IMU to the gimbal". An IMU that physically sits +// yawed +90 deg is therefore corrected with z = -90. +// +// Composition is intrinsic Z-X-Y: turn about Z, then about the X axis as it now +// stands, then the new Y. +struct ImuMount { + double x_deg = 0.0; // roll - about forward + double y_deg = 0.0; // pitch - about right + double z_deg = 0.0; // yaw - about down + + // No rotation at all. The common case, and worth testing for: an unmounted + // rig then pays nothing and its samples pass through bit for bit. + bool identity() const; +}; + +// Row-major 3x3. m[r][c]. +using Mat3 = std::array, 3>; + +// The rotation carrying the device's local earth frame onto NED. Identity for a +// device already reporting NED. +Mat3 nedFromReference(ImuReference r); + +// Short name for logs and the Sensors view ("NED", "NWU", "ENU"). +const char* referenceName(ImuReference r); + +// The mount rotation: R = Rz(z) * Rx(x) * Ry(y). +// +// R carries the IMU frame onto the gimbal frame. Two transforms follow from it, +// and they are NOT the same matrix: +// +// - a sensor-frame vector's components in the gimbal frame: v_gimbal = R^T v_sensor +// - the gimbal's orientation: R_earth<-gimbal = R_earth<-sensor * R +Mat3 mountRotationZXY(const ImuMount& m); + +// The MTi's own orientation output is the ordinary aerospace Z-Y-X sequence +// (yaw, then pitch, then roll), which is a DIFFERENT order from the mount above. +// These two helpers exist as a named pair so the two conventions cannot quietly +// swap places: R = Rz(yaw) * Ry(pitch) * Rx(roll). +Mat3 rotationFromEulerZYX(double roll_deg, double pitch_deg, double yaw_deg); + +// Inverse of the above. Angles come back in the ranges atan2/asin produce, which +// are the MTi's own: roll and yaw in -180..180, pitch in -90..90. Degenerate at +// pitch = +/-90, where roll and yaw are no longer separable: roll is pinned to 0 +// and the whole rotation is carried in yaw. +void eulerZYXFromRotation(const Mat3& R, double& roll_deg, double& pitch_deg, double& yaw_deg); + +// The whole reading moved into the gimbal frame: orientation, acceleration, +// rate of turn and magnetic field. Temperature, sample counter and the valid +// flag are properties of the device rather than of a frame, and pass through. +// +// Returns the sample unchanged when the mount is the identity. +ImuSample applyMount(const ImuMount& m, const ImuSample& s); + +// The full frame correction: re-reference the orientation from the device's earth +// frame to NED, then move the whole reading into the gimbal's body frame. +// +// R_ned<-gimbal = nedFromReference(ref) * R_local<-sensor * mountRotationZXY(m) +// \_____ earth _____/ \____ body ____/ +// +// Only the orientation is re-referenced. acc, gyr and mag are BODY-frame +// quantities - they describe directions in the sensor, not relative to the earth +// - so the earth frame does not touch them and only the mount rotates them. +// +// Returns the sample unchanged when there is nothing to do (NED already, and an +// identity mount). +ImuSample applyFrames(ImuReference ref, const ImuMount& m, const ImuSample& s); + +} // namespace fgc diff --git a/include/fgc/MountedImuSource.h b/include/fgc/MountedImuSource.h new file mode 100644 index 0000000..072aed0 --- /dev/null +++ b/include/fgc/MountedImuSource.h @@ -0,0 +1,56 @@ +#pragma once + +#include "fgc/IImuSource.h" +#include "fgc/ImuMount.h" + +#include +#include + +namespace fgc { + +// Wraps any IImuSource and puts its readings into the gimbal's NED body frame: +// the device's earth reference is re-referenced to NED, and the [IMU] x/y/z mount +// carries the body frame onto the gimbal's. Both are needed and they are not +// interchangeable - see applyFrames(). +// +// This sits directly on the factory's output, which is the whole point: every +// consumer - the calibration routine, the self-test suite, the TUI, `imu status`, +// the MQTT path - already reads through IImuSource::sample(), so one wrap covers +// all of them and no consumer needs to know a mount exists. It wraps the mock as +// readily as the MTi, which is what makes the feature exercisable with +// --mock-imu on a machine with no hardware. +// +// Application skips the wrapper entirely when there is nothing to do (the device +// already reports NED and the mount is the identity), so such a rig carries +// neither the indirection nor the arithmetic. +class MountedImuSource : public IImuSource { +public: + MountedImuSource(std::unique_ptr inner, ImuReference reference, ImuMount mount) + : inner_(std::move(inner)), reference_(reference), mount_(mount) {} + + void start() override { inner_->start(); } + void stop() override { inner_->stop(); } + bool connected() const override { return inner_->connected(); } + + std::optional sample() override { + auto s = inner_->sample(); + if (!s) return s; + return applyFrames(reference_, mount_, *s); + } + + // Everything below describes the device, not a frame, so it passes straight + // through. The mount is a host-side correction and never reaches the MTi. + std::optional config() const override { return inner_->config(); } + std::vector configProblems() const override { return inner_->configProblems(); } + void refreshConfig() override { inner_->refreshConfig(); } + void noRotation(int seconds) override { inner_->noRotation(seconds); } + void headingReset() override { inner_->headingReset(); } + bool setFilterProfile(int type) override { return inner_->setFilterProfile(type); } + +private: + std::unique_ptr inner_; + ImuReference reference_; + ImuMount mount_; +}; + +} // namespace fgc diff --git a/include/fgc/MtiImuSource.h b/include/fgc/MtiImuSource.h index eaee8d9..efdb2b1 100644 --- a/include/fgc/MtiImuSource.h +++ b/include/fgc/MtiImuSource.h @@ -21,6 +21,7 @@ public: bool connected() const override; std::optional sample() override; std::optional config() const override; + std::vector configProblems() const override; void refreshConfig() override; bool setFilterProfile(int type) override; void noRotation(int seconds) override; diff --git a/include/fgc/MtiProtocol.h b/include/fgc/MtiProtocol.h index 40812b1..eb7fa78 100644 --- a/include/fgc/MtiProtocol.h +++ b/include/fgc/MtiProtocol.h @@ -57,14 +57,135 @@ inline constexpr uint8_t kMidResetOrientationAck = 0xA5; inline constexpr uint16_t kResetHeading = 0x0001; // current heading becomes yaw 0 inline constexpr uint16_t kResetStore = 0x0000; // persist current reset (Config state) -// OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04). -inline constexpr uint16_t kOutputMode = 0x0007; -// OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp -// SampleCounter (bits1:0=01 => 0x01), float, all calibrated channels enabled. -inline constexpr uint32_t kOutputSettings = 0x00000005; -// Expected MTData payload with the above config: Temp(4) + Acc(12) + Gyr(12) + -// Mag(12) + Euler(12) + SampleCounter(2). -inline constexpr uint8_t kMTDataLen = 54; +// Which EARTH frame a device measures its orientation against. +// +// A different axis entirely from the [IMU] x/y/z mount, and the distinction is +// load-bearing: the mount says how the sensor is bolted to the gimbal (a BODY +// frame), while this says what the reported angles are measured relative to. +// +// NED X North Y East Z Down - what the gimbal frame assumes +// NWU X North Y West Z Up - the classic MTi's factory local frame +// ENU X East Y North Z Up - the newer MTi series' default +// +// A Z-up reference is what makes reported pitch FALL as the nose rises: with the +// body Y axis pointing left, a right-hand rotation about it tips the nose DOWN, +// so "nose up" is reported negative. +// +// This CANNOT be corrected by any mount value. Re-referencing the earth frame +// left-multiplies the reported orientation, whereas a mount right-multiplies it, +// and no fixed right-multiplication reproduces a left-multiplication across all +// attitudes. Both are applied, by applyFrames() in ImuMount.h. +enum class ImuReference { Ned, Nwu, Enu }; + +// --- What the host asks the device to stream ------------------------------- +// +// These are a wire-format contract, not a preference: parseMTData below decodes +// a fixed field order that follows from exactly these bits, so the two must be +// changed together. That is what mtDataLen() and the static_assert enforce. +// Deliberately not configurable from config.ini for the same reason. + +// OutputMode bits: which blocks the device streams, in this fixed order. +namespace mtmode { +inline constexpr uint16_t kTemperature = 0x0001; +inline constexpr uint16_t kCalibrated = 0x0002; // acc + gyr + mag +inline constexpr uint16_t kOrientation = 0x0004; +inline constexpr uint16_t kAuxiliary = 0x0008; // AIN1/AIN2, inserted before orientation +inline constexpr uint16_t kStatus = 0x0800; // one byte, before the sample counter +} // namespace mtmode + +inline constexpr uint16_t kOutputMode = + mtmode::kTemperature | mtmode::kCalibrated | mtmode::kOrientation; + +// OutputSettings bits: how those blocks are formatted. +namespace mtsettings { +inline constexpr uint32_t kTimestampMask = 0x00000003u; // bits 1:0 +inline constexpr uint32_t kTimestampNone = 0x00000000u; +inline constexpr uint32_t kTimestampSampleCounter = 0x00000001u; + +inline constexpr uint32_t kOrientationMask = 0x0000000Cu; // bits 3:2 +inline constexpr uint32_t kOrientationQuaternion = 0x00000000u; // 16 bytes +inline constexpr uint32_t kOrientationEuler = 0x00000004u; // 12 bytes +inline constexpr uint32_t kOrientationMatrix = 0x00000008u; // 36 bytes + +// Per-channel disables: a SET bit turns the channel OFF. +inline constexpr uint32_t kAccDisabled = 0x00000010u; +inline constexpr uint32_t kGyrDisabled = 0x00000020u; +inline constexpr uint32_t kMagDisabled = 0x00000040u; + +inline constexpr uint32_t kFormatMask = 0x00000300u; // bits 9:8 +inline constexpr uint32_t kFormatFloat = 0x00000000u; +inline constexpr uint32_t kFormatFixed1220 = 0x00000100u; // same length as float: + // the length gate cannot + // catch this one + +// Coordinate-system field: which earth reference the device reports against. +// +// Left at 0 - the device is NOT asked to change its reference. The correction is +// made on the host instead (kMtiReference below + applyFrames), which is +// deterministic, reversible, and unit-testable without hardware, where writing an +// unverified bit into the unit's non-volatile memory would be none of those. The +// field's position in OutputSettings is in any case not established for this +// firmware. +// +// Keeping it 0 also keeps the two ends consistent: the host rewrites this whole +// word every startup, so the device's reference is pinned to what 0x00000005 +// selects and kMtiReference can describe it with certainty. If you ever do set +// the coordinate system on the device, the readback check will flag the changed +// word - at which point kMtiReference must be updated to match. +inline constexpr uint32_t kCoordSysNed = 0x00000000u; +} // namespace mtsettings + +inline constexpr uint32_t kOutputSettings = + mtsettings::kTimestampSampleCounter | mtsettings::kOrientationEuler | + mtsettings::kFormatFloat | mtsettings::kCoordSysNed; + +// Which earth frame this device measures orientation against, under the +// OutputSettings above. +// +// The classic MTi's factory local frame is X North, Y West, Z Up. Because the +// host rewrites OutputSettings on every startup, the device's reference is +// whatever that word selects - i.e. deterministic, not "whatever was left in the +// unit" - so this is a fact about kOutputSettings and belongs beside it. +// +// The consequence, and the reason this constant exists: a Z-up reference reports +// pitch FALLING as the nose rises. ImuReference / applyFrames re-reference it to +// NED on the host (see ImuMount.h); no [IMU] x/y/z value can, because the earth +// frame is on the other side of the multiplication. +// +// If the yaw the Sensors view reports is 90 deg off a compass, this is ENU rather +// than NWU - roll and pitch come out identical either way, so yaw is the only +// thing that distinguishes them. Changing it here is the whole fix. +inline constexpr ImuReference kMtiReference = ImuReference::Nwu; + +// The MTData payload length implied by a (mode, settings) pair, in the device's +// fixed field order. Deriving it is what keeps kMTDataLen from drifting away +// from the settings above: change a bit and the length follows, so the +// static_assert fires instead of the parser silently mismatching at runtime. +constexpr unsigned mtDataLen(uint16_t mode, uint32_t settings) { + unsigned n = 0; + if (mode & mtmode::kTemperature) n += 4; + if (mode & mtmode::kCalibrated) { + if (!(settings & mtsettings::kAccDisabled)) n += 12; + if (!(settings & mtsettings::kGyrDisabled)) n += 12; + if (!(settings & mtsettings::kMagDisabled)) n += 12; + } + if (mode & mtmode::kOrientation) { + switch (settings & mtsettings::kOrientationMask) { + case mtsettings::kOrientationQuaternion: n += 16; break; + case mtsettings::kOrientationEuler: n += 12; break; + case mtsettings::kOrientationMatrix: n += 36; break; + default: break; // reserved encoding: length unknown + } + } + if (mode & mtmode::kStatus) n += 1; + if ((settings & mtsettings::kTimestampMask) == mtsettings::kTimestampSampleCounter) n += 2; + return n; +} + +// Expected payload with the config above: Temp(4) + Acc(12) + Gyr(12) + Mag(12) +// + Euler(12) + SampleCounter(2). +inline constexpr uint8_t kMTDataLen = static_cast(mtDataLen(kOutputMode, kOutputSettings)); +static_assert(kMTDataLen == 54, "MTData layout changed; parseMTData must change with it"); // One fully decoded IMU reading. struct ImuSample { @@ -75,7 +196,7 @@ struct ImuSample { float mag[3] = {0, 0, 0}; // a.u. (normalized to earth field) float roll_deg = 0.f; // -180..180 float pitch_deg = 0.f; // -90..90 - float yaw_deg = 0.f; // 0..360 heading (MTi native -180..180 is shifted) + float yaw_deg = 0.f; // -180..180, exactly as the device reports it uint16_t sample_counter = 0; }; @@ -164,6 +285,41 @@ bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::s // against available_profiles by type) and sample_rate_hz from the period. void finalizeImuConfig(ImuDeviceConfig& c); +// One way the device disagrees with what the host asked it for. +struct ImuConfigMismatch { + std::string field; // "output mode" / "output settings" + uint32_t expected = 0; + uint32_t actual = 0; + bool answered = true; // false: the device never reported this field + std::string detail; // decoded difference, e.g. "orientation is Quaternion, asked for Euler" + + // One line for a log or the UI, e.g. + // output settings: asked for 0x00000005, device reports 0x00000105 (data + // format is Fixed 12.20, asked for Float) + std::string describe() const; +}; + +// Compare what the device reports against what configure() asked it for. Empty +// means the device is doing exactly what the host configured. +// +// This is the only thing standing between a device that ignored or rejected the +// Set messages and a session that silently streams the wrong thing: the payload +// length gate in parseMTData cannot see a wrong data format or a wrong +// coordinate system, since neither changes the length. +// +// A field the device did not answer is reported as unverifiable (`answered = +// false`) rather than as a match - older firmware that does not respond to the +// Req* queries must not be mistaken for a device that agreed. Identity fields +// (product code, device ID, firmware) and anything the host does not set (period, +// XKF profile) are deliberately not compared. +std::vector checkImuDeviceConfig(const ImuDeviceConfig& c); + +// Human text for an Error message (MID 0x42) payload byte - how the device says +// it rejected a message outright, as opposed to quietly not applying it. Codes +// are from the MT Low-Level Protocol error table; anything unrecognized comes +// back as "unknown error 0xNN" rather than being swallowed. +std::string mtiErrorText(uint8_t code); + // Lower byte of the sum of all bytes from BID through the end of DATA. The CS // byte that makes the running total ≡ 0 (mod 256) is (256 - mtiChecksum) & 0xFF. uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len); diff --git a/include/fgc/mock/MockImuSource.h b/include/fgc/mock/MockImuSource.h index 92d0b4a..5dbc2d0 100644 --- a/include/fgc/mock/MockImuSource.h +++ b/include/fgc/mock/MockImuSource.h @@ -27,7 +27,9 @@ public: s.valid = true; s.roll_deg = static_cast(15.0 * std::sin(t * 0.5)); s.pitch_deg = static_cast(10.0 * std::sin(t * 0.3 + 1.0)); - s.yaw_deg = static_cast(std::fmod(t * 8.0, 360.0)); // slow spin + // Slow spin through the MTi's own -180..180, wrap included, so anything + // consuming yaw meets the same discontinuity it will meet on the rig. + s.yaw_deg = static_cast(std::fmod(t * 8.0 + 180.0, 360.0) - 180.0); // Gravity tilts with roll/pitch; small free accel noise. const double r = s.roll_deg * M_PI / 180.0, p = s.pitch_deg * M_PI / 180.0; s.acc[0] = static_cast(9.81 * -std::sin(p)); @@ -57,6 +59,18 @@ public: return true; } + // Runs the same check the real handshake runs, over the synthetic config + // below. It reports nothing as long as the mock mirrors the constants - which + // is the point: perturb config()'s output_settings and the error surfaces all + // the way to the Sensors view, so that path is exercisable without hardware. + std::vector configProblems() const override { + std::vector out; + auto c = config(); + if (!c) return out; + for (const auto& m : checkImuDeviceConfig(*c)) out.push_back(m.describe()); + return out; + } + // 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 diff --git a/include/fgc/ui/UiSnapshot.h b/include/fgc/ui/UiSnapshot.h index 933a315..8c90cd2 100644 --- a/include/fgc/ui/UiSnapshot.h +++ b/include/fgc/ui/UiSnapshot.h @@ -1,6 +1,7 @@ #pragma once #include "fgc/IControlChannel.h" +#include "fgc/ImuMount.h" #include "fgc/IMotorController.h" // AxisState #include "fgc/Logger.h" // LogLevel @@ -88,6 +89,11 @@ struct SensorGroup { struct SensorsView { SensorGroup imu; // Xsens MTi: Roll/Pitch/Yaw SensorGroup env; // ambient temperature + humidity (SHT41) + + // The MTi is not streaming what the host configured (the detail is in + // ImuConfigView::problems). Flagged on the compact panel too, because the + // readings above are wrong in a way that reading them cannot reveal. + bool imu_config_alert = false; }; // One auto-sweep scan-grid waypoint, for the expanded camera view. @@ -210,6 +216,22 @@ struct ImuConfigView { // Available XKF profiles with the active one flagged. Empty if the device // did not report them. std::vector xkf_profiles; + + // The [IMU] x/y/z mounting rotation being applied to every reading, already + // formatted (e.g. "x 180.0 y 0.0 z -90.0"). Empty when no mount is + // configured. Unlike everything above it comes from config.ini rather than + // from the device, so the view labels it as such - the MTi has its own + // internal alignment setting and the two must not be confused. + std::string mount; + + // How the device disagreed with what the host asked it for at startup, one + // finding per line, already formatted. Empty when it agreed. + // + // Like `mount` this must survive the config cache, and for a sharper reason: + // the handshake runs before the TUI installs its log sink, so the LOG_ERROR + // it emits is wiped by the alt-screen. Without this field the operator would + // have no way of learning that the IMU is streaming the wrong thing. + std::vector problems; }; // Full Xsens MTi reading for the expanded Sensors view (units: acc m/s^2, @@ -324,6 +346,11 @@ UiColor axisStateColor(AxisState s); // "12.3°" (one decimal, sign preserved). std::string formatDegrees(double deg); +// The [IMU] mount as one line: "x 180.0 y 0.0 z -90.0", or "" for no mount. +// Empty rather than "none" so a caller can tell "not configured" from a value +// and decide for itself how to say so. +std::string formatImuMount(const ImuMount& m); + // Human "Ns ago" / "Nm ago" for an absolute ms timestamp relative to now_ms. // Returns "—" when then_ms is 0 (never). std::string formatTimeAgo(long long now_ms, long long then_ms); diff --git a/src/core/Application.cpp b/src/core/Application.cpp index afcb725..56d8820 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -15,6 +15,7 @@ #include "fgc/IMotorController.h" #include "fgc/ImagePipeline.h" #include "fgc/Logger.h" +#include "fgc/MountedImuSource.h" #include "fgc/MtiImuSource.h" #include "fgc/ScanGrid.h" #include "fgc/SerialMotorController.h" @@ -264,12 +265,29 @@ struct Application::Impl { std::unique_ptr makeImu() { if (!cfg.features.enable_imu) return nullptr; // sensors panel stays "pending" - if (cfg.features.mock_imu) return std::make_unique(); - if (cfg.imu.device.empty()) { + + std::unique_ptr src; + if (cfg.features.mock_imu) { + src = std::make_unique(); + } else if (cfg.imu.device.empty()) { LOG_WARN << "IMU enabled but [IMU] device is empty; disabling IMU"; return nullptr; + } else { + src = std::make_unique(cfg.imu.device, cfg.imu.baud); } - return std::make_unique(cfg.imu.device, cfg.imu.baud); + + // Wrapping here is what makes the mount universal: every consumer reads + // through IImuSource::sample(), so none of them can see the sensor frame. + // An unmounted rig skips the wrapper entirely rather than rotating by + // zero on every sample. + // The mock fabricates gimbal-frame readings directly, so only the real + // device carries an earth reference that has to be corrected. + const ImuReference ref = cfg.features.mock_imu ? ImuReference::Ned : kMtiReference; + if (ref == ImuReference::Ned && cfg.imu.mount.identity()) return src; + LOG_INFO << "IMU frames: reference " << referenceName(ref) << " -> NED; mount x " + << cfg.imu.mount.x_deg << " y " << cfg.imu.mount.y_deg << " z " + << cfg.imu.mount.z_deg << " (deg, Z-X-Y); readings are in the gimbal frame"; + return std::make_unique(std::move(src), ref, cfg.imu.mount); } std::unique_ptr makeEnv() { @@ -502,6 +520,12 @@ struct Application::Impl { << (c->scenario_label.empty() ? std::string("(unnamed)") : c->scenario_label) << " (type " << static_cast(c->scenario_type) << ')'; LOG_INFO << " available (* = active): " << imuProfileList(); + const auto problems = imu->configProblems(); + if (problems.empty()) { + LOG_INFO << " device matches the configuration the host asked for"; + } else { + for (const auto& p : problems) LOG_ERROR << " " << p; + } } // `imu` command family, mirroring handleGimbal: the operator-facing surface of @@ -648,6 +672,15 @@ struct Application::Impl { } } s.imu.config = imu_config_view_; + // From config.ini, not from the device, so it is available even when + // the MTi never answered the configuration queries. + s.imu.config.mount = formatImuMount(cfg.imu.mount); + // Also outside the cache above, and for a sharper reason: the handshake + // that produces these runs before the TUI installs its log sink, so the + // errors it logs are wiped by the alt-screen. This is the only place the + // operator can still see them. + s.imu.config.problems = imu->configProblems(); + s.sensors.imu_config_alert = !s.imu.config.problems.empty(); if (auto m = imu->sample()) { // Full reading for the expanded view. s.imu.present = true; diff --git a/src/core/CalibrationRoutine.cpp b/src/core/CalibrationRoutine.cpp index 49b2643..453ada9 100644 --- a/src/core/CalibrationRoutine.cpp +++ b/src/core/CalibrationRoutine.cpp @@ -121,9 +121,10 @@ double dwellAndMeasure(IImuSource& imu, char axis, int dwell_ms, int sample_ms, } if (vals.empty()) return 0.0; if (axis == 'Y') { - double m = circularMeanDeg(vals); // robust to wrap within the dwell - if (m < 0.0) m += 360.0; // report a 0..360 heading (matches IMU) - return m; + // Robust to a wrap within the dwell, and already in the device's own + // -180..180. The sweep's per-sample unwrapNear below is what keeps the + // fit continuous, so no range shift is needed - or wanted - here. + return circularMeanDeg(vals); } double m = 0; for (double v : vals) m += v; diff --git a/src/core/Config.cpp b/src/core/Config.cpp index 5dec1c3..09dd2eb 100644 --- a/src/core/Config.cpp +++ b/src/core/Config.cpp @@ -1,6 +1,7 @@ #include "fgc/Config.h" #include "fgc/Paths.h" +#include #include #include #include @@ -309,6 +310,21 @@ AppConfig ConfigLoader::fromMap(const std::map& kv) { cfg.imu.device = get(kv, "IMU.device", cfg.imu.device); cfg.imu.baud = static_cast(getInt(kv, "IMU.baud", cfg.imu.baud)); + // [IMU] x/y/z: how the unit is bolted on, in degrees about forward/right/down. + // A value outside +/-360 is a typo far more often than an intent, and one that + // is accepted silently is invisible afterwards - the readings just quietly + // describe the wrong axes. + cfg.imu.mount.x_deg = getDouble(kv, "IMU.x", cfg.imu.mount.x_deg); + cfg.imu.mount.y_deg = getDouble(kv, "IMU.y", cfg.imu.mount.y_deg); + cfg.imu.mount.z_deg = getDouble(kv, "IMU.z", cfg.imu.mount.z_deg); + for (const auto& [axis, value] : {std::pair{"x", cfg.imu.mount.x_deg}, + std::pair{"y", cfg.imu.mount.y_deg}, + std::pair{"z", cfg.imu.mount.z_deg}}) { + if (!std::isfinite(value) || std::abs(value) > 360.0) + throw std::runtime_error(std::string("IMU.") + axis + + " must be a rotation in degrees within +/-360"); + } + cfg.env.i2c_device = get(kv, "Env.i2c_device", cfg.env.i2c_device); cfg.env.i2c_addr = static_cast( getIntAutoBase(kv, "Env.i2c_addr", static_cast(cfg.env.i2c_addr))); diff --git a/src/core/ImuMount.cpp b/src/core/ImuMount.cpp new file mode 100644 index 0000000..eee48e5 --- /dev/null +++ b/src/core/ImuMount.cpp @@ -0,0 +1,148 @@ +#include "fgc/ImuMount.h" + +#include +#include + +namespace fgc { + +namespace { + +constexpr double kDeg2Rad = M_PI / 180.0; +constexpr double kRad2Deg = 180.0 / M_PI; + +// Below this the mount is not worth applying: 1e-9 deg is far under any angle an +// operator can measure, and treating it as zero keeps the untouched-sample +// guarantee exact rather than approximate. +constexpr double kAngleEpsilon = 1e-9; + +Mat3 rotX(double rad) { + const double c = std::cos(rad), s = std::sin(rad); + return {{{1, 0, 0}, {0, c, -s}, {0, s, c}}}; +} + +Mat3 rotY(double rad) { + const double c = std::cos(rad), s = std::sin(rad); + return {{{c, 0, s}, {0, 1, 0}, {-s, 0, c}}}; +} + +Mat3 rotZ(double rad) { + const double c = std::cos(rad), s = std::sin(rad); + return {{{c, -s, 0}, {s, c, 0}, {0, 0, 1}}}; +} + +Mat3 mul(const Mat3& a, const Mat3& b) { + Mat3 r{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + r[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j]; + return r; +} + +// v' = R^T v, i.e. re-express a vector's components in the frame R carries the +// original onto. Written out rather than transposing then multiplying, since the +// transpose only ever appears here. +void applyTranspose(const Mat3& R, const float in[3], float out[3]) { + double v[3] = {in[0], in[1], in[2]}; + for (int i = 0; i < 3; ++i) + out[i] = static_cast(R[0][i] * v[0] + R[1][i] * v[1] + R[2][i] * v[2]); +} + +} // namespace + +bool ImuMount::identity() const { + return std::abs(x_deg) < kAngleEpsilon && std::abs(y_deg) < kAngleEpsilon && + std::abs(z_deg) < kAngleEpsilon; +} + +Mat3 mountRotationZXY(const ImuMount& m) { + return mul(mul(rotZ(m.z_deg * kDeg2Rad), rotX(m.x_deg * kDeg2Rad)), rotY(m.y_deg * kDeg2Rad)); +} + +Mat3 rotationFromEulerZYX(double roll_deg, double pitch_deg, double yaw_deg) { + return mul(mul(rotZ(yaw_deg * kDeg2Rad), rotY(pitch_deg * kDeg2Rad)), rotX(roll_deg * kDeg2Rad)); +} + +void eulerZYXFromRotation(const Mat3& R, double& roll_deg, double& pitch_deg, double& yaw_deg) { + // For R = Rz(yaw) Ry(pitch) Rx(roll), R[2][0] is exactly -sin(pitch). + // Repeated multiplication can push it a hair past +/-1, and asin() of that is + // NaN - which would then propagate silently through every consumer. + const double sin_pitch = std::clamp(-R[2][0], -1.0, 1.0); + const double pitch = std::asin(sin_pitch); + const double cos_pitch = std::cos(pitch); + + double roll, yaw; + if (std::abs(cos_pitch) < 1e-9) { + // Gimbal lock: pointing straight up or straight down, roll and yaw are the + // same rotation and only their sum is observable. Attribute all of it to + // yaw, which is the axis the operator is steering. + roll = 0.0; + yaw = std::atan2(-R[0][1], R[1][1]); + } else { + roll = std::atan2(R[2][1], R[2][2]); + yaw = std::atan2(R[1][0], R[0][0]); + } + + // atan2/asin already land in -180..180 and -90..90, which is exactly the + // range the MTi reports, so nothing is re-ranged on the way out. + roll_deg = roll * kRad2Deg; + pitch_deg = pitch * kRad2Deg; + yaw_deg = yaw * kRad2Deg; +} + +Mat3 nedFromReference(ImuReference r) { + switch (r) { + // Already NED: nothing to do. + case ImuReference::Ned: return {{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}}; + // NWU -> NED: North stays, West becomes East, Up becomes Down. That is a + // 180 deg turn about North, i.e. Rx(180). + case ImuReference::Nwu: return {{{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}}; + // ENU -> NED: North is ENU's Y, East is ENU's X, Down is -Up. A 180 deg + // turn about the horizontal axis halfway between North and East. + case ImuReference::Enu: return {{{0, 1, 0}, {1, 0, 0}, {0, 0, -1}}}; + } + return {{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}}; +} + +const char* referenceName(ImuReference r) { + switch (r) { + case ImuReference::Ned: return "NED"; + case ImuReference::Nwu: return "NWU"; + case ImuReference::Enu: return "ENU"; + } + return "?"; +} + +ImuSample applyFrames(ImuReference ref, const ImuMount& m, const ImuSample& s) { + if (ref == ImuReference::Ned && m.identity()) return s; + + const Mat3 R = mountRotationZXY(m); + + ImuSample out = s; // temp, counter and valid are frame-independent + // acc/gyr/mag are body-frame directions, so only the mount touches them. The + // earth reference is about what the ORIENTATION is measured against and has + // no bearing on which way the sensor felt an acceleration. + applyTranspose(R, s.acc, out.acc); + applyTranspose(R, s.gyr, out.gyr); + applyTranspose(R, s.mag, out.mag); + + // The device reports local_earth<-sensor. Left-multiplying re-references the + // earth frame to NED; right-multiplying by the mount swaps the body frame for + // the gimbal's. They are independent and both are needed - which is exactly + // why a mount alone could never fix a Z-up reference. + const Mat3 local_from_sensor = rotationFromEulerZYX(s.roll_deg, s.pitch_deg, s.yaw_deg); + const Mat3 ned_from_gimbal = + mul(mul(nedFromReference(ref), local_from_sensor), R); + + double roll = 0, pitch = 0, yaw = 0; + eulerZYXFromRotation(ned_from_gimbal, roll, pitch, yaw); + out.roll_deg = static_cast(roll); + out.pitch_deg = static_cast(pitch); + out.yaw_deg = static_cast(yaw); + return out; +} + +ImuSample applyMount(const ImuMount& m, const ImuSample& s) { + return applyFrames(ImuReference::Ned, m, s); +} + +} // namespace fgc diff --git a/src/core/MtiProtocol.cpp b/src/core/MtiProtocol.cpp index 3c9f772..bfa00fc 100644 --- a/src/core/MtiProtocol.cpp +++ b/src/core/MtiProtocol.cpp @@ -239,6 +239,141 @@ void finalizeImuConfig(ImuDeviceConfig& c) { } } +namespace { + +const char* orientationName(uint32_t s) { + switch (s & mtsettings::kOrientationMask) { + case mtsettings::kOrientationQuaternion: return "Quaternion"; + case mtsettings::kOrientationEuler: return "Euler"; + case mtsettings::kOrientationMatrix: return "Matrix"; + default: return "reserved"; + } +} + +const char* formatName(uint32_t s) { + switch (s & mtsettings::kFormatMask) { + case mtsettings::kFormatFloat: return "Float"; + case mtsettings::kFormatFixed1220: return "Fixed 12.20"; + default: return "Fixed 16.32"; + } +} + +const char* timestampName(uint32_t s) { + return (s & mtsettings::kTimestampMask) == mtsettings::kTimestampSampleCounter + ? "Sample counter" + : "none"; +} + +std::string join(const std::vector& parts) { + std::string out; + for (const auto& p : parts) { + if (!out.empty()) out += "; "; + out += p; + } + return out; +} + +std::string modeDetail(uint16_t expected, uint16_t actual) { + const std::pair blocks[] = { + {mtmode::kTemperature, "temperature"}, {mtmode::kCalibrated, "calibrated data"}, + {mtmode::kOrientation, "orientation"}, {mtmode::kAuxiliary, "auxiliary"}, + {mtmode::kStatus, "status"}, + }; + std::vector parts; + for (const auto& [bit, name] : blocks) { + if ((expected & bit) == (actual & bit)) continue; + parts.push_back(std::string(name) + ((actual & bit) ? " is on, was not asked for" + : " is off, but was asked for")); + } + return join(parts); +} + +std::string settingsDetail(uint32_t expected, uint32_t actual) { + std::vector parts; + if ((expected & mtsettings::kOrientationMask) != (actual & mtsettings::kOrientationMask)) + parts.push_back(std::string("orientation is ") + orientationName(actual) + + ", asked for " + orientationName(expected)); + if ((expected & mtsettings::kFormatMask) != (actual & mtsettings::kFormatMask)) + parts.push_back(std::string("data format is ") + formatName(actual) + ", asked for " + + formatName(expected)); + if ((expected & mtsettings::kTimestampMask) != (actual & mtsettings::kTimestampMask)) + parts.push_back(std::string("timestamp is ") + timestampName(actual) + ", asked for " + + timestampName(expected)); + + const std::pair channels[] = { + {mtsettings::kAccDisabled, "acceleration"}, + {mtsettings::kGyrDisabled, "rate of turn"}, + {mtsettings::kMagDisabled, "magnetic field"}, + }; + for (const auto& [bit, name] : channels) { + if ((expected & bit) == (actual & bit)) continue; + parts.push_back(std::string(name) + ((actual & bit) ? " is disabled on the device" + : " is enabled but was not asked for")); + } + + // Anything outside the fields decoded above. The coordinate system lives in + // here, so while mtsettings::kCoordSysNed is still 0 this is the branch that + // reports a device whose earth reference is not what the gimbal frame assumes. + const uint32_t known = mtsettings::kOrientationMask | mtsettings::kFormatMask | + mtsettings::kTimestampMask | mtsettings::kAccDisabled | + mtsettings::kGyrDisabled | mtsettings::kMagDisabled; + if ((expected ^ actual) & ~known) { + char buf[96]; + std::snprintf(buf, sizeof(buf), + "other bits differ (0x%08X); the coordinate system is one of these", + (expected ^ actual) & ~known); + parts.push_back(buf); + } + return join(parts); +} + +} // namespace + +std::string ImuConfigMismatch::describe() const { + if (!answered) return field + ": the device did not report it, so it could not be verified"; + char buf[80]; + std::snprintf(buf, sizeof(buf), ": asked for 0x%08X, device reports 0x%08X", expected, actual); + std::string out = field + buf; + if (!detail.empty()) out += " (" + detail + ")"; + return out; +} + +std::vector checkImuDeviceConfig(const ImuDeviceConfig& c) { + std::vector out; + + if (!c.has_output_mode) { + out.push_back({"output mode", kOutputMode, 0, false, ""}); + } else if (c.output_mode != kOutputMode) { + out.push_back({"output mode", kOutputMode, c.output_mode, true, + modeDetail(kOutputMode, c.output_mode)}); + } + + if (!c.has_output_settings) { + out.push_back({"output settings", kOutputSettings, 0, false, ""}); + } else if (c.output_settings != kOutputSettings) { + out.push_back({"output settings", kOutputSettings, c.output_settings, true, + settingsDetail(kOutputSettings, c.output_settings)}); + } + + return out; +} + +std::string mtiErrorText(uint8_t code) { + switch (code) { + case 0x03: return "invalid period"; + case 0x04: return "invalid message"; + case 0x1E: return "timer overflow"; + case 0x20: return "invalid baud rate"; + case 0x21: return "invalid parameter"; + case 0x28: return "device error"; + default: { + char buf[32]; + std::snprintf(buf, sizeof(buf), "unknown error 0x%02X", code); + return buf; + } + } +} + std::optional parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) { if (mid != kMidMTData || len != kMTDataLen) return std::nullopt; ImuSample s; @@ -250,8 +385,10 @@ std::optional parseMTData(uint8_t mid, const uint8_t* data, std::size s.roll_deg = beFloat(data + o); o += 4; s.pitch_deg = beFloat(data + o); o += 4; s.yaw_deg = beFloat(data + o); o += 4; - // Report yaw as a 0..360 heading rather than the MTi's native -180..180. - if (s.yaw_deg < 0.f) s.yaw_deg += 360.f; + // Reported exactly as the device sends it. Roll, pitch and yaw are all raw: + // the only transformation applied anywhere is the [IMU] x/y/z mount rotation, + // so what reaches a consumer is the sensor's own numbers in the frame the + // operator declared - nothing else. s.sample_counter = beU16(data + o); o += 2; s.valid = true; return s; diff --git a/src/core/TestRunner.cpp b/src/core/TestRunner.cpp index 2c91fe5..c3edfdb 100644 --- a/src/core/TestRunner.cpp +++ b/src/core/TestRunner.cpp @@ -1,5 +1,7 @@ #include "fgc/TestRunner.h" +#include "fgc/Calibration.h" // unwrapNear + #include "fgc/DiagParser.h" #include "fgc/DumpParser.h" #include "fgc/HostMetrics.h" @@ -672,14 +674,25 @@ void TestRunner::testImuDrift(TestReport& r) { const auto t0 = clock_t_::now(); const auto deadline = t0 + window; double peak_min = 1e9, peak_max = -1e9; + bool have_prev = false; + double prev_deg = 0.0; while (clock_t_::now() < deadline && !cancelled()) { auto smp = imu_->sample(); if (smp && smp->valid) { double tmin = std::chrono::duration(clock_t_::now() - t0).count() / 60.0; + // Yaw is reported raw, so it wraps at ±180. A rig parked on that + // boundary would otherwise show a ~360° "drift" between consecutive + // still samples; unwrapping makes the fit and the spread independent + // of which heading the tower happens to face. + double deg = smp->yaw_deg; + if (have_prev) deg = unwrapNear(prev_deg, deg); + prev_deg = deg; + have_prev = true; + ts.push_back(tmin); - yaw.push_back(smp->yaw_deg); - peak_min = std::min(peak_min, (double)smp->yaw_deg); - peak_max = std::max(peak_max, (double)smp->yaw_deg); + yaw.push_back(deg); + peak_min = std::min(peak_min, deg); + peak_max = std::max(peak_max, deg); } std::this_thread::sleep_for(20ms); } diff --git a/src/serial/MtiImuSource.cpp b/src/serial/MtiImuSource.cpp index 7e824c6..63b3f82 100644 --- a/src/serial/MtiImuSource.cpp +++ b/src/serial/MtiImuSource.cpp @@ -7,8 +7,11 @@ #include #include #include +#include #include +#include #include +#include #include #include @@ -37,6 +40,9 @@ struct MtiImuSource::Impl { ImuDeviceConfig cfg; // device config read back at startup bool have_cfg = false; + // Formatted findings from the last handshake: ways the device disagreed with + // what we asked it for, plus any Error frame it sent back. Empty = it agreed. + std::vector cfg_problems; MtiFramer framer; unsigned bad_len_warned = 0; @@ -82,6 +88,11 @@ struct MtiImuSource::Impl { if (ec) LOG_WARN << "MTi config write failed: " << ec.message(); }; write(msgGoToConfig()); std::this_thread::sleep_for(60ms); + // The device stops streaming on entering Config state, so drop the buffered + // MTData here - before the Sets rather than after them. From this point the + // only bytes arriving are answers to what we send, which is what lets + // queryConfig see an Error frame refusing a Set instead of flushing it away. + ::tcflush(serial.native_handle(), TCIFLUSH); if (set_profile >= 0) { write(msgSetFilterProfile(static_cast(set_profile))); std::this_thread::sleep_for(60ms); @@ -121,14 +132,25 @@ struct MtiImuSource::Impl { void queryConfig(WriteFn&& write) { using namespace std::chrono_literals; ImuDeviceConfig local; + std::vector problems; MtiFramer cf([&](uint8_t mid, const uint8_t* d, std::size_t n) { + // An Error frame is the device rejecting a message outright, as + // opposed to quietly not applying it. It was dropped on the floor + // before, which made a refused SetOutputSettings invisible. + if (mid == kMidError) { + const std::string what = + n >= 1 ? mtiErrorText(d[0]) : std::string("error with no code"); + LOG_ERROR << "MTi rejected a configuration message: " << what; + problems.push_back("device rejected a configuration message: " + what); + return; + } 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); - + // Deliberately NOT flushing here: configure() already dropped the buffered + // pre-config stream on entering Config state, and anything waiting now is + // an ack or an Error from the Sets that ran in between - which is exactly + // what this window is for. write(msgReqProductCode()); write(msgReqDID()); write(msgReqFWRev()); @@ -160,17 +182,42 @@ struct MtiImuSource::Impl { finalizeImuConfig(local); if (local.valid) { - std::lock_guard lock(mutex); - cfg = local; - have_cfg = true; + // The raw settings word is printed because it is the ground truth for + // what the device is actually doing - and, while + // mtsettings::kCoordSysNed is still unconfirmed, it is how the + // coordinate-system bits get identified: set NED once in MT Manager + // and read off what changed here. + char words[48]; + std::snprintf(words, sizeof(words), "mode 0x%04X settings 0x%08X", local.output_mode, + local.output_settings); 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"; + << " @ " << local.sample_rate_hz << " Hz, " << words; + + // Did the device actually take what configure() just asked for? Nothing + // else catches a refused or ignored Set: a wrong data format or a wrong + // coordinate system leaves the payload length unchanged, so the length + // gate in parseMTData sees nothing wrong. + for (const auto& m : checkImuDeviceConfig(local)) { + LOG_ERROR << "MTi " << m.describe(); + problems.push_back(m.describe()); + } } else { LOG_WARN << "MTi: no configuration acks received (device may not answer Req* in this firmware)"; } + + { + std::lock_guard lock(mutex); + if (local.valid) { + cfg = local; + have_cfg = true; + } + // Replaced wholesale, not appended: these describe the handshake that + // just ran, so a refresh that now succeeds must clear the old findings. + cfg_problems = std::move(problems); + } } }; @@ -232,6 +279,11 @@ std::optional MtiImuSource::config() const { return impl_->cfg; } +std::vector MtiImuSource::configProblems() const { + std::lock_guard lock(impl_->mutex); + return impl_->cfg_problems; +} + // 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 diff --git a/src/ui/TuiUi.cpp b/src/ui/TuiUi.cpp index 3e1aa0f..85733d7 100644 --- a/src/ui/TuiUi.cpp +++ b/src/ui/TuiUi.cpp @@ -271,12 +271,12 @@ Element sensorsPanel(const SensorsView& s) { // One labelled subsection per physical sensor: a title + status header, then // its readings. Keeps MTi (orientation/device temp) and the ambient env // sensor (SHT41) clearly separated. - auto group = [](const SensorGroup& g, const std::string& hint) { + auto group = [](const SensorGroup& g, Element trailing) { std::vector rows; Element status = g.present ? (text(g.status) | color(Color::Green)) : (text(g.status) | dim); rows.push_back(hbox({text(g.title) | bold, text(" "), status, filler(), - text(hint) | dim})); + std::move(trailing)})); for (const auto& f : g.fields) { Element val = text(f.value + (f.unit.empty() ? "" : " " + f.unit)); val = f.present ? (val | bold) : (val | dim); @@ -285,8 +285,14 @@ Element sensorsPanel(const SensorsView& s) { return vbox(std::move(rows)); }; // The title already says "[i]"; repeating "i to expand" in the body was noise. + // A misconfigured MTi reports plausible-looking numbers about the wrong axes, + // so the readings themselves can never reveal it - it has to be said here. + Element imu_badge = s.imu_config_alert + ? (text(" IMU CFG ") | color(Color::Red) | bold) + : text(""); return panel("[i] SENSORS", Color::Magenta, - vbox({group(s.imu, ""), separator(), group(s.env, "")})); + vbox({group(s.imu, std::move(imu_badge)), separator(), + group(s.env, text(""))})); } Element cameraPanel(const CaptureView& c) { @@ -977,7 +983,10 @@ Element sensorsDetailPanel(const ImuView& v, const EnvView& e, ScrollState& st) // 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) { + // A configured mount is worth showing even when the device never answered the + // configuration queries - it is the one line here that comes from config.ini + // and is what explains why the readings above are not the sensor's own. + if (c.present || !c.mount.empty() || !c.problems.empty()) { auto cfgRow = [](const std::string& k, const std::string& val, Color vc = Color::Default) { return hbox({text(k) | dim | size(WIDTH, EQUAL, 16), @@ -985,6 +994,34 @@ Element sensorsDetailPanel(const ImuView& v, const EnvView& e, ScrollState& st) }; body.push_back(separator()); body.push_back(text("IMU CONFIG") | bold | color(Color::Magenta)); + // Before anything else: the device is not doing what it was told to do, so + // every orientation number above is suspect. Red rather than the Mount + // row's yellow, which is a normal state and would blunt this one. + for (const auto& p : c.problems) + body.push_back(hbox({text("MISCONFIGURED") | color(Color::Red) | bold + | size(WIDTH, EQUAL, 16), + paragraph(p) | color(Color::Red)})); + // Mounting rotation first: it changes the meaning of every number above, + // and it is the one row here the operator sets rather than the device. + // (The earth-frame correction is deliberately not shown: it is a fixed + // property of the device, not something the operator sets or acts on. It + // is stated once on the startup log line.) + if (c.mount.empty()) + body.push_back(hbox({text("Mount (cfg)") | dim | size(WIDTH, EQUAL, 16), + text("none") | dim, + text(" \xE2\x80\x94 readings are as the sensor reports them") + | dim})); + else + body.push_back(hbox({text("Mount (cfg)") | dim | size(WIDTH, EQUAL, 16), + text(c.mount) | color(Color::Yellow) | bold, + text(" deg, rotations from IMU frame to gimbal frame") + | dim})); + if (!c.present) { + // Everything below this point is device-reported and unavailable. + body.push_back(hbox({text("") | size(WIDTH, EQUAL, 16), + text("(device configuration not reported)") | dim})); + } + if (c.present) { body.push_back(cfgRow("Product", c.product_code)); body.push_back(cfgRow("Firmware", c.firmware)); body.push_back(cfgRow("Device ID", c.device_id)); @@ -1008,6 +1045,7 @@ Element sensorsDetailPanel(const ImuView& v, const EnvView& e, ScrollState& st) text(p.name) | dim})); } } + } } Element imu_status = v.present ? (text(" MTi live ") | color(Color::Green) | bold) diff --git a/src/ui/UiSnapshot.cpp b/src/ui/UiSnapshot.cpp index 110a4a6..150fa2e 100644 --- a/src/ui/UiSnapshot.cpp +++ b/src/ui/UiSnapshot.cpp @@ -30,6 +30,13 @@ UiColor axisStateColor(AxisState s) { } } +std::string formatImuMount(const ImuMount& m) { + if (m.identity()) return ""; + char b[64]; + std::snprintf(b, sizeof(b), "x %.1f y %.1f z %.1f", m.x_deg, m.y_deg, m.z_deg); + return b; +} + std::string formatDegrees(double deg) { char buf[32]; std::snprintf(buf, sizeof(buf), "%.1f\xC2\xB0", deg); // UTF-8 degree sign diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0cd68ab..59d5ca8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -35,6 +35,7 @@ add_executable(fgc_tests test_uisnapshot.cpp test_scroll.cpp test_mtiprotocol.cpp + test_imumount.cpp test_sht41.cpp test_dumpparser.cpp test_helptext.cpp diff --git a/tests/test_config.cpp b/tests/test_config.cpp index 365b50d..ce98337 100644 --- a/tests/test_config.cpp +++ b/tests/test_config.cpp @@ -344,3 +344,36 @@ TEST_CASE("loadFromFile anchors a relative profile_dir to the config file's dire fs::remove_all(dir); } + +TEST_CASE("[IMU] x/y/z describe the mount and default to no rotation") { + // Absent keys must leave the identity, so an existing config.ini keeps + // behaving exactly as it did before the mount existed. + AppConfig none = ConfigLoader::fromMap({{"Camera.rgb.id", "D"}, + {"Camera.rgb.profile_dir", "/tmp/p"}}); + CHECK(none.imu.mount.identity()); + + AppConfig c = ConfigLoader::fromMap({{"Camera.rgb.id", "D"}, + {"Camera.rgb.profile_dir", "/tmp/p"}, + {"IMU.x", "180"}, + {"IMU.y", "-0.5"}, + {"IMU.z", "-90"}}); + CHECK(c.imu.mount.x_deg == doctest::Approx(180.0)); + CHECK(c.imu.mount.y_deg == doctest::Approx(-0.5)); + CHECK(c.imu.mount.z_deg == doctest::Approx(-90.0)); + CHECK_FALSE(c.imu.mount.identity()); +} + +TEST_CASE("A mount angle that cannot be a rotation is rejected") { + auto with = [](const std::string& k, const std::string& v) { + return ConfigLoader::fromMap({{"Camera.rgb.id", "D"}, + {"Camera.rgb.profile_dir", "/tmp/p"}, + {k, v}}); + }; + // Out of range is a typo far more often than an intent, and one accepted + // silently is invisible afterwards - the readings just describe wrong axes. + CHECK_THROWS(with("IMU.z", "900")); + CHECK_THROWS(with("IMU.x", "-361")); + CHECK_THROWS(with("IMU.y", "nan")); + CHECK_NOTHROW(with("IMU.z", "-360")); + CHECK_NOTHROW(with("IMU.z", "360")); +} diff --git a/tests/test_imumount.cpp b/tests/test_imumount.cpp new file mode 100644 index 0000000..9962047 --- /dev/null +++ b/tests/test_imumount.cpp @@ -0,0 +1,347 @@ +#include + +#include "fgc/ImuMount.h" + +#include + +using namespace fgc; + +namespace { + +// A sample with distinguishable values in every slot, so a transform that +// touches the wrong field is visible. +ImuSample makeSample() { + ImuSample s; + s.valid = true; + s.roll_deg = 0.f; + s.pitch_deg = 0.f; + s.yaw_deg = 0.f; + s.acc[0] = 1.f; + s.acc[1] = 2.f; + s.acc[2] = 3.f; + s.gyr[0] = 0.1f; + s.gyr[1] = 0.2f; + s.gyr[2] = 0.3f; + s.mag[0] = 0.4f; + s.mag[1] = 0.5f; + s.mag[2] = 0.6f; + s.temp_c = 24.5f; + s.sample_counter = 4321; + return s; +} + +double norm(const float v[3]) { + return std::sqrt(double(v[0]) * v[0] + double(v[1]) * v[1] + double(v[2]) * v[2]); +} + +// Heading difference on the circle, so 359.9 and 0.1 are 0.2 apart, not 359.8. +double headingDiff(double a, double b) { + double d = std::fmod(std::abs(a - b), 360.0); + return d > 180.0 ? 360.0 - d : d; +} + +} // namespace + +TEST_CASE("an identity mount changes nothing at all") { + const ImuSample in = makeSample(); + const ImuSample out = applyMount(ImuMount{}, in); + + // Exact, not approximate: a rig with no mount configured must be bit-for-bit + // what it was before this feature existed. + CHECK(out.roll_deg == in.roll_deg); + CHECK(out.pitch_deg == in.pitch_deg); + CHECK(out.yaw_deg == in.yaw_deg); + for (int i = 0; i < 3; ++i) { + CHECK(out.acc[i] == in.acc[i]); + CHECK(out.gyr[i] == in.gyr[i]); + CHECK(out.mag[i] == in.mag[i]); + } + CHECK(ImuMount{}.identity()); + CHECK_FALSE(ImuMount{0.0, 0.0, 90.0}.identity()); +} + +TEST_CASE("the mount rotation is a rotation: orthonormal, determinant +1") { + for (const ImuMount& m : {ImuMount{0, 0, 0}, ImuMount{180, 0, 0}, ImuMount{0, 0, -90}, + ImuMount{30, -45, 120}, ImuMount{-90, 17, 240}}) { + const Mat3 R = mountRotationZXY(m); + + // R^T R = I + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) { + const double dot = R[0][i] * R[0][j] + R[1][i] * R[1][j] + R[2][i] * R[2][j]; + CHECK(dot == doctest::Approx(i == j ? 1.0 : 0.0).epsilon(1e-12)); + } + + const double det = R[0][0] * (R[1][1] * R[2][2] - R[1][2] * R[2][1]) - + R[0][1] * (R[1][0] * R[2][2] - R[1][2] * R[2][0]) + + R[0][2] * (R[1][0] * R[2][1] - R[1][1] * R[2][0]); + CHECK(det == doctest::Approx(1.0)); // +1, not -1: a rotation, never a reflection + } +} + +TEST_CASE("z = 90: the IMU's forward axis reads as the gimbal's left") { + // The angles are what you apply TO THE IMU to bring it onto the gimbal. If it + // takes a +90 turn about down to align it, the IMU is currently pointing 90 + // deg to the LEFT of the gimbal's nose - so what the sensor calls forward is + // what the gimbal calls left (-Y). + ImuSample s = makeSample(); + s.acc[0] = 1.f; s.acc[1] = 0.f; s.acc[2] = 0.f; // sensor-frame forward + + const ImuSample out = applyMount(ImuMount{0, 0, 90}, s); + CHECK(out.acc[0] == doctest::Approx(0.0).scale(1)); + CHECK(out.acc[1] == doctest::Approx(-1.0)); + CHECK(out.acc[2] == doctest::Approx(0.0).scale(1)); +} + +TEST_CASE("x = 180: an upside-down IMU has its gravity put back the right way up") { + // Mounted inverted, the sensor's own Z points at the sky, so a stationary unit + // reports gravity along -Z. In the gimbal frame it must come back as +Z (down). + ImuSample s = makeSample(); + s.acc[0] = 0.f; s.acc[1] = 0.f; s.acc[2] = -9.81f; + + const ImuSample out = applyMount(ImuMount{180, 0, 0}, s); + CHECK(out.acc[0] == doctest::Approx(0.0).scale(1)); + CHECK(out.acc[1] == doctest::Approx(0.0).scale(1)); + CHECK(out.acc[2] == doctest::Approx(9.81)); +} + +TEST_CASE("a pure yaw mount shifts the reported heading by exactly that much") { + for (double sensor_yaw : {0.0, 45.0, 170.0, 300.0, 359.0}) { + ImuSample s = makeSample(); + s.yaw_deg = static_cast(sensor_yaw); + + const ImuSample out = applyMount(ImuMount{0, 0, 90}, s); + CHECK(headingDiff(out.yaw_deg, sensor_yaw + 90.0) == doctest::Approx(0.0).scale(1)); + CHECK(out.roll_deg == doctest::Approx(0.0).scale(1)); + CHECK(out.pitch_deg == doctest::Approx(0.0).scale(1)); + // Stays in the device's own range: the mount rotates, it does not re-range. + CHECK(out.yaw_deg >= -180.f); + CHECK(out.yaw_deg <= 180.f); + } +} + +TEST_CASE("the composition order is Z-X-Y, not any other permutation") { + // z=90 and x=180 together are what separate the orders: intrinsic Z-X-Y rolls + // about the X axis the yaw has ALREADY carried around, which lands sensor- + // forward somewhere a Y-X-Z composition never would. Without this, the order + // could be silently swapped and every single-axis test above would still pass. + ImuSample s = makeSample(); + s.acc[0] = 1.f; s.acc[1] = 0.f; s.acc[2] = 0.f; + + // R = Rz(90)Rx(180) = [[0,1,0],[1,0,0],[0,0,-1]], so R^T sends sensor-forward + // to gimbal +Y (right). + const ImuSample zxy = applyMount(ImuMount{180, 0, 90}, s); + CHECK(zxy.acc[0] == doctest::Approx(0.0).scale(1)); + CHECK(zxy.acc[1] == doctest::Approx(1.0)); + CHECK(zxy.acc[2] == doctest::Approx(0.0).scale(1)); + + // Feeding the same two angles to the other axes gives a different answer - + // R = Rz(180)Rx(90) sends sensor-forward to gimbal -X instead - which is what + // proves the order is load-bearing rather than incidental. + const ImuSample swapped = applyMount(ImuMount{90, 0, 180}, s); + CHECK(swapped.acc[0] == doctest::Approx(-1.0)); + CHECK(swapped.acc[1] == doctest::Approx(0.0).scale(1)); +} + +TEST_CASE("rotating a vector never changes its length") { + // The cheapest guard against a malformed matrix: a rotation preserves + // magnitude, a typo in a sine almost never does. + const ImuSample in = makeSample(); + for (const ImuMount& m : {ImuMount{30, -45, 120}, ImuMount{180, 0, -90}, ImuMount{-12, 88, 7}}) { + const ImuSample out = applyMount(m, in); + CHECK(norm(out.acc) == doctest::Approx(norm(in.acc))); + CHECK(norm(out.gyr) == doctest::Approx(norm(in.gyr))); + CHECK(norm(out.mag) == doctest::Approx(norm(in.mag))); + } +} + +TEST_CASE("everything that is not a frame passes through untouched") { + const ImuSample in = makeSample(); + const ImuSample out = applyMount(ImuMount{30, -45, 120}, in); + CHECK(out.temp_c == in.temp_c); + CHECK(out.sample_counter == in.sample_counter); + CHECK(out.valid == in.valid); +} + +TEST_CASE("Euler round-trips through the rotation matrix") { + struct { double roll, pitch, yaw; } cases[] = { + {0, 0, 0}, {10, 20, 30}, {-45, 12, 200}, {179, -80, 359}, {-170, 0, 45}, + }; + for (const auto& c : cases) { + const Mat3 R = rotationFromEulerZYX(c.roll, c.pitch, c.yaw); + double roll = 0, pitch = 0, yaw = 0; + eulerZYXFromRotation(R, roll, pitch, yaw); + CHECK(roll == doctest::Approx(c.roll).epsilon(1e-9)); + CHECK(pitch == doctest::Approx(c.pitch).epsilon(1e-9)); + CHECK(headingDiff(yaw, c.yaw) == doctest::Approx(0.0).scale(1)); + } +} + +TEST_CASE("applying a mount and then its inverse gives back the original") { + const ImuMount m{30, -45, 120}; + // The inverse of Rz(z)Rx(x)Ry(y) is Ry(-y)Rx(-x)Rz(-z), which is NOT another + // Z-X-Y mount - so undo it by rotating the matrix back rather than by + // negating the angles, and check against the composed identity. + const Mat3 R = mountRotationZXY(m); + Mat3 Rt{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) Rt[i][j] = R[j][i]; + + // R^T R must be the identity, i.e. the round trip is exact by construction. + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) { + const double v = Rt[i][0] * R[0][j] + Rt[i][1] * R[1][j] + Rt[i][2] * R[2][j]; + CHECK(v == doctest::Approx(i == j ? 1.0 : 0.0).epsilon(1e-12)); + } + + // And a yaw mount really is undone by the opposite yaw mount. + const ImuSample in = makeSample(); + const ImuSample there_and_back = applyMount(ImuMount{0, 0, -35}, applyMount(ImuMount{0, 0, 35}, in)); + for (int i = 0; i < 3; ++i) CHECK(there_and_back.acc[i] == doctest::Approx(in.acc[i])); +} + +TEST_CASE("pitch at the poles produces angles, not NaN") { + // Straight up and straight down are gimbal lock: roll and yaw stop being + // separable. The result must still be finite - a NaN here would propagate + // into the calibration fit and the drift test with no warning. + for (double pitch : {90.0, -90.0, 89.9999999}) { + ImuSample s = makeSample(); + s.pitch_deg = static_cast(pitch); + s.yaw_deg = 137.f; + + const ImuSample out = applyMount(ImuMount{0, 0, 25}, s); + CHECK(std::isfinite(out.roll_deg)); + CHECK(std::isfinite(out.pitch_deg)); + CHECK(std::isfinite(out.yaw_deg)); + CHECK(out.yaw_deg >= -180.f); + CHECK(out.yaw_deg <= 180.f); + } +} + +// --- The earth reference ---------------------------------------------------- + +namespace { + +Mat3 matMul(const Mat3& a, const Mat3& b) { + Mat3 r{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + r[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j]; + return r; +} + +// What a device with earth frame `ref` and body mount `m` would report, given the +// gimbal's true attitude in NED. The inverse of what applyFrames has to undo. +ImuSample deviceReports(ImuReference ref, const ImuMount& m, double roll, double pitch, + double yaw) { + const Mat3 ned_from_local = nedFromReference(ref); + Mat3 local_from_ned{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) local_from_ned[i][j] = ned_from_local[j][i]; + + const Mat3 ned_from_gimbal = rotationFromEulerZYX(roll, pitch, yaw); + // R_gimbal<-sensor is the transpose of the mount (which is R_sensor<-gimbal). + const Mat3 R = mountRotationZXY(m); + Mat3 gimbal_from_sensor{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) gimbal_from_sensor[i][j] = R[j][i]; + + const Mat3 local_from_sensor = + matMul(local_from_ned, matMul(ned_from_gimbal, gimbal_from_sensor)); + double r = 0, p = 0, y = 0; + eulerZYXFromRotation(local_from_sensor, r, p, y); + ImuSample s = makeSample(); + s.roll_deg = static_cast(r); + s.pitch_deg = static_cast(p); + s.yaw_deg = static_cast(y); + return s; +} + +} // namespace + +TEST_CASE("a Z-up earth reference is what makes pitch fall as the nose rises") { + // The bug this whole path exists for. An upside-down IMU on a NWU-referenced + // device reports pitch with the wrong sign, and - the crucial part - the mount + // alone cannot fix it, because a mount only ever shifts roll here. + const ImuMount upside_down{180, 0, 0}; + + for (double nose_up : {5.0, 10.0, 20.0}) { + const ImuSample raw = deviceReports(ImuReference::Nwu, upside_down, 0.0, nose_up, 0.0); + CHECK(raw.pitch_deg == doctest::Approx(-nose_up)); // falls, as observed + CHECK(raw.roll_deg == doctest::Approx(0.0).scale(1)); // and reads level + + // Mount only: pitch is untouched, roll merely picks up 180. + const ImuSample mount_only = applyMount(upside_down, raw); + CHECK(mount_only.pitch_deg == doctest::Approx(-nose_up)); + CHECK(std::abs(mount_only.roll_deg) == doctest::Approx(180.0)); + + // Earth reference + mount: correct. + const ImuSample fixed = applyFrames(ImuReference::Nwu, upside_down, raw); + CHECK(fixed.pitch_deg == doctest::Approx(nose_up)); + CHECK(fixed.roll_deg == doctest::Approx(0.0).scale(1)); + CHECK(fixed.yaw_deg == doctest::Approx(0.0).scale(1)); + } +} + +TEST_CASE("applyFrames recovers the true attitude for every reference and mount") { + struct { double roll, pitch, yaw; } poses[] = { + {0, 0, 0}, {0, 10, 0}, {0, -25, 45}, {15, 0, 0}, {-30, 12, -120}, {0, 0, 179}, + }; + for (ImuReference ref : {ImuReference::Ned, ImuReference::Nwu, ImuReference::Enu}) { + for (const ImuMount& m : {ImuMount{}, ImuMount{180, 0, 0}, ImuMount{0, 0, -90}, + ImuMount{90, 0, 180}}) { + for (const auto& p : poses) { + const ImuSample raw = deviceReports(ref, m, p.roll, p.pitch, p.yaw); + const ImuSample fixed = applyFrames(ref, m, raw); + CHECK(fixed.roll_deg == doctest::Approx(p.roll).epsilon(1e-4)); + CHECK(fixed.pitch_deg == doctest::Approx(p.pitch).epsilon(1e-4)); + CHECK(headingDiff(fixed.yaw_deg, p.yaw) == doctest::Approx(0.0).scale(1)); + } + } + } +} + +TEST_CASE("the earth reference leaves body-frame vectors alone") { + // acc/gyr/mag say which way the sensor felt something, which has nothing to do + // with what the orientation is measured against. Only the mount may touch them. + const ImuSample in = makeSample(); + for (ImuReference ref : {ImuReference::Ned, ImuReference::Nwu, ImuReference::Enu}) { + const ImuSample out = applyFrames(ref, ImuMount{}, in); + for (int i = 0; i < 3; ++i) { + CHECK(out.acc[i] == doctest::Approx(in.acc[i])); + CHECK(out.gyr[i] == doctest::Approx(in.gyr[i])); + CHECK(out.mag[i] == doctest::Approx(in.mag[i])); + } + } +} + +TEST_CASE("NED plus an identity mount is an exact no-op, and the matrices are rotations") { + const ImuSample in = makeSample(); + const ImuSample out = applyFrames(ImuReference::Ned, ImuMount{}, in); + CHECK(out.roll_deg == in.roll_deg); + CHECK(out.pitch_deg == in.pitch_deg); + CHECK(out.yaw_deg == in.yaw_deg); + + for (ImuReference ref : {ImuReference::Ned, ImuReference::Nwu, ImuReference::Enu}) { + const Mat3 R = nedFromReference(ref); + const double det = R[0][0] * (R[1][1] * R[2][2] - R[1][2] * R[2][1]) - + R[0][1] * (R[1][0] * R[2][2] - R[1][2] * R[2][0]) + + R[0][2] * (R[1][0] * R[2][1] - R[1][1] * R[2][0]); + CHECK(det == doctest::Approx(1.0)); // a rotation, never a reflection + } + CHECK(std::string(referenceName(ImuReference::Nwu)) == "NWU"); +} + +TEST_CASE("NWU and ENU differ only in yaw, by exactly 90 degrees") { + // Which of the two the device uses cannot be told from roll or pitch - only a + // compass check on yaw distinguishes them. That is worth pinning, because it + // is the whole risk of picking the wrong one. + for (double pitch : {0.0, 15.0, -20.0}) { + const ImuSample raw = deviceReports(ImuReference::Nwu, ImuMount{180, 0, 0}, 0, pitch, 30); + const ImuSample as_nwu = applyFrames(ImuReference::Nwu, ImuMount{180, 0, 0}, raw); + const ImuSample as_enu = applyFrames(ImuReference::Enu, ImuMount{180, 0, 0}, raw); + CHECK(as_enu.roll_deg == doctest::Approx(as_nwu.roll_deg)); + CHECK(as_enu.pitch_deg == doctest::Approx(as_nwu.pitch_deg)); + CHECK(headingDiff(as_enu.yaw_deg, as_nwu.yaw_deg + 90.0) == doctest::Approx(0.0).scale(1)); + } +} diff --git a/tests/test_mtiprotocol.cpp b/tests/test_mtiprotocol.cpp index 605f3d3..a8082e9 100644 --- a/tests/test_mtiprotocol.cpp +++ b/tests/test_mtiprotocol.cpp @@ -121,7 +121,7 @@ TEST_CASE("framer reassembles frames split across feeds and back-to-back frames" CHECK(count == 2); } -TEST_CASE("parseMTData reports yaw as a 0..360 heading") { +TEST_CASE("parseMTData passes roll, pitch and yaw through untouched") { auto frameWithYaw = [](float yaw) { std::vector d; for (int i = 0; i < 10; ++i) putBEFloat(d, 0.f); // temp + acc3 + gyr3 + mag3 @@ -136,12 +136,46 @@ TEST_CASE("parseMTData reports yaw as a 0..360 heading") { if (auto s = parseMTData(mid, p, n)) got = *s; }); + // No range shift, no sign flip: the device's own -180..180 reaches the + // consumer verbatim. The only thing that ever alters an IMU angle is the + // [IMU] x/y/z mount rotation, and that is applied later and only if set. auto fn = frameWithYaw(-90.f); fr.feed(fn.data(), fn.size()); - CHECK(got.yaw_deg == doctest::Approx(270.0f)); + CHECK(got.yaw_deg == doctest::Approx(-90.0f)); auto fp = frameWithYaw(45.f); fr.feed(fp.data(), fp.size()); CHECK(got.yaw_deg == doctest::Approx(45.0f)); auto fb = frameWithYaw(-179.f); fr.feed(fb.data(), fb.size()); - CHECK(got.yaw_deg == doctest::Approx(181.0f)); + CHECK(got.yaw_deg == doctest::Approx(-179.0f)); +} + +TEST_CASE("no axis is negated or re-ranged on the way out of the parser") { + // The rig's own convention is X forward / Y right / Z down with positive + // pitch = nose up. Whether the device agrees is the device's business: the + // parser's job is to not have an opinion, so a negative angle stays negative + // and a positive one stays positive on every axis. + std::vector d; + putBEFloat(d, 21.5f); // temp + for (float v : {1.f, -2.f, 3.f}) putBEFloat(d, v); // acc + for (float v : {-4.f, 5.f, -6.f}) putBEFloat(d, v); // gyr + for (float v : {7.f, -8.f, 9.f}) putBEFloat(d, v); // mag + putBEFloat(d, -30.f); // roll + putBEFloat(d, -12.5f); // pitch (nose down) + putBEFloat(d, -95.f); // yaw + d.push_back(0x12); d.push_back(0x34); + auto msg = mtiMessage(kMidMTData, d); + + ImuSample got; + MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) { + if (auto s = parseMTData(mid, p, n)) got = *s; + }); + fr.feed(msg.data(), msg.size()); + + CHECK(got.roll_deg == doctest::Approx(-30.0f)); + CHECK(got.pitch_deg == doctest::Approx(-12.5f)); + CHECK(got.yaw_deg == doctest::Approx(-95.0f)); + CHECK(got.acc[1] == doctest::Approx(-2.0f)); // vectors too: no axis flipped + CHECK(got.gyr[0] == doctest::Approx(-4.0f)); + CHECK(got.mag[1] == doctest::Approx(-8.0f)); + CHECK(got.temp_c == doctest::Approx(21.5f)); } TEST_CASE("orientation-control command builders are well-formed") { @@ -364,3 +398,140 @@ TEST_CASE("framer rejects a bad checksum and a wrong-length payload") { CHECK_FALSE(parseMTData(kMidMTData, shortData.data(), shortData.size()).has_value()); } } + +// --- What the host asks for, and proving the device took it ----------------- + +TEST_CASE("the MTData length is derived from the output settings, not asserted") { + // The constant the parser gates on must follow the bits, so that changing a + // setting can never silently leave parseMTData decoding the old layout. + CHECK(mtDataLen(kOutputMode, kOutputSettings) == 54); + CHECK(kMTDataLen == 54); + + using namespace mtsettings; + const uint32_t base = kOutputSettings & ~kOrientationMask; + + // Orientation mode dominates the length: Temp(4) + acc/gyr/mag(36) + N + counter(2). + CHECK(mtDataLen(kOutputMode, base | kOrientationQuaternion) == 58); + CHECK(mtDataLen(kOutputMode, base | kOrientationEuler) == 54); + CHECK(mtDataLen(kOutputMode, base | kOrientationMatrix) == 78); + + // A disabled calibrated channel drops its 12 bytes. + CHECK(mtDataLen(kOutputMode, kOutputSettings | kMagDisabled) == 42); + CHECK(mtDataLen(kOutputMode, kOutputSettings | kAccDisabled | kGyrDisabled) == 30); + + // No timestamp drops the trailing sample counter. + CHECK(mtDataLen(kOutputMode, kOutputSettings & ~kTimestampMask) == 52); + + // Mode bits drop whole blocks. + CHECK(mtDataLen(kOutputMode & ~mtmode::kTemperature, kOutputSettings) == 50); + CHECK(mtDataLen(kOutputMode & ~mtmode::kOrientation, kOutputSettings) == 42); + CHECK(mtDataLen(kOutputMode | mtmode::kStatus, kOutputSettings) == 55); +} + +TEST_CASE("the settings word the host writes still decodes as Euler + counter + float") { + // kOutputSettings gained named fields (and a slot for the coordinate system); + // it must still describe the stream parseMTData actually decodes. + ImuDeviceConfig c; + std::vector d = {uint8_t(kOutputSettings >> 24), uint8_t(kOutputSettings >> 16), + uint8_t(kOutputSettings >> 8), uint8_t(kOutputSettings)}; + CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d.data(), d.size())); + 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); +} + +namespace { + +// A device config reporting exactly what the host asked for. +ImuDeviceConfig agreeingConfig() { + ImuDeviceConfig c; + c.valid = true; + c.has_output_mode = true; + c.output_mode = kOutputMode; + c.has_output_settings = true; + c.output_settings = kOutputSettings; + return c; +} + +bool mentions(const std::vector& v, const std::string& field) { + for (const auto& m : v) + if (m.field == field) return true; + return false; +} + +} // namespace + +TEST_CASE("checkImuDeviceConfig passes a device that took the configuration") { + CHECK(checkImuDeviceConfig(agreeingConfig()).empty()); +} + +TEST_CASE("checkImuDeviceConfig catches what the payload length cannot") { + // These are the dangerous ones: neither changes the MTData length, so the + // length gate in parseMTData sees nothing wrong and the readings are silently + // decoded from the wrong frame or the wrong number format. + SUBCASE("a coordinate system that is not what was asked for") { + auto c = agreeingConfig(); + c.output_settings = kOutputSettings ^ 0x80000000u; // a high bit, whatever it means + const auto found = checkImuDeviceConfig(c); + REQUIRE(found.size() == 1); + CHECK(found[0].field == "output settings"); + CHECK(found[0].answered); + CHECK(found[0].expected == kOutputSettings); + CHECK(found[0].actual == c.output_settings); + // The detail must point at the coordinate system, since that is where an + // unrecognized high bit lives. + CHECK(found[0].detail.find("coordinate system") != std::string::npos); + CHECK(found[0].describe().find("output settings") != std::string::npos); + } + SUBCASE("fixed-point instead of float, which keeps the length at 54") { + auto c = agreeingConfig(); + c.output_settings = (kOutputSettings & ~mtsettings::kFormatMask) | + mtsettings::kFormatFixed1220; + CHECK(mtDataLen(kOutputMode, c.output_settings) == kMTDataLen); // invisible to the gate + const auto found = checkImuDeviceConfig(c); + REQUIRE(found.size() == 1); + CHECK(found[0].detail.find("Fixed 12.20") != std::string::npos); + CHECK(found[0].detail.find("Float") != std::string::npos); + } +} + +TEST_CASE("an unanswered field is unverifiable, not a match") { + // Older firmware that ignores the Req* queries must never be mistaken for a + // device that agreed - that is the whole difference between "checked" and + // "could not check". + auto c = agreeingConfig(); + c.has_output_settings = false; + c.output_settings = 0; + + const auto found = checkImuDeviceConfig(c); + REQUIRE(found.size() == 1); + CHECK(found[0].field == "output settings"); + CHECK_FALSE(found[0].answered); + CHECK(found[0].describe().find("could not be verified") != std::string::npos); +} + +TEST_CASE("both fields are reported when both disagree") { + ImuDeviceConfig c; + c.valid = true; + c.has_output_mode = true; + c.output_mode = mtmode::kTemperature; // orientation is off + c.has_output_settings = true; + c.output_settings = (kOutputSettings & ~mtsettings::kOrientationMask) | + mtsettings::kOrientationQuaternion; + + const auto found = checkImuDeviceConfig(c); + CHECK(found.size() == 2); + CHECK(mentions(found, "output mode")); + CHECK(mentions(found, "output settings")); + for (const auto& m : found) CHECK_FALSE(m.detail.empty()); +} + +TEST_CASE("mtiErrorText names known codes and does not swallow unknown ones") { + CHECK(mtiErrorText(0x04) == "invalid message"); + CHECK(mtiErrorText(0x21) == "invalid parameter"); + // An unrecognized code must still carry the number through to the log. + CHECK(mtiErrorText(0x7B).find("0x7B") != std::string::npos); +}