IMU readings fixed; IMU mounting is part of config.ini; device config verified at startup
This commit is contained in:
parent
9023f79c26
commit
5fce32aee7
GPG Key ID:
D4A6C394DF13F1D7
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.<name>]` block. Nothing is implied by position:
|
||||
|
|
|
|||
|
|
@ -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 <name>` 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
|
||||
|
|
|
|||
|
|
@ -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 `<RemoteDevice>` 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.
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include "fgc/ImuMount.h"
|
||||
#include "fgc/Geometry.h"
|
||||
|
||||
#include <map>
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -3,6 +3,8 @@
|
|||
#include "fgc/MtiProtocol.h" // ImuSample
|
||||
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace fgc {
|
||||
|
||||
|
|
@ -27,6 +29,15 @@ public:
|
|||
// backend cannot report it.
|
||||
virtual std::optional<ImuDeviceConfig> 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<std::string> 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.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,85 @@
|
|||
#pragma once
|
||||
|
||||
#include "fgc/MtiProtocol.h" // ImuSample
|
||||
|
||||
#include <array>
|
||||
|
||||
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<std::array<double, 3>, 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
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
#pragma once
|
||||
|
||||
#include "fgc/IImuSource.h"
|
||||
#include "fgc/ImuMount.h"
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
|
||||
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<IImuSource> 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<ImuSample> 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<ImuDeviceConfig> config() const override { return inner_->config(); }
|
||||
std::vector<std::string> 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<IImuSource> inner_;
|
||||
ImuReference reference_;
|
||||
ImuMount mount_;
|
||||
};
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -21,6 +21,7 @@ public:
|
|||
bool connected() const override;
|
||||
std::optional<ImuSample> sample() override;
|
||||
std::optional<ImuDeviceConfig> config() const override;
|
||||
std::vector<std::string> configProblems() const override;
|
||||
void refreshConfig() override;
|
||||
bool setFilterProfile(int type) override;
|
||||
void noRotation(int seconds) override;
|
||||
|
|
|
|||
|
|
@ -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<uint8_t>(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<ImuConfigMismatch> 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);
|
||||
|
|
|
|||
|
|
@ -27,7 +27,9 @@ public:
|
|||
s.valid = true;
|
||||
s.roll_deg = static_cast<float>(15.0 * std::sin(t * 0.5));
|
||||
s.pitch_deg = static_cast<float>(10.0 * std::sin(t * 0.3 + 1.0));
|
||||
s.yaw_deg = static_cast<float>(std::fmod(t * 8.0, 360.0)); // slow spin
|
||||
// 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<float>(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<float>(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<std::string> configProblems() const override {
|
||||
std::vector<std::string> 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
|
||||
|
|
|
|||
|
|
@ -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<ImuProfileView> 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<std::string> 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);
|
||||
|
|
|
|||
|
|
@ -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<IImuSource> makeImu() {
|
||||
if (!cfg.features.enable_imu) return nullptr; // sensors panel stays "pending"
|
||||
if (cfg.features.mock_imu) return std::make_unique<MockImuSource>();
|
||||
if (cfg.imu.device.empty()) {
|
||||
|
||||
std::unique_ptr<IImuSource> src;
|
||||
if (cfg.features.mock_imu) {
|
||||
src = std::make_unique<MockImuSource>();
|
||||
} else if (cfg.imu.device.empty()) {
|
||||
LOG_WARN << "IMU enabled but [IMU] device is empty; disabling IMU";
|
||||
return nullptr;
|
||||
} else {
|
||||
src = std::make_unique<MtiImuSource>(cfg.imu.device, cfg.imu.baud);
|
||||
}
|
||||
return std::make_unique<MtiImuSource>(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<MountedImuSource>(std::move(src), ref, cfg.imu.mount);
|
||||
}
|
||||
|
||||
std::unique_ptr<IEnvSensor> makeEnv() {
|
||||
|
|
@ -502,6 +520,12 @@ struct Application::Impl {
|
|||
<< (c->scenario_label.empty() ? std::string("(unnamed)") : c->scenario_label)
|
||||
<< " (type " << static_cast<int>(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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#include "fgc/Config.h"
|
||||
#include "fgc/Paths.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <cstdio>
|
||||
|
|
@ -309,6 +310,21 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
|
|||
cfg.imu.device = get(kv, "IMU.device", cfg.imu.device);
|
||||
cfg.imu.baud = static_cast<unsigned>(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<unsigned>(
|
||||
getIntAutoBase(kv, "Env.i2c_addr", static_cast<int>(cfg.env.i2c_addr)));
|
||||
|
|
|
|||
|
|
@ -0,0 +1,148 @@
|
|||
#include "fgc/ImuMount.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
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<float>(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<float>(roll);
|
||||
out.pitch_deg = static_cast<float>(pitch);
|
||||
out.yaw_deg = static_cast<float>(yaw);
|
||||
return out;
|
||||
}
|
||||
|
||||
ImuSample applyMount(const ImuMount& m, const ImuSample& s) {
|
||||
return applyFrames(ImuReference::Ned, m, s);
|
||||
}
|
||||
|
||||
} // namespace fgc
|
||||
|
|
@ -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<std::string>& 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<uint16_t, const char*> blocks[] = {
|
||||
{mtmode::kTemperature, "temperature"}, {mtmode::kCalibrated, "calibrated data"},
|
||||
{mtmode::kOrientation, "orientation"}, {mtmode::kAuxiliary, "auxiliary"},
|
||||
{mtmode::kStatus, "status"},
|
||||
};
|
||||
std::vector<std::string> 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<std::string> 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<uint32_t, const char*> 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<ImuConfigMismatch> checkImuDeviceConfig(const ImuDeviceConfig& c) {
|
||||
std::vector<ImuConfigMismatch> 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<ImuSample> 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<ImuSample> 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;
|
||||
|
|
|
|||
|
|
@ -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<double>(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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,8 +7,11 @@
|
|||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <cstdio>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <boost/asio.hpp>
|
||||
#include <termios.h>
|
||||
|
|
@ -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<std::string> 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<uint16_t>(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<std::string> 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<std::mutex> 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<std::mutex> 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<ImuDeviceConfig> MtiImuSource::config() const {
|
|||
return impl_->cfg;
|
||||
}
|
||||
|
||||
std::vector<std::string> MtiImuSource::configProblems() const {
|
||||
std::lock_guard<std::mutex> 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
|
||||
|
|
|
|||
|
|
@ -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<Element> rows;
|
||||
Element status = g.present ? (text(g.status) | color(Color::Green))
|
||||
: (text(g.status) | dim);
|
||||
rows.push_back(hbox({text(g.title) | bold, text(" "), status, filler(),
|
||||
text(hint) | dim}));
|
||||
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));
|
||||
|
|
@ -1009,6 +1046,7 @@ Element sensorsDetailPanel(const ImuView& v, const EnvView& e, ScrollState& st)
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Element imu_status = v.present ? (text(" MTi live ") | color(Color::Green) | bold)
|
||||
: (text(" MTi offline ") | color(Color::Red) | bold);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,347 @@
|
|||
#include <doctest/doctest.h>
|
||||
|
||||
#include "fgc/ImuMount.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
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<float>(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<float>(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<float>(r);
|
||||
s.pitch_deg = static_cast<float>(p);
|
||||
s.yaw_deg = static_cast<float>(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));
|
||||
}
|
||||
}
|
||||
|
|
@ -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<uint8_t> 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<uint8_t> 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<uint8_t> 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<ImuConfigMismatch>& 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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue