361 lines
18 KiB
C++
361 lines
18 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;
|
|
|
|
// 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<uint8_t>(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<ImuFilterProfile> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<ImuFilterProfile>& 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);
|
|
|
|
// 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);
|
|
|
|
// 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
|