#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) // Which EARTH frame a device measures its orientation against. // // A different axis entirely from the [IMU] x/y/z mount, and the distinction is // load-bearing: the mount says how the sensor is bolted to the gimbal (a BODY // frame), while this says what the reported angles are measured relative to. // // NED X North Y East Z Down - what the gimbal frame assumes // NWU X North Y West Z Up - the classic MTi's factory local frame // ENU X East Y North Z Up - the newer MTi series' default // // A Z-up reference is what makes reported pitch FALL as the nose rises: with the // body Y axis pointing left, a right-hand rotation about it tips the nose DOWN, // so "nose up" is reported negative. // // This CANNOT be corrected by any mount value. Re-referencing the earth frame // left-multiplies the reported orientation, whereas a mount right-multiplies it, // and no fixed right-multiplication reproduces a left-multiplication across all // attitudes. Both are applied, by applyFrames() in ImuMount.h. enum class ImuReference { Ned, Nwu, Enu }; // --- What the host asks the device to stream ------------------------------- // // These are a wire-format contract, not a preference: parseMTData below decodes // a fixed field order that follows from exactly these bits, so the two must be // changed together. That is what mtDataLen() and the static_assert enforce. // Deliberately not configurable from config.ini for the same reason. // OutputMode bits: which blocks the device streams, in this fixed order. namespace mtmode { inline constexpr uint16_t kTemperature = 0x0001; inline constexpr uint16_t kCalibrated = 0x0002; // acc + gyr + mag inline constexpr uint16_t kOrientation = 0x0004; inline constexpr uint16_t kAuxiliary = 0x0008; // AIN1/AIN2, inserted before orientation inline constexpr uint16_t kStatus = 0x0800; // one byte, before the sample counter } // namespace mtmode inline constexpr uint16_t kOutputMode = mtmode::kTemperature | mtmode::kCalibrated | mtmode::kOrientation; // OutputSettings bits: how those blocks are formatted. namespace mtsettings { inline constexpr uint32_t kTimestampMask = 0x00000003u; // bits 1:0 inline constexpr uint32_t kTimestampNone = 0x00000000u; inline constexpr uint32_t kTimestampSampleCounter = 0x00000001u; inline constexpr uint32_t kOrientationMask = 0x0000000Cu; // bits 3:2 inline constexpr uint32_t kOrientationQuaternion = 0x00000000u; // 16 bytes inline constexpr uint32_t kOrientationEuler = 0x00000004u; // 12 bytes inline constexpr uint32_t kOrientationMatrix = 0x00000008u; // 36 bytes // Per-channel disables: a SET bit turns the channel OFF. inline constexpr uint32_t kAccDisabled = 0x00000010u; inline constexpr uint32_t kGyrDisabled = 0x00000020u; inline constexpr uint32_t kMagDisabled = 0x00000040u; inline constexpr uint32_t kFormatMask = 0x00000300u; // bits 9:8 inline constexpr uint32_t kFormatFloat = 0x00000000u; inline constexpr uint32_t kFormatFixed1220 = 0x00000100u; // same length as float: // the length gate cannot // catch this one // Coordinate-system field: which earth reference the device reports against. // // Left at 0 - the device is NOT asked to change its reference. The correction is // made on the host instead (kMtiReference below + applyFrames), which is // deterministic, reversible, and unit-testable without hardware, where writing an // unverified bit into the unit's non-volatile memory would be none of those. The // field's position in OutputSettings is in any case not established for this // firmware. // // Keeping it 0 also keeps the two ends consistent: the host rewrites this whole // word every startup, so the device's reference is pinned to what 0x00000005 // selects and kMtiReference can describe it with certainty. If you ever do set // the coordinate system on the device, the readback check will flag the changed // word - at which point kMtiReference must be updated to match. inline constexpr uint32_t kCoordSysNed = 0x00000000u; } // namespace mtsettings inline constexpr uint32_t kOutputSettings = mtsettings::kTimestampSampleCounter | mtsettings::kOrientationEuler | mtsettings::kFormatFloat | mtsettings::kCoordSysNed; // Which earth frame this device measures orientation against, under the // OutputSettings above. // // The classic MTi's factory local frame is X North, Y West, Z Up. Because the // host rewrites OutputSettings on every startup, the device's reference is // whatever that word selects - i.e. deterministic, not "whatever was left in the // unit" - so this is a fact about kOutputSettings and belongs beside it. // // The consequence, and the reason this constant exists: a Z-up reference reports // pitch FALLING as the nose rises. ImuReference / applyFrames re-reference it to // NED on the host (see ImuMount.h); no [IMU] x/y/z value can, because the earth // frame is on the other side of the multiplication. // // If the yaw the Sensors view reports is 90 deg off a compass, this is ENU rather // than NWU - roll and pitch come out identical either way, so yaw is the only // thing that distinguishes them. Changing it here is the whole fix. inline constexpr ImuReference kMtiReference = ImuReference::Nwu; // The MTData payload length implied by a (mode, settings) pair, in the device's // fixed field order. Deriving it is what keeps kMTDataLen from drifting away // from the settings above: change a bit and the length follows, so the // static_assert fires instead of the parser silently mismatching at runtime. constexpr unsigned mtDataLen(uint16_t mode, uint32_t settings) { unsigned n = 0; if (mode & mtmode::kTemperature) n += 4; if (mode & mtmode::kCalibrated) { if (!(settings & mtsettings::kAccDisabled)) n += 12; if (!(settings & mtsettings::kGyrDisabled)) n += 12; if (!(settings & mtsettings::kMagDisabled)) n += 12; } if (mode & mtmode::kOrientation) { switch (settings & mtsettings::kOrientationMask) { case mtsettings::kOrientationQuaternion: n += 16; break; case mtsettings::kOrientationEuler: n += 12; break; case mtsettings::kOrientationMatrix: n += 36; break; default: break; // reserved encoding: length unknown } } if (mode & mtmode::kStatus) n += 1; if ((settings & mtsettings::kTimestampMask) == mtsettings::kTimestampSampleCounter) n += 2; return n; } // Expected payload with the config above: Temp(4) + Acc(12) + Gyr(12) + Mag(12) // + Euler(12) + SampleCounter(2). inline constexpr uint8_t kMTDataLen = static_cast(mtDataLen(kOutputMode, kOutputSettings)); static_assert(kMTDataLen == 54, "MTData layout changed; parseMTData must change with it"); // One fully decoded IMU reading. struct ImuSample { 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; // -180..180, exactly as the device reports it 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); // One way the device disagrees with what the host asked it for. struct ImuConfigMismatch { std::string field; // "output mode" / "output settings" uint32_t expected = 0; uint32_t actual = 0; bool answered = true; // false: the device never reported this field std::string detail; // decoded difference, e.g. "orientation is Quaternion, asked for Euler" // One line for a log or the UI, e.g. // output settings: asked for 0x00000005, device reports 0x00000105 (data // format is Fixed 12.20, asked for Float) std::string describe() const; }; // Compare what the device reports against what configure() asked it for. Empty // means the device is doing exactly what the host configured. // // This is the only thing standing between a device that ignored or rejected the // Set messages and a session that silently streams the wrong thing: the payload // length gate in parseMTData cannot see a wrong data format or a wrong // coordinate system, since neither changes the length. // // A field the device did not answer is reported as unverifiable (`answered = // false`) rather than as a match - older firmware that does not respond to the // Req* queries must not be mistaken for a device that agreed. Identity fields // (product code, device ID, firmware) and anything the host does not set (period, // XKF profile) are deliberately not compared. std::vector checkImuDeviceConfig(const ImuDeviceConfig& c); // Human text for an Error message (MID 0x42) payload byte - how the device says // it rejected a message outright, as opposed to quietly not applying it. Codes // are from the MT Low-Level Protocol error table; anything unrecognized comes // back as "unknown error 0xNN" rather than being swallowed. std::string mtiErrorText(uint8_t code); // Lower byte of the sum of all bytes from BID through the end of DATA. The CS // byte that makes the running total ≡ 0 (mod 256) is (256 - mtiChecksum) & 0xFF. uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len); // 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