314 lines
12 KiB
C++
314 lines
12 KiB
C++
#include <doctest/doctest.h>
|
|
|
|
#include "fgc/MtiProtocol.h"
|
|
|
|
#include <cstring>
|
|
#include <vector>
|
|
|
|
using namespace fgc;
|
|
|
|
namespace {
|
|
void putBEFloat(std::vector<uint8_t>& v, float f) {
|
|
uint32_t u;
|
|
std::memcpy(&u, &f, 4);
|
|
v.push_back(static_cast<uint8_t>(u >> 24));
|
|
v.push_back(static_cast<uint8_t>(u >> 16));
|
|
v.push_back(static_cast<uint8_t>(u >> 8));
|
|
v.push_back(static_cast<uint8_t>(u));
|
|
}
|
|
|
|
// Sum of all bytes from BID through CS must be ≡ 0 (mod 256) for a valid frame.
|
|
bool frameChecksumOk(const std::vector<uint8_t>& m) {
|
|
unsigned s = 0;
|
|
for (size_t i = 1; i < m.size(); ++i) s += m[i];
|
|
return (s & 0xFF) == 0;
|
|
}
|
|
} // namespace
|
|
|
|
TEST_CASE("config messages are well-formed with correct payloads") {
|
|
auto cfg = msgGoToConfig();
|
|
auto mode = msgSetOutputMode();
|
|
auto set = msgSetOutputSettings();
|
|
auto meas = msgGoToMeasurement();
|
|
|
|
for (const auto& m : {cfg, mode, set, meas}) {
|
|
CHECK(m[0] == kMtiPreamble);
|
|
CHECK(m[1] == kMtiBid);
|
|
CHECK(frameChecksumOk(m));
|
|
}
|
|
// GoToConfig / GoToMeasurement: no data.
|
|
CHECK(cfg[2] == kMidGoToConfig);
|
|
CHECK(cfg[3] == 0);
|
|
CHECK(meas[2] == kMidGoToMeasurement);
|
|
CHECK(meas[3] == 0);
|
|
// SetOutputMode = 0x0007 (Temp|Calibrated|Orientation), 2-byte big-endian.
|
|
CHECK(mode[2] == kMidSetOutputMode);
|
|
CHECK(mode[3] == 2);
|
|
CHECK(mode[4] == 0x00);
|
|
CHECK(mode[5] == 0x07);
|
|
// SetOutputSettings = 0x00000005 (Euler + sample counter), 4-byte big-endian.
|
|
CHECK(set[2] == kMidSetOutputSettings);
|
|
CHECK(set[3] == 4);
|
|
CHECK(set[4] == 0x00);
|
|
CHECK(set[5] == 0x00);
|
|
CHECK(set[6] == 0x00);
|
|
CHECK(set[7] == 0x05);
|
|
}
|
|
|
|
TEST_CASE("framer decodes a combined MTData frame into a full sample") {
|
|
std::vector<uint8_t> d;
|
|
putBEFloat(d, 24.5f); // temp
|
|
putBEFloat(d, 0.10f); putBEFloat(d, -0.20f); putBEFloat(d, 9.81f); // acc
|
|
putBEFloat(d, 0.01f); putBEFloat(d, 0.02f); putBEFloat(d, -0.03f); // gyr
|
|
putBEFloat(d, 0.45f); putBEFloat(d, -0.88f); putBEFloat(d, 0.21f); // mag
|
|
putBEFloat(d, -1.5f); putBEFloat(d, 3.25f); putBEFloat(d, 187.0f); // roll/pitch/yaw
|
|
d.push_back(0x12); d.push_back(0x34); // sample counter
|
|
REQUIRE(d.size() == kMTDataLen);
|
|
|
|
auto frame = mtiMessage(kMidMTData, d);
|
|
|
|
ImuSample got;
|
|
bool fired = false;
|
|
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
|
if (auto s = parseMTData(mid, p, n)) { got = *s; fired = true; }
|
|
});
|
|
|
|
// Leading noise must not break resync.
|
|
const uint8_t noise[] = {0x00, 0xAB, 0xFA, 0x01};
|
|
fr.feed(noise, sizeof(noise));
|
|
fr.feed(frame.data(), frame.size());
|
|
|
|
REQUIRE(fired);
|
|
CHECK(got.valid);
|
|
CHECK(got.temp_c == doctest::Approx(24.5f));
|
|
CHECK(got.acc[2] == doctest::Approx(9.81f));
|
|
CHECK(got.gyr[0] == doctest::Approx(0.01f));
|
|
CHECK(got.mag[1] == doctest::Approx(-0.88f));
|
|
CHECK(got.roll_deg == doctest::Approx(-1.5f));
|
|
CHECK(got.pitch_deg == doctest::Approx(3.25f));
|
|
CHECK(got.yaw_deg == doctest::Approx(187.0f));
|
|
CHECK(got.sample_counter == 0x1234);
|
|
}
|
|
|
|
TEST_CASE("framer reassembles frames split across feeds and back-to-back frames") {
|
|
// Real serial reads arrive in arbitrary chunks; the framer must not depend
|
|
// on frame boundaries aligning with feed() calls.
|
|
std::vector<uint8_t> d;
|
|
putBEFloat(d, 7.5f); // temp
|
|
d.resize(kMTDataLen, 0); // rest of the 54-byte payload = 0
|
|
auto frame = mtiMessage(kMidMTData, d);
|
|
|
|
int count = 0;
|
|
float last_temp = 0;
|
|
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
|
if (auto s = parseMTData(mid, p, n)) { ++count; last_temp = s->temp_c; }
|
|
});
|
|
|
|
// Two frames fed one byte at a time (worst-case fragmentation).
|
|
for (uint8_t b : frame) fr.feed(&b, 1);
|
|
for (uint8_t b : frame) fr.feed(&b, 1);
|
|
CHECK(count == 2);
|
|
CHECK(last_temp == doctest::Approx(7.5f));
|
|
|
|
// Two frames concatenated in a single feed.
|
|
std::vector<uint8_t> two = frame;
|
|
two.insert(two.end(), frame.begin(), frame.end());
|
|
count = 0;
|
|
MtiFramer fr2([&](uint8_t mid, const uint8_t* p, size_t n) {
|
|
if (parseMTData(mid, p, n)) ++count;
|
|
});
|
|
fr2.feed(two.data(), two.size());
|
|
CHECK(count == 2);
|
|
}
|
|
|
|
TEST_CASE("parseMTData reports yaw as a 0..360 heading") {
|
|
auto frameWithYaw = [](float yaw) {
|
|
std::vector<uint8_t> d;
|
|
for (int i = 0; i < 10; ++i) putBEFloat(d, 0.f); // temp + acc3 + gyr3 + mag3
|
|
putBEFloat(d, 0.f); // roll
|
|
putBEFloat(d, 0.f); // pitch
|
|
putBEFloat(d, yaw); // yaw
|
|
d.push_back(0); d.push_back(0); // counter
|
|
return mtiMessage(kMidMTData, d);
|
|
};
|
|
ImuSample got;
|
|
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
|
if (auto s = parseMTData(mid, p, n)) got = *s;
|
|
});
|
|
|
|
auto fn = frameWithYaw(-90.f); fr.feed(fn.data(), fn.size());
|
|
CHECK(got.yaw_deg == doctest::Approx(270.0f));
|
|
auto fp = frameWithYaw(45.f); fr.feed(fp.data(), fp.size());
|
|
CHECK(got.yaw_deg == doctest::Approx(45.0f));
|
|
auto fb = frameWithYaw(-179.f); fr.feed(fb.data(), fb.size());
|
|
CHECK(got.yaw_deg == doctest::Approx(181.0f));
|
|
}
|
|
|
|
TEST_CASE("config-readback query builders are well-formed requests (empty data)") {
|
|
struct Q { std::vector<uint8_t> m; uint8_t mid; };
|
|
Q qs[] = {
|
|
{msgReqProductCode(), kMidReqProductCode},
|
|
{msgReqDID(), kMidReqDID},
|
|
{msgReqFWRev(), kMidReqFWRev},
|
|
{msgReqPeriod(), kMidReqPeriod},
|
|
{msgReqOutputMode(), kMidSetOutputMode},
|
|
{msgReqOutputSettings(), kMidSetOutputSettings},
|
|
{msgReqFilterProfile(), kMidReqFilterProfile},
|
|
{msgReqAvailFilterProfiles(), kMidReqAvailFilterProf},
|
|
};
|
|
for (const auto& q : qs) {
|
|
CHECK(q.m[0] == kMtiPreamble);
|
|
CHECK(q.m[1] == kMtiBid);
|
|
CHECK(q.m[2] == q.mid);
|
|
CHECK(q.m[3] == 0); // request => empty data field
|
|
CHECK(frameChecksumOk(q.m));
|
|
}
|
|
}
|
|
|
|
TEST_CASE("applyImuConfigAck decodes each ack type") {
|
|
ImuDeviceConfig c;
|
|
|
|
SUBCASE("DeviceID is a 32-bit big-endian serial") {
|
|
uint8_t d[] = {0x00, 0x99, 0x0A, 0xBC};
|
|
CHECK(applyImuConfigAck(c, kMidDeviceID, d, sizeof(d)));
|
|
CHECK(c.has_device_id);
|
|
CHECK(c.device_id == 0x00990ABCu);
|
|
}
|
|
|
|
SUBCASE("ProductCode trims trailing spaces") {
|
|
const char* s = "MTi-28A53G35 ";
|
|
CHECK(applyImuConfigAck(c, kMidProductCode,
|
|
reinterpret_cast<const uint8_t*>(s), 15));
|
|
CHECK(c.product_code == "MTi-28A53G35");
|
|
}
|
|
|
|
SUBCASE("FirmwareRev with build number") {
|
|
uint8_t d[] = {2, 8, 1, 0, 0, 0, 25}; // 2.8.1 build 25
|
|
CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d)));
|
|
CHECK(c.firmware == "2.8.1 build 25");
|
|
}
|
|
|
|
SUBCASE("FirmwareRev short form (major.minor.rev only)") {
|
|
uint8_t d[] = {1, 2, 3};
|
|
CHECK(applyImuConfigAck(c, kMidFirmwareRev, d, sizeof(d)));
|
|
CHECK(c.firmware == "1.2.3");
|
|
}
|
|
|
|
SUBCASE("Period yields 100 Hz from 0x0480") {
|
|
uint8_t d[] = {0x04, 0x80}; // 1152 => 115200/1152 = 100 Hz
|
|
CHECK(applyImuConfigAck(c, kMidReqPeriodAck, d, sizeof(d)));
|
|
finalizeImuConfig(c);
|
|
CHECK(c.has_period);
|
|
CHECK(c.sample_rate_hz == doctest::Approx(100.0f));
|
|
}
|
|
|
|
SUBCASE("OutputMode 0x0007 = Temp + Calibrated + Orientation") {
|
|
uint8_t d[] = {0x00, 0x07};
|
|
CHECK(applyImuConfigAck(c, kMidSetOutputModeAck, d, sizeof(d)));
|
|
CHECK(c.out_temperature);
|
|
CHECK(c.out_calibrated);
|
|
CHECK(c.out_orientation);
|
|
CHECK_FALSE(c.out_auxiliary);
|
|
}
|
|
|
|
SUBCASE("OutputSettings 0x00000005 = Euler + sample counter + float, all channels") {
|
|
uint8_t d[] = {0x00, 0x00, 0x00, 0x05};
|
|
CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d)));
|
|
CHECK(c.orientation_mode == "Euler");
|
|
CHECK(c.timestamp_mode == "Sample counter");
|
|
CHECK(c.data_format == "Float");
|
|
CHECK(c.acc_enabled);
|
|
CHECK(c.gyr_enabled);
|
|
CHECK(c.mag_enabled);
|
|
}
|
|
|
|
SUBCASE("OutputSettings with disabled mag and fixed-point format") {
|
|
// bit6 set (disable mag), output format bits 9:8 = 01 (Fixed 12.20).
|
|
uint32_t s = 0x05 | 0x40 | 0x100;
|
|
uint8_t d[] = {uint8_t(s >> 24), uint8_t(s >> 16), uint8_t(s >> 8), uint8_t(s)};
|
|
CHECK(applyImuConfigAck(c, kMidSetOutputSettingsAck, d, sizeof(d)));
|
|
CHECK(c.acc_enabled);
|
|
CHECK(c.gyr_enabled);
|
|
CHECK_FALSE(c.mag_enabled);
|
|
CHECK(c.data_format == "Fixed 12.20");
|
|
}
|
|
|
|
SUBCASE("unknown MID is ignored") {
|
|
uint8_t d[] = {0};
|
|
CHECK_FALSE(applyImuConfigAck(c, 0x99, d, sizeof(d)));
|
|
CHECK_FALSE(c.valid);
|
|
}
|
|
}
|
|
|
|
TEST_CASE("scenario label resolves against the available-profiles list") {
|
|
ImuDeviceConfig c;
|
|
// AvailableFilterProfiles: two 22-byte records (type, version, 20-byte label).
|
|
std::vector<uint8_t> d;
|
|
auto addProfile = [&](uint8_t type, uint8_t ver, const std::string& label) {
|
|
d.push_back(type);
|
|
d.push_back(ver);
|
|
std::string padded = label;
|
|
padded.resize(20, ' ');
|
|
d.insert(d.end(), padded.begin(), padded.end());
|
|
};
|
|
addProfile(39, 11, "General");
|
|
addProfile(40, 11, "High_mag_dep");
|
|
CHECK(applyImuConfigAck(c, kMidAvailFilterProf, d.data(), d.size()));
|
|
REQUIRE(c.available_profiles.size() == 2);
|
|
CHECK(c.available_profiles[0].label == "General");
|
|
CHECK(c.available_profiles[1].label == "High_mag_dep");
|
|
|
|
// Current filter profile ack: VERSION, FILTERPROFILE(type).
|
|
uint8_t fp[] = {11, 40};
|
|
CHECK(applyImuConfigAck(c, kMidReqFilterProfileAck, fp, sizeof(fp)));
|
|
finalizeImuConfig(c);
|
|
CHECK(c.has_scenario);
|
|
CHECK(c.scenario_type == 40);
|
|
CHECK(c.scenario_label == "High_mag_dep");
|
|
}
|
|
|
|
TEST_CASE("a full ack stream decodes through the framer") {
|
|
// Concatenate realistic acks and run them through MtiFramer, mirroring how
|
|
// the live handshake collects them.
|
|
ImuDeviceConfig c;
|
|
MtiFramer fr([&](uint8_t mid, const uint8_t* p, size_t n) {
|
|
applyImuConfigAck(c, mid, p, n);
|
|
});
|
|
auto feed = [&](const std::vector<uint8_t>& m) { fr.feed(m.data(), m.size()); };
|
|
|
|
feed(mtiMessage(kMidProductCode,
|
|
{'M','T','i','-','2','8'}));
|
|
feed(mtiMessage(kMidDeviceID, {0x00, 0x99, 0x0A, 0xBC}));
|
|
feed(mtiMessage(kMidSetOutputModeAck, {0x00, 0x07}));
|
|
feed(mtiMessage(kMidSetOutputSettingsAck, {0x00, 0x00, 0x00, 0x05}));
|
|
feed(mtiMessage(kMidReqPeriodAck, {0x04, 0x80}));
|
|
feed(mtiMessage(kMidReqFilterProfileAck, {11, 39}));
|
|
finalizeImuConfig(c);
|
|
|
|
CHECK(c.valid);
|
|
CHECK(c.product_code == "MTi-28");
|
|
CHECK(c.device_id == 0x00990ABCu);
|
|
CHECK(c.out_orientation);
|
|
CHECK(c.orientation_mode == "Euler");
|
|
CHECK(c.sample_rate_hz == doctest::Approx(100.0f));
|
|
CHECK(c.scenario_type == 39);
|
|
}
|
|
|
|
TEST_CASE("framer rejects a bad checksum and a wrong-length payload") {
|
|
std::vector<uint8_t> d(kMTDataLen, 0);
|
|
auto frame = mtiMessage(kMidMTData, d);
|
|
|
|
SUBCASE("corrupt checksum") {
|
|
auto bad = frame;
|
|
bad.back() ^= 0xFF;
|
|
bool fired = false;
|
|
MtiFramer fr([&](uint8_t, const uint8_t*, size_t) { fired = true; });
|
|
fr.feed(bad.data(), bad.size());
|
|
CHECK_FALSE(fired);
|
|
}
|
|
SUBCASE("wrong-length MTData parses to nullopt") {
|
|
std::vector<uint8_t> shortData(10, 0);
|
|
CHECK_FALSE(parseMTData(kMidMTData, shortData.data(), shortData.size()).has_value());
|
|
}
|
|
}
|