Enotrium · Aiden Young (ajyds) · Jun 27, 2026 · Jun 16, 2026 · Jun 16, 2026 · Jun 27, 2026
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -2,6 +2,16 @@
 
 All notable changes to the FPGA Event-Based Drone Collision Avoidance project.
 
+## [Unreleased]
+
+### Added
+- ARM MAVLink v2 `.bin` telemetry logger (`arm/mavlink_sd_logger.h`): buffered, crash-safe SD-card-over-SPI logging of the 100 Hz control loop as CRC-checked MAVLink v2 frames (`CA_TELEM` record per tick), with optional ground-station-decodable `NAMED_VALUE_*` output in `MAVLINK_AVAILABLE` builds
+- Telemetry logging integrated into `arm/drone_main.cpp` control loop (per-tick log, periodic `fsync`, flush-on-shutdown)
+- `arm/test_mavlink_logger.cpp`: 11 unit tests (CRC known-answer, framing, field round-trip, buffering, crash/write-failure handling) wired into `make test` and CI
+- Control-loop latency profiler (`arm/loop_profiler.h`) with per-stage instrumentation in `arm/drone_main.cpp`, enabled via `make profile` (`-DPROFILE_LOOP`); zero overhead in production builds
+- Link-time optimization (`-flto`) added to the ARM release build (`arm/Makefile` `RELEASE_FLAGS`)
+- `docs/loop-latency.md`: worst-case 100 Hz control-loop latency analysis (measured ~482 µs / 4.8 % of budget) and the SD `fsync` real-hardware caveat
+
 ## [1.0.0] — 2026-06-08
 
 ### Added

diff --git a/Makefile b/Makefile
@@ -60,6 +60,7 @@ test-arm:
 	@echo "=== ARM C++ Unit Tests ==="
 	cd arm && $(MAKE) sim
 	cd test && g++ -std=c++17 -O2 -I.. -I../arm -o test_arm_cp test_arm_collision_predictor.cpp -lpthread && ./test_arm_cp
+	cd arm && $(MAKE) test-logger
 
 test-fpga:
 	@echo "=== FPGA C++ Testbench ==="

diff --git a/arm/Makefile b/arm/Makefile
@@ -23,8 +23,11 @@ ARM_ARCH_FLAGS = -march=armv8-a -mcpu=cortex-a53
 # Debug build
 DEBUG_FLAGS = -g -O0 -DDEBUG
 
-# Release build  
-RELEASE_FLAGS = -O3 -DNDEBUG -ffast-math -funroll-loops
+# Release build
+# -flto enables link-time optimization (cross-TU inlining + dead-code removal).
+# It is carried in both the compile and link steps because the hardware/sim
+# link rules pass $(CXXFLAGS), which includes RELEASE_FLAGS.
+RELEASE_FLAGS = -O3 -DNDEBUG -ffast-math -funroll-loops -flto
 
 CXXFLAGS = $(CXXFLAGS_COMMON) $(ARM_ARCH_FLAGS) $(RELEASE_FLAGS)
 
@@ -46,6 +49,7 @@ INCLUDES = -I. -I$(FREERTOS_PATH)/include -I$(FREERTOS_PATH)/portable/GCC/ARM_CR
 TARGET = drone_control
 TARGET_SIM = drone_control_sim
 TEST_TARGET = test_arm_cp
+TEST_LOGGER_TARGET = test_mavlink_logger
 
 # Source files (header-only for collision_predictor.h, evasion_controller.h)
 SOURCES = drone_main.cpp
@@ -56,7 +60,7 @@ OBJECTS = $(SOURCES:.cpp=.o)
 # ---------------------------------------------------------------------------
 # Build rules
 # ---------------------------------------------------------------------------
-.PHONY: all sim freertos test clean
+.PHONY: all sim freertos profile test test-cp test-logger clean
 
 # Default: hardware target
 all: $(TARGET)
@@ -76,6 +80,16 @@ $(TARGET_SIM): $(OBJECTS)
 	g++ $(CXXFLAGS) $(INCLUDES) -o $@ $^ -lpthread
 	@echo "Built $@ (simulation mode — no FPGA required)"
 
+# Profiling build: simulation binary with per-stage loop instrumentation
+# (-DPROFILE_LOOP). Compiled in one shot (no shared .o) so it never collides
+# with the cached object from `make sim`. Run it, let it spin, Ctrl+C, and it
+# prints the worst-case per-stage latency table. Uses the same RELEASE_FLAGS
+# (incl. -flto) as the flight binary so the numbers reflect production codegen.
+profile:
+	g++ $(CXXFLAGS_COMMON) $(RELEASE_FLAGS) $(SIM_FLAGS) -DPROFILE_LOOP $(INCLUDES) \
+		-o $(TARGET_SIM)_prof drone_main.cpp -lpthread
+	@echo "Built $(TARGET_SIM)_prof — run ./$(TARGET_SIM)_prof, Ctrl+C for the latency report"
+
 # FreeRTOS bare-metal target (Cortex-R5)
 freertos: CXXFLAGS += $(FREERTOS_FLAGS)
 freertos: INCLUDES += -I$(FREERTOS_PATH)/include -I$(FREERTOS_PATH)/portable/GCC/ARM_CR5
@@ -93,17 +107,31 @@ freertos: $(OBJECTS)
 	$(CXX) $(CXXFLAGS) $(INCLUDES) -c $< -o $@
 
 # Unit tests (native g++ — runs on dev machine, no ARM cross-compiler needed)
-# Output: ./test_arm_cp  exit 0 = all pass, 1 = any failure
-test: $(TEST_TARGET)
+# `make test` builds and runs every suite; exit 0 = all pass, 1 = any failure.
+test: test-cp test-logger
+
+# Kalman tracker integration tests
+test-cp: $(TEST_TARGET)
 	./$(TEST_TARGET)
 
 $(TEST_TARGET): test_arm_cp.cpp collision_predictor.h evasion_controller.h kalman_tracker.h
 	g++ -std=c++17 -O0 -g -Wall -Wextra -I. -o $(TEST_TARGET) test_arm_cp.cpp
 	@echo "Built $(TEST_TARGET)"
 
+# MAVLink v2 .bin telemetry logger tests
+test-logger: $(TEST_LOGGER_TARGET)
+	./$(TEST_LOGGER_TARGET)
+
+$(TEST_LOGGER_TARGET): test_mavlink_logger.cpp mavlink_sd_logger.h mavlink_bridge.h \
+                       evasion_controller.h collision_predictor.h
+	g++ -std=c++17 -O0 -g -Wall -Wextra -I. -o $(TEST_LOGGER_TARGET) test_mavlink_logger.cpp
+	@echo "Built $(TEST_LOGGER_TARGET)"
+
 # Clean
 clean:
-	rm -f $(TARGET) $(TARGET_SIM) $(TARGET)_rtos.elf $(TEST_TARGET) *.o
+	rm -f $(TARGET) $(TARGET_SIM) $(TARGET_SIM)_prof $(TARGET)_rtos.elf \
+		$(TEST_TARGET) $(TEST_LOGGER_TARGET) *.o
+	rm -rf *.dSYM
 	@echo "Clean complete"
 
 # ---------------------------------------------------------------------------
@@ -127,6 +155,7 @@ help:
 	@echo "Targets:"
 	@echo "  make          Build binary for Zynq ARM Linux"
 	@echo "  make sim      Build simulation binary (run on desktop, no FPGA needed)"
+	@echo "  make profile  Build sim binary with control-loop latency profiling"
 	@echo "  make freertos Build bare-metal binary for Zynq R5 with FreeRTOS"
 	@echo "  make test     Build and run unit tests (native g++, no FPGA needed)"
 	@echo "  make deploy   SCP binary to Zynq board"

diff --git a/arm/drone_main.cpp b/arm/drone_main.cpp
@@ -27,6 +27,9 @@
 #include "evasion_controller.h"
 #include "fpga_interface.h"
 #include "kalman_tracker.h"
+#include "mavlink_bridge.h"
+#include "mavlink_sd_logger.h"
+#include "loop_profiler.h"
 
 // FreeRTOS or bare-metal alternatives
 #ifdef FREERTOS
@@ -48,6 +51,74 @@ using namespace drone;
 static std::atomic<bool> g_running{true};
 static std::atomic<bool> g_motors_armed{false};
 
+// ---------------------------------------------------------------------------
+// Monotonic clocks (ms for scheduling, µs for loop-latency measurement)
+// ---------------------------------------------------------------------------
+#ifdef FREERTOS
+static uint32_t now_ms() { return (uint32_t)(xTaskGetTickCount() * portTICK_PERIOD_MS); }
+static uint64_t now_us() { return (uint64_t)now_ms() * 1000ULL; }
+#else
+static uint32_t now_ms() {
+    using namespace std::chrono;
+    static const auto t0 = steady_clock::now();
+    return (uint32_t)duration_cast<milliseconds>(steady_clock::now() - t0).count();
+}
+static uint64_t now_us() {
+    using namespace std::chrono;
+    static const auto t0 = steady_clock::now();
+    return (uint64_t)duration_cast<microseconds>(steady_clock::now() - t0).count();
+}
+#endif
+
+// ---------------------------------------------------------------------------
+// Control-loop latency profiling (compiled in only with -DPROFILE_LOOP).
+//
+// PROF_DECL() at the top of the loop snapshots the clock; each PROF_LAP(stage)
+// records the time since the previous lap and advances the cursor; PROF_TOTAL()
+// records the whole-iteration compute time (sleep excluded). In production
+// builds every macro expands to a no-op, so the flight binary is untouched.
+// ---------------------------------------------------------------------------
+#ifdef PROFILE_LOOP
+static drone::LoopProfiler g_profiler;
+#define PROF_DECL()              \
+    uint64_t _p_prev = now_us(); \
+    const uint64_t _p_start = _p_prev
+#define PROF_LAP(stage)                                                              \
+    do {                                                                             \
+        uint64_t _p_now = now_us();                                                  \
+        g_profiler.record(drone::LoopProfiler::stage, (uint32_t)(_p_now - _p_prev)); \
+        _p_prev = _p_now;                                                            \
+    } while (0)
+#define PROF_TOTAL() g_profiler.record(drone::LoopProfiler::kTotal, (uint32_t)(now_us() - _p_start))
+#else
+#define PROF_DECL() ((void)0)
+#define PROF_LAP(stage) ((void)0)
+#define PROF_TOTAL() ((void)0)
+#endif
+
+// ---------------------------------------------------------------------------
+// SD-card-over-SPI sink for the telemetry logger.
+//
+// On Zynq Linux the SD card is a mounted FAT filesystem, so a buffered FILE*
+// stands in for the FatFs FIL handle: fwrite == f_write, fflush == f_sync.
+// On bare-metal these callbacks would wrap FatFs f_write()/f_sync() directly.
+// ---------------------------------------------------------------------------
+static bool sd_write_cb(void* ctx, const uint8_t* data, uint32_t len) {
+    FILE* f = static_cast<FILE*>(ctx);
+    return f != nullptr && fwrite(data, 1, len, f) == len;
+}
+static bool sd_sync_cb(void* ctx) {
+    FILE* f = static_cast<FILE*>(ctx);
+    if (f == nullptr || fflush(f) != 0) return false;  // fflush: libc -> OS only
+#ifndef FREERTOS
+    // fsync forces the SD/MMC controller to commit, so a power loss can't lose
+    // telemetry the kernel was still buffering. (On bare-metal FatFs the
+    // SyncFn would call f_sync() instead.)
+    if (fsync(fileno(f)) != 0) return false;
+#endif
+    return true;
+}
+
 // ---------------------------------------------------------------------------
 // Telemetry / logging
 // ---------------------------------------------------------------------------
@@ -118,9 +189,29 @@ int main(int /*argc*/, char** /*argv*/) {
     // Persistent track list — lives across loop iterations, passed by ref into tracker.update()
     std::vector<TrackedObject> tracks;
 
+    // Initialize MAVLink v2 .bin telemetry logger (SD card over SPI).
+    // Path is overridable via DRONE_TELEMETRY_BIN; defaults to the cwd so the
+    // sim build works without an SD mount. Telemetry is non-critical: if the
+    // file can't be opened we log a warning and fly without it.
+    const char* telem_path = getenv("DRONE_TELEMETRY_BIN");
+    if (telem_path == nullptr) telem_path = "telemetry.bin";
+    FILE* telem_file = fopen(telem_path, "wb");
+    MavlinkSdLogger::Config log_cfg;  // 2 KiB buffer, 1 s sync interval
+    MavlinkSdLogger logger(log_cfg, sd_write_cb, sd_sync_cb, telem_file);
+    if (telem_file != nullptr) {
+        logger.open(now_ms());
+        printf("Telemetry logging to %s (MAVLink v2 .bin)\n", telem_path);
+    } else {
+        printf("WARNING: could not open %s — telemetry logging disabled\n", telem_path);
+    }
+    // Vehicle state from the flight controller (populated once the MAVLink
+    // bridge RX path is wired into this loop; zero-initialized until then).
+    VehicleState vehicle{};
+
     // Control loop timing
     const int CONTROL_FREQ_HZ = 100;  // 100Hz control loop
     const int CONTROL_PERIOD_MS = 1000 / CONTROL_FREQ_HZ;
+    uint64_t prev_tick_us = 0;  // for per-iteration loop-period measurement
 
     // Enable FPGA pipeline (motors initially disarmed)
     fpga.enable(false);
@@ -137,14 +228,18 @@ int main(int /*argc*/, char** /*argv*/) {
     uint32_t arm_countdown = CONTROL_FREQ_HZ * 2;  // Arm after 2 seconds
 
     while (g_running) {
+        PROF_DECL();
+
         // Step 1: Read flow vectors from FPGA encoder
         int n_events = fpga.read_flow_vectors(events, 4096);
+        PROF_LAP(kReadFlow);
 
         // Step 2: Run collision prediction
         ThreatAssessment assessment;
         if (n_events > 0) {
             assessment = predictor.assess(events);
         }
+        PROF_LAP(kPredict);
 
         // Step 2b: Kalman tracker — always called, even with zero detections.
         // Empty detections = no new tracks created, but existing tracks age toward pruning.
@@ -167,17 +262,38 @@ int main(int /*argc*/, char** /*argv*/) {
         }
         // threat_detected now reflects confirmed track presence, not raw noisy detections
         assessment.threat_detected = !confirmed.empty();
+        PROF_LAP(kTrack);
 
         // Step 3: Compute evasion command
         EvasionCommand cmd = evader.compute_command(assessment);
+        PROF_LAP(kEvade);
 
         // Step 4: Send velocity command to FPGA PWM module
         if (g_motors_armed) {
             fpga.write_velocity_command(cmd);
         }
+        PROF_LAP(kWriteCmd);
 
         // Step 5: Telemetry
         log_telemetry(assessment, cmd, tracks.size(), confirmed.size());
+        PROF_LAP(kConsoleLog);
+
+        // Step 5b: Persist a MAVLink v2 record to the SD card. loop_dt_us is
+        // the measured period of the previous iteration (0 on the first tick).
+        if (logger.is_open()) {
+            uint64_t tick_us = now_us();
+            uint32_t loop_dt_us = (prev_tick_us == 0) ? 0u : (uint32_t)(tick_us - prev_tick_us);
+            prev_tick_us = tick_us;
+            logger.log_control_tick(now_ms(), loop_dt_us, cmd, assessment, (uint16_t)tracks.size(),
+                                    (uint16_t)confirmed.size(), g_motors_armed.load(), vehicle);
+            // Stock NAMED_VALUE_* for ground stations (active only in
+            // MAVLINK_AVAILABLE builds; throttled to the configured rate).
+            logger.log_standard_tick(now_ms(), cmd, assessment, (uint16_t)tracks.size(),
+                                     (uint16_t)confirmed.size());
+            PROF_LAP(kBinLog);
+            logger.update(now_ms());  // periodic flush/f_sync (crash safety)
+            PROF_LAP(kFlush);
+        }
 
         // Auto-arm after startup countdown
         if (!g_motors_armed && arm_countdown > 0) {
@@ -189,13 +305,30 @@ int main(int /*argc*/, char** /*argv*/) {
             }
         }
 
+        PROF_TOTAL();
+
         // Step 6: Sleep for control period
         SLEEP_MS(CONTROL_PERIOD_MS);
     }
 
     // Graceful shutdown
     printf("\nDisabling motors and FPGA pipeline...\n");
     fpga.disable();
+
+    // Flush and close the telemetry log so no buffered records are lost.
+    if (logger.is_open()) {
+        logger.close();
+        const auto& s = logger.stats();
+        printf("Telemetry: %u msgs, %llu bytes, %u flushes, %u dropped%s\n", s.messages_logged,
+               (unsigned long long)s.bytes_written, s.flush_count, s.dropped_messages,
+               s.error ? " (write errors occurred)" : "");
+    }
+    if (telem_file != nullptr) fclose(telem_file);
+
+#ifdef PROFILE_LOOP
+    g_profiler.report(stdout);
+#endif
+
     printf("Shutdown complete.\n");
 
     return 0;

diff --git a/arm/loop_profiler.h b/arm/loop_profiler.h
@@ -0,0 +1,78 @@
+// arm/loop_profiler.h — Per-Stage Latency Profiler for the Control Loop
+//
+// Minimal, zero-heap instrument for the 100 Hz control loop in drone_main.cpp.
+// Records call count, summed time, and worst-case (max) time per stage in
+// microseconds, then prints a table on shutdown.
+//
+// It carries no clock of its own: the caller passes already-measured elapsed
+// microseconds to record(). That keeps it free of any platform timing
+// dependency (steady_clock vs. FreeRTOS ticks) and trivially testable.
+//
+// Active only in PROFILE_LOOP builds (`make profile`). Production builds never
+// instantiate it, so it adds zero runtime cost to the flight binary.
+
+#pragma once
+
+#include <cstdint>
+#include <cstdio>
+
+namespace drone {
+
+class LoopProfiler {
+   public:
+    // Stages of one control-loop iteration, in execution order. kTotal spans
+    // the whole loop body excluding the control-period sleep.
+    enum Stage {
+        kReadFlow = 0,  // FPGA flow-vector readback
+        kPredict,       // CollisionPredictor::assess
+        kTrack,         // KalmanTracker::update + confirmed-track rebuild
+        kEvade,         // EvasionController::compute_command
+        kWriteCmd,      // FPGA PWM velocity write
+        kConsoleLog,    // human-readable stdout telemetry
+        kBinLog,        // MAVLink frame build + buffer staging
+        kFlush,         // logger.update(): periodic fwrite + fsync (SD commit)
+        kTotal,         // whole loop body, excluding the control-period sleep
+        kStageCount
+    };
+
+    // Fold one elapsed-time sample (microseconds) into a stage's accumulators.
+    void record(Stage s, uint32_t dt_us) {
+        Acc& a = acc_[s];
+        a.count++;
+        a.sum_us += dt_us;
+        if (dt_us > a.max_us) a.max_us = dt_us;
+    }
+
+    // Worst-case (max) microseconds seen for a stage; 0 if never recorded.
+    uint32_t worst_us(Stage s) const { return acc_[s].max_us; }
+
+    // Print a count / mean / max table plus the 100 Hz budget headroom.
+    void report(FILE* out) const {
+        static const char* kNames[kStageCount] = {"read_flow", "predict",   "track",
+                                                  "evade",     "write_cmd", "console_log",
+                                                  "bin_log",   "flush",     "TOTAL"};
+        fprintf(out, "\n=== Control-loop latency profile (microseconds) ===\n");
+        fprintf(out, "%-12s %10s %12s %10s\n", "stage", "count", "mean_us", "max_us");
+        for (int i = 0; i < kStageCount; ++i) {
+            const Acc& a = acc_[i];
+            double mean = a.count ? static_cast<double>(a.sum_us) / a.count : 0.0;
+            fprintf(out, "%-12s %10llu %12.2f %10u\n", kNames[i],
+                    static_cast<unsigned long long>(a.count), mean, a.max_us);
+        }
+        const uint32_t budget_us = 10000;  // 100 Hz period
+        uint32_t worst = acc_[kTotal].max_us;
+        fprintf(out, "Control-period budget: %u us (100 Hz).\n", budget_us);
+        fprintf(out, "Worst-case compute: %u us (%.2f%% of budget, %u us headroom).\n", worst,
+                100.0 * worst / budget_us, worst < budget_us ? budget_us - worst : 0u);
+    }
+
+   private:
+    struct Acc {
+        uint64_t count = 0;
+        uint64_t sum_us = 0;
+        uint32_t max_us = 0;
+    };
+    Acc acc_[kStageCount];
+};
+
+}  // namespace drone