94 lines
3.9 KiB
C++
94 lines
3.9 KiB
C++
#pragma once
|
|
|
|
#include <cstdint>
|
|
#include <functional>
|
|
#include <optional>
|
|
#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;
|
|
|
|
// 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;
|
|
};
|
|
|
|
// 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
|