diff --git a/.gitignore b/.gitignore index 9b1f263..a986f46 100644 --- a/.gitignore +++ b/.gitignore @@ -29,3 +29,5 @@ bin/x64/Release/NIR/ # Real configs hold plaintext MQTT credentials. Commit config/config.example.ini instead. config.ini bin/x64/Release/config.ini +build-test/ +build/ diff --git a/docs/configuration.md b/docs/configuration.md index 886e29b..c71bbef 100644 --- a/docs/configuration.md +++ b/docs/configuration.md @@ -79,6 +79,18 @@ was left in Config state. Yaw is reported as a **0..360 heading** (not the MTi's `[Features] mock_imu = true` to use a synthetic IMU on dev machines (no hardware). Protocol/units are documented in the modules reference (`MtiProtocol`). +During that same Config-state handshake the host also **reads back the device configuration** +(`ReqProductCode`, `ReqDID`, `ReqFWRev`, `ReqPeriod`, `ReqOutputMode`, `ReqOutputSettings`, and the +filter-profile / **XKF profile** via `ReqFilterProfile` + `ReqAvailableFilterProfiles`). The decoded +values — product code, firmware, device ID, output mode/format, calibration channels, sample rate, and +the list of supported **Xsens Kalman Filter (XKF) profiles** with the active one marked — are surfaced +in the **IMU CONFIG** section of the expanded Sensors view (press `i`). This is read-only: it reports +what the device is actually configured to do, which is the place to confirm the active XKF profile +(e.g. `General` vs `VRU_general`) when diagnosing yaw drift. On the legacy MTi the same concept is +called a "scenario" in the device manual (e.g. `Machine_nomagfield`); it is the same setting and shares +the wire MIDs. Older MTi firmware that does not answer the `Req*` queries simply leaves the section +absent (the host logs `no configuration acks received`). + ### Secrets `mqtt_user` / `mqtt_pw` are read from the environment variables **`FGC_MQTT_USER` / `FGC_MQTT_PW`** first, @@ -157,7 +169,10 @@ one node in [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp). (`d` requests a fresh one), the homing limits, and the **last calibration** result (per-axis `counts_per_deg` / `zero_count` / R² / age). - **IMU** (`i`) — every MTi channel with units: orientation (°), acceleration (m/s²), rate-of-turn - (rad/s), magnetic field (a.u.), temperature, sample counter. + (rad/s), magnetic field (a.u.), temperature, sample counter, plus an **IMU CONFIG** section + (read back from the device at startup): product code, firmware, device ID, output mode/format, + calibration channels, sample rate, and the **Xsens Kalman Filter (XKF) profile** list — every + profile the device supports, by name, with the active one marked `●` (selected). **Activity strip** — a compact section between the log and the key bar that shows the currently-running special operation with live progress (`gimbal calib`, `gimbal diag`, homing, capture diff --git a/docs/modules-reference.md b/docs/modules-reference.md index 87d4a3f..6cb6e57 100644 --- a/docs/modules-reference.md +++ b/docs/modules-reference.md @@ -18,7 +18,7 @@ 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: sweeps each axis, dwells reading the IMU, fits degrees↔counts; exposes `progress()`/`report()`/`takeResult()` | -| [include/fgc/MtiProtocol.h](../include/fgc/MtiProtocol.h), [src/core/MtiProtocol.cpp](../src/core/MtiProtocol.cpp) | Xsens MTi binary protocol: `MtiFramer` (checksum framing), config-message builders, `parseMTData` → `ImuSample` (temp/acc/gyr/mag/euler) | +| [include/fgc/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**) | | [include/fgc/CaptureScheduler.h](../include/fgc/CaptureScheduler.h), [src/core/CaptureScheduler.cpp](../src/core/CaptureScheduler.cpp) | Capture state machine over the interfaces; injectable clock; `setGeometry` adopts a recalibration | | [include/fgc/Application.h](../include/fgc/Application.h), [src/core/Application.cpp](../src/core/Application.cpp) | Factory (real vs mock, headless vs TUI), wiring, control loop, `gimbal …` commands, background-result polling, `buildSnapshot()` | | [include/fgc/ui/UiSnapshot.h](../include/fgc/ui/UiSnapshot.h), [src/ui/UiSnapshot.cpp](../src/ui/UiSnapshot.cpp) | Plain-data view model (incl. `ImuView`, `ActivityView`, `CalibResultView`, `DumpView`) + pure formatting helpers (state label/colour, degrees, time-ago, pending-sensors) | @@ -40,7 +40,7 @@ Per-file reference for the refactored tree, plus the shared data structures. | File | Implements | Built when | |------|-----------|-----------| | [src/serial/SerialMotorController.cpp](../src/serial/SerialMotorController.cpp) | `IMotorController` over Boost.Asio serial (pImpl) | always | -| [src/serial/MtiImuSource.cpp](../src/serial/MtiImuSource.cpp) | `IImuSource` over Boost.Asio serial: configures the MTi to Euler+calibrated, then frames the MTData stream | always | +| [src/serial/MtiImuSource.cpp](../src/serial/MtiImuSource.cpp) | `IImuSource` over Boost.Asio serial: configures the MTi to Euler+calibrated, reads back its config (`config()` → `ImuDeviceConfig`), then frames the MTData stream | always | | [src/mqtt/MqttControlChannel.cpp](../src/mqtt/MqttControlChannel.cpp) | `IControlChannel` over Eclipse Paho | `WITH_MQTT` | | [src/ui/TuiUi.cpp](../src/ui/TuiUi.cpp) | `IUserInterface` over FTXUI: panels, key bar, log pane | `WITH_TUI` | | [src/camera/VimbaCameraSource.cpp](../src/camera/VimbaCameraSource.cpp) | `ICameraSource` over Vimba X (pImpl) | `WITH_VIMBA` | @@ -90,6 +90,16 @@ One decoded Xsens MTi reading: `temp_c` (°C), `acc[3]` (m/s², incl. gravity), `gimbal calib` phase-unwraps the swept yaw (`unwrapNear`) so a sweep crossing 0/360 still fits a clean line. +### `ImuDeviceConfig` ([MtiProtocol.h](../include/fgc/MtiProtocol.h)) +The device configuration read back during the Config-state handshake (each `has_*` flag marks whether +the device actually answered): `product_code`, `device_id`, `firmware`, output mode flags +(temp/calibrated/orientation/aux/status), output settings (`orientation_mode`, `timestamp_mode`, +per-channel `acc/gyr/mag_enabled`, `data_format`), `period`/`sample_rate_hz`, and the **XKF scenario** +(`scenario_type`/`version`, resolved to `scenario_label` against `available_profiles`). Built by +`applyImuConfigAck` (one ack frame at a time) + `finalizeImuConfig` (derive rate, resolve label). +Surfaced read-only in the TUI's `i` view (IMU CONFIG section); the legacy "scenario" is the modern +"filter profile" — same MIDs (`0x62/0x64/0x65`). + ## On-disk artifacts | Artifact | Path | Format | diff --git a/include/fgc/IImuSource.h b/include/fgc/IImuSource.h index 55fa2ea..6261351 100644 --- a/include/fgc/IImuSource.h +++ b/include/fgc/IImuSource.h @@ -21,6 +21,11 @@ public: // Latest reading, or nullopt if none/stale. virtual std::optional sample() = 0; + + // Device configuration read back during start-up (output mode/settings, + // sample rate, identity, XKF scenario). nullopt if not yet known or the + // backend cannot report it. + virtual std::optional config() const { return std::nullopt; } }; } // namespace fgc diff --git a/include/fgc/MtiImuSource.h b/include/fgc/MtiImuSource.h index 07cdbcb..c048e89 100644 --- a/include/fgc/MtiImuSource.h +++ b/include/fgc/MtiImuSource.h @@ -20,6 +20,7 @@ public: void stop() override; bool connected() const override; std::optional sample() override; + std::optional config() const override; private: struct Impl; diff --git a/include/fgc/MtiProtocol.h b/include/fgc/MtiProtocol.h index ac4c4e8..5a5a209 100644 --- a/include/fgc/MtiProtocol.h +++ b/include/fgc/MtiProtocol.h @@ -3,6 +3,7 @@ #include #include #include +#include #include namespace fgc { @@ -31,6 +32,22 @@ inline constexpr uint8_t kMidSetOutputSettingsAck = 0xD3; inline constexpr uint8_t kMidMTData = 0x32; inline constexpr uint8_t kMidError = 0x42; +// Config-readback queries (sent with an empty data field in Config State; the +// device replies with the matching ack MID = request MID + 1). ReqOutputMode / +// ReqOutputSettings reuse the Set MIDs above (len 0 => request, not set). +inline constexpr uint8_t kMidReqDID = 0x00; +inline constexpr uint8_t kMidDeviceID = 0x01; +inline constexpr uint8_t kMidReqPeriod = 0x04; +inline constexpr uint8_t kMidReqPeriodAck = 0x05; +inline constexpr uint8_t kMidReqFWRev = 0x12; +inline constexpr uint8_t kMidFirmwareRev = 0x13; +inline constexpr uint8_t kMidReqProductCode = 0x1C; +inline constexpr uint8_t kMidProductCode = 0x1D; +inline constexpr uint8_t kMidReqAvailFilterProf = 0x62; +inline constexpr uint8_t kMidAvailFilterProf = 0x63; +inline constexpr uint8_t kMidReqFilterProfile = 0x64; // SetScenario shares this MID +inline constexpr uint8_t kMidReqFilterProfileAck = 0x65; + // OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04). inline constexpr uint16_t kOutputMode = 0x0007; // OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp @@ -53,6 +70,73 @@ struct ImuSample { uint16_t sample_counter = 0; }; +// One available filter profile (a.k.a. XKF "scenario") as reported by the device +// in the AvailableFilterProfiles message. +struct ImuFilterProfile { + uint8_t type = 0; + uint8_t version = 0; + std::string label; // human name, e.g. "General" / "VRU_general" +}; + +// Device configuration read back during the Config-state handshake. Each `has_*` +// flag marks whether the corresponding ack was actually received and decoded, so +// the UI can show "—" for anything the device did not answer. +struct ImuDeviceConfig { + bool valid = false; // at least one field was populated + + // Identity. + std::string product_code; // e.g. "MTi-28A33G85" + bool has_device_id = false; + uint32_t device_id = 0; // serial / device ID + std::string firmware; // "2.3.1 build 25" ("" if unknown) + + // Output mode (which data the device streams). + bool has_output_mode = false; + uint16_t output_mode = 0; + bool out_temperature = false, out_calibrated = false; + bool out_orientation = false, out_auxiliary = false, out_status = false; + + // Output settings (how the data is formatted). + bool has_output_settings = false; + uint32_t output_settings = 0; + std::string orientation_mode; // "Quaternion" / "Euler" / "Matrix" + std::string timestamp_mode; // "Sample counter" / "None" + bool acc_enabled = true, gyr_enabled = true, mag_enabled = true; + std::string data_format; // "Float" / "Fixed 12.20" + + // Sample rate. + bool has_period = false; + uint16_t period = 0; // raw, resolution 1/115200 s + float sample_rate_hz = 0.f; + + // Filter profile / XKF scenario. + bool has_scenario = false; + uint8_t scenario_type = 0, scenario_version = 0; + std::string scenario_label; // resolved from available_profiles, else "" + std::vector available_profiles; +}; + +// Config-readback query builders (empty data field => "request", not "set"). +std::vector msgReqProductCode(); +std::vector msgReqDID(); +std::vector msgReqFWRev(); +std::vector msgReqPeriod(); +std::vector msgReqOutputMode(); // kMidSetOutputMode, len 0 +std::vector msgReqOutputSettings(); // kMidSetOutputSettings, len 0 +std::vector msgReqFilterProfile(); +std::vector msgReqAvailFilterProfiles(); + +// Decode a single config-ack frame into `c`. Recognizes DeviceID(0x01), +// ProductCode(0x1D), FirmwareRev(0x13), ReqPeriodAck(0x05), output-mode ack +// (0xD1), output-settings ack (0xD3), filter-profile ack (0x65), and the +// available-profiles list (0x63). Unknown MIDs are ignored. Returns true if the +// frame was recognized and applied. +bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n); + +// Resolve derived fields once all acks are applied: the scenario label (matched +// against available_profiles by type) and sample_rate_hz from the period. +void finalizeImuConfig(ImuDeviceConfig& c); + // Lower byte of the sum of all bytes from BID through the end of DATA. The CS // byte that makes the running total ≡ 0 (mod 256) is (256 - mtiChecksum) & 0xFF. uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len); diff --git a/include/fgc/mock/MockImuSource.h b/include/fgc/mock/MockImuSource.h index 289941d..cb38931 100644 --- a/include/fgc/mock/MockImuSource.h +++ b/include/fgc/mock/MockImuSource.h @@ -44,6 +44,35 @@ public: return s; } + // Synthetic config mirroring the real handshake (Euler + calibrated, 100 Hz), + // so the expanded view's IMU CONFIG section renders without hardware. + std::optional config() const override { + ImuDeviceConfig c; + c.valid = true; + c.product_code = "MTi-28A53G35 (mock)"; + c.has_device_id = true; + c.device_id = 0x00990ABC; + c.firmware = "2.8.1 build 0"; + c.has_output_mode = true; + c.output_mode = kOutputMode; + c.out_temperature = c.out_calibrated = c.out_orientation = true; + c.has_output_settings = true; + c.output_settings = kOutputSettings; + c.orientation_mode = "Euler"; + c.timestamp_mode = "Sample counter"; + c.data_format = "Float"; + c.has_period = true; + c.period = 1152; // 100 Hz + c.sample_rate_hz = 100.0f; + c.has_scenario = true; + c.scenario_type = 39; + c.scenario_version = 11; + c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"}, + {41, 11, "Dynamic"}}; + c.scenario_label = "General"; + return c; + } + private: using clock = std::chrono::steady_clock; clock::time_point start_ = clock::now(); diff --git a/include/fgc/ui/UiSnapshot.h b/include/fgc/ui/UiSnapshot.h index 01d34a2..5b3419e 100644 --- a/include/fgc/ui/UiSnapshot.h +++ b/include/fgc/ui/UiSnapshot.h @@ -118,16 +118,41 @@ struct DumpView { std::string text; }; +// Device configuration read back from the MTi at startup, formatted for the +// expanded Sensors view. `present` is false until the handshake reports it (or +// for backends that cannot report it). +// One XKF (Xsens Kalman Filter) profile the device supports, with whether it is +// the active one. +struct ImuProfileView { + std::string name; // human label, no numeric IDs ("General") + bool selected = false; // the profile currently in use +}; + +struct ImuConfigView { + bool present = false; + std::string product_code; // "MTi-28A53G35" + std::string device_id; // "0x00990ABC" + std::string firmware; // "2.8.1 build 0" + std::string output_mode; // "Temp · Calibrated · Orientation" + std::string output_settings; // "Euler · Sample counter · Float" + std::string channels; // "acc gyr mag" + std::string sample_rate; // "100 Hz" + // Available XKF profiles with the active one flagged. Empty if the device + // did not report them. + std::vector xkf_profiles; +}; + // Full Xsens MTi reading for the expanded Sensors view (units: acc m/s^2, // gyr rad/s, mag a.u., angles deg, temp °C). struct ImuView { - bool present = false; - float roll_deg = 0, pitch_deg = 0, yaw_deg = 0; - float acc[3] = {0, 0, 0}; - float gyr[3] = {0, 0, 0}; - float mag[3] = {0, 0, 0}; - float temp_c = 0; - unsigned sample_counter = 0; + bool present = false; + float roll_deg = 0, pitch_deg = 0, yaw_deg = 0; + float acc[3] = {0, 0, 0}; + float gyr[3] = {0, 0, 0}; + float mag[3] = {0, 0, 0}; + float temp_c = 0; + unsigned sample_counter = 0; + ImuConfigView config; }; // Status of the currently-running special operation (calibration, diagnostics, diff --git a/src/core/Application.cpp b/src/core/Application.cpp index d66f924..049e8a4 100644 --- a/src/core/Application.cpp +++ b/src/core/Application.cpp @@ -73,6 +73,70 @@ std::string traceNames(unsigned mask) { return out; } +// Build the expanded-view ImuConfigView from the device's decoded config. +ImuConfigView formatImuConfig(const ImuDeviceConfig& c) { + ImuConfigView v; + v.present = true; + v.product_code = c.product_code.empty() ? "—" : c.product_code; + if (c.has_device_id) { + char b[16]; + std::snprintf(b, sizeof(b), "0x%08X", c.device_id); + v.device_id = b; + } else { + v.device_id = "—"; + } + v.firmware = c.firmware.empty() ? "—" : c.firmware; + + if (c.has_output_mode) { + std::string m; + auto add = [&](bool on, const char* name) { + if (on) { if (!m.empty()) m += " · "; m += name; } + }; + add(c.out_temperature, "Temp"); + add(c.out_calibrated, "Calibrated"); + add(c.out_orientation, "Orientation"); + add(c.out_auxiliary, "Auxiliary"); + add(c.out_status, "Status"); + v.output_mode = m.empty() ? "—" : m; + } else { + v.output_mode = "—"; + } + + if (c.has_output_settings) { + v.output_settings = c.orientation_mode + " · " + c.timestamp_mode + " · " + c.data_format; + std::string ch; + if (c.acc_enabled) ch += "acc "; + if (c.gyr_enabled) ch += "gyr "; + if (c.mag_enabled) ch += "mag"; + v.channels = ch.empty() ? "none" : ch; + } else { + v.output_settings = "—"; + v.channels = "—"; + } + + if (c.has_period && c.sample_rate_hz > 0) { + char b[24]; + std::snprintf(b, sizeof(b), "%.0f Hz", c.sample_rate_hz); + v.sample_rate = b; + } else { + v.sample_rate = "—"; + } + + // XKF profiles: list every available profile by name (no numeric IDs) and + // flag the active one (matched by profile type). + for (const auto& p : c.available_profiles) { + ImuProfileView pv; + pv.name = p.label.empty() ? "(unnamed)" : p.label; + pv.selected = c.has_scenario && p.type == c.scenario_type; + v.xkf_profiles.push_back(std::move(pv)); + } + // If the device reported the active profile but not the full list, still show + // the one in use (by its resolved label, when known). + if (v.xkf_profiles.empty() && c.has_scenario && !c.scenario_label.empty()) + v.xkf_profiles.push_back({c.scenario_label, true}); + return v; +} + } // namespace struct Application::Impl { @@ -102,6 +166,8 @@ struct Application::Impl { std::vector last_calib_summary; long long last_calib_ts = 0; // epoch ms bool calib_save_pending_ = false; // awaiting y/n to persist to config + mutable ImuConfigView imu_config_view_; // formatted MTi config (read once) + mutable bool imu_config_done_ = false; std::atomic running{true}; std::mutex cmd_mutex; @@ -243,6 +309,14 @@ struct Application::Impl { f.value = v; f.present = true; }; + // Device configuration (read once at startup): format and cache. + if (!imu_config_done_) { + if (auto c = imu->config()) { + imu_config_view_ = formatImuConfig(*c); + imu_config_done_ = true; + } + } + s.imu.config = imu_config_view_; if (auto m = imu->sample()) { // Full reading for the expanded view. s.imu.present = true; diff --git a/src/core/MtiProtocol.cpp b/src/core/MtiProtocol.cpp index d5861ae..db70d48 100644 --- a/src/core/MtiProtocol.cpp +++ b/src/core/MtiProtocol.cpp @@ -1,6 +1,8 @@ #include "fgc/MtiProtocol.h" +#include #include +#include namespace fgc { @@ -19,6 +21,11 @@ uint16_t beU16(const uint8_t* p) { return static_cast((uint16_t(p[0]) << 8) | uint16_t(p[1])); } +uint32_t beU32(const uint8_t* p) { + return (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) | (uint32_t(p[2]) << 8) | + uint32_t(p[3]); +} + // Append a big-endian value to a byte vector. void putBE(std::vector& v, uint16_t x) { v.push_back(static_cast(x >> 8)); @@ -68,6 +75,133 @@ std::vector msgSetOutputSettings() { return mtiMessage(kMidSetOutputSettings, d); } +std::vector msgReqProductCode() { return mtiMessage(kMidReqProductCode); } +std::vector msgReqDID() { return mtiMessage(kMidReqDID); } +std::vector msgReqFWRev() { return mtiMessage(kMidReqFWRev); } +std::vector msgReqPeriod() { return mtiMessage(kMidReqPeriod); } +std::vector msgReqOutputMode() { return mtiMessage(kMidSetOutputMode); } +std::vector msgReqOutputSettings() { return mtiMessage(kMidSetOutputSettings); } +std::vector msgReqFilterProfile() { return mtiMessage(kMidReqFilterProfile); } +std::vector msgReqAvailFilterProfiles(){ return mtiMessage(kMidReqAvailFilterProf); } + +bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n) { + switch (mid) { + case kMidDeviceID: + if (n < 4) return false; + c.device_id = beU32(d); + c.has_device_id = true; + c.valid = true; + return true; + + case kMidProductCode: { + // ASCII string, possibly space-padded; trim trailing spaces/NULs. + std::size_t end = n; + while (end > 0 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end; + c.product_code.assign(reinterpret_cast(d), end); + c.valid = true; + return true; + } + + case kMidFirmwareRev: { + // MAJOR MINOR REV [BUILDNR(4) SCMREF(4)] — older firmware sends only 3. + if (n < 3) return false; + char buf[48]; + if (n >= 7) { + uint32_t build = beU32(d + 3); + std::snprintf(buf, sizeof(buf), "%u.%u.%u build %u", d[0], d[1], d[2], build); + } else { + std::snprintf(buf, sizeof(buf), "%u.%u.%u", d[0], d[1], d[2]); + } + c.firmware = buf; + c.valid = true; + return true; + } + + case kMidReqPeriodAck: + if (n < 2) return false; + c.period = beU16(d); + c.has_period = true; + c.valid = true; + return true; + + case kMidSetOutputModeAck: { // 0xD1, ack to ReqOutputMode + if (n < 2) return false; + uint16_t m = beU16(d); + c.output_mode = m; + c.out_temperature = m & 0x0001; + c.out_calibrated = m & 0x0002; + c.out_orientation = m & 0x0004; + c.out_auxiliary = m & 0x0008; + c.out_status = m & 0x0800; + c.has_output_mode = true; + c.valid = true; + return true; + } + + case kMidSetOutputSettingsAck: { // 0xD3, ack to ReqOutputSettings + if (n < 4) return false; + uint32_t s = beU32(d); + c.output_settings = s; + switch (s & 0x0003) { + case 0x1: c.timestamp_mode = "Sample counter"; break; + default: c.timestamp_mode = "None"; break; + } + switch ((s >> 2) & 0x0003) { + case 0x0: c.orientation_mode = "Quaternion"; break; + case 0x1: c.orientation_mode = "Euler"; break; + case 0x2: c.orientation_mode = "Matrix"; break; + default: c.orientation_mode = "?"; break; + } + // Bits 4/5/6: 1 = output DISABLED. + c.acc_enabled = !(s & 0x0010); + c.gyr_enabled = !(s & 0x0020); + c.mag_enabled = !(s & 0x0040); + c.data_format = ((s >> 8) & 0x0003) == 0x1 ? "Fixed 12.20" : "Float"; + c.has_output_settings = true; + c.valid = true; + return true; + } + + case kMidReqFilterProfileAck: // VERSION, FILTERPROFILE(type) + if (n < 2) return false; + c.scenario_version = d[0]; + c.scenario_type = d[1]; + c.has_scenario = true; + c.valid = true; + return true; + + case kMidAvailFilterProf: { + // Repeating 22-byte records: TYPE(1) VERSION(1) LABEL(20, space-padded). + c.available_profiles.clear(); + for (std::size_t o = 0; o + 22 <= n; o += 22) { + ImuFilterProfile p; + p.type = d[o]; + p.version = d[o + 1]; + if (p.type == 0) continue; // empty slot + std::size_t end = o + 22; + while (end > o + 2 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end; + p.label.assign(reinterpret_cast(d + o + 2), end - (o + 2)); + c.available_profiles.push_back(std::move(p)); + } + c.valid = true; + return true; + } + + default: + return false; + } +} + +void finalizeImuConfig(ImuDeviceConfig& c) { + if (c.has_period && c.period > 0) + c.sample_rate_hz = 115200.0f / static_cast(c.period); + if (c.has_scenario) { + for (const auto& p : c.available_profiles) { + if (p.type == c.scenario_type) { c.scenario_label = p.label; break; } + } + } +} + std::optional parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) { if (mid != kMidMTData || len != kMTDataLen) return std::nullopt; ImuSample s; diff --git a/src/serial/MtiImuSource.cpp b/src/serial/MtiImuSource.cpp index e205ec4..7a2ddff 100644 --- a/src/serial/MtiImuSource.cpp +++ b/src/serial/MtiImuSource.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -34,6 +35,9 @@ struct MtiImuSource::Impl { clock::time_point last_rx{}; std::atomic open{false}; + ImuDeviceConfig cfg; // device config read back at startup + bool have_cfg = false; + MtiFramer framer; unsigned bad_len_warned = 0; @@ -78,10 +82,68 @@ struct MtiImuSource::Impl { write(msgGoToConfig()); std::this_thread::sleep_for(60ms); write(msgSetOutputMode()); std::this_thread::sleep_for(60ms); write(msgSetOutputSettings()); std::this_thread::sleep_for(60ms); + // Read back the device configuration while still in Config State. + queryConfig(write); write(msgGoToMeasurement()); std::this_thread::sleep_for(60ms); // Drop any pre-config (old-format) bytes so the framer starts clean. ::tcflush(serial.native_handle(), TCIFLUSH); } + + // Query the device configuration in Config State and decode the acks. Runs on + // the calling thread before the streaming io_thread starts; it drives the + // io_context itself with run_for() and leaves it clean (restarted, all + // pending ops cancelled) so the later io.run() streams normally. + template + void queryConfig(WriteFn&& write) { + using namespace std::chrono_literals; + ImuDeviceConfig local; + MtiFramer cf([&](uint8_t mid, const uint8_t* d, std::size_t n) { + applyImuConfigAck(local, mid, d, n); + }); + + write(msgReqProductCode()); + write(msgReqDID()); + write(msgReqFWRev()); + write(msgReqPeriod()); + write(msgReqOutputMode()); + write(msgReqOutputSettings()); + write(msgReqFilterProfile()); + write(msgReqAvailFilterProfiles()); + + std::array buf{}; + std::function rd = [&]() { + serial.async_read_some(boost::asio::buffer(buf), + [&](const boost::system::error_code& ec, std::size_t n) { + if (ec) return; // cancelled / error: stop + cf.feed(buf.data(), n); + rd(); + }); + }; + io.restart(); + rd(); + static_cast(io.run_for(500ms)); // collect acks for up to half a second + // Drain the pending read so its by-ref handler can't fire later with the + // local buffer/framer already destroyed. + boost::system::error_code ec; + serial.cancel(ec); + io.restart(); + static_cast(io.run()); + io.restart(); + + finalizeImuConfig(local); + if (local.valid) { + std::lock_guard lock(mutex); + cfg = local; + have_cfg = true; + LOG_INFO << "MTi config: " << (local.product_code.empty() ? "?" : local.product_code) + << " fw " << (local.firmware.empty() ? "?" : local.firmware) + << " scenario " << int(local.scenario_type) + << (local.scenario_label.empty() ? "" : " (" + local.scenario_label + ")") + << " @ " << local.sample_rate_hz << " Hz"; + } else { + LOG_WARN << "MTi: no configuration acks received (device may not answer Req* in this firmware)"; + } + } }; MtiImuSource::MtiImuSource(std::string device, unsigned int baud) @@ -136,4 +198,10 @@ std::optional MtiImuSource::sample() { return impl_->latest; } +std::optional MtiImuSource::config() const { + std::lock_guard lock(impl_->mutex); + if (!impl_->have_cfg) return std::nullopt; + return impl_->cfg; +} + } // namespace fgc diff --git a/src/ui/TuiUi.cpp b/src/ui/TuiUi.cpp index f582a74..4e7a966 100644 --- a/src/ui/TuiUi.cpp +++ b/src/ui/TuiUi.cpp @@ -387,20 +387,30 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump, const Calib // Full-screen IMU view (toggled with 'i'): every MTi channel with units. Element imuDetailPanel(const ImuView& v) { - auto f2 = [](float x) { + // Fixed-width, right-aligned to 2 decimals. The constant width keeps the sign + // column and decimal point from jumping as values cross zero or change digit + // count, so the readout stays steady instead of flickering. The width is kept + // just wide enough for the field's range so the number sits close to its + // x/y/z label: 6 for the vectors (accel/gyro/mag stay well under ±100), 7 for + // orientation (so a 3-digit "-180.00" still fits without widening). + auto f2 = [](float x, int w) { char b[24]; - std::snprintf(b, sizeof(b), "%.2f", x); + std::snprintf(b, sizeof(b), "%*.2f", w, x); return std::string(b); }; - // One "LABEL (unit) x=.. y=.. z=.." row for a 3-vector. + // A dim " │ " divider between value columns. + auto vsep = [] { return text(" \xE2\x94\x82 ") | dim; }; + // One "LABEL (unit) x:.. │ y:.. │ z:.." row for a 3-vector. auto vecRow = [&](const std::string& label, const char* unit, const float xyz[3], Color c = Color::Default) { return hbox({ text(label) | dim | size(WIDTH, EQUAL, 14), text(std::string(unit)) | dim | size(WIDTH, EQUAL, 9), - text("x " + f2(xyz[0])) | color(c) | size(WIDTH, EQUAL, 12), - text("y " + f2(xyz[1])) | color(c) | size(WIDTH, EQUAL, 12), - text("z " + f2(xyz[2])) | color(c) | size(WIDTH, EQUAL, 12), + text("x:" + f2(xyz[0], 6)) | color(c) | size(WIDTH, EQUAL, 8), + vsep(), + text("y:" + f2(xyz[1], 6)) | color(c) | size(WIDTH, EQUAL, 8), + vsep(), + text("z:" + f2(xyz[2], 6)) | color(c) | size(WIDTH, EQUAL, 8), }); }; const float ori[3] = {v.roll_deg, v.pitch_deg, v.yaw_deg}; @@ -413,9 +423,11 @@ Element imuDetailPanel(const ImuView& v) { body.push_back(hbox({ text("ORIENTATION") | dim | size(WIDTH, EQUAL, 14), text("deg") | dim | size(WIDTH, EQUAL, 9), - text("roll " + f2(ori[0])) | bold | size(WIDTH, EQUAL, 14), - text("pitch " + f2(ori[1])) | bold | size(WIDTH, EQUAL, 14), - text("yaw " + f2(ori[2])) | bold | size(WIDTH, EQUAL, 14), + text("roll:" + f2(ori[0], 6)) | bold | size(WIDTH, EQUAL, 11), + vsep(), + text("pitch:" + f2(ori[1], 6)) | bold | size(WIDTH, EQUAL, 12), + vsep(), + text("yaw:" + f2(ori[2], 6)) | bold | size(WIDTH, EQUAL, 10), })); body.push_back(separator()); body.push_back(vecRow("ACCEL", "m/s2", v.acc, Color::Cyan)); @@ -424,11 +436,47 @@ Element imuDetailPanel(const ImuView& v) { body.push_back(separator()); body.push_back(hbox({ text("TEMP") | dim | size(WIDTH, EQUAL, 14), - text(f2(v.temp_c) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18), + text(f2(v.temp_c, 6) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18), text("sample #" + std::to_string(v.sample_counter)) | dim, })); } + // IMU CONFIG section (device configuration read back at startup). Shown + // whenever it is known, even if the live stream is offline. + const auto& c = v.config; + if (c.present) { + auto cfgRow = [](const std::string& k, const std::string& val, + Color vc = Color::Default) { + return hbox({text(k) | dim | size(WIDTH, EQUAL, 16), + text(val) | color(vc)}); + }; + body.push_back(separator()); + body.push_back(text("IMU CONFIG") | bold | color(Color::Magenta)); + body.push_back(cfgRow("Product", c.product_code)); + body.push_back(cfgRow("Firmware", c.firmware)); + body.push_back(cfgRow("Device ID", c.device_id)); + body.push_back(cfgRow("Output mode", c.output_mode)); + body.push_back(cfgRow("Output fmt", c.output_settings)); + body.push_back(cfgRow("Calib channels", c.channels)); + body.push_back(cfgRow("Sample rate", c.sample_rate)); + // XKF profile list: the active one is marked "●" and highlighted; the + // rest are dim "○". (Numeric profile IDs are intentionally hidden.) + body.push_back(text("Xsens Kalman Filter (XKF) profile") | dim); + if (c.xkf_profiles.empty()) { + body.push_back(hbox({text(" "), text("(not reported by device)") | dim})); + } else { + for (const auto& p : c.xkf_profiles) { + if (p.selected) + body.push_back(hbox({text(" \xE2\x97\x8F ") | color(Color::Yellow), + text(p.name) | color(Color::Yellow) | bold, + text(" (selected)") | dim})); + else + body.push_back(hbox({text(" \xE2\x97\x8B ") | dim, + text(p.name) | dim})); + } + } + } + Element status = v.present ? (text(" MTi live ") | color(Color::Green) | bold) : (text(" MTi offline ") | color(Color::Red) | bold); return window(text(" IMU (i/Esc:close) ") | bold | color(Color::Magenta), diff --git a/tests/test_mtiprotocol.cpp b/tests/test_mtiprotocol.cpp index 7e7a54c..6aa6df9 100644 --- a/tests/test_mtiprotocol.cpp +++ b/tests/test_mtiprotocol.cpp @@ -144,6 +144,156 @@ TEST_CASE("parseMTData reports yaw as a 0..360 heading") { CHECK(got.yaw_deg == doctest::Approx(181.0f)); } +TEST_CASE("config-readback query builders are well-formed requests (empty data)") { + struct Q { std::vector m; uint8_t mid; }; + Q qs[] = { + {msgReqProductCode(), kMidReqProductCode}, + {msgReqDID(), kMidReqDID}, + {msgReqFWRev(), kMidReqFWRev}, + {msgReqPeriod(), kMidReqPeriod}, + {msgReqOutputMode(), kMidSetOutputMode}, + {msgReqOutputSettings(), kMidSetOutputSettings}, + {msgReqFilterProfile(), kMidReqFilterProfile}, + {msgReqAvailFilterProfiles(), kMidReqAvailFilterProf}, + }; + for (const auto& q : qs) { + CHECK(q.m[0] == kMtiPreamble); + CHECK(q.m[1] == kMtiBid); + CHECK(q.m[2] == q.mid); + CHECK(q.m[3] == 0); // request => empty data field + CHECK(frameChecksumOk(q.m)); + } +} + +TEST_CASE("applyImuConfigAck decodes each ack type") { + ImuDeviceConfig c; + + SUBCASE("DeviceID is a 32-bit big-endian serial") { + uint8_t d[] = {0x00, 0x99, 0x0A, 0xBC}; + CHECK(applyImuConfigAck(c, kMidDeviceID, d, sizeof(d))); + CHECK(c.has_device_id); + CHECK(c.device_id == 0x00990ABCu); + } + + SUBCASE("ProductCode trims trailing spaces") { + const char* s = "MTi-28A53G35 "; + CHECK(applyImuConfigAck(c, kMidProductCode, + reinterpret_cast(s), 15)); + CHECK(c.product_code == "MTi-28A53G35"); + } + + SUBCASE("FirmwareRev with build number") { + uint8_t d[] = {2, 8, 1, 0, 0, 0, 25}; // 2.8.1 build 25 + CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d))); + CHECK(c.firmware == "2.8.1 build 25"); + } + + SUBCASE("FirmwareRev short form (major.minor.rev only)") { + uint8_t d[] = {1, 2, 3}; + CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d))); + CHECK(c.firmware == "1.2.3"); + } + + SUBCASE("Period yields 100 Hz from 0x0480") { + uint8_t d[] = {0x04, 0x80}; // 1152 => 115200/1152 = 100 Hz + CHECK(applyImuConfigAck(c, kMidReqPeriodAck, d, sizeof(d))); + finalizeImuConfig(c); + CHECK(c.has_period); + CHECK(c.sample_rate_hz == doctest::Approx(100.0f)); + } + + SUBCASE("OutputMode 0x0007 = Temp + Calibrated + Orientation") { + uint8_t d[] = {0x00, 0x07}; + CHECK(applyImuConfigAck(c, kMidSetOutputModeAck, d, sizeof(d))); + CHECK(c.out_temperature); + CHECK(c.out_calibrated); + CHECK(c.out_orientation); + CHECK_FALSE(c.out_auxiliary); + } + + SUBCASE("OutputSettings 0x00000005 = Euler + sample counter + float, all channels") { + uint8_t d[] = {0x00, 0x00, 0x00, 0x05}; + CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d))); + CHECK(c.orientation_mode == "Euler"); + CHECK(c.timestamp_mode == "Sample counter"); + CHECK(c.data_format == "Float"); + CHECK(c.acc_enabled); + CHECK(c.gyr_enabled); + CHECK(c.mag_enabled); + } + + SUBCASE("OutputSettings with disabled mag and fixed-point format") { + // bit6 set (disable mag), output format bits 9:8 = 01 (Fixed 12.20). + uint32_t s = 0x05 | 0x40 | 0x100; + uint8_t d[] = {uint8_t(s >> 24), uint8_t(s >> 16), uint8_t(s >> 8), uint8_t(s)}; + CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d))); + CHECK(c.acc_enabled); + CHECK(c.gyr_enabled); + CHECK_FALSE(c.mag_enabled); + CHECK(c.data_format == "Fixed 12.20"); + } + + SUBCASE("unknown MID is ignored") { + uint8_t d[] = {0}; + CHECK_FALSE(applyImuConfigAck(c, 0x99, d, sizeof(d))); + CHECK_FALSE(c.valid); + } +} + +TEST_CASE("scenario label resolves against the available-profiles list") { + ImuDeviceConfig c; + // AvailableFilterProfiles: two 22-byte records (type, version, 20-byte label). + std::vector d; + auto addProfile = [&](uint8_t type, uint8_t ver, const std::string& label) { + d.push_back(type); + d.push_back(ver); + std::string padded = label; + padded.resize(20, ' '); + d.insert(d.end(), padded.begin(), padded.end()); + }; + addProfile(39, 11, "General"); + addProfile(40, 11, "High_mag_dep"); + CHECK(applyImuConfigAck(c, kMidAvailFilterProf, d.data(), d.size())); + REQUIRE(c.available_profiles.size() == 2); + CHECK(c.available_profiles[0].label == "General"); + CHECK(c.available_profiles[1].label == "High_mag_dep"); + + // Current filter profile ack: VERSION, FILTERPROFILE(type). + uint8_t fp[] = {11, 40}; + CHECK(applyImuConfigAck(c, kMidReqFilterProfileAck, fp, sizeof(fp))); + finalizeImuConfig(c); + CHECK(c.has_scenario); + CHECK(c.scenario_type == 40); + CHECK(c.scenario_label == "High_mag_dep"); +} + +TEST_CASE("a full ack stream decodes through the framer") { + // Concatenate realistic acks and run them through MtiFramer, mirroring how + // the live handshake collects them. + ImuDeviceConfig c; + MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) { + applyImuConfigAck(c, mid, p, n); + }); + auto feed = [&](const std::vector& m) { fr.feed(m.data(), m.size()); }; + + feed(mtiMessage(kMidProductCode, + {'M','T','i','-','2','8'})); + feed(mtiMessage(kMidDeviceID, {0x00, 0x99, 0x0A, 0xBC})); + feed(mtiMessage(kMidSetOutputModeAck, {0x00, 0x07})); + feed(mtiMessage(kMidSetOutputSettingsAck, {0x00, 0x00, 0x00, 0x05})); + feed(mtiMessage(kMidReqPeriodAck, {0x04, 0x80})); + feed(mtiMessage(kMidReqFilterProfileAck, {11, 39})); + finalizeImuConfig(c); + + CHECK(c.valid); + CHECK(c.product_code == "MTi-28"); + CHECK(c.device_id == 0x00990ABCu); + CHECK(c.out_orientation); + CHECK(c.orientation_mode == "Euler"); + CHECK(c.sample_rate_hz == doctest::Approx(100.0f)); + CHECK(c.scenario_type == 39); +} + TEST_CASE("framer rejects a bad checksum and a wrong-length payload") { std::vector d(kMTDataLen, 0); auto frame = mtiMessage(kMidMTData, d);