diff --git a/CHANGELOG.md b/CHANGELOG.md index 6108b99..e60a64c 100644 --- 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 index c42ae88..e3a144b 100644 --- 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 index b196a0b..3252d4b 100644 --- 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 index b4fae41..4201db3 100644 --- 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 g_running{true}; static std::atomic 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(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(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(ctx); + return f != nullptr && fwrite(data, 1, len, f) == len; +} +static bool sd_sync_cb(void* ctx) { + FILE* f = static_cast(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 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,6 +305,8 @@ int main(int /*argc*/, char** /*argv*/) { } } + PROF_TOTAL(); + // Step 6: Sleep for control period SLEEP_MS(CONTROL_PERIOD_MS); } @@ -196,6 +314,21 @@ int main(int /*argc*/, char** /*argv*/) { // 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 new file mode 100644 index 0000000..938ad56 --- /dev/null +++ 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 +#include + +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(a.sum_us) / a.count : 0.0; + fprintf(out, "%-12s %10llu %12.2f %10u\n", kNames[i], + static_cast(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 diff --git a/arm/mavlink_bridge.h b/arm/mavlink_bridge.h index ffa9a2f..fcb1617 100644 --- a/arm/mavlink_bridge.h +++ b/arm/mavlink_bridge.h @@ -287,6 +287,7 @@ class MAVLinkBridge { uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); return send_(buf, len); #else + (void)arm; return true; // Stub for compilation #endif } @@ -312,6 +313,9 @@ class MAVLinkBridge { // Send heartbeat: tells flight controller we're alive // ------------------------------------------------------------------ void send_heartbeat(uint32_t current_time_ms) { +#ifndef MAVLINK_AVAILABLE + (void)current_time_ms; +#endif #ifdef MAVLINK_AVAILABLE mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; diff --git a/arm/mavlink_sd_logger.h b/arm/mavlink_sd_logger.h new file mode 100644 index 0000000..e5baba8 --- /dev/null +++ b/arm/mavlink_sd_logger.h @@ -0,0 +1,399 @@ +// arm/mavlink_sd_logger.h — MAVLink v2 .bin Telemetry Logger (SD card over SPI) +// +// Records the collision-avoidance control loop's state to an on-board SD card +// as a stream of MAVLink v2 frames (".bin" flight log). The file is a raw +// MAVLink v2 byte stream — every record is a fully framed, CRC-checked v2 +// packet — so it can be replayed/parsed with standard tooling +// (e.g. pymavlink `mavlogdump`, MAVLink Inspector) after the flight. +// +// Transport: the SD card is reached over SPI. Rather than bake in a specific +// driver, the sink is abstracted behind two callbacks that map 1:1 onto FatFs: +// +// WriteFn(ctx, data, len) -> f_write(&fil, data, len, &bw); bw == len +// SyncFn(ctx) -> f_sync(&fil) == FR_OK +// +// On a desktop/sim build the same callbacks can wrap a FILE* (fwrite/fflush). +// +// Design notes: +// - Writes are buffered (default 2 KiB ~= 4 SD blocks) so the control loop +// never blocks on a per-message SPI transaction. The buffer is flushed +// when it fills or on a periodic interval (crash-safety f_sync). +// - Logging is best-effort: a failing SD write is counted and the buffer is +// dropped rather than allowed to grow without bound or stall the loop. +// - Header-only, matches the rest of arm/. Reuses FlightCommand / +// VehicleState from mavlink_bridge.h and the threat/evasion types. + +#pragma once + +#include +#include +#include + +#include "evasion_controller.h" // EvasionCommand, EvasionLevel, ThreatAssessment, ObjectCluster +#include "mavlink_bridge.h" // FlightCommand, VehicleState + +namespace drone { + +// --------------------------------------------------------------------------- +// MAVLink v2 framing constants +// --------------------------------------------------------------------------- +// STX | LEN | INCOMPAT | COMPAT | SEQ | SYSID | COMPID | MSGID[3] | payload | CRC[2] +// 0 1 2 3 4 5 6 7..9 10.. tail +static constexpr uint8_t kMavStxV2 = 0xFD; +static constexpr uint8_t kMavHeaderLen = 10; // bytes 0..9 inclusive +static constexpr uint8_t kMavChecksumLen = 2; // CRC-16 little-endian +static constexpr uint16_t kMavMaxPayload = 255; +static constexpr uint16_t kMavMaxFrame = kMavHeaderLen + kMavMaxPayload + kMavChecksumLen; // 267 + +// --------------------------------------------------------------------------- +// Canonical MAVLink CRC-16/MCRF4XX (byte-for-byte identical to the +// crc_accumulate() used by PX4 / ArduPilot / pymavlink). Init 0xFFFF, +// reflected, the per-message crc_extra is folded in last. +// --------------------------------------------------------------------------- +inline void mav_crc_accumulate(uint8_t data, uint16_t& crc) { + uint8_t tmp = data ^ static_cast(crc & 0xFF); + tmp ^= static_cast(tmp << 4); + crc = static_cast((crc >> 8) ^ (static_cast(tmp) << 8) ^ + (static_cast(tmp) << 3) ^ + (static_cast(tmp) >> 4)); +} + +inline uint16_t mav_crc_calculate(const uint8_t* buf, uint16_t len, uint8_t crc_extra) { + uint16_t crc = 0xFFFF; + for (uint16_t i = 0; i < len; ++i) mav_crc_accumulate(buf[i], crc); + mav_crc_accumulate(crc_extra, crc); + return crc; +} + +// --------------------------------------------------------------------------- +// CA_TELEM — collision-avoidance telemetry record (one per control-loop tick). +// +// Logged in the autopilot-specific message-id range so it never collides with +// a standard MAVLink message. crc_extra is a fixed project constant: any +// consumer that knows this layout can CRC-validate the record. The payload is +// tightly packed in MAVLink field order (largest types first) so it matches a +// generated message definition with no implicit padding. +// --------------------------------------------------------------------------- +static constexpr uint32_t kMsgIdCaTelem = 0x0CAA; // 3242 +static constexpr uint8_t kCrcExtraCaTelem = 0x5B; // 91 (fixed for this layout) + +#pragma pack(push, 1) +struct CaTelemPayload { + uint32_t time_boot_ms; // 0 : ms since control loop start + uint32_t loop_dt_us; // 4 : measured control-loop period (µs) + float velocity_x; // 8 : commanded forward velocity (m/s) + float velocity_y; // 12 : commanded lateral velocity (m/s) + float velocity_z; // 16 : commanded vertical velocity (m/s, +up) + float yaw_rate; // 20 : commanded yaw rate (rad/s) + float max_urgency; // 24 : highest collision urgency [0,1] + float time_to_collision; // 28 : TTC of nearest threat (s) + float safe_bearing; // 32 : clearest-path bearing (rad) + float altitude_m; // 36 : vehicle barometric altitude (m) + float battery_voltage; // 40 : vehicle battery (V) + uint16_t n_tracks; // 44 : total Kalman tracks + uint16_t n_confirmed; // 46 : confirmed (>=3 hit) tracks + uint8_t evasion_level; // 48 : EvasionLevel enum + uint8_t threat_detected; // 49 : 1 = confirmed threat present + uint8_t motors_armed; // 50 : 1 = our motor-arm latch is set + uint8_t vehicle_armed; // 51 : 1 = FC reports armed +}; +#pragma pack(pop) +static_assert(sizeof(CaTelemPayload) == 52, "CaTelemPayload must be tightly packed (52 bytes)"); + +// --------------------------------------------------------------------------- +// MavlinkSdLogger +// --------------------------------------------------------------------------- +class MavlinkSdLogger { + public: + // SD-over-SPI sink. `ctx` is an opaque handle (e.g. a FatFs FIL* or FILE*). + // write must persist all `len` bytes (return false on short/failed write). + // sync forces buffered card data to non-volatile storage (may be null). + using WriteFn = bool (*)(void* ctx, const uint8_t* data, uint32_t len); + using SyncFn = bool (*)(void* ctx); + + struct Config { + uint8_t system_id = 1; // companion computer system id + uint8_t component_id = 191; // MAV_COMP_ID_MISSIONPLANNER range + + // Flush when the staging buffer reaches this many bytes. Keep it a + // multiple of the SD block size (512 B) to avoid read-modify-write. + uint32_t buffer_capacity = 2048; + + // Force an f_sync at least this often so a crash loses < interval of + // telemetry. 0 disables periodic sync (still synced on close()). + uint32_t flush_interval_ms = 1000; + + // Rate (Hz) for the standard, ground-station-decodable messages emitted + // by log_standard_tick(). Mirrors fpga/config.yaml telemetry_rate_hz. + // 0 disables them. Only takes effect in MAVLINK_AVAILABLE builds. + uint32_t standard_telemetry_hz = 10; + }; + + struct Stats { + uint32_t messages_logged = 0; // frames successfully staged + uint32_t bytes_buffered = 0; // currently staged, not yet on card + uint64_t bytes_written = 0; // total bytes handed to the sink ok + uint32_t flush_count = 0; // successful buffer->sink flushes + uint32_t dropped_messages = 0; // frames lost to write failure / oversize + uint32_t write_errors = 0; // failed write/sync sink calls + bool error = false; // sticky: a sink call has failed + }; + + MavlinkSdLogger(const Config& cfg, WriteFn write_fn, SyncFn sync_fn, void* ctx) + : cfg_(cfg), + write_(write_fn), + sync_(sync_fn), + ctx_(ctx), + seq_(0), + last_flush_ms_(0), + last_standard_ms_(0), + opened_(false) { + buffer_.reserve(cfg_.buffer_capacity + kMavMaxFrame); + } + + // Begin a logging session. Resets the staging buffer and stats (the + // MAVLink sequence counter is intentionally preserved across sessions). + bool open(uint32_t now_ms = 0) { + buffer_.clear(); + stats_ = Stats{}; + last_flush_ms_ = now_ms; + last_standard_ms_ = now_ms; + opened_ = (write_ != nullptr); + return opened_; + } + + bool is_open() const { return opened_; } + const Stats& stats() const { return stats_; } + + // ------------------------------------------------------------------ + // High-level: log one control-loop tick as a CA_TELEM record. + // ------------------------------------------------------------------ + bool log_control_tick(uint32_t time_boot_ms, uint32_t loop_dt_us, const EvasionCommand& cmd, + const ThreatAssessment& threat, uint16_t n_tracks, uint16_t n_confirmed, + bool motors_armed, const VehicleState& vehicle) { + CaTelemPayload p{}; + p.time_boot_ms = time_boot_ms; + p.loop_dt_us = loop_dt_us; + p.velocity_x = cmd.velocity_x; + p.velocity_y = cmd.velocity_y; + p.velocity_z = cmd.velocity_z; + p.yaw_rate = cmd.yaw_rate; + p.max_urgency = threat.max_urgency; + p.time_to_collision = threat.time_to_first_collision; + p.safe_bearing = threat.safe_bearing; + p.altitude_m = vehicle.altitude_m; + p.battery_voltage = vehicle.battery_voltage; + p.n_tracks = n_tracks; + p.n_confirmed = n_confirmed; + p.evasion_level = static_cast(cmd.level); + p.threat_detected = threat.threat_detected ? 1 : 0; + p.motors_armed = motors_armed ? 1 : 0; + p.vehicle_armed = vehicle.armed ? 1 : 0; + + return log_message(kMsgIdCaTelem, kCrcExtraCaTelem, reinterpret_cast(&p), + sizeof(p)); + } + + // ------------------------------------------------------------------ + // Standard, ground-station-decodable telemetry (QGroundControl / Mission + // Planner) — the same control-loop state as CA_TELEM, but expressed as + // stock NAMED_VALUE_FLOAT / NAMED_VALUE_INT messages so no custom message + // definition is required to read the log. + // + // These are produced by the *generated* MAVLink library so their layout + // and crc_extra are correct by construction — we never hand-pack a stock + // message. In stub builds (no library) this is a no-op: the CA_TELEM + // record already carries the identical data. Throttled to + // standard_telemetry_hz to honour the configured ground-station rate. + // ------------------------------------------------------------------ + bool log_standard_tick(uint32_t time_boot_ms, const EvasionCommand& cmd, + const ThreatAssessment& threat, uint16_t n_tracks, + uint16_t n_confirmed) { + if (!opened_) return false; +#ifdef MAVLINK_AVAILABLE + if (cfg_.standard_telemetry_hz == 0) return true; + if (time_boot_ms - last_standard_ms_ < 1000u / cfg_.standard_telemetry_hz) return true; + last_standard_ms_ = time_boot_ms; + emit_named_float_(time_boot_ms, "ca_urg", threat.max_urgency); + emit_named_float_(time_boot_ms, "ca_ttc", threat.time_to_first_collision); + emit_named_float_(time_boot_ms, "ca_vx", cmd.velocity_x); + emit_named_float_(time_boot_ms, "ca_vy", cmd.velocity_y); + emit_named_float_(time_boot_ms, "ca_vz", cmd.velocity_z); + emit_named_int_(time_boot_ms, "ca_trk", static_cast(n_tracks)); + emit_named_int_(time_boot_ms, "ca_cnf", static_cast(n_confirmed)); + emit_named_int_(time_boot_ms, "ca_lvl", static_cast(cmd.level)); + return true; +#else + (void)time_boot_ms; + (void)cmd; + (void)threat; + (void)n_tracks; + (void)n_confirmed; + return true; // CA_TELEM already carries this data in stub builds +#endif + } + + // ------------------------------------------------------------------ + // Mid-level: frame an arbitrary payload as a MAVLink v2 message and + // stage it. Use this for additional message types as they're added. + // ------------------------------------------------------------------ + bool log_message(uint32_t msgid, uint8_t crc_extra, const uint8_t* payload, + uint8_t payload_len) { + if (!opened_) return false; + uint8_t frame[kMavMaxFrame]; + uint16_t n = build_frame_(frame, msgid, crc_extra, payload, payload_len); + return stage_(frame, n); + } + + // ------------------------------------------------------------------ + // Low-level: tee an already-framed MAVLink packet into the log (e.g. the + // exact bytes the MAVLinkBridge sends to the flight controller). The frame + // is stored verbatim — no re-framing, no sequence rewrite. + // ------------------------------------------------------------------ + bool log_raw_frame(const uint8_t* frame, uint16_t len) { + if (!opened_ || frame == nullptr || len == 0) return false; + return stage_(frame, len); + } + + // ------------------------------------------------------------------ + // Periodic maintenance — call once per control-loop iteration. Flushes + // the staging buffer to the card when the flush interval has elapsed. + // ------------------------------------------------------------------ + void update(uint32_t now_ms) { + if (cfg_.flush_interval_ms == 0) return; + if (now_ms - last_flush_ms_ >= cfg_.flush_interval_ms) { + flush(); + last_flush_ms_ = now_ms; + } + } + + // ------------------------------------------------------------------ + // Force the staging buffer to the card and f_sync. Returns false (and + // sets the sticky error flag) if any sink call fails. + // ------------------------------------------------------------------ + bool flush() { + if (!opened_) return false; + bool ok = drain_(); + if (sync_ != nullptr) { + if (!sync_(ctx_)) { + stats_.write_errors++; + stats_.error = true; + ok = false; + } + } + return ok; + } + + // Flush remaining data and end the session. + bool close() { + if (!opened_) return false; + bool ok = flush(); + opened_ = false; + return ok; + } + + private: +#ifdef MAVLINK_AVAILABLE + // Pack + stage a stock NAMED_VALUE_FLOAT / NAMED_VALUE_INT via the + // generated library (guaranteed-correct framing, seq, and crc_extra). + void emit_named_float_(uint32_t t, const char* name, float v) { + mavlink_message_t msg; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + mavlink_msg_named_value_float_pack(cfg_.system_id, cfg_.component_id, &msg, t, name, v); + stage_(buf, mavlink_msg_to_send_buffer(buf, &msg)); + } + void emit_named_int_(uint32_t t, const char* name, int32_t v) { + mavlink_message_t msg; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + mavlink_msg_named_value_int_pack(cfg_.system_id, cfg_.component_id, &msg, t, name, v); + stage_(buf, mavlink_msg_to_send_buffer(buf, &msg)); + } +#endif + + // Build a MAVLink v2 frame into `out`; returns total frame length. + uint16_t build_frame_(uint8_t* out, uint32_t msgid, uint8_t crc_extra, const uint8_t* payload, + uint8_t payload_len) { + out[0] = kMavStxV2; + out[1] = payload_len; + out[2] = 0; // incompat_flags (no signing) + out[3] = 0; // compat_flags + out[4] = seq_++; + out[5] = cfg_.system_id; + out[6] = cfg_.component_id; + out[7] = static_cast(msgid & 0xFF); + out[8] = static_cast((msgid >> 8) & 0xFF); + out[9] = static_cast((msgid >> 16) & 0xFF); + if (payload_len > 0 && payload != nullptr) { + memcpy(out + kMavHeaderLen, payload, payload_len); + } + // CRC covers everything from LEN (byte 1) through the payload, plus crc_extra. + uint16_t crc = + mav_crc_calculate(out + 1, static_cast(9 + payload_len), crc_extra); + uint16_t tail = static_cast(kMavHeaderLen + payload_len); + out[tail] = static_cast(crc & 0xFF); + out[tail + 1] = static_cast((crc >> 8) & 0xFF); + return static_cast(tail + kMavChecksumLen); + } + + // Stage a complete frame, flushing first if it would overflow the buffer. + bool stage_(const uint8_t* frame, uint16_t len) { + // A frame larger than the whole buffer can never be staged: flush what + // we have, then write it straight through to the sink. + if (len > cfg_.buffer_capacity) { + bool ok = drain_(); + if (write_(ctx_, frame, len)) { + stats_.bytes_written += len; + stats_.messages_logged++; + } else { + stats_.write_errors++; + stats_.dropped_messages++; + stats_.error = true; + ok = false; + } + return ok; + } + + if (buffer_.size() + len > cfg_.buffer_capacity) { + if (!drain_()) { + // Drain failed: the buffer was dropped. Fall through and stage + // this frame so logging can recover on the next good write. + stats_.dropped_messages++; + } + } + + buffer_.insert(buffer_.end(), frame, frame + len); + stats_.bytes_buffered = static_cast(buffer_.size()); + stats_.messages_logged++; + return !stats_.error; + } + + // Write the staging buffer to the sink and clear it. On failure the buffer + // is dropped (best-effort logging) and the sticky error flag is set. + bool drain_() { + if (buffer_.empty()) return true; + bool ok = write_(ctx_, buffer_.data(), static_cast(buffer_.size())); + if (ok) { + stats_.bytes_written += buffer_.size(); + stats_.flush_count++; + } else { + stats_.write_errors++; + stats_.error = true; + } + buffer_.clear(); + stats_.bytes_buffered = 0; + return ok; + } + + Config cfg_; + WriteFn write_; + SyncFn sync_; + void* ctx_; + std::vector buffer_; + uint8_t seq_; + uint32_t last_flush_ms_; + uint32_t last_standard_ms_; + bool opened_; + Stats stats_; +}; + +} // namespace drone diff --git a/arm/test_mavlink_logger.cpp b/arm/test_mavlink_logger.cpp new file mode 100644 index 0000000..84a1267 --- /dev/null +++ b/arm/test_mavlink_logger.cpp @@ -0,0 +1,508 @@ +// arm/test_mavlink_logger.cpp — MAVLink v2 .bin telemetry logger tests +// +// Exercises arm/mavlink_sd_logger.h end to end against an in-memory SD sink +// and an independent MAVLink v2 readback parser: +// CRC : MCRF4XX known-answer vector, parser CRC validation +// Framing : STX/len/ids/msgid layout, sequence numbering + wrap +// Round-trip : every CA_TELEM field survives encode -> .bin -> decode +// Buffering : nothing hits the card until threshold/flush/close +// Robustness : oversize frames, write failures, recovery, raw-frame tee +// +// Build + run: make test-logger (or: make test, which runs all suites) +// Binary: ./test_mavlink_logger (exit 0 = all pass, 1 = any failure) + +#include +#include +#include +#include + +#include "mavlink_sd_logger.h" + +using namespace drone; + +// --------------------------------------------------------------------------- +// Minimal test framework (same shape as test_arm_cp.cpp) +// --------------------------------------------------------------------------- +static int g_total = 0; +static int g_passed = 0; +static bool g_test_failed = false; +static const char* g_test_name = ""; + +#define RUN_TEST(name) \ + do { \ + g_test_name = #name; \ + g_test_failed = false; \ + g_total++; \ + printf(" [ RUN ] %s\n", g_test_name); \ + } while (0) + +#define END_TEST \ + do { \ + if (!g_test_failed) { \ + printf(" [ PASS ] %s\n\n", g_test_name); \ + g_passed++; \ + } else { \ + printf(" [ FAIL ] %s\n\n", g_test_name); \ + } \ + } while (0) + +#define EXPECT_EQ(a, b) \ + do { \ + if ((a) != (b)) { \ + printf(" EXPECT_EQ failed %s:%d\n left: %s = %lld\n right: %s = %lld\n", \ + __FILE__, __LINE__, #a, (long long)(a), #b, (long long)(b)); \ + g_test_failed = true; \ + } \ + } while (0) + +#define EXPECT_TRUE(cond) \ + do { \ + if (!(cond)) { \ + printf(" EXPECT_TRUE failed %s:%d: %s\n", __FILE__, __LINE__, #cond); \ + g_test_failed = true; \ + } \ + } while (0) + +#define EXPECT_FALSE(cond) \ + do { \ + if ((cond)) { \ + printf(" EXPECT_FALSE failed %s:%d: %s\n", __FILE__, __LINE__, #cond); \ + g_test_failed = true; \ + } \ + } while (0) + +#define EXPECT_FEQ(a, b) \ + do { \ + float _fa = (float)(a), _fb = (float)(b); \ + if (memcmp(&_fa, &_fb, sizeof(float)) != 0) { \ + printf(" EXPECT_FEQ failed %s:%d\n %s = %.7g vs %s = %.7g\n", __FILE__, \ + __LINE__, #a, (double)_fa, #b, (double)_fb); \ + g_test_failed = true; \ + } \ + } while (0) + +// --------------------------------------------------------------------------- +// In-memory SD-over-SPI sink (stands in for FatFs f_write / f_sync) +// --------------------------------------------------------------------------- +struct MemSink { + std::vector data; // everything successfully "written to the card" + int write_calls = 0; + int sync_calls = 0; + bool fail_writes = false; // simulate a card/SPI write fault +}; + +static bool mem_write(void* ctx, const uint8_t* d, uint32_t n) { + auto* s = static_cast(ctx); + s->write_calls++; + if (s->fail_writes) return false; + s->data.insert(s->data.end(), d, d + n); + return true; +} + +static bool mem_sync(void* ctx) { + static_cast(ctx)->sync_calls++; + return true; +} + +// --------------------------------------------------------------------------- +// Independent MAVLink v2 readback parser +// --------------------------------------------------------------------------- +struct ParsedFrame { + uint8_t len = 0; + uint8_t seq = 0; + uint8_t sysid = 0; + uint8_t compid = 0; + uint32_t msgid = 0; + std::vector payload; + bool crc_ok = false; +}; + +// Parse one frame at `pos`. Validates CRC using the supplied crc_extra. +// Returns the offset of the next frame, or 0 on malformed input. +static size_t parse_frame(const std::vector& buf, size_t pos, uint8_t crc_extra, + ParsedFrame& out) { + if (pos + kMavHeaderLen + kMavChecksumLen > buf.size()) return 0; + if (buf[pos] != kMavStxV2) return 0; + out.len = buf[pos + 1]; + out.seq = buf[pos + 4]; + out.sysid = buf[pos + 5]; + out.compid = buf[pos + 6]; + out.msgid = + (uint32_t)buf[pos + 7] | ((uint32_t)buf[pos + 8] << 8) | ((uint32_t)buf[pos + 9] << 16); + size_t tail = pos + kMavHeaderLen + out.len; + if (tail + kMavChecksumLen > buf.size()) return 0; + out.payload.assign(buf.begin() + pos + kMavHeaderLen, buf.begin() + tail); + + uint16_t want = mav_crc_calculate(&buf[pos + 1], (uint16_t)(9 + out.len), crc_extra); + uint16_t got = (uint16_t)buf[tail] | ((uint16_t)buf[tail + 1] << 8); + out.crc_ok = (want == got); + return tail + kMavChecksumLen; +} + +// Parse a whole stream of CA_TELEM frames; returns false if any frame is +// malformed or fails CRC. Fills `frames`. +static bool parse_stream(const std::vector& buf, std::vector& frames) { + size_t pos = 0; + while (pos < buf.size()) { + ParsedFrame f; + size_t next = parse_frame(buf, pos, kCrcExtraCaTelem, f); + if (next == 0 || !f.crc_ok) return false; + frames.push_back(f); + pos = next; + } + return true; +} + +// --------------------------------------------------------------------------- +// Fixtures with distinctive, individually-identifiable values +// --------------------------------------------------------------------------- +static EvasionCommand make_cmd() { + EvasionCommand c{}; + c.velocity_x = 1.25f; + c.velocity_y = -2.5f; + c.velocity_z = 0.75f; + c.yaw_rate = -0.5f; + c.level = EvasionLevel::WARNING; // 2 + return c; +} + +static ThreatAssessment make_threat() { + ThreatAssessment t{}; + t.max_urgency = 0.875f; + t.safe_bearing = 1.5f; + t.safe_bearing_confidence = 0.9f; + t.threat_detected = true; + t.time_to_first_collision = 0.625f; + return t; +} + +static VehicleState make_vehicle() { + VehicleState v{}; + v.altitude_m = 12.5f; + v.battery_voltage = 11.1f; + v.armed = 1; + return v; +} + +static MavlinkSdLogger::Config cfg_with_capacity(uint32_t cap, uint32_t flush_ms = 0) { + MavlinkSdLogger::Config c; + c.buffer_capacity = cap; + c.flush_interval_ms = flush_ms; + return c; +} + +static const uint16_t kCaFrameLen = kMavHeaderLen + sizeof(CaTelemPayload) + kMavChecksumLen; // 64 + +// --------------------------------------------------------------------------- +// Test 1: CRC-16/MCRF4XX known-answer vector. +// +// The documented check value of CRC-16/MCRF4XX over the ASCII string +// "123456789" is 0x6F91. mav_crc_accumulate must reproduce it exactly — this +// pins the algorithm to the one PX4/ArduPilot/pymavlink use. +// --------------------------------------------------------------------------- +void test_crc_known_answer() { + RUN_TEST(test_crc_known_answer); + const char* s = "123456789"; + uint16_t crc = 0xFFFF; + for (const char* p = s; *p; ++p) mav_crc_accumulate((uint8_t)*p, crc); + EXPECT_EQ(crc, (uint16_t)0x6F91); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 2: Frame layout — one CA_TELEM decodes to the right header fields. +// --------------------------------------------------------------------------- +void test_frame_layout() { + RUN_TEST(test_frame_layout); + MemSink sink; + MavlinkSdLogger::Config cfg; + cfg.system_id = 7; + cfg.component_id = 191; + MavlinkSdLogger logger(cfg, mem_write, mem_sync, &sink); + logger.open(); + + logger.log_control_tick(1000, 10000, make_cmd(), make_threat(), 4, 2, true, make_vehicle()); + logger.flush(); + + std::vector frames; + EXPECT_TRUE(parse_stream(sink.data, frames)); + EXPECT_EQ(frames.size(), 1u); + if (!frames.empty()) { + EXPECT_EQ(frames[0].len, (uint8_t)sizeof(CaTelemPayload)); + EXPECT_EQ(frames[0].sysid, (uint8_t)7); + EXPECT_EQ(frames[0].compid, (uint8_t)191); + EXPECT_EQ(frames[0].msgid, kMsgIdCaTelem); + EXPECT_TRUE(frames[0].crc_ok); + } + EXPECT_EQ(sink.data.size(), (size_t)kCaFrameLen); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 3: Full payload round-trip — every field survives encode -> decode. +// --------------------------------------------------------------------------- +void test_payload_roundtrip() { + RUN_TEST(test_payload_roundtrip); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(4096), mem_write, mem_sync, &sink); + logger.open(); + + EvasionCommand cmd = make_cmd(); + ThreatAssessment threat = make_threat(); + VehicleState veh = make_vehicle(); + logger.log_control_tick(123456, 9876, cmd, threat, 5, 3, true, veh); + logger.close(); + + std::vector frames; + EXPECT_TRUE(parse_stream(sink.data, frames)); + EXPECT_EQ(frames.size(), 1u); + if (frames.empty()) { + END_TEST; + return; + } + EXPECT_EQ(frames[0].payload.size(), sizeof(CaTelemPayload)); + + CaTelemPayload p{}; + memcpy(&p, frames[0].payload.data(), sizeof(p)); + EXPECT_EQ(p.time_boot_ms, 123456u); + EXPECT_EQ(p.loop_dt_us, 9876u); + EXPECT_FEQ(p.velocity_x, cmd.velocity_x); + EXPECT_FEQ(p.velocity_y, cmd.velocity_y); + EXPECT_FEQ(p.velocity_z, cmd.velocity_z); + EXPECT_FEQ(p.yaw_rate, cmd.yaw_rate); + EXPECT_FEQ(p.max_urgency, threat.max_urgency); + EXPECT_FEQ(p.time_to_collision, threat.time_to_first_collision); + EXPECT_FEQ(p.safe_bearing, threat.safe_bearing); + EXPECT_FEQ(p.altitude_m, veh.altitude_m); + EXPECT_FEQ(p.battery_voltage, veh.battery_voltage); + EXPECT_EQ(p.n_tracks, (uint16_t)5); + EXPECT_EQ(p.n_confirmed, (uint16_t)3); + EXPECT_EQ(p.evasion_level, (uint8_t)EvasionLevel::WARNING); + EXPECT_EQ(p.threat_detected, (uint8_t)1); + EXPECT_EQ(p.motors_armed, (uint8_t)1); + EXPECT_EQ(p.vehicle_armed, (uint8_t)1); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 4: Buffering — bytes reach the card only on flush, not per message. +// --------------------------------------------------------------------------- +void test_buffering_defers_writes() { + RUN_TEST(test_buffering_defers_writes); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(4096), mem_write, mem_sync, &sink); + logger.open(); + + for (int i = 0; i < 3; i++) + logger.log_control_tick(i, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + + // Still buffered: no write call yet, sink empty. + EXPECT_EQ(sink.write_calls, 0); + EXPECT_EQ(sink.data.size(), 0u); + EXPECT_EQ(logger.stats().bytes_buffered, (uint32_t)(3 * kCaFrameLen)); + + EXPECT_TRUE(logger.flush()); + EXPECT_EQ(sink.write_calls, 1); // single contiguous block write + EXPECT_EQ(sink.sync_calls, 1); // flush() also f_syncs + EXPECT_EQ(sink.data.size(), (size_t)(3 * kCaFrameLen)); + EXPECT_EQ(logger.stats().bytes_buffered, 0u); + + std::vector frames; + EXPECT_TRUE(parse_stream(sink.data, frames)); + EXPECT_EQ(frames.size(), 3u); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 5: Threshold auto-flush — staging a frame that would overflow the +// buffer first drains the existing contents. +// --------------------------------------------------------------------------- +void test_threshold_autoflush() { + RUN_TEST(test_threshold_autoflush); + MemSink sink; + // Capacity holds exactly one 64-byte frame but not two. + MavlinkSdLogger logger(cfg_with_capacity(100), mem_write, mem_sync, &sink); + logger.open(); + + logger.log_control_tick(0, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + EXPECT_EQ(sink.write_calls, 0); // first frame fits, stays buffered + + logger.log_control_tick(1, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + EXPECT_EQ(sink.write_calls, 1); // second frame forced a drain + EXPECT_EQ(sink.data.size(), (size_t)kCaFrameLen); // exactly the first frame on card + EXPECT_EQ(logger.stats().bytes_buffered, (uint32_t)kCaFrameLen); // second frame buffered + + logger.close(); + EXPECT_EQ(sink.data.size(), (size_t)(2 * kCaFrameLen)); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 6: Sequence numbers increment per frame and wrap at 256. +// --------------------------------------------------------------------------- +void test_sequence_numbering() { + RUN_TEST(test_sequence_numbering); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(8192), mem_write, mem_sync, &sink); + logger.open(); + + const int N = 260; // forces a wrap past 255 + for (int i = 0; i < N; i++) + logger.log_control_tick(i, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + logger.close(); + + std::vector frames; + EXPECT_TRUE(parse_stream(sink.data, frames)); + EXPECT_EQ(frames.size(), (size_t)N); + bool seq_ok = true; + for (int i = 0; i < (int)frames.size(); i++) + if (frames[i].seq != (uint8_t)(i & 0xFF)) seq_ok = false; + EXPECT_TRUE(seq_ok); + EXPECT_EQ(logger.stats().messages_logged, (uint32_t)N); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 7: Oversize frame — a frame larger than the whole buffer is written +// straight through to the sink without corruption. +// --------------------------------------------------------------------------- +void test_oversize_frame_passthrough() { + RUN_TEST(test_oversize_frame_passthrough); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(16), mem_write, mem_sync, &sink); // < 64 + logger.open(); + + EXPECT_TRUE( + logger.log_control_tick(0, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle())); + EXPECT_EQ(sink.write_calls, 1); + EXPECT_EQ(sink.data.size(), (size_t)kCaFrameLen); + EXPECT_EQ(logger.stats().bytes_buffered, 0u); + + std::vector frames; + EXPECT_TRUE(parse_stream(sink.data, frames)); + EXPECT_EQ(frames.size(), 1u); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 8: Write failure is contained, and the logger recovers afterward. +// --------------------------------------------------------------------------- +void test_write_failure_and_recovery() { + RUN_TEST(test_write_failure_and_recovery); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(4096), mem_write, mem_sync, &sink); + logger.open(); + + sink.fail_writes = true; + logger.log_control_tick(0, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + EXPECT_FALSE(logger.flush()); // sink rejected the write + EXPECT_TRUE(logger.stats().error); // sticky error flag set + EXPECT_TRUE(logger.stats().write_errors >= 1); + EXPECT_EQ(sink.data.size(), 0u); // nothing persisted + + // Card recovers: subsequent logging must reach it and parse cleanly. + sink.fail_writes = false; + logger.log_control_tick(1, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + EXPECT_TRUE(logger.flush()); + EXPECT_EQ(sink.data.size(), (size_t)kCaFrameLen); + + std::vector frames; + EXPECT_TRUE(parse_stream(sink.data, frames)); + EXPECT_EQ(frames.size(), 1u); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 9: Parser CRC validation actually rejects corruption. +// +// Guards against a false-positive parser: flip one payload byte and confirm +// the recomputed CRC no longer matches. +// --------------------------------------------------------------------------- +void test_crc_detects_corruption() { + RUN_TEST(test_crc_detects_corruption); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(4096), mem_write, mem_sync, &sink); + logger.open(); + logger.log_control_tick(42, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + logger.close(); + + ParsedFrame f; + EXPECT_TRUE(parse_frame(sink.data, 0, kCrcExtraCaTelem, f) != 0); + EXPECT_TRUE(f.crc_ok); + + // Corrupt a payload byte (offset 12 = velocity_x) and re-parse. + sink.data[kMavHeaderLen + 2] ^= 0xFF; + ParsedFrame f2; + parse_frame(sink.data, 0, kCrcExtraCaTelem, f2); + EXPECT_FALSE(f2.crc_ok); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 10: Raw-frame tee — pre-framed bytes are stored verbatim. +// --------------------------------------------------------------------------- +void test_raw_frame_tee() { + RUN_TEST(test_raw_frame_tee); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(4096), mem_write, mem_sync, &sink); + logger.open(); + + const uint8_t raw[] = {0xFD, 0x02, 0x00, 0x00, 0x05, 0x01, 0xBF, + 0x00, 0x00, 0x00, 0xAB, 0xCD, 0x11, 0x22}; + EXPECT_TRUE(logger.log_raw_frame(raw, sizeof(raw))); + logger.flush(); + EXPECT_EQ(sink.data.size(), sizeof(raw)); + EXPECT_EQ(memcmp(sink.data.data(), raw, sizeof(raw)), 0); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 11: Guard rails — logging before open() and after close() is a no-op. +// --------------------------------------------------------------------------- +void test_open_close_guards() { + RUN_TEST(test_open_close_guards); + MemSink sink; + MavlinkSdLogger logger(cfg_with_capacity(4096), mem_write, mem_sync, &sink); + + // Not opened yet. + EXPECT_FALSE( + logger.log_control_tick(0, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle())); + EXPECT_EQ(sink.write_calls, 0); + + logger.open(); + logger.log_control_tick(0, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle()); + logger.close(); + size_t after_close = sink.data.size(); + + // After close(): further logging rejected, nothing more written. + EXPECT_FALSE(logger.is_open()); + EXPECT_FALSE( + logger.log_control_tick(1, 0, make_cmd(), make_threat(), 1, 1, true, make_vehicle())); + EXPECT_EQ(sink.data.size(), after_close); + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test runner +// --------------------------------------------------------------------------- +int main() { + printf("ARM MAVLink v2 .bin Telemetry Logger — Tests\n"); + printf("=============================================\n\n"); + + test_crc_known_answer(); + test_frame_layout(); + test_payload_roundtrip(); + test_buffering_defers_writes(); + test_threshold_autoflush(); + test_sequence_numbering(); + test_oversize_frame_passthrough(); + test_write_failure_and_recovery(); + test_crc_detects_corruption(); + test_raw_frame_tee(); + test_open_close_guards(); + + printf("=============================================\n"); + printf("Results: %d/%d passed\n\n", g_passed, g_total); + + return (g_passed == g_total) ? 0 : 1; +} diff --git a/docs/loop-latency.md b/docs/loop-latency.md new file mode 100644 index 0000000..43690d4 --- /dev/null +++ b/docs/loop-latency.md @@ -0,0 +1,121 @@ +# 100 Hz Control-Loop Latency + +This note documents the worst-case latency of the ARM control loop in +[`arm/drone_main.cpp`](../arm/drone_main.cpp), how to reproduce the measurement, +and where the real-hardware risk lies. + +The loop runs at **100 Hz** — a **10 000 µs (10 ms) period**. Every iteration +must finish its compute work and still leave time for the control-period sleep, +otherwise the effective rate drops below 100 Hz and command updates to the FPGA +PWM stage are delayed. + +## Build flags + +The flight and simulation binaries are compiled with +`-O3 -ffast-math -funroll-loops -flto` (see +[`arm/Makefile`](../arm/Makefile), `RELEASE_FLAGS`). `-O3` was already in place; +`-flto` (link-time optimization) was added so the optimizer can inline and +dead-strip across translation units. The current ARM code is effectively a +single translation unit (`drone_main.cpp` pulls in the header-only predictor, +tracker, evasion, and logger), so LTO's benefit today is modest, but it keeps +cross-module inlining automatic as the codebase grows and costs nothing. + +## How to measure + +```sh +cd arm +make profile # builds drone_control_sim_prof with -DPROFILE_LOOP +./drone_control_sim_prof # let it spin for a few seconds, then Ctrl+C +``` + +`make profile` compiles the simulation binary with the same `RELEASE_FLAGS` +(including `-flto`) as the flight build, plus `-DPROFILE_LOOP`. The instrument +is [`arm/loop_profiler.h`](../arm/loop_profiler.h): a zero-heap accumulator that +records count / mean / **max** microseconds for each loop stage. In production +builds (`make`, `make sim`) `PROFILE_LOOP` is undefined and every probe macro +expands to a no-op, so the flight binary carries no profiling overhead. + +The loop body is timed in nine stages, in execution order: + +| stage | work measured | +|---------------|-----------------------------------------------------------| +| `read_flow` | `FpgaInterface::read_flow_vectors` (AXI4-Lite readback) | +| `predict` | `CollisionPredictor::assess` | +| `track` | `KalmanTracker::update` + confirmed-track rebuild | +| `evade` | `EvasionController::compute_command` | +| `write_cmd` | `FpgaInterface::write_velocity_command` (when armed) | +| `console_log` | human-readable `printf` telemetry to stdout | +| `bin_log` | MAVLink v2 frame build + SD buffer staging | +| `flush` | `logger.update()` — periodic `fwrite` + `fsync` (SD commit) | +| `TOTAL` | whole loop body, **excluding** the control-period sleep | + +## Measured worst case + +396 iterations, 14 flush cycles, simulation build with `-flto`, on the +development host (x86-64, clang). All times in microseconds: + +| stage | mean | **max** | +|---------------|-------:|--------:| +| read_flow | 0.51 | 11 | +| predict | 0.11 | 3 | +| track | 1.72 | 40 | +| evade | 0.83 | 17 | +| write_cmd | 0.08 | 2 | +| console_log | 6.94 | 246 | +| bin_log | 0.70 | 40 | +| **flush** | 4.58 | **478** | +| **TOTAL** | 15.56 | **482** | + +**Worst-case compute: 482 µs — 4.8 % of the 10 ms budget (~9.5 ms headroom).** +Mean compute is ~16 µs; the loop spends the remaining ~99.8 % of each period +asleep. + +Observations: + +- The two largest spikes are **`flush` (478 µs)** and **`console_log` + (246 µs)**, and the worst `TOTAL` tick (482 µs) is a flush tick — the periodic + `fsync` dominates. Pure control compute (`read_flow` → `evade`) never exceeds + ~70 µs combined. +- `flush` is paid only when the logger's flush interval elapses (every ~1 s, + ~14 ticks of 396 above) or its buffer fills, **not every tick** — by design, + so the SD commit cost is amortized rather than charged to all 100 ticks/s. +- `console_log` is the human-readable `printf` stream. In a real deployment + stdout is typically redirected or disabled; if loop jitter ever became a + concern it is the cheapest stage to drop. + +## Real-hardware caveats + +The numbers above were taken on the dev host, not the Zynq Cortex-A53, and the +`flush` figure is the most optimistic of all: + +1. **CPU.** The Cortex-A53 runs slower than the dev host. Even assuming a + conservative 5–10× slowdown, worst-case compute stays in the low + single-digit milliseconds — still inside the 10 ms budget, but the headroom + margin shrinks and should be re-measured on-target. +2. **`fsync` to SD-over-SPI ≠ `fsync` to an SSD.** On the dev host the flush + commits to an SSD in ~0.5 ms. A real SD card over SPI can stall for **tens of + milliseconds** on a block erase / wear-levelling cycle — long enough to + overrun a single 10 ms period on the tick where it lands. The control loop + tolerates an occasional late tick (commands are idempotent and the tracker + ages by wall-clock `dt`, not tick count), but a flush stall is the one event + that can exceed the budget on real hardware. + +### Mitigation already in place / recommended + +- Flushing is **periodic, not per-tick** (`Config::flush_interval_ms`, default + 1 s), so at most ~1 tick/second can be hit by an SD stall. +- The logger is **crash-safe** regardless: a power loss between flushes loses at + most the last interval of buffered telemetry, never the file. +- If on-target measurement shows flush stalls causing unacceptable jitter, move + the SD `fwrite`/`fsync` off the control thread (double-buffer + a lower-priority + writer task) so the 100 Hz loop never blocks on storage. The logger's + callback seam (`WriteFn`/`SyncFn`) already makes this a drop-in change without + touching the control logic. + +## Bottom line + +With LTO enabled and the loop instrumented, worst-case **control compute is +under 5 % of the 100 Hz budget** on the dev host. The only stage that can +threaten the budget on real hardware is the periodic SD `fsync`, which is +already amortized to ~1 Hz and isolated behind a callback seam that allows +moving it off the control path if on-target profiling requires it.