295 lines
10 KiB
C++
295 lines
10 KiB
C++
#include "fgc/MtiProtocol.h"
|
|
|
|
#include <cctype>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
#include <string>
|
|
#include <utility>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
// Big-endian readers (MTi is big-endian; host x86 is little-endian).
|
|
float beFloat(const uint8_t* p) {
|
|
uint32_t u = (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) |
|
|
(uint32_t(p[2]) << 8) | uint32_t(p[3]);
|
|
float f;
|
|
std::memcpy(&f, &u, sizeof(f));
|
|
return f;
|
|
}
|
|
|
|
uint16_t beU16(const uint8_t* p) {
|
|
return static_cast<uint16_t>((uint16_t(p[0]) << 8) | uint16_t(p[1]));
|
|
}
|
|
|
|
uint32_t beU32(const uint8_t* p) {
|
|
return (uint32_t(p[0]) << 24) | (uint32_t(p[1]) << 16) | (uint32_t(p[2]) << 8) |
|
|
uint32_t(p[3]);
|
|
}
|
|
|
|
// Append a big-endian value to a byte vector.
|
|
void putBE(std::vector<uint8_t>& v, uint16_t x) {
|
|
v.push_back(static_cast<uint8_t>(x >> 8));
|
|
v.push_back(static_cast<uint8_t>(x & 0xFF));
|
|
}
|
|
void putBE(std::vector<uint8_t>& v, uint32_t x) {
|
|
v.push_back(static_cast<uint8_t>((x >> 24) & 0xFF));
|
|
v.push_back(static_cast<uint8_t>((x >> 16) & 0xFF));
|
|
v.push_back(static_cast<uint8_t>((x >> 8) & 0xFF));
|
|
v.push_back(static_cast<uint8_t>(x & 0xFF));
|
|
}
|
|
|
|
} // namespace
|
|
|
|
uint8_t mtiChecksum(const uint8_t* from_bid, std::size_t len) {
|
|
unsigned sum = 0;
|
|
for (std::size_t i = 0; i < len; ++i) sum += from_bid[i];
|
|
return static_cast<uint8_t>(sum & 0xFF);
|
|
}
|
|
|
|
std::vector<uint8_t> mtiMessage(uint8_t mid, const std::vector<uint8_t>& data) {
|
|
std::vector<uint8_t> m;
|
|
m.reserve(5 + data.size());
|
|
m.push_back(kMtiPreamble);
|
|
m.push_back(kMtiBid);
|
|
m.push_back(mid);
|
|
m.push_back(static_cast<uint8_t>(data.size()));
|
|
m.insert(m.end(), data.begin(), data.end());
|
|
// Checksum covers BID..DATA; the CS byte makes the total ≡ 0 (mod 256).
|
|
uint8_t s = mtiChecksum(m.data() + 1, m.size() - 1);
|
|
m.push_back(static_cast<uint8_t>((0x100 - s) & 0xFF));
|
|
return m;
|
|
}
|
|
|
|
std::vector<uint8_t> msgGoToConfig() { return mtiMessage(kMidGoToConfig); }
|
|
std::vector<uint8_t> msgGoToMeasurement() { return mtiMessage(kMidGoToMeasurement); }
|
|
|
|
std::vector<uint8_t> msgSetOutputMode() {
|
|
std::vector<uint8_t> d;
|
|
putBE(d, kOutputMode);
|
|
return mtiMessage(kMidSetOutputMode, d);
|
|
}
|
|
|
|
std::vector<uint8_t> msgSetOutputSettings() {
|
|
std::vector<uint8_t> d;
|
|
putBE(d, kOutputSettings);
|
|
return mtiMessage(kMidSetOutputSettings, d);
|
|
}
|
|
|
|
std::vector<uint8_t> msgSetNoRotation(uint16_t seconds) {
|
|
std::vector<uint8_t> d;
|
|
putBE(d, seconds);
|
|
return mtiMessage(kMidSetNoRotation, d);
|
|
}
|
|
|
|
std::vector<uint8_t> msgResetOrientation(uint16_t code) {
|
|
std::vector<uint8_t> d;
|
|
putBE(d, code);
|
|
return mtiMessage(kMidResetOrientation, d);
|
|
}
|
|
|
|
std::vector<uint8_t> msgSetFilterProfile(uint16_t profile) {
|
|
std::vector<uint8_t> d;
|
|
putBE(d, profile);
|
|
return mtiMessage(kMidReqFilterProfile, d); // same MID; non-empty data => "set"
|
|
}
|
|
|
|
int pickNoMagProfile(const std::vector<ImuFilterProfile>& profiles) {
|
|
auto lower = [](std::string s) {
|
|
for (char& c : s) c = static_cast<char>(std::tolower((unsigned char)c));
|
|
return s;
|
|
};
|
|
int vru = -1;
|
|
for (const auto& p : profiles) {
|
|
const std::string l = lower(p.label);
|
|
if (l.find("nomag") != std::string::npos || l.find("no_mag") != std::string::npos ||
|
|
l.find("no mag") != std::string::npos)
|
|
return p.type; // explicit no-magnetometer
|
|
if (vru < 0 && l.find("vru") != std::string::npos) // gyro-tracked heading
|
|
vru = p.type;
|
|
}
|
|
return vru;
|
|
}
|
|
|
|
std::vector<uint8_t> msgReqProductCode() { return mtiMessage(kMidReqProductCode); }
|
|
std::vector<uint8_t> msgReqDID() { return mtiMessage(kMidReqDID); }
|
|
std::vector<uint8_t> msgReqFWRev() { return mtiMessage(kMidReqFWRev); }
|
|
std::vector<uint8_t> msgReqPeriod() { return mtiMessage(kMidReqPeriod); }
|
|
std::vector<uint8_t> msgReqOutputMode() { return mtiMessage(kMidSetOutputMode); }
|
|
std::vector<uint8_t> msgReqOutputSettings() { return mtiMessage(kMidSetOutputSettings); }
|
|
std::vector<uint8_t> msgReqFilterProfile() { return mtiMessage(kMidReqFilterProfile); }
|
|
std::vector<uint8_t> msgReqAvailFilterProfiles(){ return mtiMessage(kMidReqAvailFilterProf); }
|
|
|
|
bool applyImuConfigAck(ImuDeviceConfig& c, uint8_t mid, const uint8_t* d, std::size_t n) {
|
|
switch (mid) {
|
|
case kMidDeviceID:
|
|
if (n < 4) return false;
|
|
c.device_id = beU32(d);
|
|
c.has_device_id = true;
|
|
c.valid = true;
|
|
return true;
|
|
|
|
case kMidProductCode: {
|
|
// ASCII string, possibly space-padded; trim trailing spaces/NULs.
|
|
std::size_t end = n;
|
|
while (end > 0 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
|
|
c.product_code.assign(reinterpret_cast<const char*>(d), end);
|
|
c.valid = true;
|
|
return true;
|
|
}
|
|
|
|
case kMidFirmwareRev: {
|
|
// MAJOR MINOR REV [BUILDNR(4) SCMREF(4)] — older firmware sends only 3.
|
|
if (n < 3) return false;
|
|
char buf[48];
|
|
if (n >= 7) {
|
|
uint32_t build = beU32(d + 3);
|
|
std::snprintf(buf, sizeof(buf), "%u.%u.%u build %u", d[0], d[1], d[2], build);
|
|
} else {
|
|
std::snprintf(buf, sizeof(buf), "%u.%u.%u", d[0], d[1], d[2]);
|
|
}
|
|
c.firmware = buf;
|
|
c.valid = true;
|
|
return true;
|
|
}
|
|
|
|
case kMidReqPeriodAck:
|
|
if (n < 2) return false;
|
|
c.period = beU16(d);
|
|
c.has_period = true;
|
|
c.valid = true;
|
|
return true;
|
|
|
|
case kMidSetOutputModeAck: { // 0xD1, ack to ReqOutputMode
|
|
if (n < 2) return false;
|
|
uint16_t m = beU16(d);
|
|
c.output_mode = m;
|
|
c.out_temperature = m & 0x0001;
|
|
c.out_calibrated = m & 0x0002;
|
|
c.out_orientation = m & 0x0004;
|
|
c.out_auxiliary = m & 0x0008;
|
|
c.out_status = m & 0x0800;
|
|
c.has_output_mode = true;
|
|
c.valid = true;
|
|
return true;
|
|
}
|
|
|
|
case kMidSetOutputSettingsAck: { // 0xD3, ack to ReqOutputSettings
|
|
if (n < 4) return false;
|
|
uint32_t s = beU32(d);
|
|
c.output_settings = s;
|
|
switch (s & 0x0003) {
|
|
case 0x1: c.timestamp_mode = "Sample counter"; break;
|
|
default: c.timestamp_mode = "None"; break;
|
|
}
|
|
switch ((s >> 2) & 0x0003) {
|
|
case 0x0: c.orientation_mode = "Quaternion"; break;
|
|
case 0x1: c.orientation_mode = "Euler"; break;
|
|
case 0x2: c.orientation_mode = "Matrix"; break;
|
|
default: c.orientation_mode = "?"; break;
|
|
}
|
|
// Bits 4/5/6: 1 = output DISABLED.
|
|
c.acc_enabled = !(s & 0x0010);
|
|
c.gyr_enabled = !(s & 0x0020);
|
|
c.mag_enabled = !(s & 0x0040);
|
|
c.data_format = ((s >> 8) & 0x0003) == 0x1 ? "Fixed 12.20" : "Float";
|
|
c.has_output_settings = true;
|
|
c.valid = true;
|
|
return true;
|
|
}
|
|
|
|
case kMidReqFilterProfileAck: // VERSION, FILTERPROFILE(type)
|
|
if (n < 2) return false;
|
|
c.scenario_version = d[0];
|
|
c.scenario_type = d[1];
|
|
c.has_scenario = true;
|
|
c.valid = true;
|
|
return true;
|
|
|
|
case kMidAvailFilterProf: {
|
|
// Repeating 22-byte records: TYPE(1) VERSION(1) LABEL(20, space-padded).
|
|
c.available_profiles.clear();
|
|
for (std::size_t o = 0; o + 22 <= n; o += 22) {
|
|
ImuFilterProfile p;
|
|
p.type = d[o];
|
|
p.version = d[o + 1];
|
|
if (p.type == 0) continue; // empty slot
|
|
std::size_t end = o + 22;
|
|
while (end > o + 2 && (d[end - 1] == ' ' || d[end - 1] == 0)) --end;
|
|
p.label.assign(reinterpret_cast<const char*>(d + o + 2), end - (o + 2));
|
|
c.available_profiles.push_back(std::move(p));
|
|
}
|
|
c.valid = true;
|
|
return true;
|
|
}
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void finalizeImuConfig(ImuDeviceConfig& c) {
|
|
if (c.has_period && c.period > 0)
|
|
c.sample_rate_hz = 115200.0f / static_cast<float>(c.period);
|
|
if (c.has_scenario) {
|
|
for (const auto& p : c.available_profiles) {
|
|
if (p.type == c.scenario_type) { c.scenario_label = p.label; break; }
|
|
}
|
|
}
|
|
}
|
|
|
|
std::optional<ImuSample> parseMTData(uint8_t mid, const uint8_t* data, std::size_t len) {
|
|
if (mid != kMidMTData || len != kMTDataLen) return std::nullopt;
|
|
ImuSample s;
|
|
std::size_t o = 0;
|
|
s.temp_c = beFloat(data + o); o += 4;
|
|
for (int i = 0; i < 3; ++i) { s.acc[i] = beFloat(data + o); o += 4; }
|
|
for (int i = 0; i < 3; ++i) { s.gyr[i] = beFloat(data + o); o += 4; }
|
|
for (int i = 0; i < 3; ++i) { s.mag[i] = beFloat(data + o); o += 4; }
|
|
s.roll_deg = beFloat(data + o); o += 4;
|
|
s.pitch_deg = beFloat(data + o); o += 4;
|
|
s.yaw_deg = beFloat(data + o); o += 4;
|
|
// Report yaw as a 0..360 heading rather than the MTi's native -180..180.
|
|
if (s.yaw_deg < 0.f) s.yaw_deg += 360.f;
|
|
s.sample_counter = beU16(data + o); o += 2;
|
|
s.valid = true;
|
|
return s;
|
|
}
|
|
|
|
void MtiFramer::feed(const uint8_t* p, std::size_t n) {
|
|
for (std::size_t i = 0; i < n; ++i) {
|
|
uint8_t b = p[i];
|
|
switch (state_) {
|
|
case S::Pre:
|
|
if (b == kMtiPreamble) state_ = S::Bid;
|
|
break;
|
|
case S::Bid:
|
|
// After PRE we expect BID; otherwise resync (allow back-to-back PRE).
|
|
if (b == kMtiBid) { sum_ = b; state_ = S::Mid; }
|
|
else if (b == kMtiPreamble) { /* stay */ }
|
|
else state_ = S::Pre;
|
|
break;
|
|
case S::Mid:
|
|
mid_ = b; sum_ += b; state_ = S::Len;
|
|
break;
|
|
case S::Len:
|
|
len_ = b; sum_ += b; data_.clear();
|
|
state_ = (len_ == 0) ? S::Cs : S::Data;
|
|
break;
|
|
case S::Data:
|
|
data_.push_back(b); sum_ += b;
|
|
if (data_.size() == len_) state_ = S::Cs;
|
|
break;
|
|
case S::Cs:
|
|
sum_ += b;
|
|
if ((sum_ & 0xFF) == 0 && sink_)
|
|
sink_(mid_, data_.data(), data_.size());
|
|
state_ = S::Pre;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace fgc
|