Integration of IMU

CMakeLists.txt

@@ -52,6 +52,7 @@ add_library(fgc_core STATIC
   src/core/CommandParser.cpp
   src/core/HelpText.cpp
   src/core/DumpParser.cpp
+  src/core/MtiProtocol.cpp
   src/ui/UiSnapshot.cpp
   src/ui/HeadlessUi.cpp
   ini.c
@@ -73,6 +74,7 @@ set(FGC_SOURCES
   src/camera/JpegXlEncoder.cpp
   src/camera/ImagePipeline.cpp
   src/serial/SerialMotorController.cpp
+  src/serial/MtiImuSource.cpp
 )
 if(WITH_MQTT)
   list(APPEND FGC_SOURCES src/mqtt/MqttControlChannel.cpp)

config/config.example.ini

@@ -35,6 +35,16 @@ id_Cam4 =
 device = /dev/ttyACM0
 baud = 115200
 
+[IMU]
+; Xsens MTi orientation/inertial sensor. Wired to the LattePanda's RS-232 header
+; (an onboard hardware UART), so this is a stable /dev/ttyS* node - NOT a USB
+; device and NOT /dev/ttyUSB0-3 (those are the Quectel modem). Find it with
+;   ls /dev/ttyS*  ;  dmesg | grep -iE 'ttyS|LPSS|HSUART'  (often /dev/ttyS4)
+; Enable with [Features] enable_imu = true. The host reconfigures the MTi to a
+; Euler + calibrated 100 Hz stream at startup.
+device =
+baud = 115200
+
 [Motor]
 ; Degrees<->encoder-counts calibration for each axis. The firmware speaks only
 ; in absolute encoder counts; these map them to the heading/elevation degrees
@@ -76,8 +86,10 @@ output_dir =
 enable_mqtt = true
 enable_camera = true
 enable_serial = true
+enable_imu = false
 mock_camera = false
 mock_serial = false
+mock_imu = false
 
 [UI]
 ; Full-screen terminal dashboard (sectioned, colored, live status + log pane).

include/fgc/Config.h

@@ -43,8 +43,17 @@ struct FeaturesConfig {
     bool enable_mqtt   = true;
     bool enable_camera = true;
     bool enable_serial = true;
+    bool enable_imu    = false;  // Xsens MTi orientation/IMU (off by default)
     bool mock_camera   = false;  // use a simulated camera instead of Vimba X
     bool mock_serial   = false;  // use a simulated motor controller
+    bool mock_imu      = false;  // use a simulated IMU instead of the MTi
+};
+
+// [IMU]: Xsens MTi connected over the LattePanda's RS-232 UART (a hardware
+// /dev/ttyS* node, stable across reboots — not a USB device).
+struct ImuConfig {
+    std::string  device = "";       // e.g. /dev/ttyS4; empty => required when enabled
+    unsigned int baud   = 115200;
 };
 
 struct LoggingConfig {
@@ -78,6 +87,7 @@ struct AppConfig {
     UiConfig       ui;        // [UI] terminal dashboard toggle
     Geometry       geometry;  // [Motor] degrees<->counts maps (yaw + pitch)
     ScanConfig     scan;      // [Scan] grid source
+    ImuConfig      imu;       // [IMU] Xsens MTi serial device
 
     // Capture rate in images/second (derived from general.image_interval).
     double image_rate() const;

include/fgc/IImuSource.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "fgc/MtiProtocol.h"  // ImuSample
+
+#include <optional>
+
+namespace fgc {
+
+// Abstraction over the orientation/inertial sensor (Xsens MTi). Implemented by
+// MtiImuSource (RS-232/UART binary stream) and MockImuSource (synthetic). Runs
+// on its own thread; start() must not block the control loop.
+class IImuSource {
+public:
+    virtual ~IImuSource() = default;
+
+    virtual void start() = 0;
+    virtual void stop()  = 0;
+
+    // Whether a valid, recent reading is available.
+    virtual bool connected() const = 0;
+
+    // Latest reading, or nullopt if none/stale.
+    virtual std::optional<ImuSample> sample() = 0;
+};
+
+}  // namespace fgc

include/fgc/MtiImuSource.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "fgc/IImuSource.h"
+
+#include <memory>
+#include <string>
+
+namespace fgc {
+
+// Real IMU backend: reads the Xsens MTi binary MTData stream over a serial port
+// (RS-232 via the LattePanda UART). Configures the device to the Euler +
+// calibrated output at startup, then streams read-only. Boost.Asio detail is
+// hidden behind a pImpl (mirrors SerialMotorController).
+class MtiImuSource : public IImuSource {
+public:
+    MtiImuSource(std::string device, unsigned int baud);
+    ~MtiImuSource() override;
+
+    void                     start() override;
+    void                     stop() override;
+    bool                     connected() const override;
+    std::optional<ImuSample> sample() override;
+
+private:
+    struct Impl;
+    std::unique_ptr<Impl> impl_;
+};
+
+}  // namespace fgc

include/fgc/MtiProtocol.h

@@ -0,0 +1,93 @@
+#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;
+    float    pitch_deg = 0.f;
+    float    yaw_deg = 0.f;
+    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

include/fgc/mock/MockImuSource.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "fgc/IImuSource.h"
+#include "fgc/Logger.h"
+
+#include <chrono>
+#include <cmath>
+
+namespace fgc {
+
+// Simulated IMU for development without hardware: synthesizes a slowly varying,
+// physically plausible full sample (orientation sweeps, ~1 g on accZ, small
+// gyro/mag) so the Sensors panel and its expanded view animate.
+class MockImuSource : public IImuSource {
+public:
+    void start() override {
+        start_ = clock::now();
+        LOG_INFO << "[mock] IMU started";
+    }
+    void stop() override { LOG_INFO << "[mock] IMU stopped"; }
+
+    bool connected() const override { return true; }
+
+    std::optional<ImuSample> sample() override {
+        const double t = std::chrono::duration<double>(clock::now() - start_).count();
+        ImuSample s;
+        s.valid     = true;
+        s.roll_deg  = static_cast<float>(15.0 * std::sin(t * 0.5));
+        s.pitch_deg = static_cast<float>(10.0 * std::sin(t * 0.3 + 1.0));
+        s.yaw_deg   = static_cast<float>(std::fmod(t * 8.0, 360.0));      // slow spin
+        // Gravity tilts with roll/pitch; small free accel noise.
+        const double r = s.roll_deg * M_PI / 180.0, p = s.pitch_deg * M_PI / 180.0;
+        s.acc[0] = static_cast<float>(9.81 * -std::sin(p));
+        s.acc[1] = static_cast<float>(9.81 * std::sin(r) * std::cos(p));
+        s.acc[2] = static_cast<float>(9.81 * std::cos(r) * std::cos(p));
+        s.gyr[0] = static_cast<float>(0.13 * std::cos(t * 0.5));
+        s.gyr[1] = static_cast<float>(0.05 * std::cos(t * 0.3 + 1.0));
+        s.gyr[2] = 0.14f;
+        s.mag[0] = static_cast<float>(std::cos(s.yaw_deg * M_PI / 180.0));
+        s.mag[1] = static_cast<float>(-std::sin(s.yaw_deg * M_PI / 180.0));
+        s.mag[2] = 0.35f;
+        s.temp_c = static_cast<float>(24.5 + 0.5 * std::sin(t * 0.05));
+        s.sample_counter = static_cast<uint16_t>(static_cast<unsigned>(t * 100.0) & 0xFFFF);
+        return s;
+    }
+
+private:
+    using clock = std::chrono::steady_clock;
+    clock::time_point start_ = clock::now();
+};
+
+}  // namespace fgc

include/fgc/ui/UiSnapshot.h

@@ -109,6 +109,18 @@ struct DumpView {
     std::string text;
 };
 
+// Full Xsens MTi reading for the expanded Sensors view (units: acc m/s^2,
+// gyr rad/s, mag a.u., angles deg, temp °C).
+struct ImuView {
+    bool     present = false;
+    float    roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
+    float    acc[3] = {0, 0, 0};
+    float    gyr[3] = {0, 0, 0};
+    float    mag[3] = {0, 0, 0};
+    float    temp_c = 0;
+    unsigned sample_counter = 0;
+};
+
 struct UiSnapshot {
     HeaderView           header;
     GimbalView           gimbal;
@@ -117,6 +129,7 @@ struct UiSnapshot {
     ConnView             conn;
     std::vector<LogLine> log;
     DumpView             dump;
+    ImuView              imu;
 };
 
 // ---- Pure formatting helpers (unit-tested in tests/test_uisnapshot.cpp) ----

src/core/Application.cpp

@@ -6,12 +6,15 @@
 #include "fgc/HelpText.h"
 #include "fgc/ICameraSource.h"
 #include "fgc/IControlChannel.h"
+#include "fgc/IImuSource.h"
 #include "fgc/IMotorController.h"
 #include "fgc/ImagePipeline.h"
 #include "fgc/Logger.h"
+#include "fgc/MtiImuSource.h"
 #include "fgc/ScanGrid.h"
 #include "fgc/SerialMotorController.h"
 #include "fgc/mock/MockCameraSource.h"
+#include "fgc/mock/MockImuSource.h"
 #include "fgc/mock/MockMotorController.h"
 #include "fgc/mock/NullControlChannel.h"
 #include "fgc/ui/HeadlessUi.h"
@@ -20,6 +23,7 @@
 
 #include <atomic>
 #include <chrono>
+#include <cstdio>
 #include <memory>
 #include <mutex>
 #include <queue>
@@ -69,6 +73,7 @@ struct Application::Impl {
     std::unique_ptr<IControlChannel>  channel;
     std::unique_ptr<IMotorController> motor;
     std::unique_ptr<ICameraSource>    camera;
+    std::unique_ptr<IImuSource>       imu;
     std::unique_ptr<ImagePipeline>    pipeline;
     std::unique_ptr<CaptureScheduler> scheduler;
     std::unique_ptr<IUserInterface>   ui;
@@ -105,6 +110,16 @@ struct Application::Impl {
         return std::make_unique<SerialMotorController>(cfg.serial.device, cfg.serial.baud);
     }
 
+    std::unique_ptr<IImuSource> makeImu() {
+        if (!cfg.features.enable_imu) return nullptr;  // sensors panel stays "pending"
+        if (cfg.features.mock_imu) return std::make_unique<MockImuSource>();
+        if (cfg.imu.device.empty()) {
+            LOG_WARN << "IMU enabled but [IMU] device is empty; disabling IMU";
+            return nullptr;
+        }
+        return std::make_unique<MtiImuSource>(cfg.imu.device, cfg.imu.baud);
+    }
+
     std::unique_ptr<ICameraSource> makeCamera() {
         bool mock = opts.mock_camera.value_or(cfg.features.mock_camera);
 #if !FGC_WITH_VIMBA
@@ -192,8 +207,48 @@ struct Application::Impl {
             }
         }
 
-        // --- Sensors (DHT11 + MTi not integrated yet) ---
+        // --- Sensors: DHT11 still pending; MTi orientation/IMU live if present ---
         s.sensors = pendingSensorsView();
+        if (imu) {
+            auto fmt1 = [](float v) {
+                char b[24];
+                std::snprintf(b, sizeof(b), "%.1f", v);
+                return std::string(b);
+            };
+            auto setField = [&](size_t i, const std::string& v) {
+                if (i < s.sensors.fields.size()) {
+                    s.sensors.fields[i].value   = v;
+                    s.sensors.fields[i].present = true;
+                }
+            };
+            if (auto m = imu->sample()) {
+                // Full reading for the expanded view.
+                s.imu.present        = true;
+                s.imu.roll_deg       = m->roll_deg;
+                s.imu.pitch_deg      = m->pitch_deg;
+                s.imu.yaw_deg        = m->yaw_deg;
+                for (int i = 0; i < 3; ++i) {
+                    s.imu.acc[i] = m->acc[i];
+                    s.imu.gyr[i] = m->gyr[i];
+                    s.imu.mag[i] = m->mag[i];
+                }
+                s.imu.temp_c         = m->temp_c;
+                s.imu.sample_counter = m->sample_counter;
+                // Compact panel: Temp (field 0), Roll/Pitch/Yaw (2-4), status (5).
+                // Field units are already set, so values are bare numbers.
+                s.sensors.imu_present = true;
+                setField(0, fmt1(m->temp_c));
+                setField(2, fmt1(m->roll_deg));
+                setField(3, fmt1(m->pitch_deg));
+                setField(4, fmt1(m->yaw_deg));
+                if (s.sensors.fields.size() > 5) {
+                    s.sensors.fields[5].value   = "live";
+                    s.sensors.fields[5].present = true;
+                }
+            } else if (s.sensors.fields.size() > 5) {
+                s.sensors.fields[5].value = "no fix";
+            }
+        }
 
         // --- Camera / capture ---
         s.capture.present      = true;
@@ -411,6 +466,7 @@ struct Application::Impl {
         channel = makeChannel();
         motor   = makeMotor();
         camera  = makeCamera();
+        imu     = makeImu();
 
         if (!channel->connect())
             LOG_WARN << "Control channel not connected; continuing in degraded mode";
@@ -443,6 +499,7 @@ struct Application::Impl {
                                                        cfg.geometry, grid);
 
         motor->start();
+        if (imu) imu->start();
         camera->open();
         pipeline->start();
         channel->publishStatus(0);
@@ -481,6 +538,7 @@ struct Application::Impl {
         pipeline->stop();
         camera->stop();
         camera->close();
+        if (imu) imu->stop();
         motor->stop();
         channel->disconnect();
         return 0;

src/core/Config.cpp

@@ -106,8 +106,13 @@ AppConfig ConfigLoader::fromMap(const std::map<std::string, std::string>& kv) {
     cfg.features.enable_mqtt   = getBool(kv, "Features.enable_mqtt", cfg.features.enable_mqtt);
     cfg.features.enable_camera = getBool(kv, "Features.enable_camera", cfg.features.enable_camera);
     cfg.features.enable_serial = getBool(kv, "Features.enable_serial", cfg.features.enable_serial);
+    cfg.features.enable_imu    = getBool(kv, "Features.enable_imu", cfg.features.enable_imu);
     cfg.features.mock_camera   = getBool(kv, "Features.mock_camera", cfg.features.mock_camera);
     cfg.features.mock_serial   = getBool(kv, "Features.mock_serial", cfg.features.mock_serial);
+    cfg.features.mock_imu      = getBool(kv, "Features.mock_imu", cfg.features.mock_imu);
+
+    cfg.imu.device = get(kv, "IMU.device", cfg.imu.device);
+    cfg.imu.baud   = static_cast<unsigned>(getInt(kv, "IMU.baud", cfg.imu.baud));
 
     cfg.logging.level = get(kv, "Logging.level", cfg.logging.level);
     cfg.logging.trace = get(kv, "Logging.trace", cfg.logging.trace);

src/core/MtiProtocol.cpp

@@ -0,0 +1,121 @@
+#include "fgc/MtiProtocol.h"
+
+#include <cstring>
+
+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]));
+}
+
+// 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::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;
+    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

src/serial/MtiImuSource.cpp

@@ -0,0 +1,139 @@
+#include "fgc/MtiImuSource.h"
+
+#include "fgc/Logger.h"
+#include "fgc/MtiProtocol.h"
+
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <mutex>
+#include <thread>
+
+#include <boost/asio.hpp>
+#include <termios.h>
+#include <unistd.h>
+
+namespace fgc {
+
+struct MtiImuSource::Impl {
+    Impl(std::string dev, unsigned int b)
+        : device(std::move(dev)), baud(b), serial(io),
+          framer([this](uint8_t mid, const uint8_t* d, std::size_t n) { onFrame(mid, d, n); }) {}
+
+    using clock = std::chrono::steady_clock;
+
+    std::string              device;
+    unsigned int             baud;
+    boost::asio::io_context  io;
+    boost::asio::serial_port serial;
+    std::thread              io_thread;
+    std::array<uint8_t, 256> rxbuf{};
+
+    std::mutex        mutex;
+    ImuSample         latest;
+    clock::time_point last_rx{};
+    std::atomic<bool> open{false};
+
+    MtiFramer framer;
+    unsigned  bad_len_warned = 0;
+
+    void onFrame(uint8_t mid, const uint8_t* d, std::size_t n) {
+        if (mid != kMidMTData) return;  // ignore acks/other during streaming
+        auto s = parseMTData(mid, d, n);
+        if (!s) {
+            if (bad_len_warned++ < 1)
+                LOG_WARN << "MTi: unexpected MTData length " << n << " (expected "
+                         << int(kMTDataLen) << "); device may not be in the configured mode";
+            return;
+        }
+        std::lock_guard<std::mutex> lock(mutex);
+        latest  = *s;
+        last_rx = clock::now();
+    }
+
+    void doRead() {
+        serial.async_read_some(
+            boost::asio::buffer(rxbuf),
+            [this](const boost::system::error_code& ec, std::size_t n) {
+                if (ec) {
+                    if (ec != boost::asio::error::operation_aborted)
+                        LOG_WARN << "MTi serial read failed: " << ec.message();
+                    return;
+                }
+                framer.feed(rxbuf.data(), n);
+                doRead();
+            });
+    }
+
+    // Synchronous, best-effort config handshake (no ack parsing): runs on the
+    // calling thread BEFORE the io_thread starts, so there is no concurrent
+    // access to the serial port. Small delays let the device switch states.
+    void configure() {
+        using namespace std::chrono_literals;
+        auto write = [this](const std::vector<uint8_t>& m) {
+            boost::system::error_code ec;
+            boost::asio::write(serial, boost::asio::buffer(m), ec);
+            if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
+        };
+        write(msgGoToConfig());        std::this_thread::sleep_for(60ms);
+        write(msgSetOutputMode());     std::this_thread::sleep_for(60ms);
+        write(msgSetOutputSettings());  std::this_thread::sleep_for(60ms);
+        write(msgGoToMeasurement());    std::this_thread::sleep_for(60ms);
+        // Drop any pre-config (old-format) bytes so the framer starts clean.
+        ::tcflush(serial.native_handle(), TCIFLUSH);
+    }
+};
+
+MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
+    : impl_(std::make_unique<Impl>(std::move(device), baud)) {}
+
+MtiImuSource::~MtiImuSource() { stop(); }
+
+void MtiImuSource::start() {
+    namespace asio = boost::asio;
+    boost::system::error_code ec;
+    impl_->serial.open(impl_->device, ec);
+    if (ec) {
+        LOG_ERROR << "MTi: failed to open " << impl_->device << ": " << ec.message();
+        return;
+    }
+    impl_->serial.set_option(asio::serial_port_base::baud_rate(impl_->baud));
+    impl_->serial.set_option(asio::serial_port_base::character_size(8));
+    impl_->serial.set_option(asio::serial_port_base::parity(asio::serial_port_base::parity::none));
+    impl_->serial.set_option(
+        asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
+    impl_->serial.set_option(
+        asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));
+
+    impl_->open = true;
+    impl_->configure();
+    impl_->doRead();
+    impl_->io_thread = std::thread([this] { impl_->io.run(); });
+    LOG_INFO << "MTi IMU started on " << impl_->device << " @ " << impl_->baud;
+}
+
+void MtiImuSource::stop() {
+    if (!impl_) return;
+    impl_->io.stop();
+    if (impl_->io_thread.joinable()) impl_->io_thread.join();
+    boost::system::error_code ec;
+    if (impl_->serial.is_open()) impl_->serial.close(ec);
+    impl_->open = false;
+}
+
+bool MtiImuSource::connected() const {
+    if (!impl_->open) return false;
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    return impl_->latest.valid &&
+           (Impl::clock::now() - impl_->last_rx) < std::chrono::seconds(1);
+}
+
+std::optional<ImuSample> MtiImuSource::sample() {
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    if (!impl_->latest.valid ||
+        (Impl::clock::now() - impl_->last_rx) >= std::chrono::seconds(1))
+        return std::nullopt;
+    return impl_->latest;
+}
+
+}  // namespace fgc

src/ui/TuiUi.cpp

@@ -82,7 +82,8 @@ Element gimbalPanel(const GimbalView& g) {
 
 Element sensorsPanel(const SensorsView& s) {
     std::vector<Element> rows;
-    rows.push_back(text("pending integration") | dim);
+    rows.push_back(s.imu_present ? (text("MTi: live   (i to expand)") | color(Color::Green))
+                                 : (text("MTi: --   DHT11 pending") | dim));
     rows.push_back(separator());
     for (const auto& f : s.fields) {
         Element val = text(f.value + (f.unit.empty() ? "" : " " + f.unit));
@@ -324,6 +325,57 @@ Element gimbalDetailPanel(const GimbalView& g, const DumpView& dump) {
                   vbox({header, separator(), hbox(std::move(cols)) | flex}));
 }
 
+// Full-screen IMU view (toggled with 'i'): every MTi channel with units.
+Element imuDetailPanel(const ImuView& v) {
+    auto f2 = [](float x) {
+        char b[24];
+        std::snprintf(b, sizeof(b), "%.2f", x);
+        return std::string(b);
+    };
+    // One "LABEL (unit)  x=.. y=.. z=.." row for a 3-vector.
+    auto vecRow = [&](const std::string& label, const char* unit, const float xyz[3],
+                      Color c = Color::Default) {
+        return hbox({
+            text(label) | dim | size(WIDTH, EQUAL, 14),
+            text(std::string(unit)) | dim | size(WIDTH, EQUAL, 9),
+            text("x " + f2(xyz[0])) | color(c) | size(WIDTH, EQUAL, 12),
+            text("y " + f2(xyz[1])) | color(c) | size(WIDTH, EQUAL, 12),
+            text("z " + f2(xyz[2])) | color(c) | size(WIDTH, EQUAL, 12),
+        });
+    };
+    const float ori[3] = {v.roll_deg, v.pitch_deg, v.yaw_deg};
+
+    std::vector<Element> body;
+    if (!v.present) {
+        body.push_back(text("(no IMU data)") | color(Color::Red) | bold);
+        body.push_back(text("check [Features] enable_imu and [IMU] device, or --mock-imu") | dim);
+    } else {
+        body.push_back(hbox({
+            text("ORIENTATION") | dim | size(WIDTH, EQUAL, 14),
+            text("deg") | dim | size(WIDTH, EQUAL, 9),
+            text("roll " + f2(ori[0])) | bold | size(WIDTH, EQUAL, 14),
+            text("pitch " + f2(ori[1])) | bold | size(WIDTH, EQUAL, 14),
+            text("yaw " + f2(ori[2])) | bold | size(WIDTH, EQUAL, 14),
+        }));
+        body.push_back(separator());
+        body.push_back(vecRow("ACCEL", "m/s2", v.acc, Color::Cyan));
+        body.push_back(vecRow("RATE OF TURN", "rad/s", v.gyr, Color::Cyan));
+        body.push_back(vecRow("MAG FIELD", "a.u.", v.mag, Color::Cyan));
+        body.push_back(separator());
+        body.push_back(hbox({
+            text("TEMP") | dim | size(WIDTH, EQUAL, 14),
+            text(f2(v.temp_c) + " \xC2\xB0""C") | bold | size(WIDTH, EQUAL, 18),
+            text("sample #" + std::to_string(v.sample_counter)) | dim,
+        }));
+    }
+
+    Element status = v.present ? (text(" MTi live ") | color(Color::Green) | bold)
+                               : (text(" MTi offline ") | color(Color::Red) | bold);
+    return window(text(" IMU  (i/Esc:close) ") | bold | color(Color::Magenta),
+                  vbox({hbox({status, filler()}), separator(),
+                        vbox(std::move(body)) | flex}));
+}
+
 }  // namespace
 
 TuiUi::TuiUi() = default;
@@ -368,7 +420,7 @@ void TuiUi::refreshLoop() {
 void TuiUi::uiLoop() {
     std::string cmd_buffer;
     bool        command_mode = false;
-    enum class Overlay { None, Help, Gimbal };
+    enum class Overlay { None, Help, Gimbal, Sensors };
     Overlay     overlay      = Overlay::None;  // which takeover panel owns the main area
     int         help_sel     = 0;
     bool        gimbal_dump_requested = false;  // auto-pull a dump the first time
@@ -401,8 +453,8 @@ void TuiUi::uiLoop() {
         } else {
             bottom = hbox({
                 keyHint("s", "Start"), keyHint("x", "Stop"), keyHint("h", "Home"),
-                keyHint("r", "Reset"), keyHint("g", "Gimbal"), keyHint(":", "Cmd"),
+                keyHint("r", "Reset"), keyHint("g", "Gimbal"), keyHint("i", "IMU"),
-                keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
+                keyHint(":", "Cmd"), keyHint("?", "Help"), filler(), keyHint("q", "Quit"),
             });
         }
 
@@ -415,6 +467,8 @@ void TuiUi::uiLoop() {
             case Overlay::Gimbal:
                 return vbox({header, separator(),
                              gimbalDetailPanel(s.gimbal, s.dump) | flex, bottom});
+            case Overlay::Sensors:
+                return vbox({header, separator(), imuDetailPanel(s.imu) | flex, bottom});
             default:
                 return vbox({header, separator(), top, middle, logPanel(s.log) | flex, bottom});
         }
@@ -463,6 +517,10 @@ void TuiUi::uiLoop() {
             if (sink_) sink_("dump");
             return true;
         }
+        if (c == "i") {
+            overlay = (overlay == Overlay::Sensors) ? Overlay::None : Overlay::Sensors;
+            return true;
+        }
         if (overlay == Overlay::Help) {  // vim-style section nav while help is open
             if (c == "j") { help_sel = (help_sel + 1) % n; return true; }
             if (c == "k") { help_sel = (help_sel - 1 + n) % n; return true; }

tests/CMakeLists.txt

@@ -28,6 +28,9 @@ add_executable(fgc_tests
   test_geometry.cpp
   test_scangrid.cpp
   test_uisnapshot.cpp
+  test_mtiprotocol.cpp
+  test_dumpparser.cpp
+  test_helptext.cpp
 )
 target_link_libraries(fgc_tests PRIVATE fgc_core doctest::doctest)
 

tests/test_dumpparser.cpp

@@ -0,0 +1,131 @@
+#include <doctest/doctest.h>
+
+#include "fgc/DumpParser.h"
+
+#include <algorithm>
+#include <string>
+
+using namespace fgc;
+
+namespace {
+
+// A realistic two-axis firmware DUMP block (the on-wire format from
+// firmware/src/motor.cpp printDump): header, then per axis a state line and a
+// TMC register line, bracketed by DUMP BEGIN / DUMP END.
+const char* kDump =
+    "DUMP BEGIN build=11dd3ce-dirty uptime=11740067 mcusr=0x01 free_ram=1852\n"
+    "DUMP Y state=3 hsub=10 enabled=1 lim_neg=-82919 lim_pos=98687 hold_target=-90 "
+    "speed=50000 eeprom_restored=0 has_encoder=1\n"
+    "DUMP Y TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
+    "RAMPMODE=0x00000000 XACTUAL=0xFFFFFFA6 VACTUAL=0x00000000 XTARGET=0xFFFFFFA6 "
+    "SW_MODE=0x000008A0 RAMP_STAT=0x00001680 X_ENC=0xFFFFFF7F ENC_STATUS=0x00000002 "
+    "CHOPCONF=0x00410043 DRV_STATUS=0x80084000 PWM_SCALE=0x00000011 PWM_AUTO=0x003F0039\n"
+    "DUMP P state=3 hsub=10 enabled=1 lim_neg=495183 lim_pos=1069042 hold_target=500183 "
+    "speed=150000 eeprom_restored=0 has_encoder=1\n"
+    "DUMP P TMC GCONF=0x0000000C GSTAT=0x00000000 IOIN=0x30000008 TSTEP=0x000FFFFF "
+    "RAMPMODE=0x00000000 XACTUAL=0x0007A1C8 VACTUAL=0x00000000 XTARGET=0x0007A1C8 "
+    "SW_MODE=0x000008A3 RAMP_STAT=0x00001688 X_ENC=0x0007A231 ENC_STATUS=0x00000002 "
+    "CHOPCONF=0x00410043 DRV_STATUS=0x80084000 PWM_SCALE=0x00010011 PWM_AUTO=0x0021003C\n"
+    "DUMP END\n";
+
+bool has(const std::vector<std::string>& v, const std::string& s) {
+    return std::find(v.begin(), v.end(), s) != v.end();
+}
+
+const DumpAxis* axis(const DumpData& d, char a) {
+    for (const auto& ax : d.axes)
+        if (ax.axis == a) return &ax;
+    return nullptr;
+}
+
+}  // namespace
+
+TEST_CASE("parseDump decodes the header") {
+    DumpData d = parseDump(kDump);
+    REQUIRE(d.valid);
+    CHECK(d.build == "11dd3ce-dirty");
+    CHECK(d.uptime_ms == 11740067);
+    CHECK(d.mcusr == 0x01u);
+    CHECK(d.free_ram == 1852);
+    CHECK(has(d.reset_flags, "PORF (power-on)"));
+    REQUIRE(d.axes.size() == 2);
+}
+
+TEST_CASE("parseDump decodes the per-axis state line") {
+    DumpData d = parseDump(kDump);
+    const DumpAxis* y = axis(d, 'Y');
+    REQUIRE(y != nullptr);
+    CHECK(y->state_name == "READY");   // state=3
+    CHECK(y->enabled);
+    CHECK(y->has_encoder);
+    CHECK_FALSE(y->eeprom_restored);
+    CHECK(y->lim_neg == -82919);
+    CHECK(y->lim_pos == 98687);
+    CHECK(y->hold_target == -90);
+    CHECK(y->speed == 50000);
+    CHECK(y->hsub == 10);
+
+    const DumpAxis* p = axis(d, 'P');
+    REQUIRE(p != nullptr);
+    CHECK(p->lim_neg == 495183);
+    CHECK(p->lim_pos == 1069042);
+}
+
+TEST_CASE("parseDump keeps all 16 TMC registers (regression: TMC-line whitespace)") {
+    // The "DUMP Y TMC ..." line was once misclassified due to a leading-space
+    // slice, dropping every register. Guard that all of them survive.
+    DumpData d = parseDump(kDump);
+    const DumpAxis* y = axis(d, 'Y');
+    REQUIRE(y != nullptr);
+    for (const char* r : {"GCONF", "GSTAT", "IOIN", "TSTEP", "RAMPMODE", "XACTUAL",
+                          "VACTUAL", "XTARGET", "SW_MODE", "RAMP_STAT", "X_ENC",
+                          "ENC_STATUS", "CHOPCONF", "DRV_STATUS", "PWM_SCALE", "PWM_AUTO"}) {
+        CHECK_MESSAGE(y->regs.count(r) == 1, "missing register ", r);
+    }
+    CHECK(y->regs.at("RAMP_STAT") == "0x00001680");
+    CHECK(y->regs.at("DRV_STATUS") == "0x80084000");
+}
+
+TEST_CASE("parseDump decodes status-register bit flags") {
+    DumpData d = parseDump(kDump);
+    const DumpAxis* y = axis(d, 'Y');
+    REQUIRE(y != nullptr);
+
+    // DRV_STATUS = 0x80084000 -> bit31 stst, bit14 stealth; CS_ACTUAL=(>>16)&0x1F=8.
+    CHECK(y->drv_status == 0x80084000u);
+    CHECK(has(y->drv_flags, "stst"));
+    CHECK(has(y->drv_flags, "stealth"));
+    CHECK(y->cs_actual == 8);
+    CHECK(y->sg_result == 0);
+
+    // GSTAT = 0 -> no flags (placeholder "-").
+    CHECK(has(y->gstat_flags, "-"));
+
+    // RAMP_STAT = 0x1680 -> bit7 event_pos_reached, bit9 position_reached, bit10 vzero.
+    CHECK(y->ramp_stat == 0x00001680u);
+    CHECK(has(y->ramp_flags, "event_pos_reached"));
+    CHECK(has(y->ramp_flags, "position_reached"));
+    CHECK(has(y->ramp_flags, "vzero"));
+}
+
+TEST_CASE("parseDump tolerates a corrupted/merged DUMP END (best-effort)") {
+    // The host detects DUMP END anywhere in a line; a clean header still parses
+    // even when later content is mangled — but a block with no BEGIN/END is invalid.
+    CHECK_FALSE(parseDump("").valid);
+    CHECK_FALSE(parseDump("garbage with no markers\n").valid);
+    CHECK_FALSE(parseDump("DUMP Y state=3\nno end marker\n").valid);
+}
+
+TEST_CASE("formatDump renders valid and invalid dumps") {
+    auto bad = formatDump(DumpData{});
+    REQUIRE(bad.size() == 1);
+    CHECK(bad[0].find("no firmware dump") != std::string::npos);
+
+    auto good = formatDump(parseDump(kDump));
+    std::string joined;
+    for (const auto& l : good) joined += l + "\n";
+    CHECK(joined.find("build=11dd3ce-dirty") != std::string::npos);
+    CHECK(joined.find("[Y]") != std::string::npos);
+    CHECK(joined.find("[P]") != std::string::npos);
+    CHECK(joined.find("RAMP_STAT") != std::string::npos);
+}

tests/test_helptext.cpp

@@ -0,0 +1,62 @@
+#include <doctest/doctest.h>
+
+#include "fgc/HelpText.h"
+
+#include <string>
+
+using namespace fgc;
+
+namespace {
+std::string join(const std::vector<std::string>& v) {
+    std::string s;
+    for (const auto& l : v) s += l + "\n";
+    return s;
+}
+}  // namespace
+
+TEST_CASE("helpCatalog is well-formed") {
+    const auto& cat = helpCatalog();
+    REQUIRE_FALSE(cat.empty());
+    for (const auto& sec : cat) {
+        CHECK_FALSE(sec.title.empty());
+        CHECK_FALSE(sec.entries.empty());
+        for (const auto& e : sec.entries) {
+            CHECK_FALSE(e.syntax.empty());
+            CHECK_FALSE(e.summary.empty());
+        }
+    }
+}
+
+TEST_CASE("renderHelp() with no topic lists every section and entry") {
+    std::string out = join(renderHelp(""));
+    CHECK(out.find("help <topic>") != std::string::npos);  // the usage hint
+    // Each catalog section title and entry syntax should appear.
+    for (const auto& sec : helpCatalog()) {
+        CHECK(out.find(sec.title) != std::string::npos);
+        for (const auto& e : sec.entries)
+            CHECK(out.find(e.syntax) != std::string::npos);
+    }
+    // A couple of the commands added this session.
+    CHECK(out.find("goto") != std::string::npos);
+    CHECK(out.find("dump") != std::string::npos);
+}
+
+TEST_CASE("renderHelp(<section>) expands that section with detail") {
+    std::string out = join(renderHelp("positioning"));
+    CHECK(out.find("goto <yaw_deg> <pitch_deg>") != std::string::npos);
+    // Detail lines (example) are only emitted in topic mode.
+    CHECK(out.find("goto 30 -10") != std::string::npos);
+}
+
+TEST_CASE("renderHelp(<verb>) matches a single command, case-insensitively") {
+    std::string lower = join(renderHelp("goto"));
+    std::string upper = join(renderHelp("GOTO"));
+    CHECK(lower.find("goto <yaw_deg> <pitch_deg>") != std::string::npos);
+    CHECK(upper.find("goto <yaw_deg> <pitch_deg>") != std::string::npos);
+    CHECK(lower == upper);
+}
+
+TEST_CASE("renderHelp(unknown) reports no match") {
+    std::string out = join(renderHelp("definitely-not-a-command"));
+    CHECK(out.find("No help topic") != std::string::npos);
+}

tests/test_mtiprotocol.cpp

@@ -0,0 +1,109 @@
+#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 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());
+    }
+}