#pragma once #include #include #include #include #include namespace fgc { // Xsens MTi (legacy MT0100P) binary protocol helpers. The MTi streams MTData // messages over RS-232/UART; this module frames the byte stream, validates // checksums, builds the config messages, and decodes a combined MTData payload // into an ImuSample. Kept pure (no I/O) so it is unit-testable and lives in // fgc_core (mirrors TelemetryParser / DumpParser). // // Frame layout: PRE(0xFA) BID(0xFF) MID LEN DATA[LEN] CS // Checksum: (BID + MID + LEN + ΣDATA + CS) & 0xFF == 0. All multi-byte values // are big-endian. // Protocol constants. inline constexpr uint8_t kMtiPreamble = 0xFA; inline constexpr uint8_t kMtiBid = 0xFF; inline constexpr uint8_t kMidGoToConfig = 0x30; inline constexpr uint8_t kMidGoToConfigAck = 0x31; inline constexpr uint8_t kMidGoToMeasurement = 0x10; inline constexpr uint8_t kMidGoToMeasAck = 0x11; inline constexpr uint8_t kMidSetOutputMode = 0xD0; inline constexpr uint8_t kMidSetOutputModeAck = 0xD1; inline constexpr uint8_t kMidSetOutputSettings = 0xD2; inline constexpr uint8_t kMidSetOutputSettingsAck = 0xD3; inline constexpr uint8_t kMidMTData = 0x32; inline constexpr uint8_t kMidError = 0x42; // Config-readback queries (sent with an empty data field in Config State; the // device replies with the matching ack MID = request MID + 1). ReqOutputMode / // ReqOutputSettings reuse the Set MIDs above (len 0 => request, not set). inline constexpr uint8_t kMidReqDID = 0x00; inline constexpr uint8_t kMidDeviceID = 0x01; inline constexpr uint8_t kMidReqPeriod = 0x04; inline constexpr uint8_t kMidReqPeriodAck = 0x05; inline constexpr uint8_t kMidReqFWRev = 0x12; inline constexpr uint8_t kMidFirmwareRev = 0x13; inline constexpr uint8_t kMidReqProductCode = 0x1C; inline constexpr uint8_t kMidProductCode = 0x1D; inline constexpr uint8_t kMidReqAvailFilterProf = 0x62; inline constexpr uint8_t kMidAvailFilterProf = 0x63; inline constexpr uint8_t kMidReqFilterProfile = 0x64; // SetScenario shares this MID inline constexpr uint8_t kMidReqFilterProfileAck = 0x65; // Orientation-control commands (valid in Measurement State). inline constexpr uint8_t kMidSetNoRotation = 0x22; // 2-byte duration (seconds) inline constexpr uint8_t kMidSetNoRotationAck = 0x23; inline constexpr uint8_t kMidResetOrientation = 0xA4; // 2-byte CODE (Table 33) inline constexpr uint8_t kMidResetOrientationAck = 0xA5; // ResetOrientation CODE values we use. inline constexpr uint16_t kResetHeading = 0x0001; // current heading becomes yaw 0 inline constexpr uint16_t kResetStore = 0x0000; // persist current reset (Config state) // OutputMode = Temperature(0x01) | Calibrated(0x02) | Orientation(0x04). inline constexpr uint16_t kOutputMode = 0x0007; // OutputSettings: orientation mode Euler (bits3:2=01 => 0x04) + timestamp // SampleCounter (bits1:0=01 => 0x01), float, all calibrated channels enabled. inline constexpr uint32_t kOutputSettings = 0x00000005; // Expected MTData payload with the above config: Temp(4) + Acc(12) + Gyr(12) + // Mag(12) + Euler(12) + SampleCounter(2). inline constexpr uint8_t kMTDataLen = 54; // One fully decoded IMU reading. struct ImuSample { bool valid = false; float temp_c = 0.f; // °C float acc[3] = {0, 0, 0}; // m/s^2 (incl. gravity), sensor frame float gyr[3] = {0, 0, 0}; // rad/s float mag[3] = {0, 0, 0}; // a.u. (normalized to earth field) float roll_deg = 0.f; // -180..180 float pitch_deg = 0.f; // -90..90 float yaw_deg = 0.f; // 0..360 heading (MTi native -180..180 is shifted) uint16_t sample_counter = 0; }; // 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; }; // Orientation-control command builders. // SetNoRotation: run the "no rotation" gyro-bias update for `seconds` (the device // must be held still during it) to cut heading drift, esp. in no-magnetometer use. std::vector msgSetNoRotation(uint16_t seconds); // ResetOrientation: CODE 0x0001 redefines the *current* heading as yaw 0 // (bore-sighting); CODE 0x0000 (in Config state) stores it across power cycles. std::vector msgResetOrientation(uint16_t code); // SetFilterProfile (classic): select the XKF profile/scenario by type number. // Valid in Config state; settings changed in Config state persist to the device's // non-volatile memory automatically. std::vector msgSetFilterProfile(uint16_t profile); // Pick the no-magnetometer XKF profile from the device's reported list: prefers a // label containing "nomag"/"no_mag", else a "VRU" (gyro-tracked heading) profile. // Returns the profile `type` number, or -1 if none looks magnetometer-free. int pickNoMagProfile(const std::vector& 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); // Build a complete message (PRE BID MID LEN DATA CS) ready to write. std::vector mtiMessage(uint8_t mid, const std::vector& data = {}); // The four config messages for the orientation+calibrated Euler stream. std::vector msgGoToConfig(); std::vector msgSetOutputMode(); // kOutputMode std::vector msgSetOutputSettings(); // kOutputSettings std::vector msgGoToMeasurement(); // Decode an MTData payload (the DATA bytes, big-endian) into an ImuSample. // Returns nullopt if mid != MTData or len != kMTDataLen. std::optional parseMTData(uint8_t mid, const uint8_t* data, std::size_t len); // Incremental framer: feed raw bytes; for each complete, checksum-valid frame it // invokes the sink with (mid, data, len). Tolerates noise/resync by re-scanning // for the preamble. class MtiFramer { public: using FrameSink = std::function; explicit MtiFramer(FrameSink sink) : sink_(std::move(sink)) {} void feed(const uint8_t* p, std::size_t n); private: enum class S { Pre, Bid, Mid, Len, Data, Cs }; FrameSink sink_; S state_ = S::Pre; uint8_t mid_ = 0; uint8_t len_ = 0; std::vector data_; unsigned sum_ = 0; // running checksum sum (BID..DATA) }; } // namespace fgc