178 lines
7.9 KiB
C++
178 lines
7.9 KiB
C++
#pragma once
|
|
|
|
#include <cstdint>
|
|
#include <functional>
|
|
#include <optional>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
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;
|
|
|
|
// 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<ImuFilterProfile> available_profiles;
|
|
};
|
|
|
|
// Config-readback query builders (empty data field => "request", not "set").
|
|
std::vector<uint8_t> msgReqProductCode();
|
|
std::vector<uint8_t> msgReqDID();
|
|
std::vector<uint8_t> msgReqFWRev();
|
|
std::vector<uint8_t> msgReqPeriod();
|
|
std::vector<uint8_t> msgReqOutputMode(); // kMidSetOutputMode, len 0
|
|
std::vector<uint8_t> msgReqOutputSettings(); // kMidSetOutputSettings, len 0
|
|
std::vector<uint8_t> msgReqFilterProfile();
|
|
std::vector<uint8_t> 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<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data = {});
|
|
|
|
// The four config messages for the orientation+calibrated Euler stream.
|
|
std::vector<uint8_t> msgGoToConfig();
|
|
std::vector<uint8_t> msgSetOutputMode(); // kOutputMode
|
|
std::vector<uint8_t> msgSetOutputSettings(); // kOutputSettings
|
|
std::vector<uint8_t> msgGoToMeasurement();
|
|
|
|
// Decode an MTData payload (the DATA bytes, big-endian) into an ImuSample.
|
|
// Returns nullopt if mid != MTData or len != kMTDataLen.
|
|
std::optional<ImuSample> 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<void(uint8_t mid, const uint8_t* data, std::size_t len)>;
|
|
|
|
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<uint8_t> data_;
|
|
unsigned sum_ = 0; // running checksum sum (BID..DATA)
|
|
};
|
|
|
|
} // namespace fgc
|