diff --git a/CMakeLists.txt b/CMakeLists.txt index 0de783f..1adf89b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ set(MAVLINK_DIR ${CMAKE_SOURCE_DIR}/lib/c_library_v2) add_library(hawkeye-core STATIC src/ulog_parser.c src/ulog_replay.c + src/ulog_replay_apply.c src/data_source_ulog.c ) target_include_directories(hawkeye-core PUBLIC src/) diff --git a/docs/wasm-pre-extraction-design.md b/docs/wasm-pre-extraction-design.md new file mode 100644 index 0000000..74e9818 --- /dev/null +++ b/docs/wasm-pre-extraction-design.md @@ -0,0 +1,335 @@ +# WASM ULog pre-extraction — design + +Design document for the WASM build's ULog replay path. Input to Task #2 of `velvety-dazzling-dolphin` plan. Not a commit artifact yet; lives here for user review before any code is written. + +## Goal + +On the WASM build, walk each ULog once at load time, extract every field the replay engine actually consumes into a flat in-memory timeline, then release the raw file bytes. Playback consumes the flat arrays instead of re-reading the file. + +**Why:** a multi-drone swarm (Lucky Seven or similar) with typical ULog sizes would otherwise keep 80-300 MB of raw file bytes per drone resident in WASM heap. Mobile browsers OOM. Desktop browsers waste memory. The existing file-backed parser is excellent on native (leave it alone) and wrong on WASM (replace it for playback). + +**Non-goal:** native behavior changes. Native keeps the exact code path it has today. Every `#ifdef __EMSCRIPTEN__` branch in the parser and replay code must have a byte-identical native fallthrough. + +## Memory target + +Target: ≤5 MB per drone for a 1000-second flight at typical PX4 logging rates. 16-drone case ≤80 MB total. + +Rough math for one drone, 1000 s flight: +- `vehicle_attitude` at 100 Hz: 100,000 events × 24 bytes = 2.4 MB +- `vehicle_local_position` at 50 Hz: 50,000 events × 32 bytes = 1.6 MB +- `vehicle_global_position` at 10 Hz: 10,000 events × 32 bytes = 320 KB +- `airspeed_validated` at 20 Hz: 20,000 events × 16 bytes = 320 KB +- `vehicle_status` at 1 Hz: 1,000 events × 8 bytes = 8 KB +- `home_position` (rare): <1,000 events × 32 bytes < 32 KB +- `STATUSTEXT`: typically <200 entries, ~144 bytes each = <29 KB +- Subtotal: ~4.7 MB + +Well within target. The plan's original 1-5 MB estimate is realistic. + +## Event data model + +Separate arrays per event type, not a tagged union. A union would pad every event to the size of the largest member (STATUSTEXT's 128-byte text buffer), bloating attitude from 24 B to ~144 B — a 6× waste on the dominant event type. Per-type arrays also let us seek efficiently: each array is sorted by timestamp, binary search per type. + +### Struct definitions (C) + +Proposed shape. Field ordering picked for packing; `uint64_t` timestamps aligned to 8. These go in a new `ulog_events.h` header that both native and WASM can include (native never instantiates the arrays, only uses the apply helpers). + +```c +// --- per-event structs --- + +typedef struct { + uint64_t timestamp_us; + float q[4]; // w, x, y, z +} ulog_att_event_t; // 24 bytes + +typedef struct { + uint64_t timestamp_us; + double lat_deg; + double lon_deg; + float alt_m; + float _pad; // keep 8-byte alignment on 32-bit WASM +} ulog_gpos_event_t; // 32 bytes + +typedef struct { + uint64_t timestamp_us; + float x, y, z; + float vx, vy, vz; +} ulog_lpos_event_t; // 32 bytes + +typedef struct { + uint64_t timestamp_us; + uint16_t ias_cms; // indicated_airspeed * 100 + uint16_t tas_cms; // true_airspeed * 100 + uint32_t _pad; +} ulog_aspd_event_t; // 16 bytes + +typedef struct { + uint64_t timestamp_us; + uint8_t vehicle_type; // PX4 enum, pre-translated to MAV_TYPE + uint8_t is_vtol; + uint8_t nav_state; + uint8_t _pad[5]; +} ulog_vstatus_event_t; // 16 bytes + +typedef struct { + uint64_t timestamp_us; + double lat_deg; + double lon_deg; + float alt_m; + uint8_t valid_hpos; + uint8_t _pad[3]; +} ulog_home_event_t; // 32 bytes + +typedef struct { + uint64_t timestamp_us; + uint8_t severity; + uint8_t _pad[7]; + char text[STATUSTEXT_MSG_MAX]; // 128 bytes +} ulog_statustext_event_t; // 144 bytes +``` + +### Timeline container + +A single `ulog_timeline_t` holds all arrays plus the one-shot LPOS reference point (which is set once from the first LPOS message that has `xy_global`). Arrays grow with `realloc` during extraction and get final-sized at the end of the walk. + +```c +typedef struct { + // Per-type event arrays (grown during extraction, final-sized after) + ulog_att_event_t *att; int att_count; int att_cap; + ulog_lpos_event_t *lpos; int lpos_count; int lpos_cap; + ulog_gpos_event_t *gpos; int gpos_count; int gpos_cap; + ulog_aspd_event_t *aspd; int aspd_count; int aspd_cap; + ulog_vstatus_event_t *vstatus; int vstatus_count; int vstatus_cap; + ulog_home_event_t *home; int home_count; int home_cap; + ulog_statustext_event_t *statustext; int statustext_count; int statustext_cap; + + // One-shot LPOS reference point (captured on first LPOS with xy_global) + double ref_lat_deg; + double ref_lon_deg; + float ref_alt_m; + uint8_t ref_set; + uint8_t ref_rejected; +} ulog_timeline_t; +``` + +### Where the timeline lives + +On WASM, `ulog_replay_ctx_t` gains a `ulog_timeline_t timeline;` field (gated by `#ifdef __EMSCRIPTEN__` so native sizeof stays unchanged). Playback cursors per array (`att_cursor`, `lpos_cursor`, etc.) also live on the ctx under the same ifdef. + +## Extraction pass + +The existing init already performs a full-file scan at `src/ulog_replay.c:377` (`while (ulog_parser_next(&ctx->parser, &scan_msg))`). Current scan purposes: `mode_changes`, CUSUM takeoff, home resolution, position tier. The WASM extraction **piggybacks on this same walk** — it does not add a second file read. + +Inside the scan loop, after the existing native-side work is done, a new `#ifdef __EMSCRIPTEN__` block dispatches the current message to an appender: + +```c +#ifdef __EMSCRIPTEN__ + if (scan_msg.msg_id == att_msg_id) + extract_attitude(&ctx->timeline, &scan_msg, &ctx->cache); + else if (scan_msg.msg_id == lpos_msg_id) + extract_lpos(&ctx->timeline, &scan_msg, &ctx->cache); + // ... etc +#endif +``` + +Each `extract_*` helper decodes its subscription's fields using the existing `ulog_parser_get_*` accessors (which work on in-memory `ulog_data_msg_t` structs and have nothing to do with file I/O) and appends a fully-populated event struct to the corresponding array. Amortized O(1) append via doubling `realloc`. + +**STATUSTEXT special case**: the current `logging_cb` (line 8) is enabled *after* the pre-scan via `ulog_parser_set_logging_callback` so the ring buffer only captures messages encountered during real playback, not the pre-scan walk. On WASM this needs to change — we need to capture all STATUSTEXT entries during the pre-scan walk into the flat `statustext` array, then dispatch them to the ring buffer at playback time when the cursor crosses each one. Simple enough to gate. + +**After the extraction walk**: + +- Native path (unchanged): `ulog_parser_rewind(&ctx->parser)` at line 507. Playback re-reads the file during `_advance`. +- WASM path: `ulog_parser_close(&ctx->parser)` right after the walk (or immediately free `p->read_buf` and the raw file bytes that `--preload-file` loaded into VFS). Playback never re-reads the file. The parser state (formats, subs, index) can be freed too — it's only needed by `ulog_parser_next`. + +Actually — subtle point worth flagging. The WASM build under `emcc` with `--preload-file` bakes the asset into `.data` which Emscripten's MEMFS stages into the WASM heap at startup. For bundled shipped assets (models, shaders, fonts, themes, textures) that's fine — they live for the lifetime of the process. For a user-uploaded ULog passed through `hawkeye_load_ulog_bytes`, the bytes come in via JS → `HEAPU8.set` → a C buffer we own. We're responsible for freeing that buffer. We free it immediately after extraction. No MEMFS involvement for ULogs. + +## Playback + +### Native path — byte-identical to today + +`ulog_replay_advance` on native keeps calling `ulog_parser_next` exactly as it does now. No change. Literal zero diff in the execution path. + +### WASM path — cursor iteration over flat arrays + +`ulog_replay_advance` on WASM iterates each array in parallel, advancing cursors while `timeline.X[cursor].timestamp_us <= target_usec`. For each advance, it calls the shared apply helper. This is a linear scan per call; cursors only move forward during play. On seek, cursors are binary-searched to the new target. + +```c +#ifdef __EMSCRIPTEN__ +static bool replay_advance_wasm(ulog_replay_ctx_t *ctx, uint64_t target_usec) { + ulog_timeline_t *tl = &ctx->timeline; + + while (ctx->att_cursor < tl->att_count && + tl->att[ctx->att_cursor].timestamp_us <= target_usec) { + apply_attitude(ctx, &tl->att[ctx->att_cursor]); + ctx->att_cursor++; + } + while (ctx->lpos_cursor < tl->lpos_count && + tl->lpos[ctx->lpos_cursor].timestamp_us <= target_usec) { + apply_lpos(ctx, &tl->lpos[ctx->lpos_cursor]); + ctx->lpos_cursor++; + } + // ... similarly for gpos, aspd, vstatus, home, statustext + return ctx->att_cursor < tl->att_count || + ctx->lpos_cursor < tl->lpos_count; // etc +} +#endif +``` + +## Shared apply helpers (drift prevention) + +This is the critical part. If native and WASM have separate state-update implementations, they will desync. Instead, both call the same per-type apply helpers. Native paths *decode* fields from a raw `ulog_data_msg_t` then call the helper; WASM paths read fields directly from the pre-extracted event struct then call the helper. + +### Refactor to extract + +`process_message` at `src/ulog_replay.c:69-258` currently mixes field decoding and state update. Split each branch into: + +- `decode_attitude(dmsg, cache) → ulog_att_event_t` (decode only, no state touched) +- `apply_attitude(ctx, event)` (update ctx->state, touch nothing that requires dmsg) + +Then `process_message` on native becomes: + +```c +static void process_message(ulog_replay_ctx_t *ctx, const ulog_data_msg_t *dmsg) { + int sub_idx = find_subscription(ctx, dmsg->msg_id); + if (sub_idx < 0) return; + ctx->state.time_usec = dmsg->timestamp; + if (ctx->first_pos_set) ctx->state.valid = true; + + if (sub_idx == ctx->sub_attitude) { + ulog_att_event_t ev; + decode_attitude(dmsg, &ctx->cache, &ev); + apply_attitude(ctx, &ev); + } + else if (sub_idx == ctx->sub_global_pos) { /* ... */ } + // etc +} +``` + +WASM's `replay_advance_wasm` skips the decode step entirely and calls `apply_*` directly on the pre-extracted events. + +**Why this is safe:** the apply functions take fully-decoded field values, not raw bytes. They don't know or care whether the data came from a live parser read or an in-memory array. Any bug fix to `apply_attitude` (e.g. state calculation, first-pos flag handling) automatically applies to both paths because there's exactly one implementation. + +**Native cost of the refactor**: two function calls per message instead of one. No heap allocation (the decoded event goes on the stack — ~32 bytes). Inlining will likely collapse this back to the original code size. Benchmarks would be nice but the overhead is negligible. + +### The extraction path reuses decode_* too + +This is the elegant part: `extract_attitude` during the pre-scan walk is literally `decode_attitude` + `timeline_append_att`. Same decode logic as native playback. One source of truth for field decoding, one source of truth for state update. The only thing that differs between native and WASM is **where the data lives** — on disk (re-read per frame) vs in RAM (iterated per frame). + +## Seek semantics + +### Native path — unchanged + +`ulog_replay_seek` already calls `ulog_parser_seek_early` + forward-scan up to 2000 messages (line 653). Leave it alone. + +### WASM path + +`ulog_replay_seek` under `#ifdef __EMSCRIPTEN__` binary-searches each per-type cursor to the largest index where `timestamp_us <= target_usec`. Then it rewinds the cursors slightly and replays forward to rebuild `ctx->state` coherently — same principle as the native path's "seek_early + forward scan" trick, but on in-memory arrays. + +To rebuild state coherently after seek, all arrays must be replayed in timestamp order from some point before `target_usec`. Easiest implementation: on seek, set all cursors to zero (or binary-search each back by N seconds to a chosen resync point), then call `replay_advance_wasm(ctx, target_usec)` which advances each cursor forward to the target. This is O(messages-since-resync), same asymptotic cost as the native forward scan. + +Decision to make during implementation: fully-from-zero rewind on every seek (simple, correct, O(N) per seek) vs. banded resync with a snapshot every K seconds (faster for long flights, more complex). Start with from-zero; profile if it feels sluggish. + +## Subscription indices and cache + +Both native and WASM need `ctx->sub_attitude`, `ctx->sub_local_pos`, etc. and the corresponding field offset cache (`ctx->cache`). These are computed by `ulog_parser_find_subscription` and `ulog_parser_find_field` during init, **before** the walk. That stays unchanged. Even on WASM, the cache gets populated because we need it for the decode step during extraction. + +After extraction, on WASM, `ctx->cache` and `ctx->parser.subs` could be freed since they're only used for decoding raw messages. Keeping them is cheap (~a few KB) and leaves the ctx structurally symmetric between native and WASM, which simplifies debugging. Recommend keeping. + +## Memory lifecycle on WASM + +1. JS handles user-supplied ULog: reads file into a `Uint8Array`, calls `hawkeye_load_ulog_bytes(ptr, len)` +2. C side: `malloc(len)`, `memcpy` from `HEAPU8`, releases the JS ArrayBuffer pressure +3. C side: opens an in-memory "file" (or the parser uses the buffer directly if we pick Option A plumbing — see next section) +4. `ulog_replay_init` runs the normal init, pre-scan extracts events into `ctx->timeline` +5. C side: frees the raw ULog buffer from step 2 +6. Playback runs against `ctx->timeline` with zero file I/O +7. On `hawkeye_destroy`, `ulog_replay_close` frees all timeline arrays + +Peak memory during step 2-4 is the raw file size plus the growing timeline arrays. After step 5, only the timeline. For a typical big ULog, peak might be ~90 MB briefly, settling to ~5 MB. Per drone loaded sequentially, peak is contained. + +## Plumbing: how does the parser read from memory during extraction? + +The parser currently uses `FILE *fp`. For the extraction walk to even run on WASM, the parser needs a memory backend. **Good news**: Emscripten provides `fmemopen(buf, len, "rb")` via its libc, which returns a `FILE *` backed by a memory buffer. All `fread`/`fseek`/`ftell` calls work unchanged against it. + +This means the parser code does **not** need to be modified for WASM at all. A single `#ifdef __EMSCRIPTEN__` at the `fopen` site in `ulog_parser_open` can route to `fmemopen` when called from a new `ulog_parser_open_mem(parser, buf, len)` API: + +```c +int ulog_parser_open_mem(ulog_parser_t *p, const uint8_t *buf, size_t len); +``` + +This wrapper does the header validation and Pass 1/2 logic exactly like `ulog_parser_open`, but uses `fmemopen` instead of `fopen`. Or more elegantly, refactor `ulog_parser_open` to take a `FILE *` already-opened, and add two thin wrappers (`_open_file`, `_open_mem`). Native uses `_open_file`; WASM uses `_open_mem`. + +**Verification needed before committing to this**: confirm Emscripten's `fmemopen` is reliable and supports `fseek` correctly. I'm 95% sure it does (it's a glibc API Emscripten has supported for years), but this is exactly the kind of claim I should verify before building on it rather than discovering a regression during implementation. + +## What changes to `ulog_replay_ctx_t` (header) + +New fields, all gated by `#ifdef __EMSCRIPTEN__` so native sizeof is byte-identical: + +```c +#ifdef __EMSCRIPTEN__ + ulog_timeline_t timeline; + int att_cursor; + int lpos_cursor; + int gpos_cursor; + int aspd_cursor; + int vstatus_cursor; + int home_cursor; + int statustext_cursor; +#endif +``` + +New public function: + +```c +int ulog_replay_init_from_bytes(ulog_replay_ctx_t *ctx, + const uint8_t *buf, size_t len); +``` + +Implemented only under `#ifdef __EMSCRIPTEN__`. Calls `ulog_parser_open_mem` then runs the same init body as `ulog_replay_init`. + +## What does NOT change + +- `ulog_parser.h` public API — unchanged (one new `ulog_parser_open_mem` function; existing functions untouched) +- `ulog_parser.c` core logic — unchanged; only a new wrapper function added +- Native `ulog_replay_init(ctx, filepath)` — unchanged +- Native `ulog_replay_advance` — unchanged (calls `process_message` which calls `decode_*` + `apply_*`) +- Native `ulog_replay_seek` — unchanged +- All fixtures, tests, existing replay behavior on desktop — byte-identical output + +The refactor of `process_message` into `decode_*` + `apply_*` helpers is a **pure refactor with no behavioral change**. It ships on native with no feature flag. Each fixture ULog should produce byte-identical replay state when walked through the refactored code, which is what the prerequisite's verification gate checks. + +## Resolved decisions + +1. **Parser memory backend.** Use libc `fmemopen` via Emscripten's musl-derived stdio. No shim, no custom memory FILE wrapper. Verify during implementation that the Emscripten build links the musl implementation (five-minute check of their libc config), but this is a build-flag check, not a reliability concern — `fmemopen` is a stable POSIX API and musl's implementation is battle-tested. A new `ulog_parser_open_mem(p, buf, len)` wrapper calls `fmemopen` then runs the existing Pass 1/2 validation logic unchanged. +2. **Refactor scope: WASM-only (Option B).** The `decode_*` / `apply_*` split lives entirely under `#ifdef __EMSCRIPTEN__`. Native `process_message` is byte-for-byte unchanged — not a line touched, not a function reordered. Trade-off accepted: a small ongoing drift risk between the two apply paths in exchange for zero risk of regressing native today. If native ever needs the same refactor for its own reasons, that's a separate assessment later. +3. **STATUSTEXT capture timing.** On WASM, the logging callback is registered *before* the extraction walk and routes LOGGING messages into a flat `statustext` array on `ulog_timeline_t`, each entry tagged with its original timestamp. During playback, the statustext cursor advances alongside the other cursors and dispatches entries to the `statustext_ring_t` when their timestamps are crossed. End-user behavior identical to native (ring buffer fills in the right order at the right times). Native path is untouched — it continues to register the callback post-scan and fill the ring during live re-reads of the file. +4. **Seek strategy — mirror native.** Native already solved this at `src/ulog_replay.c:639`: jump close via `ulog_parser_seek_early`, then forward-scan up to 2000 messages calling `process_message` to settle state. WASM does the direct equivalent: binary-search each cursor to a point slightly before the target (the analogue of `seek_early`), then advance forward calling `apply_*` on each event until cursors reach the target. Same bounded cost, same user-facing latency, same "replay a window to settle state" invariant. Not a new design decision — just preserve the strategy and swap the mechanism. +5. **Multi-drone memory pressure on mobile.** Measurement deferred to Step 11 (cross-browser smoke test). No preemptive caps, no preemptive struct packing, no preemptive feature deferral. We find out what iOS Safari actually does with 16 drones when we can measure on a real device, and react then if we overshoot budget. + +### Secondary decisions (deferred, premature to decide now) + +- **LPOS reference point capture timing** — native captures on-the-fly during playback at the first LPOS with `xy_global`. WASM will do the same by having the extraction walk call the same `apply_lpos` helper that sets `ref_set` on first qualifying sample. No need for a one-shot field on `ulog_timeline_t`; the `ctx->ref_*` fields get populated naturally by the extraction walk because it calls `apply_lpos`. +- **Skipping the parser's timestamp index on WASM** — the index is only used for `ulog_parser_seek_early`, which WASM doesn't call. We could skip building it. Tiny optimization; revisit after first green build if binary size or init time is an issue. + +## Verification gate for Task #2 + +This is the same gate from the plan, restated with specific checks: + +1. `git diff src/ulog_parser.h src/ulog_parser.c src/ulog_replay.h src/ulog_replay.c` shows only: + - The `decode_*` / `apply_*` refactor (pure behavioral no-op on native) + - New `#ifdef __EMSCRIPTEN__` blocks + - New `ulog_parser_open_mem` function + - New `ulog_replay_init_from_bytes` function +2. `make release` and `make test` pass on a clean checkout, output byte-identical to pre-change baseline. +3. All fixture ULogs in `tests/fixtures/` (or wherever they live) produce byte-identical replay state when loaded through the refactored native path. This is the strongest drift-prevention check — if native output drifts by even one bit after the refactor, we have a bug. +4. User reviews the diff before any commit. No push. + +--- + +**Summary of deltas from the original plan's Step 2**: same goal, same target memory footprint, same hard-prerequisite role. New specifics: + +- Uses `fmemopen` (pending verification) instead of a bespoke memory backend, keeping `ulog_parser.c` almost untouched +- Refactors `process_message` into `decode_*` + `apply_*` helpers as a pure no-op change on native, shared between both paths to prevent drift +- Per-type event arrays instead of a tagged union, for memory efficiency +- STATUSTEXT capture moves from playback-callback to extraction-callback on WASM, with a flat statustext event array feeding the ring buffer during playback +- Pre-scan walk is **reused** rather than duplicated — the extraction piggybacks on the existing init-time walk, not a separate pass diff --git a/src/ulog_events.h b/src/ulog_events.h new file mode 100644 index 0000000..1a438e2 --- /dev/null +++ b/src/ulog_events.h @@ -0,0 +1,85 @@ +#ifndef ULOG_EVENTS_H +#define ULOG_EVENTS_H + +// Per-type event structs for the pre-extracted replay timeline. On WASM these +// are populated by the ULog extractor at init time and consumed by cursor +// iteration; on native they are the interchange format between the per-branch +// decode stage of process_message() and the shared apply helpers. +// +// Each struct is laid out for tight packing and 8-byte alignment. The sizes +// in the comments are the expected sizeof on a 32-bit Emscripten target. + +#include +#include "ulog_replay.h" // STATUSTEXT_MSG_MAX, statustext_ring_t (type defs only) + +typedef struct { + uint64_t timestamp_us; + float q[4]; // w, x, y, z (direct copy from vehicle_attitude) +} ulog_att_event_t; // 24 bytes + +typedef struct { + uint64_t timestamp_us; + double lat_deg; + double lon_deg; + float alt_m; + float _pad; // explicit pad keeps sizeof stable on both 32-bit and 64-bit +} ulog_gpos_event_t; // 32 bytes + +typedef struct { + uint64_t timestamp_us; + float x, y, z; // NED meters + float vx, vy, vz; // NED m/s +} ulog_lpos_event_t; // 32 bytes + +typedef struct { + uint64_t timestamp_us; + uint16_t ias_cms; // indicated_airspeed_m_s * 100 + uint16_t tas_cms; // true_airspeed_m_s * 100 + uint32_t _pad; +} ulog_aspd_event_t; // 16 bytes + +typedef struct { + uint64_t timestamp_us; + uint8_t vehicle_type; // PX4 enum, pre-translated to MAV_TYPE + uint8_t is_vtol; + uint8_t nav_state; + uint8_t _pad[5]; +} ulog_vstatus_event_t; // 16 bytes + +typedef struct { + uint64_t timestamp_us; + double lat_deg; + double lon_deg; + float alt_m; + uint8_t valid_hpos; + uint8_t _pad[3]; +} ulog_home_event_t; // 32 bytes + +typedef struct { + uint64_t timestamp_us; + uint8_t severity; // 0=EMERG .. 7=DEBUG + uint8_t _pad[7]; + char text[STATUSTEXT_MSG_MAX]; +} ulog_statustext_event_t; // 144 bytes + +// --------------------------------------------------------------------------- +// Flat timeline container — one instance per loaded ULog, owned by the WASM +// replay context. All arrays grow with doubling realloc during extraction and +// are final-sized (realloc shrink) after the walk completes. +// --------------------------------------------------------------------------- + +typedef struct { + ulog_att_event_t *att; int att_count; int att_cap; + ulog_lpos_event_t *lpos; int lpos_count; int lpos_cap; + ulog_gpos_event_t *gpos; int gpos_count; int gpos_cap; + ulog_aspd_event_t *aspd; int aspd_count; int aspd_cap; + ulog_vstatus_event_t *vstatus; int vstatus_count; int vstatus_cap; + ulog_home_event_t *home; int home_count; int home_cap; + ulog_statustext_event_t *statustext; int statustext_count; int statustext_cap; + + // Log-level timestamp bounds (microseconds, absolute from log header) + uint64_t start_timestamp_us; + uint64_t end_timestamp_us; +} ulog_timeline_t; + +#endif diff --git a/src/ulog_replay.c b/src/ulog_replay.c index 55ef762..1be158e 100644 --- a/src/ulog_replay.c +++ b/src/ulog_replay.c @@ -1,4 +1,5 @@ #include "ulog_replay.h" +#include "ulog_replay_apply.h" #include #include @@ -19,47 +20,97 @@ static void logging_cb(const ulog_logging_msg_t *msg, void *userdata) { if (ring->count < STATUSTEXT_RING_SIZE) ring->count++; } -// PX4 nav_state display names -const char *ulog_nav_state_name(uint8_t nav_state) { - switch (nav_state) { - case 0: return "Manual"; - case 1: return "Altitude"; - case 2: return "Position"; - case 3: return "Mission"; - case 4: return "Loiter"; - case 5: return "RTL"; - case 10: return "Acro"; - case 12: return "Descend"; - case 13: return "Terminate"; - case 14: return "Offboard"; - case 15: return "Stabilized"; - case 17: return "Takeoff"; - case 18: return "Land"; - case 19: return "Follow"; - case 20: return "Precland"; - case 21: return "Orbit"; - case 22: return "VTOL Takeoff"; - default: return "Unknown"; +// --------------------------------------------------------------------------- +// Per-message field decode helpers. The event structs (ulog_*_event_t) are +// the interchange format between decode and apply; the same apply helpers +// feed both native (live parser) and WASM (pre-extracted arrays). +// --------------------------------------------------------------------------- + +static void decode_attitude(const ulog_data_msg_t *dmsg, + const ulog_field_cache_t *cache, + ulog_att_event_t *ev) { + ev->timestamp_us = dmsg->timestamp; + int off = cache->att_q_offset; + ev->q[0] = ulog_parser_get_float(dmsg, off + 0); + ev->q[1] = ulog_parser_get_float(dmsg, off + 4); + ev->q[2] = ulog_parser_get_float(dmsg, off + 8); + ev->q[3] = ulog_parser_get_float(dmsg, off + 12); +} + +static void decode_gpos(const ulog_data_msg_t *dmsg, + const ulog_field_cache_t *cache, + ulog_gpos_event_t *ev) { + ev->timestamp_us = dmsg->timestamp; + ev->lat_deg = ulog_parser_get_double(dmsg, cache->gpos_lat_offset); + ev->lon_deg = ulog_parser_get_double(dmsg, cache->gpos_lon_offset); + ev->alt_m = ulog_parser_get_float (dmsg, cache->gpos_alt_offset); + ev->_pad = 0.0f; +} + +static void decode_lpos(const ulog_data_msg_t *dmsg, + const ulog_field_cache_t *cache, + ulog_lpos_event_t *ev) { + ev->timestamp_us = dmsg->timestamp; + ev->x = ulog_parser_get_float(dmsg, cache->lpos_x_offset); + ev->y = ulog_parser_get_float(dmsg, cache->lpos_y_offset); + ev->z = ulog_parser_get_float(dmsg, cache->lpos_z_offset); + ev->vx = ulog_parser_get_float(dmsg, cache->lpos_vx_offset); + ev->vy = ulog_parser_get_float(dmsg, cache->lpos_vy_offset); + ev->vz = ulog_parser_get_float(dmsg, cache->lpos_vz_offset); +} + +static void decode_aspd(const ulog_data_msg_t *dmsg, + const ulog_field_cache_t *cache, + ulog_aspd_event_t *ev) { + ev->timestamp_us = dmsg->timestamp; + float ias = ulog_parser_get_float(dmsg, cache->aspd_ias_offset); + float tas = ulog_parser_get_float(dmsg, cache->aspd_tas_offset); + ev->ias_cms = (uint16_t)(ias * 100.0f); + ev->tas_cms = (uint16_t)(tas * 100.0f); + ev->_pad = 0; +} + +static void decode_vstatus(const ulog_data_msg_t *dmsg, + const ulog_field_cache_t *cache, + ulog_vstatus_event_t *ev) { + ev->timestamp_us = dmsg->timestamp; + uint8_t px4_type = ulog_parser_get_uint8(dmsg, cache->vstatus_type_offset); + uint8_t is_vtol = (cache->vstatus_is_vtol_offset >= 0) && + ulog_parser_get_uint8(dmsg, cache->vstatus_is_vtol_offset); + ev->is_vtol = is_vtol; + + // PX4 vehicle_type enum → MAVLink MAV_TYPE, matching the inline mapping + // that used to live in process_message. + if (is_vtol) { + ev->vehicle_type = 22; // MAV_TYPE_VTOL_FIXEDROTOR + } else { + switch (px4_type) { + case 1: ev->vehicle_type = 2; break; // ROTARY_WING → QUADROTOR + case 2: ev->vehicle_type = 1; break; // FIXED_WING → FIXED_WING + case 3: ev->vehicle_type = 10; break; // ROVER → GROUND_ROVER + default: ev->vehicle_type = 2; break; + } } + + ev->nav_state = (cache->vstatus_nav_state_offset >= 0) + ? ulog_parser_get_uint8(dmsg, cache->vstatus_nav_state_offset) + : 0; + memset(ev->_pad, 0, sizeof(ev->_pad)); } -// Approximate meters-per-degree at a given latitude -#define DEG_TO_RAD (3.14159265358979323846 / 180.0) -static void local_to_global(double ref_lat, double ref_lon, float ref_alt, - float x, float y, float z, - int32_t *lat_e7, int32_t *lon_e7, int32_t *alt_mm) { - // x = North, y = East, z = Down (NED) - double meters_per_deg_lat = 111132.92; - double meters_per_deg_lon = 111132.92 * cos(ref_lat * DEG_TO_RAD); - if (meters_per_deg_lon < 1.0) meters_per_deg_lon = 1.0; - - double lat = ref_lat + (double)x / meters_per_deg_lat; - double lon = ref_lon + (double)y / meters_per_deg_lon; - float alt = ref_alt - z; // NED z is down, alt is up - - *lat_e7 = (int32_t)(lat * 1e7); - *lon_e7 = (int32_t)(lon * 1e7); - *alt_mm = (int32_t)(alt * 1000.0f); +static void decode_home(const ulog_data_msg_t *dmsg, + const ulog_field_cache_t *cache, + ulog_home_event_t *ev) { + ev->timestamp_us = dmsg->timestamp; + ev->lat_deg = (cache->home_lat_offset >= 0) + ? ulog_parser_get_double(dmsg, cache->home_lat_offset) : 0.0; + ev->lon_deg = (cache->home_lon_offset >= 0) + ? ulog_parser_get_double(dmsg, cache->home_lon_offset) : 0.0; + ev->alt_m = (cache->home_alt_offset >= 0) + ? ulog_parser_get_float(dmsg, cache->home_alt_offset) : 0.0f; + ev->valid_hpos = (cache->home_valid_hpos_offset >= 0) + ? ulog_parser_get_uint8(dmsg, cache->home_valid_hpos_offset) : 0; + memset(ev->_pad, 0, sizeof(ev->_pad)); } // --------------------------------------------------------------------------- @@ -82,178 +133,65 @@ static void process_message(ulog_replay_ctx_t *ctx, const ulog_data_msg_t *dmsg) if (ctx->first_pos_set) ctx->state.valid = true; - // home_position topic — authoritative home (Tier 1) - // Skip if pre-scan rejected this log's home (no GPOS data to confirm it) - if (sub_idx == ctx->sub_home_pos && ctx->sub_home_pos >= 0 && !ctx->home_rejected) { - double lat = 0, lon = 0; - float alt = 0; - if (ctx->cache.home_lat_offset >= 0) - lat = ulog_parser_get_double(dmsg, ctx->cache.home_lat_offset); - if (ctx->cache.home_lon_offset >= 0) - lon = ulog_parser_get_double(dmsg, ctx->cache.home_lon_offset); - if (ctx->cache.home_alt_offset >= 0) - alt = ulog_parser_get_float(dmsg, ctx->cache.home_alt_offset); - if (lat != 0.0 || lon != 0.0) { - ctx->home.lat = (int32_t)(lat * 1e7); - ctx->home.lon = (int32_t)(lon * 1e7); - ctx->home.alt = (int32_t)(alt * 1000.0f); - ctx->home.valid = true; - ctx->home_from_topic = true; - } + // home_position topic — authoritative home (Tier 1). + // Skip if pre-scan rejected this log's home (no GPOS data to confirm it). + if (sub_idx == ctx->sub_home_pos && ctx->sub_home_pos >= 0) { + ulog_home_event_t ev; + decode_home(dmsg, &ctx->cache, &ev); + ulog_apply_home(ctx, &ev); } if (sub_idx == ctx->sub_attitude) { - // float[4] q — NED quaternion (w,x,y,z) — direct copy - int off = ctx->cache.att_q_offset; - ctx->state.quaternion[0] = ulog_parser_get_float(dmsg, off + 0); - ctx->state.quaternion[1] = ulog_parser_get_float(dmsg, off + 4); - ctx->state.quaternion[2] = ulog_parser_get_float(dmsg, off + 8); - ctx->state.quaternion[3] = ulog_parser_get_float(dmsg, off + 12); + ulog_att_event_t ev; + decode_attitude(dmsg, &ctx->cache, &ev); + ulog_apply_attitude(ctx, &ev); } else if (ctx->sub_global_pos >= 0 && sub_idx == ctx->sub_global_pos) { - // lat/lon: double (degrees) -> int32_t (degE7) - // alt: float (meters) -> int32_t (mm) - double lat = ulog_parser_get_double(dmsg, ctx->cache.gpos_lat_offset); - double lon = ulog_parser_get_double(dmsg, ctx->cache.gpos_lon_offset); - float alt = ulog_parser_get_float(dmsg, ctx->cache.gpos_alt_offset); - - ctx->state.lat = (int32_t)(lat * 1e7); - ctx->state.lon = (int32_t)(lon * 1e7); - ctx->state.alt = (int32_t)(alt * 1000.0f); - - // Derive velocity from consecutive GPOS samples (GPOS has no velocity fields) - if (ctx->prev_gpos_usec > 0 && dmsg->timestamp > ctx->prev_gpos_usec) { - double dt_gpos = (double)(dmsg->timestamp - ctx->prev_gpos_usec) / 1e6; - if (dt_gpos > 0.01 && dt_gpos < 5.0) { - double meters_per_deg_lat = 111132.92; - double meters_per_deg_lon = 111132.92 * cos(lat * DEG_TO_RAD); - if (meters_per_deg_lon < 1.0) meters_per_deg_lon = 1.0; - ctx->gpos_vx = (float)((lat - ctx->prev_lat_deg) * meters_per_deg_lat / dt_gpos); - ctx->gpos_vy = (float)((lon - ctx->prev_lon_deg) * meters_per_deg_lon / dt_gpos); - ctx->gpos_vz = (float)(-(alt - ctx->prev_alt_m) / dt_gpos); // alt up, z down - } - } - ctx->prev_lat_deg = lat; - ctx->prev_lon_deg = lon; - ctx->prev_alt_m = alt; - ctx->prev_gpos_usec = dmsg->timestamp; - - ctx->has_global_pos = true; - - // Store for dead-reckoning interpolation - ctx->last_lat_deg = lat; - ctx->last_lon_deg = lon; - ctx->last_alt_m = alt; - ctx->last_pos_usec = dmsg->timestamp; - - if (!ctx->first_pos_set) { - if (!ctx->home_from_topic) { - ctx->home.lat = ctx->state.lat; - ctx->home.lon = ctx->state.lon; - ctx->home.alt = ctx->state.alt; - ctx->home.valid = true; - } - ctx->first_pos_set = true; - ctx->state.valid = true; - } + ulog_gpos_event_t ev; + decode_gpos(dmsg, &ctx->cache, &ev); + ulog_apply_gpos(ctx, &ev); } else if (sub_idx == ctx->sub_local_pos) { - // vx/vy/vz: float (m/s NED) -> int16_t (cm/s) - float vx = ulog_parser_get_float(dmsg, ctx->cache.lpos_vx_offset); - float vy = ulog_parser_get_float(dmsg, ctx->cache.lpos_vy_offset); - float vz = ulog_parser_get_float(dmsg, ctx->cache.lpos_vz_offset); - ctx->state.vx = (int16_t)(vx * 100.0f); - ctx->state.vy = (int16_t)(vy * 100.0f); - ctx->state.vz = (int16_t)(vz * 100.0f); - - // Store velocity and local position for dead-reckoning - ctx->last_vx = vx; - ctx->last_vy = vy; - ctx->last_vz = vz; - - float x = ulog_parser_get_float(dmsg, ctx->cache.lpos_x_offset); - float y = ulog_parser_get_float(dmsg, ctx->cache.lpos_y_offset); - float z = ulog_parser_get_float(dmsg, ctx->cache.lpos_z_offset); - ctx->last_x = x; - ctx->last_y = y; - ctx->last_z = z; - - if (!ctx->has_global_pos) { - ctx->last_pos_usec = dmsg->timestamp; - - // Try to get reference point from local position if not yet set - if (!ctx->ref_set && ctx->cache.lpos_xy_global_offset >= 0) { - uint8_t xy_global = ulog_parser_get_uint8(dmsg, ctx->cache.lpos_xy_global_offset); - if (xy_global) { - double rlat = ulog_parser_get_double(dmsg, ctx->cache.lpos_ref_lat_offset); - double rlon = ulog_parser_get_double(dmsg, ctx->cache.lpos_ref_lon_offset); - // Sanity check: reject garbage reference coordinates - if (rlat < -90.0 || rlat > 90.0 || rlon < -180.0 || rlon > 180.0) { - if (!ctx->ref_rejected) { - printf(" Warning: LPOS ref out of range (lat=%.1f, lon=%.1f), ignoring\n", rlat, rlon); - ctx->ref_rejected = true; - } - } else { - ctx->ref_lat = rlat; - ctx->ref_lon = rlon; - if (ctx->cache.lpos_z_global_offset >= 0 && - ulog_parser_get_uint8(dmsg, ctx->cache.lpos_z_global_offset)) { - ctx->ref_alt = ulog_parser_get_float(dmsg, ctx->cache.lpos_ref_alt_offset); - } - ctx->ref_set = true; + // LPOS reference-frame capture has to happen with raw parser bytes + // in hand (xy_global / ref_lat / ref_lon / ref_alt aren't fields on + // the compact lpos event struct). Done here, before apply_lpos, + // which reads ctx->ref_* as already-settled state. + if (!ctx->has_global_pos && !ctx->ref_set && + ctx->cache.lpos_xy_global_offset >= 0) { + uint8_t xy_global = ulog_parser_get_uint8(dmsg, ctx->cache.lpos_xy_global_offset); + if (xy_global) { + double rlat = ulog_parser_get_double(dmsg, ctx->cache.lpos_ref_lat_offset); + double rlon = ulog_parser_get_double(dmsg, ctx->cache.lpos_ref_lon_offset); + if (rlat < -90.0 || rlat > 90.0 || rlon < -180.0 || rlon > 180.0) { + if (!ctx->ref_rejected) { + printf(" Warning: LPOS ref out of range (lat=%.1f, lon=%.1f), ignoring\n", rlat, rlon); + ctx->ref_rejected = true; } - } - } - - // Require horizontal position (x/y or reference frame) to produce - // a meaningful global position. z-only (baro) without a reference - // can't give valid lat/lon and would mix incompatible altitudes. - bool has_pos = (x != 0.0f || y != 0.0f) || ctx->ref_set; - - if (has_pos) { - local_to_global(ctx->ref_lat, ctx->ref_lon, ctx->ref_alt, - x, y, z, - &ctx->state.lat, &ctx->state.lon, &ctx->state.alt); - - if (!ctx->first_pos_set) { - if (!ctx->home_from_topic) { - ctx->home.lat = ctx->state.lat; - ctx->home.lon = ctx->state.lon; - ctx->home.alt = ctx->state.alt; - ctx->home.valid = true; + } else { + ctx->ref_lat = rlat; + ctx->ref_lon = rlon; + if (ctx->cache.lpos_z_global_offset >= 0 && + ulog_parser_get_uint8(dmsg, ctx->cache.lpos_z_global_offset)) { + ctx->ref_alt = ulog_parser_get_float(dmsg, ctx->cache.lpos_ref_alt_offset); } - ctx->first_pos_set = true; - ctx->state.valid = true; + ctx->ref_set = true; } } } + + ulog_lpos_event_t ev; + decode_lpos(dmsg, &ctx->cache, &ev); + ulog_apply_lpos(ctx, &ev); } else if (ctx->sub_airspeed >= 0 && sub_idx == ctx->sub_airspeed) { - // float (m/s) -> uint16_t (cm/s) - float ias = ulog_parser_get_float(dmsg, ctx->cache.aspd_ias_offset); - float tas = ulog_parser_get_float(dmsg, ctx->cache.aspd_tas_offset); - ctx->state.ind_airspeed = (uint16_t)(ias * 100.0f); - ctx->state.true_airspeed = (uint16_t)(tas * 100.0f); + ulog_aspd_event_t ev; + decode_aspd(dmsg, &ctx->cache, &ev); + ulog_apply_aspd(ctx, &ev); } else if (ctx->sub_vehicle_status >= 0 && sub_idx == ctx->sub_vehicle_status) { - // PX4 vehicle_type enum → MAVLink MAV_TYPE - uint8_t px4_type = ulog_parser_get_uint8(dmsg, ctx->cache.vstatus_type_offset); - bool is_vtol = ctx->cache.vstatus_is_vtol_offset >= 0 && - ulog_parser_get_uint8(dmsg, ctx->cache.vstatus_is_vtol_offset); - if (is_vtol) { - ctx->vehicle_type = 22; // MAV_TYPE_VTOL_FIXEDROTOR - } else { - switch (px4_type) { - case 1: ctx->vehicle_type = 2; break; // ROTARY_WING → MAV_TYPE_QUADROTOR - case 2: ctx->vehicle_type = 1; break; // FIXED_WING → MAV_TYPE_FIXED_WING - case 3: ctx->vehicle_type = 10; break; // ROVER → MAV_TYPE_GROUND_ROVER - default: ctx->vehicle_type = 2; break; - } - } - // Track current nav_state for HUD display - if (ctx->cache.vstatus_nav_state_offset >= 0) { - ctx->current_nav_state = ulog_parser_get_uint8(dmsg, ctx->cache.vstatus_nav_state_offset); - } + ulog_vstatus_event_t ev; + decode_vstatus(dmsg, &ctx->cache, &ev); + ulog_apply_vstatus(ctx, &ev); } } @@ -584,7 +522,7 @@ bool ulog_replay_advance(ulog_replay_ctx_t *ctx, float dt, float speed, bool loo } double meters_per_deg_lat = 111132.92; - double meters_per_deg_lon = 111132.92 * cos(ctx->last_lat_deg * DEG_TO_RAD); + double meters_per_deg_lon = 111132.92 * cos(ctx->last_lat_deg * ULOG_DEG_TO_RAD); if (meters_per_deg_lon < 1.0) meters_per_deg_lon = 1.0; double lat = ctx->last_lat_deg + (double)(vx * dt_pos) / meters_per_deg_lat; @@ -600,9 +538,9 @@ bool ulog_replay_advance(ulog_replay_ctx_t *ctx, float dt, float speed, bool loo float y = ctx->last_y + ctx->last_vy * dt_pos; float z = ctx->last_z + ctx->last_vz * dt_pos; - local_to_global(ctx->ref_lat, ctx->ref_lon, ctx->ref_alt, - x, y, z, - &ctx->state.lat, &ctx->state.lon, &ctx->state.alt); + ulog_local_to_global(ctx->ref_lat, ctx->ref_lon, ctx->ref_alt, + x, y, z, + &ctx->state.lat, &ctx->state.lon, &ctx->state.alt); } } @@ -671,7 +609,7 @@ void ulog_replay_seek(ulog_replay_ctx_t *ctx, float target_s) { } double meters_per_deg_lat = 111132.92; - double meters_per_deg_lon = 111132.92 * cos(ctx->last_lat_deg * DEG_TO_RAD); + double meters_per_deg_lon = 111132.92 * cos(ctx->last_lat_deg * ULOG_DEG_TO_RAD); if (meters_per_deg_lon < 1.0) meters_per_deg_lon = 1.0; double lat = ctx->last_lat_deg + (double)(vx * dt_pos) / meters_per_deg_lat; @@ -686,9 +624,9 @@ void ulog_replay_seek(ulog_replay_ctx_t *ctx, float target_s) { float y = ctx->last_y + ctx->last_vy * dt_pos; float z = ctx->last_z + ctx->last_vz * dt_pos; - local_to_global(ctx->ref_lat, ctx->ref_lon, ctx->ref_alt, - x, y, z, - &ctx->state.lat, &ctx->state.lon, &ctx->state.alt); + ulog_local_to_global(ctx->ref_lat, ctx->ref_lon, ctx->ref_alt, + x, y, z, + &ctx->state.lat, &ctx->state.lon, &ctx->state.alt); } } } diff --git a/src/ulog_replay.h b/src/ulog_replay.h index b254f13..dd57a80 100644 --- a/src/ulog_replay.h +++ b/src/ulog_replay.h @@ -65,9 +65,14 @@ typedef struct { // Prevents runtime process_message from re-validating home during playback. typedef struct { +#ifndef __EMSCRIPTEN__ + // Parser-backed decode state. Native reads the ULog live during replay + // and uses these fields to find subscriptions + resolve field offsets. + // On WASM the timeline is pre-extracted, so this machinery is dead + // weight (~1.3 MB/drone from the formats[256] array) and is excluded + // from the build. The shared apply helpers never touch these fields. ulog_parser_t parser; - // Subscription indices (-1 if topic not found in log) int sub_attitude; int sub_global_pos; int sub_local_pos; @@ -76,6 +81,7 @@ typedef struct { int sub_home_pos; ulog_field_cache_t cache; +#endif // Current output state hil_state_t state; @@ -128,6 +134,27 @@ typedef struct { // STATUSTEXT messages (populated during playback) statustext_ring_t statustext; + +#ifdef __EMSCRIPTEN__ + // WASM-only: pre-extracted event arrays + per-type cursors. The WASM + // replay path walks the ULog once at init time, populates these arrays, + // and releases the raw file bytes. Playback then iterates cursors over + // the flat arrays instead of re-reading the file. Lives inside the + // native struct under an ifdef so native sizeof is unchanged and the + // shared apply helpers can accept a single ctx type on both targets. + // + // The ulog_timeline type and its append/find helpers are defined in + // src/ulog_events.h + src/wasm/ulog_timeline.h. We hold it as an + // opaque pointer here so ulog_replay.h doesn't have to pull in either. + void *timeline; + int att_cursor; + int lpos_cursor; + int gpos_cursor; + int aspd_cursor; + int vstatus_cursor; + int home_cursor; + int statustext_cursor; +#endif } ulog_replay_ctx_t; // Initialize replay context, parse file, build index. Returns 0 on success. diff --git a/src/ulog_replay_apply.c b/src/ulog_replay_apply.c new file mode 100644 index 0000000..084ea7e --- /dev/null +++ b/src/ulog_replay_apply.c @@ -0,0 +1,175 @@ +#include "ulog_replay_apply.h" + +#include +#include + +// PX4 nav_state display names. Lives in the shared file so both native (which +// compiles this alongside ulog_replay.c) and WASM (which compiles only this +// subset of the replay layer) link against a single copy. +const char *ulog_nav_state_name(uint8_t nav_state) { + switch (nav_state) { + case 0: return "Manual"; + case 1: return "Altitude"; + case 2: return "Position"; + case 3: return "Mission"; + case 4: return "Loiter"; + case 5: return "RTL"; + case 10: return "Acro"; + case 12: return "Descend"; + case 13: return "Terminate"; + case 14: return "Offboard"; + case 15: return "Stabilized"; + case 17: return "Takeoff"; + case 18: return "Land"; + case 19: return "Follow"; + case 20: return "Precland"; + case 21: return "Orbit"; + case 22: return "VTOL Takeoff"; + default: return "Unknown"; + } +} + +// Approximate meters-per-degree at a given latitude (matches the inline helper +// that used to live in ulog_replay.c and wasm/wasm_replay.c). +void ulog_local_to_global(double ref_lat, double ref_lon, float ref_alt, + float x, float y, float z, + int32_t *lat_e7, int32_t *lon_e7, int32_t *alt_mm) { + double meters_per_deg_lat = 111132.92; + double meters_per_deg_lon = 111132.92 * cos(ref_lat * ULOG_DEG_TO_RAD); + if (meters_per_deg_lon < 1.0) meters_per_deg_lon = 1.0; + + double lat = ref_lat + (double)x / meters_per_deg_lat; + double lon = ref_lon + (double)y / meters_per_deg_lon; + float alt = ref_alt - z; // NED z is down, alt is up + + *lat_e7 = (int32_t)(lat * 1e7); + *lon_e7 = (int32_t)(lon * 1e7); + *alt_mm = (int32_t)(alt * 1000.0f); +} + +void ulog_apply_attitude(ulog_replay_ctx_t *ctx, const ulog_att_event_t *ev) { + ctx->state.time_usec = ev->timestamp_us; + if (ctx->first_pos_set) ctx->state.valid = true; + ctx->state.quaternion[0] = ev->q[0]; + ctx->state.quaternion[1] = ev->q[1]; + ctx->state.quaternion[2] = ev->q[2]; + ctx->state.quaternion[3] = ev->q[3]; +} + +void ulog_apply_gpos(ulog_replay_ctx_t *ctx, const ulog_gpos_event_t *ev) { + ctx->state.time_usec = ev->timestamp_us; + if (ctx->first_pos_set) ctx->state.valid = true; + + ctx->state.lat = (int32_t)(ev->lat_deg * 1e7); + ctx->state.lon = (int32_t)(ev->lon_deg * 1e7); + ctx->state.alt = (int32_t)(ev->alt_m * 1000.0f); + + // GPOS has no velocity fields — derive from consecutive samples. + if (ctx->prev_gpos_usec > 0 && ev->timestamp_us > ctx->prev_gpos_usec) { + double dt_gpos = (double)(ev->timestamp_us - ctx->prev_gpos_usec) / 1e6; + if (dt_gpos > 0.01 && dt_gpos < 5.0) { + double meters_per_deg_lat = 111132.92; + double meters_per_deg_lon = 111132.92 * cos(ev->lat_deg * ULOG_DEG_TO_RAD); + if (meters_per_deg_lon < 1.0) meters_per_deg_lon = 1.0; + ctx->gpos_vx = (float)((ev->lat_deg - ctx->prev_lat_deg) * meters_per_deg_lat / dt_gpos); + ctx->gpos_vy = (float)((ev->lon_deg - ctx->prev_lon_deg) * meters_per_deg_lon / dt_gpos); + ctx->gpos_vz = (float)(-(ev->alt_m - ctx->prev_alt_m) / dt_gpos); + } + } + ctx->prev_lat_deg = ev->lat_deg; + ctx->prev_lon_deg = ev->lon_deg; + ctx->prev_alt_m = ev->alt_m; + ctx->prev_gpos_usec = ev->timestamp_us; + + ctx->has_global_pos = true; + + ctx->last_lat_deg = ev->lat_deg; + ctx->last_lon_deg = ev->lon_deg; + ctx->last_alt_m = ev->alt_m; + ctx->last_pos_usec = ev->timestamp_us; + + if (!ctx->first_pos_set) { + if (!ctx->home_from_topic) { + ctx->home.lat = ctx->state.lat; + ctx->home.lon = ctx->state.lon; + ctx->home.alt = ctx->state.alt; + ctx->home.valid = true; + } + ctx->first_pos_set = true; + ctx->state.valid = true; + } +} + +void ulog_apply_lpos(ulog_replay_ctx_t *ctx, const ulog_lpos_event_t *ev) { + ctx->state.time_usec = ev->timestamp_us; + if (ctx->first_pos_set) ctx->state.valid = true; + + ctx->state.vx = (int16_t)(ev->vx * 100.0f); + ctx->state.vy = (int16_t)(ev->vy * 100.0f); + ctx->state.vz = (int16_t)(ev->vz * 100.0f); + + ctx->last_vx = ev->vx; + ctx->last_vy = ev->vy; + ctx->last_vz = ev->vz; + ctx->last_x = ev->x; + ctx->last_y = ev->y; + ctx->last_z = ev->z; + + if (!ctx->has_global_pos) { + ctx->last_pos_usec = ev->timestamp_us; + + // Require horizontal position (x/y or reference frame) to produce a + // meaningful global position. z-only (baro) without a reference + // can't yield valid lat/lon. Ref capture is handled by the caller + // (native: inline in process_message; WASM: extractor pre-pass). + bool has_pos = (ev->x != 0.0f || ev->y != 0.0f) || ctx->ref_set; + + if (has_pos) { + ulog_local_to_global(ctx->ref_lat, ctx->ref_lon, ctx->ref_alt, + ev->x, ev->y, ev->z, + &ctx->state.lat, &ctx->state.lon, &ctx->state.alt); + + if (!ctx->first_pos_set) { + if (!ctx->home_from_topic) { + ctx->home.lat = ctx->state.lat; + ctx->home.lon = ctx->state.lon; + ctx->home.alt = ctx->state.alt; + ctx->home.valid = true; + } + ctx->first_pos_set = true; + ctx->state.valid = true; + } + } + } +} + +void ulog_apply_aspd(ulog_replay_ctx_t *ctx, const ulog_aspd_event_t *ev) { + ctx->state.time_usec = ev->timestamp_us; + if (ctx->first_pos_set) ctx->state.valid = true; + ctx->state.ind_airspeed = ev->ias_cms; + ctx->state.true_airspeed = ev->tas_cms; +} + +void ulog_apply_vstatus(ulog_replay_ctx_t *ctx, const ulog_vstatus_event_t *ev) { + ctx->state.time_usec = ev->timestamp_us; + if (ctx->first_pos_set) ctx->state.valid = true; + // vehicle_type already pre-translated to MAV_TYPE by the decode step + ctx->vehicle_type = ev->vehicle_type; + ctx->current_nav_state = ev->nav_state; +} + +void ulog_apply_home(ulog_replay_ctx_t *ctx, const ulog_home_event_t *ev) { + ctx->state.time_usec = ev->timestamp_us; + if (ctx->first_pos_set) ctx->state.valid = true; + + // home_rejected: pre-scan determined this log has no GPOS to confirm + // home, so ignore home_position topic updates (native-compatible). + if (ctx->home_rejected) return; + if (ev->lat_deg == 0.0 && ev->lon_deg == 0.0) return; + + ctx->home.lat = (int32_t)(ev->lat_deg * 1e7); + ctx->home.lon = (int32_t)(ev->lon_deg * 1e7); + ctx->home.alt = (int32_t)(ev->alt_m * 1000.0f); + ctx->home.valid = true; + ctx->home_from_topic = true; +} diff --git a/src/ulog_replay_apply.h b/src/ulog_replay_apply.h new file mode 100644 index 0000000..9244eb4 --- /dev/null +++ b/src/ulog_replay_apply.h @@ -0,0 +1,36 @@ +#ifndef ULOG_REPLAY_APPLY_H +#define ULOG_REPLAY_APPLY_H + +// Shared per-message state-update helpers for ULog replay. One source of truth +// between native (decodes live from the parser and calls apply) and WASM +// (pre-extracts events at init time, then iterates cursors calling apply). +// +// Each apply function takes a fully-decoded event struct and a ulog_replay_ctx_t. +// The decode step is platform-specific (native reads raw parser bytes, WASM +// reads pre-extracted arrays); the apply step is shared. + +#include + +#include "ulog_replay.h" +#include "ulog_events.h" + +#define ULOG_DEG_TO_RAD (3.14159265358979323846 / 180.0) + +// Convert NED local coordinates (x,y,z meters) to lat/lon/alt (e7 / mm) +// given a reference frame. Shared by native lpos handling and WASM +// interpolation/apply paths. +void ulog_local_to_global(double ref_lat, double ref_lon, float ref_alt, + float x, float y, float z, + int32_t *lat_e7, int32_t *lon_e7, int32_t *alt_mm); + +// Per-message apply helpers. Each updates ctx state from a decoded event. +// Callers must have already populated ctx->cache + subscription indices (on +// native) or ctx->ref_* (on WASM, set by the extractor pre-pass). +void ulog_apply_attitude(ulog_replay_ctx_t *ctx, const ulog_att_event_t *ev); +void ulog_apply_gpos (ulog_replay_ctx_t *ctx, const ulog_gpos_event_t *ev); +void ulog_apply_lpos (ulog_replay_ctx_t *ctx, const ulog_lpos_event_t *ev); +void ulog_apply_aspd (ulog_replay_ctx_t *ctx, const ulog_aspd_event_t *ev); +void ulog_apply_vstatus (ulog_replay_ctx_t *ctx, const ulog_vstatus_event_t *ev); +void ulog_apply_home (ulog_replay_ctx_t *ctx, const ulog_home_event_t *ev); + +#endif diff --git a/src/wasm/ulog_extractor.c b/src/wasm/ulog_extractor.c new file mode 100644 index 0000000..c2a4183 --- /dev/null +++ b/src/wasm/ulog_extractor.c @@ -0,0 +1,817 @@ +// WASM-only ULog extractor. See header for purpose. Structure mirrors +// src/ulog_parser.c + the pre-scan loop inside src/ulog_replay.c at +// init-time, but is a freshly-written implementation so that no modification +// to native source files is required. +// +// Single-pass design: one read of the file walks both the definition section +// and the data section. Format tables, subscription tables, field offset +// caches, and pre-scan state are all built up as messages arrive. Events are +// appended to the timeline as they stream past. After the walk, the raw +// file bytes can be freed by the caller. + +#include "ulog_extractor.h" +#include "ulog_timeline.h" + +#include +#include +#include + +// --------------------------------------------------------------------------- +// ULog binary format constants +// --------------------------------------------------------------------------- + +#define ULOG_HEADER_SIZE 16 +#define ULOG_MSG_HEADER_SIZE 3 + +#define ULOG_MSG_FORMAT 'F' +#define ULOG_MSG_ADD_LOGGED 'A' +#define ULOG_MSG_DATA 'D' +#define ULOG_MSG_LOGGING 'L' + +static const uint8_t ULOG_MAGIC[7] = { 'U', 'L', 'o', 'g', 0x01, 0x12, 0x35 }; + +#define EXTRACTOR_MAX_FORMATS 256 +#define EXTRACTOR_MAX_SUBS 256 +#define EXTRACTOR_MAX_FIELDS 64 + +// --------------------------------------------------------------------------- +// Internal types (freshly written, parallel to native's equivalents) +// --------------------------------------------------------------------------- + +typedef enum { + FT_UINT8, FT_INT8, + FT_UINT16, FT_INT16, + FT_UINT32, FT_INT32, + FT_UINT64, FT_INT64, + FT_FLOAT, FT_DOUBLE, + FT_BOOL, FT_CHAR, + FT_UNKNOWN, +} field_type_t; + +typedef struct { + char name[64]; + field_type_t type; + int array_size; + int offset; // byte offset within DATA payload (after msg_id) +} field_t; + +typedef struct { + char name[64]; + field_t fields[EXTRACTOR_MAX_FIELDS]; + int field_count; + int size; // total payload bytes +} format_t; + +typedef struct { + uint16_t msg_id; + uint8_t multi_id; + char message_name[64]; + int format_idx; // index into formats[] +} subscription_t; + +// Field offset cache, resolved lazily when the first DATA message for each +// subscription is seen. Matches the slots defined in ulog_field_cache_t on +// native, same field names where they carry the same meaning. +typedef struct { + int att_q_offset; + + int gpos_lat_offset, gpos_lon_offset, gpos_alt_offset; + + int lpos_x_offset, lpos_y_offset, lpos_z_offset; + int lpos_vx_offset, lpos_vy_offset, lpos_vz_offset; + int lpos_ref_lat_offset, lpos_ref_lon_offset, lpos_ref_alt_offset; + int lpos_xy_global_offset, lpos_z_global_offset; + + int aspd_ias_offset, aspd_tas_offset; + + int vstatus_type_offset, vstatus_is_vtol_offset, vstatus_nav_state_offset; + + int home_lat_offset, home_lon_offset, home_alt_offset, home_valid_hpos_offset; +} field_cache_t; + +// --------------------------------------------------------------------------- +// Walker state — everything the walk needs, kept in one struct so the +// recursive helpers don't need long argument lists +// --------------------------------------------------------------------------- + +typedef struct { + const uint8_t *buf; + size_t len; + size_t pos; // current read offset into buf + + format_t formats[EXTRACTOR_MAX_FORMATS]; + int format_count; + + subscription_t subs[EXTRACTOR_MAX_SUBS]; + int sub_count; + + field_cache_t cache; + + // Cached subscription indices (-1 if topic not present in this log) + int sub_attitude; + int sub_global_pos; + int sub_local_pos; + int sub_airspeed; + int sub_vehicle_status; + int sub_home_pos; + + // Cached msg_ids for faster dispatch + uint16_t att_msg_id; + uint16_t gpos_msg_id; + uint16_t lpos_msg_id; + uint16_t aspd_msg_id; + uint16_t vstatus_msg_id; + uint16_t home_msg_id; + + // Output (populated during walk) + ulog_extract_result_t *out; + uint64_t start_timestamp_us; // populated from first DATA message + uint8_t first_seen; + + // Pre-scan bookkeeping + uint8_t got_vehicle_type; + uint8_t got_home; + uint8_t seen_gpos_data; + uint8_t prev_nav; + + // CUSUM takeoff detection state + float cusum_s; + float cusum_k; + float cusum_h; + uint8_t cusum_triggered; + float cusum_trigger_time; + float cusum_peak; + + // read scratch buffer sized to the current message + uint8_t *read_buf; + int read_buf_cap; +} walker_t; + +// --------------------------------------------------------------------------- +// Byte-level reads (unaligned-safe via memcpy) +// --------------------------------------------------------------------------- + +static float read_float_at(const uint8_t *buf, int offset) { + float v; memcpy(&v, buf + offset, sizeof(v)); return v; +} +static double read_double_at(const uint8_t *buf, int offset) { + double v; memcpy(&v, buf + offset, sizeof(v)); return v; +} +static uint8_t read_uint8_at(const uint8_t *buf, int offset) { + return buf[offset]; +} + +// Buffer reads (bounds-checked against walker) +static int w_read(walker_t *w, void *dst, size_t n) { + if (w->pos + n > w->len) return -1; + memcpy(dst, w->buf + w->pos, n); + w->pos += n; + return 0; +} + +static int w_skip(walker_t *w, size_t n) { + if (w->pos + n > w->len) return -1; + w->pos += n; + return 0; +} + +static int ensure_read_buf(walker_t *w, int needed) { + if (needed <= w->read_buf_cap) return 0; + int new_cap = w->read_buf_cap ? w->read_buf_cap : 4096; + while (new_cap < needed) new_cap *= 2; + uint8_t *new_buf = realloc(w->read_buf, new_cap); + if (!new_buf) return -1; + w->read_buf = new_buf; + w->read_buf_cap = new_cap; + return 0; +} + +// --------------------------------------------------------------------------- +// Format string parsing (matches native's parse_format / parse_type_str) +// --------------------------------------------------------------------------- + +static field_type_t parse_type_str(const char *s) { + if (strcmp(s, "float") == 0) return FT_FLOAT; + if (strcmp(s, "double") == 0) return FT_DOUBLE; + if (strcmp(s, "uint8_t") == 0) return FT_UINT8; + if (strcmp(s, "int8_t") == 0) return FT_INT8; + if (strcmp(s, "uint16_t") == 0) return FT_UINT16; + if (strcmp(s, "int16_t") == 0) return FT_INT16; + if (strcmp(s, "uint32_t") == 0) return FT_UINT32; + if (strcmp(s, "int32_t") == 0) return FT_INT32; + if (strcmp(s, "uint64_t") == 0) return FT_UINT64; + if (strcmp(s, "int64_t") == 0) return FT_INT64; + if (strcmp(s, "bool") == 0) return FT_BOOL; + if (strcmp(s, "char") == 0) return FT_CHAR; + return FT_UNKNOWN; +} + +static int field_elem_size(field_type_t t) { + switch (t) { + case FT_UINT8: case FT_INT8: case FT_BOOL: case FT_CHAR: return 1; + case FT_UINT16: case FT_INT16: return 2; + case FT_UINT32: case FT_INT32: case FT_FLOAT: return 4; + case FT_UINT64: case FT_INT64: case FT_DOUBLE: return 8; + default: return 0; + } +} + +static int parse_format(const char *fmt_str, format_t *fmt) { + memset(fmt, 0, sizeof(*fmt)); + const char *colon = strchr(fmt_str, ':'); + if (!colon) return -1; + + int name_len = (int)(colon - fmt_str); + if (name_len >= (int)sizeof(fmt->name)) name_len = (int)sizeof(fmt->name) - 1; + memcpy(fmt->name, fmt_str, name_len); + fmt->name[name_len] = '\0'; + + const char *p = colon + 1; + int offset = 0; + + while (*p && fmt->field_count < EXTRACTOR_MAX_FIELDS) { + while (*p == ' ' || *p == '\t') p++; + if (*p == '\0' || *p == ';') { if (*p) p++; continue; } + + const char *space = strchr(p, ' '); + if (!space) break; + + char type_buf[64]; + int type_len = (int)(space - p); + if (type_len >= (int)sizeof(type_buf)) type_len = (int)sizeof(type_buf) - 1; + memcpy(type_buf, p, type_len); + type_buf[type_len] = '\0'; + + const char *name_start = space + 1; + const char *semi = strchr(name_start, ';'); + const char *name_end = semi ? semi : name_start + strlen(name_start); + + field_t *f = &fmt->fields[fmt->field_count]; + int fname_len = (int)(name_end - name_start); + if (fname_len >= (int)sizeof(f->name)) fname_len = (int)sizeof(f->name) - 1; + memcpy(f->name, name_start, fname_len); + f->name[fname_len] = '\0'; + + f->array_size = 1; + char base_type[64]; + char *bracket = strchr(type_buf, '['); + if (bracket) { + int base_len = (int)(bracket - type_buf); + memcpy(base_type, type_buf, base_len); + base_type[base_len] = '\0'; + f->array_size = atoi(bracket + 1); + if (f->array_size < 1) f->array_size = 1; + } else { + strcpy(base_type, type_buf); + } + + f->type = parse_type_str(base_type); + int elem_sz = field_elem_size(f->type); + if (elem_sz == 0) { + p = semi ? semi + 1 : name_end; + continue; + } + + f->offset = offset; + offset += elem_sz * f->array_size; + fmt->field_count++; + + p = semi ? semi + 1 : name_end; + } + + fmt->size = offset; + return 0; +} + +static int find_field(const format_t *fmt, const char *name) { + if (!fmt) return -1; + for (int i = 0; i < fmt->field_count; i++) { + if (strcmp(fmt->fields[i].name, name) == 0) + return fmt->fields[i].offset; + } + return -1; +} + +static int find_subscription(const walker_t *w, const char *name) { + for (int i = 0; i < w->sub_count; i++) { + if (strcmp(w->subs[i].message_name, name) == 0 && w->subs[i].multi_id == 0) + return i; + } + return -1; +} + +static void process_add_logged(walker_t *w, const uint8_t *buf, uint16_t msg_size) { + if (w->sub_count >= EXTRACTOR_MAX_SUBS) return; + if (msg_size < 3) return; + subscription_t *sub = &w->subs[w->sub_count]; + sub->multi_id = buf[0]; + sub->msg_id = (uint16_t)(buf[1] | (buf[2] << 8)); + int name_len = msg_size - 3; + if (name_len >= (int)sizeof(sub->message_name)) + name_len = (int)sizeof(sub->message_name) - 1; + memcpy(sub->message_name, buf + 3, name_len); + sub->message_name[name_len] = '\0'; + + sub->format_idx = -1; + for (int i = 0; i < w->format_count; i++) { + if (strcmp(w->formats[i].name, sub->message_name) == 0) { + sub->format_idx = i; + break; + } + } + w->sub_count++; +} + +// --------------------------------------------------------------------------- +// Cache resolution — called once after we've seen all the subscriptions we +// need in the definition section. Any additional subs picked up in the data +// section re-resolve individual entries. +// --------------------------------------------------------------------------- + +static void init_cache(field_cache_t *c) { + memset(c, 0xFF, sizeof(*c)); // -1 = not found +} + +static void resolve_subs_and_cache(walker_t *w) { + w->sub_attitude = find_subscription(w, "vehicle_attitude"); + w->sub_global_pos = find_subscription(w, "vehicle_global_position"); + w->sub_local_pos = find_subscription(w, "vehicle_local_position"); + w->sub_airspeed = find_subscription(w, "airspeed_validated"); + w->sub_vehicle_status = find_subscription(w, "vehicle_status"); + w->sub_home_pos = find_subscription(w, "home_position"); + + w->att_msg_id = (w->sub_attitude >= 0) ? w->subs[w->sub_attitude].msg_id : 0xFFFF; + w->gpos_msg_id = (w->sub_global_pos >= 0) ? w->subs[w->sub_global_pos].msg_id : 0xFFFF; + w->lpos_msg_id = (w->sub_local_pos >= 0) ? w->subs[w->sub_local_pos].msg_id : 0xFFFF; + w->aspd_msg_id = (w->sub_airspeed >= 0) ? w->subs[w->sub_airspeed].msg_id : 0xFFFF; + w->vstatus_msg_id = (w->sub_vehicle_status >= 0) ? w->subs[w->sub_vehicle_status].msg_id : 0xFFFF; + w->home_msg_id = (w->sub_home_pos >= 0) ? w->subs[w->sub_home_pos].msg_id : 0xFFFF; + + if (w->sub_attitude >= 0) { + const format_t *f = &w->formats[w->subs[w->sub_attitude].format_idx]; + w->cache.att_q_offset = find_field(f, "q"); + } + + if (w->sub_global_pos >= 0) { + const format_t *f = &w->formats[w->subs[w->sub_global_pos].format_idx]; + w->cache.gpos_lat_offset = find_field(f, "lat"); + w->cache.gpos_lon_offset = find_field(f, "lon"); + w->cache.gpos_alt_offset = find_field(f, "alt"); + } + + if (w->sub_local_pos >= 0) { + const format_t *f = &w->formats[w->subs[w->sub_local_pos].format_idx]; + w->cache.lpos_x_offset = find_field(f, "x"); + w->cache.lpos_y_offset = find_field(f, "y"); + w->cache.lpos_z_offset = find_field(f, "z"); + w->cache.lpos_vx_offset = find_field(f, "vx"); + w->cache.lpos_vy_offset = find_field(f, "vy"); + w->cache.lpos_vz_offset = find_field(f, "vz"); + w->cache.lpos_ref_lat_offset = find_field(f, "ref_lat"); + w->cache.lpos_ref_lon_offset = find_field(f, "ref_lon"); + w->cache.lpos_ref_alt_offset = find_field(f, "ref_alt"); + w->cache.lpos_xy_global_offset = find_field(f, "xy_global"); + w->cache.lpos_z_global_offset = find_field(f, "z_global"); + } + + if (w->sub_airspeed >= 0) { + const format_t *f = &w->formats[w->subs[w->sub_airspeed].format_idx]; + w->cache.aspd_ias_offset = find_field(f, "indicated_airspeed_m_s"); + w->cache.aspd_tas_offset = find_field(f, "true_airspeed_m_s"); + } + + if (w->sub_vehicle_status >= 0) { + const format_t *f = &w->formats[w->subs[w->sub_vehicle_status].format_idx]; + w->cache.vstatus_type_offset = find_field(f, "vehicle_type"); + w->cache.vstatus_is_vtol_offset = find_field(f, "is_vtol"); + w->cache.vstatus_nav_state_offset = find_field(f, "nav_state"); + } + + if (w->sub_home_pos >= 0) { + const format_t *f = &w->formats[w->subs[w->sub_home_pos].format_idx]; + w->cache.home_lat_offset = find_field(f, "lat"); + w->cache.home_lon_offset = find_field(f, "lon"); + w->cache.home_alt_offset = find_field(f, "alt"); + w->cache.home_valid_hpos_offset = find_field(f, "valid_hpos"); + } +} + +// --------------------------------------------------------------------------- +// Per-type extraction. Each function reads the fields it cares about from +// the payload, builds an event struct, and appends it to the timeline. It +// also does the pre-scan state updates that native does during its init walk. +// +// The `payload` pointer here is the DATA message payload AFTER the 2-byte +// msg_id (matches native's ulog_data_msg_t.data semantics: data points at +// "msg_id + 2", i.e. starts with the timestamp). +// --------------------------------------------------------------------------- + +static void extract_attitude(walker_t *w, uint64_t ts_us, const uint8_t *payload) { + if (w->cache.att_q_offset < 0) return; + ulog_att_event_t ev; + ev.timestamp_us = ts_us; + int off = w->cache.att_q_offset; + ev.q[0] = read_float_at(payload, off + 0); + ev.q[1] = read_float_at(payload, off + 4); + ev.q[2] = read_float_at(payload, off + 8); + ev.q[3] = read_float_at(payload, off + 12); + ulog_timeline_append_att(w->out->timeline, &ev); +} + +static void extract_gpos(walker_t *w, uint64_t ts_us, const uint8_t *payload) { + w->seen_gpos_data = 1; + if (w->cache.gpos_lat_offset < 0 || + w->cache.gpos_lon_offset < 0 || + w->cache.gpos_alt_offset < 0) return; + + double lat = read_double_at(payload, w->cache.gpos_lat_offset); + double lon = read_double_at(payload, w->cache.gpos_lon_offset); + float alt = read_float_at (payload, w->cache.gpos_alt_offset); + + ulog_gpos_event_t ev; + ev.timestamp_us = ts_us; + ev.lat_deg = lat; + ev.lon_deg = lon; + ev.alt_m = alt; + ev._pad = 0.0f; + ulog_timeline_append_gpos(w->out->timeline, &ev); + + // Home fallback: if we haven't resolved home yet via the home_position + // topic, first gpos sample with non-zero coordinates becomes home. + if (!w->got_home && (lat != 0.0 || lon != 0.0)) { + w->out->home.lat = (int32_t)(lat * 1e7); + w->out->home.lon = (int32_t)(lon * 1e7); + w->out->home.alt = (int32_t)(alt * 1000.0f); + w->out->home.valid = true; + w->got_home = 1; + } +} + +static void extract_lpos(walker_t *w, uint64_t ts_us, const uint8_t *payload) { + if (w->cache.lpos_x_offset < 0) return; + + ulog_lpos_event_t ev; + ev.timestamp_us = ts_us; + ev.x = read_float_at(payload, w->cache.lpos_x_offset); + ev.y = read_float_at(payload, w->cache.lpos_y_offset); + ev.z = read_float_at(payload, w->cache.lpos_z_offset); + ev.vx = read_float_at(payload, w->cache.lpos_vx_offset); + ev.vy = read_float_at(payload, w->cache.lpos_vy_offset); + ev.vz = read_float_at(payload, w->cache.lpos_vz_offset); + ulog_timeline_append_lpos(w->out->timeline, &ev); + + // LPOS reference point — capture once from first lpos sample with xy_global. + if (!w->out->ref_set && !w->out->ref_rejected && w->cache.lpos_xy_global_offset >= 0) { + uint8_t xy_global = read_uint8_at(payload, w->cache.lpos_xy_global_offset); + if (xy_global) { + double rlat = read_double_at(payload, w->cache.lpos_ref_lat_offset); + double rlon = read_double_at(payload, w->cache.lpos_ref_lon_offset); + if (rlat < -90.0 || rlat > 90.0 || rlon < -180.0 || rlon > 180.0) { + w->out->ref_rejected = 1; + } else { + w->out->ref_lat_deg = rlat; + w->out->ref_lon_deg = rlon; + if (w->cache.lpos_z_global_offset >= 0 && + read_uint8_at(payload, w->cache.lpos_z_global_offset)) { + w->out->ref_alt_m = read_float_at(payload, w->cache.lpos_ref_alt_offset); + } + w->out->ref_set = 1; + } + } + } + + // CUSUM takeoff detection on upward velocity (-vz = upward in NED) + if (!w->cusum_triggered && w->cache.lpos_vz_offset >= 0) { + float up = -ev.vz; + w->cusum_s += (up - w->cusum_k); + if (w->cusum_s < 0.0f) w->cusum_s = 0.0f; + if (w->cusum_s > w->cusum_h) { + w->cusum_trigger_time = (float)((double)(ts_us - w->start_timestamp_us) / 1e6); + w->cusum_peak = w->cusum_s; + w->cusum_triggered = 1; + } + } +} + +static void extract_aspd(walker_t *w, uint64_t ts_us, const uint8_t *payload) { + if (w->cache.aspd_ias_offset < 0 || w->cache.aspd_tas_offset < 0) return; + float ias = read_float_at(payload, w->cache.aspd_ias_offset); + float tas = read_float_at(payload, w->cache.aspd_tas_offset); + ulog_aspd_event_t ev; + ev.timestamp_us = ts_us; + ev.ias_cms = (uint16_t)(ias * 100.0f); + ev.tas_cms = (uint16_t)(tas * 100.0f); + ev._pad = 0; + ulog_timeline_append_aspd(w->out->timeline, &ev); +} + +static void extract_vstatus(walker_t *w, uint64_t ts_us, const uint8_t *payload) { + if (w->cache.vstatus_type_offset < 0) return; + + uint8_t px4_type = read_uint8_at(payload, w->cache.vstatus_type_offset); + uint8_t is_vtol = (w->cache.vstatus_is_vtol_offset >= 0) + ? read_uint8_at(payload, w->cache.vstatus_is_vtol_offset) : 0; + uint8_t nav = (w->cache.vstatus_nav_state_offset >= 0) + ? read_uint8_at(payload, w->cache.vstatus_nav_state_offset) : 0xFF; + + uint8_t mav_type; + if (is_vtol) { + mav_type = 22; // MAV_TYPE_VTOL_FIXEDROTOR + } else { + switch (px4_type) { + case 1: mav_type = 2; break; // ROTARY_WING → MAV_TYPE_QUADROTOR + case 2: mav_type = 1; break; // FIXED_WING → MAV_TYPE_FIXED_WING + case 3: mav_type = 10; break; // ROVER → MAV_TYPE_GROUND_ROVER + default: mav_type = 2; break; + } + } + + ulog_vstatus_event_t ev; + ev.timestamp_us = ts_us; + ev.vehicle_type = mav_type; + ev.is_vtol = is_vtol; + ev.nav_state = nav; + memset(ev._pad, 0, sizeof(ev._pad)); + ulog_timeline_append_vstatus(w->out->timeline, &ev); + + // First vstatus sets the extract-result vehicle_type + if (!w->got_vehicle_type) { + w->out->vehicle_type = mav_type; + w->got_vehicle_type = 1; + } + + // Mode change tracking + if (nav != w->prev_nav && w->out->mode_change_count < ULOG_MAX_MODE_CHANGES) { + float t = (float)((double)(ts_us - w->start_timestamp_us) / 1e6); + w->out->mode_changes[w->out->mode_change_count].time_s = t; + w->out->mode_changes[w->out->mode_change_count].nav_state = nav; + w->out->mode_change_count++; + w->prev_nav = nav; + } +} + +static void extract_home(walker_t *w, uint64_t ts_us, const uint8_t *payload) { + if (w->cache.home_lat_offset < 0) return; + + uint8_t hpos_valid = 1; + if (w->cache.home_valid_hpos_offset >= 0) + hpos_valid = read_uint8_at(payload, w->cache.home_valid_hpos_offset) != 0; + + double lat = read_double_at(payload, w->cache.home_lat_offset); + double lon = read_double_at(payload, w->cache.home_lon_offset); + float alt = read_float_at (payload, w->cache.home_alt_offset); + + ulog_home_event_t ev; + ev.timestamp_us = ts_us; + ev.lat_deg = lat; + ev.lon_deg = lon; + ev.alt_m = alt; + ev.valid_hpos = hpos_valid; + memset(ev._pad, 0, sizeof(ev._pad)); + ulog_timeline_append_home(w->out->timeline, &ev); + + // Pre-scan home resolution (Tier 1: authoritative home_position topic) + if (!w->got_home && hpos_valid && (lat != 0.0 || lon != 0.0)) { + w->out->home.lat = (int32_t)(lat * 1e7); + w->out->home.lon = (int32_t)(lon * 1e7); + w->out->home.alt = (int32_t)(alt * 1000.0f); + w->out->home.valid = true; + w->out->home_from_topic = 1; + w->got_home = 1; + } +} + +static void extract_statustext(walker_t *w, uint64_t ts_us, + uint8_t severity, const uint8_t *text, int text_len) { + ulog_statustext_event_t ev; + memset(&ev, 0, sizeof(ev)); + ev.timestamp_us = ts_us; + ev.severity = severity; + int n = text_len < STATUSTEXT_MSG_MAX - 1 ? text_len : STATUSTEXT_MSG_MAX - 1; + memcpy(ev.text, text, n); + ev.text[n] = '\0'; + ulog_timeline_append_statustext(w->out->timeline, &ev); +} + +// --------------------------------------------------------------------------- +// Dispatch one DATA message +// --------------------------------------------------------------------------- + +static void dispatch_data(walker_t *w, uint16_t msg_id, uint64_t ts_us, const uint8_t *payload) { + if (msg_id == w->att_msg_id) extract_attitude(w, ts_us, payload); + else if (msg_id == w->lpos_msg_id) extract_lpos(w, ts_us, payload); + else if (msg_id == w->gpos_msg_id) extract_gpos(w, ts_us, payload); + else if (msg_id == w->aspd_msg_id) extract_aspd(w, ts_us, payload); + else if (msg_id == w->vstatus_msg_id) extract_vstatus(w, ts_us, payload); + else if (msg_id == w->home_msg_id) extract_home(w, ts_us, payload); + // Other subscriptions are logged by PX4 but ignored by Hawkeye's replay. +} + +// --------------------------------------------------------------------------- +// Public entry point: ulog_extract +// --------------------------------------------------------------------------- + +int ulog_extract(const uint8_t *buf, size_t len, ulog_extract_result_t *out) { + if (!buf || !out || !out->timeline) return -1; + + // walker_t is ~1.3 MB (formats[256] of ~5 KB each) — too big for the + // WASM stack (default 1 MB). Heap-allocate to avoid stack overflow. + walker_t *w = calloc(1, sizeof(walker_t)); + if (!w) return -1; + + w->buf = buf; + w->len = len; + w->pos = 0; + w->out = out; + w->sub_attitude = -1; + w->sub_global_pos = -1; + w->sub_local_pos = -1; + w->sub_airspeed = -1; + w->sub_vehicle_status = -1; + w->sub_home_pos = -1; + w->att_msg_id = w->gpos_msg_id = w->lpos_msg_id = 0xFFFF; + w->aspd_msg_id = w->vstatus_msg_id = w->home_msg_id = 0xFFFF; + w->prev_nav = 0xFF; + w->cusum_k = 0.3f; + w->cusum_h = 2.0f; + init_cache(&w->cache); + + // Initialize extract result fields that need non-zero defaults + out->vehicle_type = 2; // default MAV_TYPE_QUADROTOR + out->current_nav_state = 0xFF; + out->mode_change_count = 0; + out->home.valid = false; + out->home_from_topic = 0; + out->home_rejected = 0; + out->ref_set = 0; + out->ref_rejected = 0; + out->ref_lat_deg = 0.0; + out->ref_lon_deg = 0.0; + out->ref_alt_m = 0.0f; + out->takeoff_detected = 0; + out->takeoff_time_s = 0.0f; + out->takeoff_conf = 0.0f; + + // --- Validate ULog header --- + uint8_t header[ULOG_HEADER_SIZE]; + if (w_read(w, header, ULOG_HEADER_SIZE) != 0) { + fprintf(stderr, "ulog_extract: file too short\n"); + goto fail; + } + if (memcmp(header, ULOG_MAGIC, 7) != 0) { + fprintf(stderr, "ulog_extract: invalid magic\n"); + goto fail; + } + + // --- Definition section: read FORMAT and ADD_LOGGED_MSG until first DATA --- + uint8_t hit_data_section = 0; + while (!hit_data_section) { + uint8_t msg_hdr[ULOG_MSG_HEADER_SIZE]; + if (w_read(w, msg_hdr, ULOG_MSG_HEADER_SIZE) != 0) goto fail; + uint16_t msg_size = (uint16_t)(msg_hdr[0] | (msg_hdr[1] << 8)); + uint8_t msg_type = msg_hdr[2]; + + if (ensure_read_buf(w, msg_size) != 0) goto fail; + if (w_read(w, w->read_buf, msg_size) != 0) goto fail; + + switch ((char)msg_type) { + case ULOG_MSG_FORMAT: { + if (w->format_count < EXTRACTOR_MAX_FORMATS) { + char fmt_str[2048]; + int n = msg_size < (int)sizeof(fmt_str) - 1 ? msg_size : (int)sizeof(fmt_str) - 1; + memcpy(fmt_str, w->read_buf, n); + fmt_str[n] = '\0'; + parse_format(fmt_str, &w->formats[w->format_count]); + w->format_count++; + } + break; + } + case ULOG_MSG_ADD_LOGGED: + process_add_logged(w, w->read_buf, msg_size); + break; + case ULOG_MSG_DATA: { + // First DATA — back up to let the main loop read it properly. + w->pos -= (ULOG_MSG_HEADER_SIZE + msg_size); + hit_data_section = 1; + break; + } + default: + // Skip other message types (INFO, PARAMETER, SYNC, etc.) + break; + } + } + + // Resolve subscription indices and field offset cache now that we have + // (most of) the definitions. Any ADD_LOGGED that shows up in the data + // section will trigger a re-resolve. + resolve_subs_and_cache(w); + + // --- Data section: walk DATA + LOGGING + in-stream ADD_LOGGED --- + for (;;) { + uint8_t msg_hdr[ULOG_MSG_HEADER_SIZE]; + if (w_read(w, msg_hdr, ULOG_MSG_HEADER_SIZE) != 0) break; // EOF — normal exit + uint16_t msg_size = (uint16_t)(msg_hdr[0] | (msg_hdr[1] << 8)); + uint8_t msg_type = msg_hdr[2]; + + if (ensure_read_buf(w, msg_size) != 0) goto fail; + if (w_read(w, w->read_buf, msg_size) != 0) break; // partial trailing msg — tolerate + + if ((char)msg_type == ULOG_MSG_ADD_LOGGED) { + process_add_logged(w, w->read_buf, msg_size); + resolve_subs_and_cache(w); // refresh cache for the new sub + continue; + } + + if ((char)msg_type == ULOG_MSG_DATA && msg_size >= 10) { + uint16_t msg_id = (uint16_t)(w->read_buf[0] | (w->read_buf[1] << 8)); + uint64_t ts_us; + memcpy(&ts_us, w->read_buf + 2, 8); + + if (!w->first_seen) { + w->start_timestamp_us = ts_us; + w->first_seen = 1; + out->timeline->start_timestamp_us = ts_us; + } + out->timeline->end_timestamp_us = ts_us; + + // payload in dispatch_data points at w->read_buf + 2, matching native's + // ulog_data_msg_t.data semantics (starts with timestamp). + dispatch_data(w, msg_id, ts_us, w->read_buf + 2); + continue; + } + + if ((char)msg_type == ULOG_MSG_LOGGING && msg_size >= 9) { + uint8_t severity = w->read_buf[0]; + uint64_t ts_us; + memcpy(&ts_us, w->read_buf + 1, 8); + const uint8_t *text = w->read_buf + 9; + int text_len = msg_size - 9; + extract_statustext(w, ts_us, severity, text, text_len); + continue; + } + + // Other message types: ignored (DROPOUT, INFO_MULTI, PARAM, SYNC, ...) + } + + // --- Finalize pre-scan results --- + + // Home Tier validation: if home came from the home_position topic but + // there's no gpos data to confirm the position is globally referenced, + // the home is unreliable (baro-only) and gets rejected. + if (w->got_home && out->home_from_topic && !w->seen_gpos_data) { + out->home.valid = false; + out->home_from_topic = 0; + out->home_rejected = 1; + } + + // Takeoff detection results + out->takeoff_detected = w->cusum_triggered; + out->takeoff_time_s = w->cusum_triggered ? w->cusum_trigger_time : 0.0f; + + // Confidence scoring: CUSUM sharpness + nav_state corroboration + float conf = 0.0f; + if (w->cusum_triggered) { + float sharpness = (w->cusum_peak - w->cusum_h) / 8.0f; + if (sharpness < 0.0f) sharpness = 0.0f; + if (sharpness > 1.0f) sharpness = 1.0f; + conf = 0.3f + 0.4f * sharpness; + + // Find the mode active at cusum_trigger_time + uint8_t active_ns = 0xFF; + for (int m = out->mode_change_count - 1; m >= 0; m--) { + if (out->mode_changes[m].time_s <= w->cusum_trigger_time) { + active_ns = out->mode_changes[m].nav_state; + break; + } + } + // Takeoff(17) Mission(3) VTOL-Takeoff(22) Offboard(14) Position(2) + if (active_ns == 17 || active_ns == 3 || active_ns == 22 || + active_ns == 14 || active_ns == 2) { + conf += 0.3f; + } + if (conf > 1.0f) conf = 1.0f; + } else { + // No CUSUM trigger — check if already airborne at log start + if (out->mode_change_count > 0) { + uint8_t ns = out->mode_changes[0].nav_state; + if (ns == 3 || ns == 4 || ns == 14) + conf = 0.5f; + } + } + out->takeoff_conf = conf; + + if (out->mode_change_count > 0) + out->current_nav_state = out->mode_changes[0].nav_state; + + // Shrink timeline arrays to final size so idle capacity is released. + ulog_timeline_shrink_to_fit(out->timeline); + + free(w->read_buf); + free(w); + return 0; + +fail: + free(w->read_buf); + free(w); + return -1; +} diff --git a/src/wasm/ulog_extractor.h b/src/wasm/ulog_extractor.h new file mode 100644 index 0000000..1964624 --- /dev/null +++ b/src/wasm/ulog_extractor.h @@ -0,0 +1,55 @@ +#ifndef WASM_ULOG_EXTRACTOR_H +#define WASM_ULOG_EXTRACTOR_H + +// WASM-only ULog extractor. Walks a ULog byte buffer once, populating a +// flat-array timeline and pre-scan state. Does not depend on src/ulog_parser.c +// or src/ulog_replay.c — this is a freshly-written parser that reuses the +// same ULog format knowledge without sharing implementation. + +#include +#include + +#include "ulog_events.h" +#include "../mavlink_receiver.h" // home_position_t (type only) +#include "../ulog_replay.h" // ulog_mode_change_t, ULOG_MAX_MODE_CHANGES (type only) + +// Pre-scan result populated alongside the timeline. Mirrors the relevant +// fields from ulog_replay_ctx_t that the replay output surface exposes to +// the rest of the application. +typedef struct { + ulog_timeline_t *timeline; // in/out: owned by caller, populated by extractor + + // Vehicle type and current mode state + uint8_t vehicle_type; + uint8_t current_nav_state; + + // Flight mode transitions (copied to replay ctx after extraction) + ulog_mode_change_t mode_changes[ULOG_MAX_MODE_CHANGES]; + int mode_change_count; + + // Home position resolution + home_position_t home; + uint8_t home_from_topic; // true = tier 1 (from home_position topic) + uint8_t home_rejected; // pre-scan rejected home (no GPOS to confirm) + + // LPOS reference point (for local→global conversion fallback) + double ref_lat_deg; + double ref_lon_deg; + float ref_alt_m; + uint8_t ref_set; + uint8_t ref_rejected; + + // CUSUM takeoff detection + float takeoff_time_s; + float takeoff_conf; + uint8_t takeoff_detected; +} ulog_extract_result_t; + +// Parse a ULog from memory, populate timeline and pre-scan state. +// Caller owns `out->timeline` and must call ulog_timeline_init on it before +// calling this function. On success, returns 0 and the timeline plus all +// pre-scan fields are populated. On failure, returns -1 and the timeline +// may be partially populated (caller should ulog_timeline_free it). +int ulog_extract(const uint8_t *buf, size_t len, ulog_extract_result_t *out); + +#endif diff --git a/src/wasm/ulog_timeline.c b/src/wasm/ulog_timeline.c new file mode 100644 index 0000000..844b8a8 --- /dev/null +++ b/src/wasm/ulog_timeline.c @@ -0,0 +1,125 @@ +#include "ulog_timeline.h" + +#include +#include + +// --------------------------------------------------------------------------- +// Generic growth helper — doubles capacity on overflow +// --------------------------------------------------------------------------- + +static int grow_if_needed(void **arr_ptr, int *cap, int count, size_t elem_size) { + if (count < *cap) return 0; + int new_cap = *cap ? *cap * 2 : 64; + void *new_arr = realloc(*arr_ptr, (size_t)new_cap * elem_size); + if (!new_arr) return -1; + *arr_ptr = new_arr; + *cap = new_cap; + return 0; +} + +static void shrink_array(void **arr_ptr, int *cap, int count, size_t elem_size) { + if (count == *cap) return; + if (count == 0) { + free(*arr_ptr); + *arr_ptr = NULL; + *cap = 0; + return; + } + void *new_arr = realloc(*arr_ptr, (size_t)count * elem_size); + // realloc shrink is allowed to fail, in which case we keep the larger + // buffer — still correct, just wastes capacity. + if (new_arr) { + *arr_ptr = new_arr; + *cap = count; + } +} + +// --------------------------------------------------------------------------- +// Binary search: returns largest index i where arr[i].timestamp_us <= target. +// Returns -1 if count == 0 or target < arr[0].timestamp_us. +// +// The timeline arrays are sorted by timestamp_us (guaranteed because ULog +// DATA messages are emitted in monotonic timestamp order during extraction), +// so binary search is valid. +// --------------------------------------------------------------------------- + +#define DEFINE_FIND(TYPE, FIELD) \ + int ulog_timeline_find_##FIELD##_at(const ulog_timeline_t *tl, \ + uint64_t target_us) { \ + if (tl->FIELD##_count == 0) return -1; \ + if (tl->FIELD[0].timestamp_us > target_us) return -1; \ + int lo = 0, hi = tl->FIELD##_count - 1, best = 0; \ + while (lo <= hi) { \ + int mid = (lo + hi) / 2; \ + if (tl->FIELD[mid].timestamp_us <= target_us) { \ + best = mid; \ + lo = mid + 1; \ + } else { \ + hi = mid - 1; \ + } \ + } \ + return best; \ + } + +DEFINE_FIND(ulog_att_event_t, att) +DEFINE_FIND(ulog_lpos_event_t, lpos) +DEFINE_FIND(ulog_gpos_event_t, gpos) +DEFINE_FIND(ulog_aspd_event_t, aspd) +DEFINE_FIND(ulog_vstatus_event_t, vstatus) +DEFINE_FIND(ulog_home_event_t, home) +DEFINE_FIND(ulog_statustext_event_t, statustext) + +#undef DEFINE_FIND + +// --------------------------------------------------------------------------- +// Init / free +// --------------------------------------------------------------------------- + +void ulog_timeline_init(ulog_timeline_t *tl) { + memset(tl, 0, sizeof(*tl)); +} + +void ulog_timeline_free(ulog_timeline_t *tl) { + free(tl->att); tl->att = NULL; tl->att_count = 0; tl->att_cap = 0; + free(tl->lpos); tl->lpos = NULL; tl->lpos_count = 0; tl->lpos_cap = 0; + free(tl->gpos); tl->gpos = NULL; tl->gpos_count = 0; tl->gpos_cap = 0; + free(tl->aspd); tl->aspd = NULL; tl->aspd_count = 0; tl->aspd_cap = 0; + free(tl->vstatus); tl->vstatus = NULL; tl->vstatus_count = 0; tl->vstatus_cap = 0; + free(tl->home); tl->home = NULL; tl->home_count = 0; tl->home_cap = 0; + free(tl->statustext); tl->statustext = NULL; tl->statustext_count = 0; tl->statustext_cap = 0; + tl->start_timestamp_us = 0; + tl->end_timestamp_us = 0; +} + +void ulog_timeline_shrink_to_fit(ulog_timeline_t *tl) { + shrink_array((void **)&tl->att, &tl->att_cap, tl->att_count, sizeof(ulog_att_event_t)); + shrink_array((void **)&tl->lpos, &tl->lpos_cap, tl->lpos_count, sizeof(ulog_lpos_event_t)); + shrink_array((void **)&tl->gpos, &tl->gpos_cap, tl->gpos_count, sizeof(ulog_gpos_event_t)); + shrink_array((void **)&tl->aspd, &tl->aspd_cap, tl->aspd_count, sizeof(ulog_aspd_event_t)); + shrink_array((void **)&tl->vstatus, &tl->vstatus_cap, tl->vstatus_count, sizeof(ulog_vstatus_event_t)); + shrink_array((void **)&tl->home, &tl->home_cap, tl->home_count, sizeof(ulog_home_event_t)); + shrink_array((void **)&tl->statustext, &tl->statustext_cap, tl->statustext_count, sizeof(ulog_statustext_event_t)); +} + +// --------------------------------------------------------------------------- +// Appenders +// --------------------------------------------------------------------------- + +#define DEFINE_APPEND(TYPE, FIELD) \ + int ulog_timeline_append_##FIELD(ulog_timeline_t *tl, const TYPE *ev) { \ + if (grow_if_needed((void **)&tl->FIELD, &tl->FIELD##_cap, \ + tl->FIELD##_count, sizeof(TYPE)) != 0) \ + return -1; \ + tl->FIELD[tl->FIELD##_count++] = *ev; \ + return 0; \ + } + +DEFINE_APPEND(ulog_att_event_t, att) +DEFINE_APPEND(ulog_lpos_event_t, lpos) +DEFINE_APPEND(ulog_gpos_event_t, gpos) +DEFINE_APPEND(ulog_aspd_event_t, aspd) +DEFINE_APPEND(ulog_vstatus_event_t, vstatus) +DEFINE_APPEND(ulog_home_event_t, home) +DEFINE_APPEND(ulog_statustext_event_t, statustext) + +#undef DEFINE_APPEND diff --git a/src/wasm/ulog_timeline.h b/src/wasm/ulog_timeline.h new file mode 100644 index 0000000..a843b33 --- /dev/null +++ b/src/wasm/ulog_timeline.h @@ -0,0 +1,36 @@ +#ifndef WASM_ULOG_TIMELINE_H +#define WASM_ULOG_TIMELINE_H + +// Append/free helpers for the flat-array event timeline used by the WASM +// replay path. All appends are amortized O(1) via doubling realloc. + +#include "ulog_events.h" + +void ulog_timeline_init(ulog_timeline_t *tl); +void ulog_timeline_free(ulog_timeline_t *tl); + +// Shrink every array's capacity to its count. Call once after extraction +// finishes so idle capacity doesn't sit in the WASM heap for the lifetime +// of the replay. +void ulog_timeline_shrink_to_fit(ulog_timeline_t *tl); + +// Appenders. Each returns 0 on success, -1 on allocation failure. +int ulog_timeline_append_att (ulog_timeline_t *tl, const ulog_att_event_t *ev); +int ulog_timeline_append_lpos (ulog_timeline_t *tl, const ulog_lpos_event_t *ev); +int ulog_timeline_append_gpos (ulog_timeline_t *tl, const ulog_gpos_event_t *ev); +int ulog_timeline_append_aspd (ulog_timeline_t *tl, const ulog_aspd_event_t *ev); +int ulog_timeline_append_vstatus (ulog_timeline_t *tl, const ulog_vstatus_event_t *ev); +int ulog_timeline_append_home (ulog_timeline_t *tl, const ulog_home_event_t *ev); +int ulog_timeline_append_statustext(ulog_timeline_t *tl, const ulog_statustext_event_t *ev); + +// Binary-search each array for the largest index where timestamp_us <= target. +// Returns -1 if the array is empty or the first event is already past target. +int ulog_timeline_find_att_at (const ulog_timeline_t *tl, uint64_t target_us); +int ulog_timeline_find_lpos_at (const ulog_timeline_t *tl, uint64_t target_us); +int ulog_timeline_find_gpos_at (const ulog_timeline_t *tl, uint64_t target_us); +int ulog_timeline_find_aspd_at (const ulog_timeline_t *tl, uint64_t target_us); +int ulog_timeline_find_vstatus_at (const ulog_timeline_t *tl, uint64_t target_us); +int ulog_timeline_find_home_at (const ulog_timeline_t *tl, uint64_t target_us); +int ulog_timeline_find_statustext_at(const ulog_timeline_t *tl, uint64_t target_us); + +#endif diff --git a/src/wasm/wasm_replay.c b/src/wasm/wasm_replay.c new file mode 100644 index 0000000..d062cb7 --- /dev/null +++ b/src/wasm/wasm_replay.c @@ -0,0 +1,280 @@ +// WASM replay path. Consumes pre-extracted events from a ulog_timeline_t via +// cursor iteration and calls the shared apply helpers (src/ulog_replay_apply) +// to update replay state. The context type is ulog_replay_ctx_t — same struct +// as native, with timeline + cursor fields gated behind #ifdef __EMSCRIPTEN__. + +#include "wasm_replay.h" +#include "ulog_extractor.h" +#include "ulog_timeline.h" +#include "ulog_events.h" +#include "ulog_replay_apply.h" + +#include +#include +#include +#include + +static ulog_timeline_t *tl_of(wasm_replay_ctx_t *ctx) { + return (ulog_timeline_t *)ctx->timeline; +} + +static const ulog_timeline_t *ctl_of(const wasm_replay_ctx_t *ctx) { + return (const ulog_timeline_t *)ctx->timeline; +} + +// --------------------------------------------------------------------------- +// STATUSTEXT ring push — WASM-only. Native has an equivalent tiny helper +// inside ulog_replay.c's logging_cb (the ring-write logic is 8 lines; not +// worth a shared-file hop for such a small widget). The inputs here come +// from a pre-extracted ulog_statustext_event_t plus the timeline's log +// start, which is the moral equivalent of native's parser.start_timestamp. +// --------------------------------------------------------------------------- + +static void apply_statustext(wasm_replay_ctx_t *ctx, + const ulog_statustext_event_t *ev, + uint64_t log_start_us) { + statustext_ring_t *ring = &ctx->statustext; + statustext_entry_t *e = &ring->entries[ring->head]; + e->severity = ev->severity; + float time_s = (float)((double)(ev->timestamp_us - log_start_us) / 1e6); + e->time_s = time_s; + int len = (int)strnlen(ev->text, STATUSTEXT_MSG_MAX - 1); + memcpy(e->text, ev->text, len); + e->text[len] = '\0'; + ring->head = (ring->head + 1) % STATUSTEXT_RING_SIZE; + if (ring->count < STATUSTEXT_RING_SIZE) ring->count++; +} + +// --------------------------------------------------------------------------- +// Cursor advance: walk every per-type cursor forward to the target timestamp, +// calling the shared apply helper for each event consumed. +// --------------------------------------------------------------------------- + +static void advance_cursors_to(wasm_replay_ctx_t *ctx, uint64_t target_us) { + const ulog_timeline_t *tl = ctl_of(ctx); + + while (ctx->att_cursor < tl->att_count && + tl->att[ctx->att_cursor].timestamp_us <= target_us) { + ulog_apply_attitude(ctx, &tl->att[ctx->att_cursor++]); + } + while (ctx->lpos_cursor < tl->lpos_count && + tl->lpos[ctx->lpos_cursor].timestamp_us <= target_us) { + ulog_apply_lpos(ctx, &tl->lpos[ctx->lpos_cursor++]); + } + while (ctx->gpos_cursor < tl->gpos_count && + tl->gpos[ctx->gpos_cursor].timestamp_us <= target_us) { + ulog_apply_gpos(ctx, &tl->gpos[ctx->gpos_cursor++]); + } + while (ctx->aspd_cursor < tl->aspd_count && + tl->aspd[ctx->aspd_cursor].timestamp_us <= target_us) { + ulog_apply_aspd(ctx, &tl->aspd[ctx->aspd_cursor++]); + } + while (ctx->vstatus_cursor < tl->vstatus_count && + tl->vstatus[ctx->vstatus_cursor].timestamp_us <= target_us) { + ulog_apply_vstatus(ctx, &tl->vstatus[ctx->vstatus_cursor++]); + } + while (ctx->home_cursor < tl->home_count && + tl->home[ctx->home_cursor].timestamp_us <= target_us) { + ulog_apply_home(ctx, &tl->home[ctx->home_cursor++]); + } + while (ctx->statustext_cursor < tl->statustext_count && + tl->statustext[ctx->statustext_cursor].timestamp_us <= target_us) { + apply_statustext(ctx, &tl->statustext[ctx->statustext_cursor++], + tl->start_timestamp_us); + } +} + +// Dead-reckon state.lat/lon/alt forward from the last position sample to +// target_us using velocity. Mirrors the interpolation block in native +// ulog_replay.c so WASM playback animates smoothly between LPOS samples at +// render rate. +static void interpolate_position(wasm_replay_ctx_t *ctx, uint64_t target_us) { + if (ctx->last_pos_usec == 0 || target_us <= ctx->last_pos_usec) return; + + float dt_pos = (float)((double)(target_us - ctx->last_pos_usec) / 1e6); + if (dt_pos > 0.5f) dt_pos = 0.5f; // clamp runaway if data is stale + + if (ctx->has_global_pos) { + float vx = ctx->last_vx, vy = ctx->last_vy, vz = ctx->last_vz; + if (vx == 0.0f && vy == 0.0f && vz == 0.0f) { + vx = ctx->gpos_vx; + vy = ctx->gpos_vy; + vz = ctx->gpos_vz; + } + double meters_per_deg_lat = 111132.92; + double meters_per_deg_lon = 111132.92 * cos(ctx->last_lat_deg * ULOG_DEG_TO_RAD); + if (meters_per_deg_lon < 1.0) meters_per_deg_lon = 1.0; + + double lat = ctx->last_lat_deg + (double)(vx * dt_pos) / meters_per_deg_lat; + double lon = ctx->last_lon_deg + (double)(vy * dt_pos) / meters_per_deg_lon; + float alt = ctx->last_alt_m - vz * dt_pos; + + ctx->state.lat = (int32_t)(lat * 1e7); + ctx->state.lon = (int32_t)(lon * 1e7); + ctx->state.alt = (int32_t)(alt * 1000.0f); + } else { + float x = ctx->last_x + ctx->last_vx * dt_pos; + float y = ctx->last_y + ctx->last_vy * dt_pos; + float z = ctx->last_z + ctx->last_vz * dt_pos; + ulog_local_to_global(ctx->ref_lat, ctx->ref_lon, ctx->ref_alt, + x, y, z, + &ctx->state.lat, &ctx->state.lon, &ctx->state.alt); + } + ctx->state.time_usec = target_us; +} + +static bool any_cursor_remaining(const wasm_replay_ctx_t *ctx) { + const ulog_timeline_t *tl = ctl_of(ctx); + return ctx->att_cursor < tl->att_count || + ctx->lpos_cursor < tl->lpos_count || + ctx->gpos_cursor < tl->gpos_count || + ctx->aspd_cursor < tl->aspd_count || + ctx->vstatus_cursor < tl->vstatus_count || + ctx->home_cursor < tl->home_count; +} + +// --------------------------------------------------------------------------- +// Public API +// --------------------------------------------------------------------------- + +int wasm_replay_init_from_bytes(wasm_replay_ctx_t *ctx, + const uint8_t *buf, size_t len) { + memset(ctx, 0, sizeof(*ctx)); + + ulog_timeline_t *tl = calloc(1, sizeof(*tl)); + if (!tl) return -1; + ulog_timeline_init(tl); + ctx->timeline = tl; + + ulog_extract_result_t result; + memset(&result, 0, sizeof(result)); + result.timeline = tl; + + if (ulog_extract(buf, len, &result) != 0) { + ulog_timeline_free(tl); + free(tl); + ctx->timeline = NULL; + return -1; + } + + // Copy pre-scan state from the extractor result to the replay ctx. + ctx->vehicle_type = result.vehicle_type; + ctx->current_nav_state = result.current_nav_state; + ctx->mode_change_count = result.mode_change_count; + memcpy(ctx->mode_changes, result.mode_changes, + sizeof(ctx->mode_changes[0]) * (size_t)result.mode_change_count); + ctx->home = result.home; + ctx->home_from_topic = result.home_from_topic != 0; + ctx->home_rejected = result.home_rejected != 0; + ctx->ref_lat = result.ref_lat_deg; + ctx->ref_lon = result.ref_lon_deg; + ctx->ref_alt = result.ref_alt_m; + ctx->ref_set = result.ref_set != 0; + ctx->ref_rejected = result.ref_rejected != 0; + ctx->takeoff_detected = result.takeoff_detected != 0; + ctx->takeoff_time_s = result.takeoff_time_s; + ctx->takeoff_conf = result.takeoff_conf; + ctx->time_offset_s = 0.0; + ctx->has_global_pos = false; + ctx->first_pos_set = false; + ctx->wall_accum = 0.0; + + // Log summary (matches native's init-time print) + uint64_t dur_us = tl->end_timestamp_us - tl->start_timestamp_us; + float dur = (float)((double)dur_us / 1e6); + printf("WASM replay: %d att, %d lpos, %d gpos events (%.1fs)\n", + tl->att_count, tl->lpos_count, tl->gpos_count, dur); + return 0; +} + +bool wasm_replay_advance(wasm_replay_ctx_t *ctx, float dt, float speed, + bool looping, bool interpolation) { + const ulog_timeline_t *tl = ctl_of(ctx); + if (!tl || tl->start_timestamp_us == 0) return false; + + // Convert accumulated wall-clock time to a target log-relative timestamp, + // then to an absolute log timestamp. + ctx->wall_accum += (double)(dt * speed); + double log_rel_s = ctx->wall_accum - ctx->time_offset_s; + uint64_t dur_us = tl->end_timestamp_us - tl->start_timestamp_us; + + if (log_rel_s < 0.0) { + return true; // still waiting for our aligned start + } + + uint64_t rel_us = (uint64_t)(log_rel_s * 1e6); + + if (rel_us >= dur_us) { + if (looping) { + ctx->wall_accum = ctx->time_offset_s; + ctx->att_cursor = ctx->lpos_cursor = ctx->gpos_cursor = 0; + ctx->aspd_cursor = ctx->vstatus_cursor = ctx->home_cursor = 0; + ctx->statustext_cursor = 0; + ctx->has_global_pos = false; + ctx->first_pos_set = false; + ctx->prev_gpos_usec = 0; + ctx->last_pos_usec = 0; + ctx->last_vx = ctx->last_vy = ctx->last_vz = 0.0f; + ctx->gpos_vx = ctx->gpos_vy = ctx->gpos_vz = 0.0f; + return true; + } + rel_us = dur_us; + } + + uint64_t target_us = tl->start_timestamp_us + rel_us; + advance_cursors_to(ctx, target_us); + + if (interpolation) interpolate_position(ctx, target_us); + + return any_cursor_remaining(ctx) || rel_us < dur_us; +} + +void wasm_replay_seek(wasm_replay_ctx_t *ctx, float target_s) { + const ulog_timeline_t *tl = ctl_of(ctx); + if (!tl || tl->start_timestamp_us == 0) return; + if (target_s < 0.0f) target_s = 0.0f; + + // Mirror native's "seek_early + forward-scan to settle state" strategy. + // Binary-search each cursor to a point slightly before the target, then + // walk forward calling the shared apply helpers to rebuild coherent state. + uint64_t target_us = tl->start_timestamp_us + (uint64_t)(target_s * 1e6); + uint64_t early_us = target_us > 1000000 ? target_us - 1000000 : tl->start_timestamp_us; + + int att_idx = ulog_timeline_find_att_at (tl, early_us); + int lpos_idx = ulog_timeline_find_lpos_at (tl, early_us); + int gpos_idx = ulog_timeline_find_gpos_at (tl, early_us); + int aspd_idx = ulog_timeline_find_aspd_at (tl, early_us); + int vstatus_idx = ulog_timeline_find_vstatus_at (tl, early_us); + int home_idx = ulog_timeline_find_home_at (tl, early_us); + int statustext_idx = ulog_timeline_find_statustext_at(tl, early_us); + + ctx->att_cursor = att_idx < 0 ? 0 : att_idx + 1; + ctx->lpos_cursor = lpos_idx < 0 ? 0 : lpos_idx + 1; + ctx->gpos_cursor = gpos_idx < 0 ? 0 : gpos_idx + 1; + ctx->aspd_cursor = aspd_idx < 0 ? 0 : aspd_idx + 1; + ctx->vstatus_cursor = vstatus_idx < 0 ? 0 : vstatus_idx + 1; + ctx->home_cursor = home_idx < 0 ? 0 : home_idx + 1; + ctx->statustext_cursor = statustext_idx < 0 ? 0 : statustext_idx + 1; + + ctx->prev_gpos_usec = 0; + + advance_cursors_to(ctx, target_us); + interpolate_position(ctx, target_us); + + ctx->wall_accum = (double)target_s + ctx->time_offset_s; +} + +void wasm_replay_close(wasm_replay_ctx_t *ctx) { + ulog_timeline_t *tl = tl_of(ctx); + if (tl) { + ulog_timeline_free(tl); + free(tl); + } + memset(ctx, 0, sizeof(*ctx)); +} + +uint64_t wasm_replay_duration_us(const wasm_replay_ctx_t *ctx) { + const ulog_timeline_t *tl = ctl_of(ctx); + if (!tl) return 0; + return tl->end_timestamp_us - tl->start_timestamp_us; +} diff --git a/src/wasm/wasm_replay.h b/src/wasm/wasm_replay.h new file mode 100644 index 0000000..b7e0727 --- /dev/null +++ b/src/wasm/wasm_replay.h @@ -0,0 +1,39 @@ +#ifndef WASM_REPLAY_H +#define WASM_REPLAY_H + +// WASM replay path API. The replay context type is an alias for the native +// ulog_replay_ctx_t — WASM-specific fields (timeline, cursors, wall_accum) +// are added to the native struct behind #ifdef __EMSCRIPTEN__ so the shared +// apply helpers (src/ulog_replay_apply.{h,c}) can take a single ctx pointer +// on both targets. + +#include +#include +#include + +#include "ulog_replay.h" // ulog_replay_ctx_t (with WASM-only fields under ifdef) +#include "ulog_events.h" // ulog_timeline_t + event structs + +typedef ulog_replay_ctx_t wasm_replay_ctx_t; + +// Initialize a replay context from a ULog byte buffer. The buffer may be +// freed by the caller immediately after this call returns — the extractor +// does not retain any reference to it. Returns 0 on success, -1 on failure. +int wasm_replay_init_from_bytes(wasm_replay_ctx_t *ctx, + const uint8_t *buf, size_t len); + +// Advance playback by dt seconds at the given speed. Returns true if still +// playing (cursors have not all reached end-of-timeline). +bool wasm_replay_advance(wasm_replay_ctx_t *ctx, float dt, float speed, + bool looping, bool interpolation); + +// Seek to an absolute position (seconds from log start). +void wasm_replay_seek(wasm_replay_ctx_t *ctx, float target_s); + +// Free all owned resources. +void wasm_replay_close(wasm_replay_ctx_t *ctx); + +// Log duration in microseconds (timeline end - start). Zero if not loaded. +uint64_t wasm_replay_duration_us(const wasm_replay_ctx_t *ctx); + +#endif diff --git a/wasm/.gitignore b/wasm/.gitignore new file mode 100644 index 0000000..c9d6701 --- /dev/null +++ b/wasm/.gitignore @@ -0,0 +1,4 @@ +build/ +*.wasm +*.data +*.js.map diff --git a/wasm/CMakeLists.txt b/wasm/CMakeLists.txt new file mode 100644 index 0000000..98497a1 --- /dev/null +++ b/wasm/CMakeLists.txt @@ -0,0 +1,176 @@ +cmake_minimum_required(VERSION 3.14) +project(hawkeye-wasm C) + +# ----------------------------------------------------------------------------- +# Hawkeye WASM build. Standalone CMake subproject invoked via: +# +# emcmake cmake -S wasm -B wasm/build -DCMAKE_BUILD_TYPE=Release +# cmake --build wasm/build -j +# +# Does NOT modify the root CMakeLists.txt or any file under src/*.c / src/*.h. +# All WASM-specific C code lives under src/wasm/ and this directory. +# ----------------------------------------------------------------------------- + +if(NOT EMSCRIPTEN) + message(FATAL_ERROR + "The Hawkeye WASM build requires the Emscripten toolchain. " + "Configure with: emcmake cmake -S wasm -B wasm/build") +endif() + +set(CMAKE_C_STANDARD 11) + +set(HAWKEYE_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/..) +set(HAWKEYE_SRC ${HAWKEYE_ROOT}/src) + +# ----------------------------------------------------------------------------- +# Raylib via FetchContent, retargeted to Web + OpenGL ES 3 +# ----------------------------------------------------------------------------- +# Raylib's own CMake responds to PLATFORM/GRAPHICS cache variables set BEFORE +# FetchContent_MakeAvailable(). Setting them here compiles Raylib's web +# backend (rcore_web.c + GLFW/emscripten glue) instead of its desktop backend. + +set(PLATFORM "Web" CACHE STRING "Raylib target platform" FORCE) +set(GRAPHICS "GRAPHICS_API_OPENGL_ES3" CACHE STRING "Raylib graphics API" FORCE) +set(BUILD_EXAMPLES OFF CACHE BOOL "" FORCE) +set(BUILD_SHARED_LIBS OFF CACHE BOOL "" FORCE) + +# Raylib module trims — every disabled module shrinks the WASM binary by a +# meaningful amount. Drop audio (no MAVLink beeps in Hawkeye). Drop image +# format parsers except PNG. Drop model format parsers except OBJ + GLTF. +set(SUPPORT_MODULE_RAUDIO OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_JPG OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_BMP OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_TGA OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_GIF OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_QOI OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_PSD OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_DDS OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_HDR OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_PIC OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_KTX OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_ASTC OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_PKM OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_PVR OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_IQM OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_VOX OFF CACHE BOOL "" FORCE) +set(SUPPORT_FILEFORMAT_M3D OFF CACHE BOOL "" FORCE) + +include(FetchContent) +FetchContent_Declare( + raylib + GIT_REPOSITORY https://github.com/raysan5/raylib.git + GIT_TAG 5.5 + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(raylib) + +# ----------------------------------------------------------------------------- +# Hawkeye WASM executable +# ----------------------------------------------------------------------------- +# Skeleton stage (Task #4 of plan): wasm_main.c is a stub that initializes +# Raylib, clears the canvas, and implements the six embed API functions as +# no-ops. Scene / vehicle / HUD / replay wire-up happens in later tasks by +# adding to this source list, not by rewriting the build. +# +# Source files included at this stage: +# - wasm/wasm_main.c (entry point, stub API impl) +# - src/wasm/ulog_timeline.c (flat-array event container) +# - src/wasm/ulog_extractor.c (ULog walker + pre-scan) +# - src/wasm/wasm_replay.c (cursor-based playback) + +add_executable(hawkeye + ${CMAKE_CURRENT_SOURCE_DIR}/wasm_main.c + ${CMAKE_CURRENT_SOURCE_DIR}/wasm_dialog.c + + # Shared apply helpers (native + WASM). Update-once source of truth for + # per-message state updates; decode stage is platform-specific. + ${HAWKEYE_SRC}/ulog_replay_apply.c + + # WASM-only C code (freshly written, separate from native src/) + ${HAWKEYE_SRC}/wasm/ulog_timeline.c + ${HAWKEYE_SRC}/wasm/ulog_extractor.c + ${HAWKEYE_SRC}/wasm/wasm_replay.c + + # Rendering spine — shared with native (no modifications to these files) + ${HAWKEYE_SRC}/scene.c + ${HAWKEYE_SRC}/vehicle.c + ${HAWKEYE_SRC}/theme.c + ${HAWKEYE_SRC}/asset_path.c + + # HUD subsystem — shared with native + ${HAWKEYE_SRC}/hud.c + ${HAWKEYE_SRC}/hud_help.c + ${HAWKEYE_SRC}/hud_instruments.c + ${HAWKEYE_SRC}/hud_transport.c + ${HAWKEYE_SRC}/hud_telemetry.c + ${HAWKEYE_SRC}/hud_annunciators.c + ${HAWKEYE_SRC}/tactical_hud.c + ${HAWKEYE_SRC}/debug_panel.c + ${HAWKEYE_SRC}/ortho_panel.c + ${HAWKEYE_SRC}/replay_trail.c + ${HAWKEYE_SRC}/replay_markers.c + ${HAWKEYE_SRC}/replay_conflict.c + ${HAWKEYE_SRC}/ui_marker_input.c +) + +target_include_directories(hawkeye PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} # wasm/wasm_api.h + ${HAWKEYE_SRC} # src/*.h (type-only includes) + ${HAWKEYE_SRC}/wasm # src/wasm/*.h + ${HAWKEYE_ROOT}/lib/c_library_v2 + ${HAWKEYE_ROOT}/lib/c_library_v2/common +) + +target_compile_definitions(hawkeye PRIVATE MAVLINK_COMM_NUM_BUFFERS=16) + +target_link_libraries(hawkeye PRIVATE raylib) + +# emcc emits .js + .wasm (+ .data when preload is used) +set_target_properties(hawkeye PROPERTIES SUFFIX ".js") + +target_link_options(hawkeye PRIVATE + "-sMODULARIZE=1" + "-sEXPORT_ES6=1" + "-sEXPORT_NAME=createHawkeye" + "-sEXPORTED_FUNCTIONS=['_hawkeye_init','_hawkeye_load_ulog_bytes','_hawkeye_begin_multi_load','_hawkeye_stage_ulog','_hawkeye_finalize_multi_load','_hawkeye_get_conflict_flags','_hawkeye_add_theme_bytes','_hawkeye_resize','_hawkeye_destroy','_hawkeye_set_playing','_hawkeye_seek','_hawkeye_get_duration','_hawkeye_get_position','_malloc','_free']" + "-sEXPORTED_RUNTIME_METHODS=['ccall','cwrap','HEAPU8']" + "-sINITIAL_MEMORY=16MB" + "-sALLOW_MEMORY_GROWTH=1" + "-sMAXIMUM_MEMORY=512MB" + "-sUSE_WEBGL2=1" + "-sFULL_ES3=1" + "-sENVIRONMENT=web" + "-sALLOW_TABLE_GROWTH=1" + "-sSTACK_SIZE=1MB" + "SHELL:--preload-file ${HAWKEYE_ROOT}/shaders@shaders" + "SHELL:--preload-file ${HAWKEYE_ROOT}/models@models" + "SHELL:--preload-file ${HAWKEYE_ROOT}/fonts@fonts" + "SHELL:--preload-file ${HAWKEYE_ROOT}/textures@textures" + "SHELL:--preload-file ${HAWKEYE_ROOT}/themes@themes" +) + +# Release: size-optimize and run the closure compiler on JS glue. +# Debug: leave symbols + assertions on for early bring-up. +if(CMAKE_BUILD_TYPE STREQUAL "Release") + target_compile_options(hawkeye PRIVATE -Os) + target_link_options(hawkeye PRIVATE -Os) +else() + target_compile_options(hawkeye PRIVATE -O0 -g3) + target_link_options(hawkeye PRIVATE -O0 -g3 "-sASSERTIONS=2" "-sSAFE_HEAP=1") +endif() + +# Copy the dev-harness shell into the build dir as index.html so the +# python http.server can serve it at the root URL. Runs on every build +# via a dedicated custom target that depends on shell.html's mtime. +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/index.html + COMMAND ${CMAKE_COMMAND} -E copy_if_different + ${CMAKE_CURRENT_SOURCE_DIR}/shell.html + ${CMAKE_CURRENT_BINARY_DIR}/index.html + DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/shell.html +) +add_custom_target(hawkeye_index ALL DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/index.html) + +add_custom_command(TARGET hawkeye POST_BUILD + COMMAND ${CMAKE_COMMAND} -E echo "hawkeye WASM build complete. See wasm/README.md for smoke-test instructions." +) diff --git a/wasm/README.md b/wasm/README.md new file mode 100644 index 0000000..68bf826 --- /dev/null +++ b/wasm/README.md @@ -0,0 +1,107 @@ +# Hawkeye WASM build + +Standalone Emscripten build of Hawkeye, targeting embedding into host web pages (PX4 Flight Review, the self-hosted builder hub, or any other viewer that wants a 3D ULog replay canvas). + +**This directory is entirely separate from the native build.** The root `CMakeLists.txt` does not reference anything here, and building the native desktop binary (`make release` from the repo root) does not require Emscripten or touch any file under `wasm/` or `src/wasm/`. + +## Layout + +``` +wasm/ +├── CMakeLists.txt Standalone emscripten build subproject +├── wasm_api.h Exported C API (interface contract for host pages) +├── wasm_main.c Entry point, embed API implementation, frame loop +├── shell.html Local dev harness (not shipping UI) +├── README.md This file +└── .gitignore build/ output + +src/wasm/ WASM-only C implementation (separate from native src/) +├── ulog_events.h Per-type event structs + flat timeline container +├── ulog_timeline.h/.c Append/free helpers for the timeline +├── ulog_extractor.h/.c Reads a ULog from memory into a timeline + pre-scan state +└── wasm_replay.h/.c Cursor-based playback over the pre-extracted timeline +``` + +Native source files (`src/*.c`, `src/*.h`) are untouched. The WASM build includes a few native headers (`mavlink_receiver.h`, `ulog_replay.h`) for type definitions only — no native `.c` files are compiled in the WASM build. + +## Prerequisites + +- [Emscripten SDK](https://emscripten.org/docs/getting_started/downloads.html) installed and `emsdk_env.sh` sourced in the current shell. +- CMake 3.14+ +- `python3` (for serving the local dev harness during smoke testing) + +Installing emsdk for the first time: + +```bash +git clone https://github.com/emscripten-core/emsdk.git +cd emsdk +./emsdk install latest +./emsdk activate latest +source ./emsdk_env.sh +``` + +## Building + +From the repo root: + +```bash +emcmake cmake -S wasm -B wasm/build -DCMAKE_BUILD_TYPE=Release +cmake --build wasm/build -j +``` + +The build emits three files into `wasm/build/`: + +- `hawkeye.js` — JS glue code exposing the WASM module +- `hawkeye.wasm` — the compiled binary +- `hawkeye.data` — preloaded asset blob (once asset preloading is wired in Task #5; the skeleton stage has no `.data` file) + +## Local smoke test + +Copy the dev harness next to the build artifacts and serve it: + +```bash +cp wasm/shell.html wasm/build/index.html +cd wasm/build +python3 -m http.server 8000 +``` + +Then open `http://localhost:8000/` in a browser. You should see: +- A dark canvas +- The log panel showing `hawkeye.js loaded` and `hawkeye_init → 0` +- Clicking Play / Pause / dragging the seek slider should not error (no log is loaded yet in the skeleton stage) + +Drag a `.ulg` file into the file input and the log panel should report `hawkeye_load_ulog_bytes → 0` and print the number of extracted events to the console. + +## Current status (Task progress) + +Tracked in `/Users/m/.claude/plans/velvety-dazzling-dolphin.md` plan file. + +- **Task #2** ULog pre-extraction — **done** (`src/wasm/ulog_extractor.*`, `wasm_replay.*`) +- **Task #3** Interface contract — **done** (`wasm/wasm_api.h`) +- **Task #4** Skeleton build — **in progress** (`wasm/CMakeLists.txt`, `wasm/wasm_main.c`, this README) +- **Task #5** Asset audit — pending (needs a first successful build to measure) +- **Task #6** Dev harness — **done** (`wasm/shell.html`) +- **Task #7** Rendering spine wire-up — pending +- **Task #8** ULog replay wire-up — pending +- **Task #9** HUD wire-up — pending +- **Task #10** Embed API polish — pending +- **Task #11** Cross-browser smoke test — pending +- **Task #12** Size measurements — pending +- **Task #13** CI workflow — pending + +## Size targets + +To be recorded here after the first green build and the asset audit: + +| Artifact | Target (Brotli-compressed) | Actual | +| ------------------ | -------------------------- | ------ | +| `hawkeye.wasm` | <1.5 MB | TBD | +| `hawkeye.data` | <1.0 MB | TBD | +| `hawkeye.js` glue | <100 KB | TBD | + +## Known limitations + +- **Single-threaded only.** No `SharedArrayBuffer`, no pthreads. Necessary because host pages (e.g. Flight Review) cannot be required to set COOP/COEP headers. +- **No MAVLink UDP.** The WASM build does not include `src/mavlink_receiver.c` or `src/data_source_mavlink.c`. Live telemetry is a native-only feature. +- **WebGL2 required.** iOS Safari supports WebGL2 on iOS 15+. Older browsers will fail `hawkeye_init`. +- **Asset preloading pending.** The skeleton stage does not yet preload models, shaders, fonts, textures, or themes. Once Task #7 (rendering spine wire-up) lands, `--preload-file` will be added to the link flags. diff --git a/wasm/TESTING.md b/wasm/TESTING.md new file mode 100644 index 0000000..090781b --- /dev/null +++ b/wasm/TESTING.md @@ -0,0 +1,58 @@ +# WASM Replay Testing Matrix + +One log per position resolution tier from `../logs/`. Tests every branch of the fallback chain documented in `POSITION_RESOLUTION_TEST_CHECKLIST.md`. The WASM extractor and replay must handle all of these identically to native. + +## Position resolution tiers + +| # | Category | Log file | Expected behavior | +|---|----------|----------|-------------------| +| 1 | **C01: Home valid** | `../logs/C01_HOME_POS_VALID/0192e367-56ff-4828-9831-1d28b2be0b9e.ulg` | `home=valid` in console. Origin set from first state after home arrives. Drone flies normally. Best case. | +| 2 | **C02: Home zeros, GPOS valid** | `../logs/C02_HOME_POS_ZEROS__GPOS_VALID/2b12d921-2048-4365-a2a2-cb95193adfe7.ulg` | `home=valid` from first GPOS (fallback). Drone positions from global position data. | +| 3 | **C02: Home zeros, LPOS ref valid** | `../logs/C02_HOME_POS_ZEROS__LPOS_REF_VALID/8a612aef-d2ba-4249-b710-261c737ea56c.ulg` | Origin derived from LPOS reference point (`ref_lat/ref_lon`). Drone positions via local-to-global conversion. | +| 4 | **C02: Home zeros, LPOS no global** | `../logs/C02_HOME_POS_ZEROS__LPOS_NO_GLOBAL/272adb09-4f3a-4ed8-9025-24c65aad1b99.ulg` | No global reference. Drone renders at local NED coordinates relative to origin. May sit near world origin. | +| 5 | **C02: Home zeros, LPOS ref zeros** | `../logs/C02_HOME_POS_ZEROS__LPOS_REF_ZEROS/f69c1496-c3f8-4d2c-9933-9c2c0cac14d9.ulg` | LPOS ref exists but is zero. Should fall back to origin from first nonzero position after ~20 frames. | +| 6 | **C03: No home, GPOS valid** | `../logs/C03_NO_HOME__GPOS_VALID/31409991-e4e0-4c90-a44e-2f35648e38ac.ulg` | `home=none` in console. Origin auto-resolves from first GPOS sample in `vehicle_update`. Drone flies normally. | +| 7 | **C05: No home, no GPOS, LPOS ref valid** | `../logs/C05_NO_HOME__NO_GPOS__LPOS_REF_VALID/3ceed808-83eb-47c1-a21c-2c08fb1ecb7e.ulg` | Position derived entirely from LPOS + reference point. No GPS. Drone should still move. | +| 8 | **C07: No home, no GPOS, LPOS no global** | `../logs/C07_NO_HOME__NO_GPOS__LPOS_NO_GLOBAL/05f68e25-b07d-4967-93a0-40a7aa496d19.ulg` | Worst viable position tier. Drone at local NED coords, no global reference. May appear at origin with local motion only. | +| 9 | **C09: No position data** | `../logs/C09_NO_HOME__NO_GPOS__NO_LPOS/66188e38-448c-442f-ac87-81e11b023a5c.ulg` | Attitude only, no position. Drone stays at origin, rotates in place. `state.valid` may never become true. Console should show `0 lpos, 0 gpos`. | +| 10 | **C10: No attitude** | `../logs/C10_NO_ATTITUDE/2c458da4-2e39-49d9-8526-79388b388930.ulg` | No vehicle_attitude topic. Extractor should report `0 att`. Drone may not render or may sit frozen. Native requires attitude to load; WASM extractor should handle gracefully (no crash). | + +## Playback controls + +Run these on any C01 log (known-good positioning): + +| # | Test | Steps | Pass criteria | +|---|------|-------|---------------| +| 11 | **Play/Pause** | Load log, click Pause, wait 3s, click Play | Drone stops on Pause, resumes on Play. No crash. | +| 12 | **Seek** | Load log, drag slider to ~50%, release | Drone jumps to mid-flight position. Playback continues from there. No crash or freeze. | +| 13 | **Reload** | Load one log, then load a different log | First log's trail clears. New log plays. No crash, no leftover state from first log. | +| 14 | **Loop** | Load log, let it play to the end | Playback restarts from the beginning. Drone returns to start position. | + +## Event extraction + +Check the browser console `WASM replay:` line for each log: + +| # | Log category | Expected counts | +|---|-------------|-----------------| +| 15 | C01 | Nonzero att, nonzero lpos, likely nonzero gpos | +| 16 | C03 | Nonzero att, likely nonzero lpos, nonzero gpos | +| 17 | C07 | Nonzero att, nonzero lpos, 0 gpos | +| 18 | C09 | 0 att OR very few, 0 lpos, 0 gpos | +| 19 | C10 | 0 att, may have lpos/gpos but no attitude to display | + +## How to test + +1. Start the dev server: `cd wasm/build && python3 -m http.server 8000` +2. Open `http://localhost:8000/` +3. For each row, use Choose File to load the log +4. Watch for ~10 seconds, check console output +5. Record: pass / fail / notes + +Quick format for reporting back: +``` +1: pass +2: pass +3: drone stuck at origin +4: pass +... +``` diff --git a/wasm/shell.html b/wasm/shell.html new file mode 100644 index 0000000..ebc029d --- /dev/null +++ b/wasm/shell.html @@ -0,0 +1,278 @@ + + + + + +Hawkeye WASM — dev harness + + + + +
+ +
+ + +
+
+

ULog

+ +
+ +
+

Log

+
+
+
+
+ + + + + diff --git a/wasm/wasm_api.h b/wasm/wasm_api.h new file mode 100644 index 0000000..985b498 --- /dev/null +++ b/wasm/wasm_api.h @@ -0,0 +1,151 @@ +#ifndef HAWKEYE_WASM_API_H +#define HAWKEYE_WASM_API_H + +// Hawkeye WASM embed API. +// +// This header is the stable interface contract between the host page +// (Flight Review embed, self-hosted builder hub, or the local dev harness) +// and the WASM build of Hawkeye. All functions are exported via +// EMSCRIPTEN_KEEPALIVE and accessible from JavaScript via Module.ccall or +// Module.cwrap. +// +// Threading +// --------- +// All functions must be called from the main browser thread. Hawkeye WASM +// is single-threaded — it does not require SharedArrayBuffer, does not set +// COOP/COEP headers, and does not use pthreads. This is intentional so the +// build can embed inside host pages (e.g. Flight Review) that cannot +// guarantee cross-origin isolation. +// +// Lifecycle +// --------- +// createHawkeye({canvas: ...}) // JS side, returns Module promise +// hawkeye_init(canvas_id, w, h) // bind to canvas, init scene+hud, start frame loop +// hawkeye_load_ulog_bytes(p, n) // (optional) extract a ULog and start playback +// hawkeye_set_playing(1/0) // play/pause +// hawkeye_seek(seconds) // jump to a time offset from log start +// hawkeye_resize(w, h) // respond to host-page layout changes +// hawkeye_destroy() // tear down and release resources +// +// Error handling +// -------------- +// Integer-returning functions return 0 on success, nonzero on failure. +// Failures are logged to the browser console via printf. They are not +// signalled via postMessage — the host JS wrapper is expected to check +// return values if it cares. + +#ifdef __EMSCRIPTEN__ +#include +#else +// Allow this header to be read by native tooling (IDEs, clangd) even though +// the functions are only defined in the WASM build. +#define EMSCRIPTEN_KEEPALIVE +#endif + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +// Query the loaded log's duration in seconds. Returns 0 if no log is loaded. +EMSCRIPTEN_KEEPALIVE +double hawkeye_get_duration(void); + +// Query the current playback position in seconds from log start. +// Returns 0 if no log is loaded. +EMSCRIPTEN_KEEPALIVE +double hawkeye_get_position(void); + +// Multi-drone loading: stage files one at a time, then finalize. +// +// Usage from JS: +// hawkeye_begin_multi_load(count) +// for each file: hawkeye_stage_ulog(index, ptr, len) +// hawkeye_finalize_multi_load(mode) +// +// mode: 0 = auto, 1 = formation, 2 = ghost, 3 = grid + +EMSCRIPTEN_KEEPALIVE +int hawkeye_begin_multi_load(int count); + +EMSCRIPTEN_KEEPALIVE +int hawkeye_stage_ulog(int index, const uint8_t *buf, size_t len); + +EMSCRIPTEN_KEEPALIVE +int hawkeye_finalize_multi_load(int mode); + +// Initialize Hawkeye, bind the renderer to the HTML canvas identified by +// canvas_id (a selector string e.g. "#hawkeye-canvas"), and start the +// per-frame update loop via emscripten_set_main_loop. Safe to call once +// per module instance. +// +// Preconditions: canvas_id is non-null; width/height > 0; WebGL2 available. +// Postconditions: on success, the main loop is running and rendering into +// the canvas. Returns 0 on success, -1 if the canvas cannot be acquired or +// WebGL2 init fails. +EMSCRIPTEN_KEEPALIVE +int hawkeye_init(const char *canvas_id, int width, int height); + +// Load a ULog from a byte buffer. The buffer must contain a complete, +// valid ULog file. On success, the WASM replay context owns the extracted +// in-memory timeline and the caller may free (or let JS garbage collect) +// the input buffer immediately after this call returns. +// +// This function walks the file once to populate the flat event timeline +// and pre-scan state (mode changes, takeoff detection, home resolution). +// Memory footprint after return: approximately 1-5 MB per typical single +// drone flight, regardless of the input file size. +// +// Calling this while a previous log is loaded releases the previous log's +// timeline and replaces it. Returns 0 on success, -1 on parse failure or +// allocation failure. +EMSCRIPTEN_KEEPALIVE +int hawkeye_load_ulog_bytes(const uint8_t *buf, size_t len); + +// Notify Hawkeye that the canvas size has changed. Updates the viewport, +// projection, and any cached framebuffer sizes. Cheap; safe to call on +// every resize event. +EMSCRIPTEN_KEEPALIVE +void hawkeye_resize(int width, int height); + +// Tear down the Hawkeye instance: stop the main loop, release the ULog +// timeline, release GL resources (textures, VBOs, framebuffers), free the +// heap. After this returns, the module should not be used except by +// re-instantiating from createHawkeye(). +EMSCRIPTEN_KEEPALIVE +void hawkeye_destroy(void); + +// Toggle playback. When paused, the scene remains rendered and the HUD +// continues to draw, but replay time does not advance. Ignored if no +// log is loaded. +EMSCRIPTEN_KEEPALIVE +void hawkeye_set_playing(int playing); + +// Seek absolute playback position, measured in seconds from the start of +// the loaded log. Clamped to [0, log_duration]. Ignored if no log is +// loaded. After seek, replay state is settled by walking cursors forward +// from a point slightly before the target, matching native's seek-early +// + forward-scan semantics. +EMSCRIPTEN_KEEPALIVE +void hawkeye_seek(double seconds); + +// Query conflict detection result after staging files but before finalize. +// Bit0 = conflict_detected, bit1 = conflict_far. Returns 0 if not applicable +// (single drone, no log, or not initialized). Intended for host-side dialog +// wiring — call after all hawkeye_stage_ulog() calls complete. +EMSCRIPTEN_KEEPALIVE +int hawkeye_get_conflict_flags(void); + +// Accept a .mvt theme file as raw bytes (drag-drop replacement for native's +// IsFileDropped handler). `name` is the filename used as the MEMFS key so +// theme_load_mvt's fopen path has something to open. Returns 0 on success. +EMSCRIPTEN_KEEPALIVE +int hawkeye_add_theme_bytes(const uint8_t *buf, size_t len, const char *name); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/wasm/wasm_dialog.c b/wasm/wasm_dialog.c new file mode 100644 index 0000000..2163bb2 --- /dev/null +++ b/wasm/wasm_dialog.c @@ -0,0 +1,106 @@ +// WASM prompt dialog implementation. Measurement and draw logic mirrors +// src/replay_conflict.c:60-132 (draw_prompt_dialog) — same theme fields, same +// geometry, same font sizes. Difference: one frame per call, state persists +// across frames via wasm_dialog_t. + +#include "wasm_dialog.h" +#include +#include +#include + +static void copy_str(char *dst, const char *src, size_t cap) { + if (!src) { dst[0] = '\0'; return; } + size_t n = strlen(src); + if (n >= cap) n = cap - 1; + memcpy(dst, src, n); + dst[n] = '\0'; +} + +void wasm_dialog_begin(wasm_dialog_t *d, + const char *title, const char *subtitle, + const char **options, int count) +{ + memset(d, 0, sizeof(*d)); + d->active = true; + copy_str(d->title, title, WASM_DIALOG_STR_MAX); + copy_str(d->subtitle, subtitle ? subtitle : "", WASM_DIALOG_STR_MAX); + if (count > WASM_DIALOG_MAX_OPTIONS) count = WASM_DIALOG_MAX_OPTIONS; + d->option_count = count; + for (int i = 0; i < count; i++) + copy_str(d->options[i], options[i], WASM_DIALOG_STR_MAX); +} + +int wasm_dialog_draw_and_poll(wasm_dialog_t *d, const theme_t *theme, + Font font_label, Font font_value) +{ + if (!d->active) return 0; + + int choice = 0; + // Poll keys 1..option_count + for (int o = 0; o < d->option_count && o < 9; o++) { + if (IsKeyPressed(KEY_ONE + o)) { choice = o + 1; break; } + } + + int sw = GetScreenWidth(); + int sh = GetScreenHeight(); + float s = powf(sh / 720.0f, 0.7f); + if (s < 1.0f) s = 1.0f; + + float fs_title = 15 * s, fs_option = 20 * s, fs_hint = 12 * s; + float line_h = 36 * s, pad = 16 * s, inner_gap = 20 * s; + + float key_num_w = MeasureTextEx(font_value, "1", fs_option, 0.5f).x; + float gap = 16 * s; + float widest = 0; + for (int o = 0; o < d->option_count; o++) { + float w = key_num_w + gap + + MeasureTextEx(font_label, d->options[o], fs_option, 0.5f).x; + if (w > widest) widest = w; + } + + Vector2 tmw = MeasureTextEx(font_label, d->title, fs_title, 0.5f); + Vector2 trw = MeasureTextEx(font_label, d->subtitle, fs_title, 0.5f); + float title_total = tmw.x + trw.x; + + float box_w = 520 * s; + float opts_block_h = line_h * d->option_count; + float box_h = pad + fs_title + inner_gap + opts_block_h + inner_gap * 0.3f + fs_hint + pad * 0.5f; + float cx = sw / 2.0f, cy = sh / 2.0f; + float bx = cx - box_w / 2.0f, by = cy - box_h / 2.0f; + + DrawRectangle(0, 0, sw, sh, theme->prompt_scrim); + + Rectangle box = {bx, by, box_w, box_h}; + DrawRectangleRounded(box, 0.06f, 8, theme->prompt_box_bg); + DrawRectangleRoundedLinesEx(box, 0.06f, 8, 1.5f * s, theme->prompt_border); + + float title_x = cx - title_total / 2.0f; + DrawTextEx(font_label, d->title, + (Vector2){title_x, by + pad}, fs_title, 0.5f, theme->prompt_title); + DrawTextEx(font_label, d->subtitle, + (Vector2){title_x + tmw.x, by + pad}, fs_title, 0.5f, theme->prompt_subtitle); + + float left = cx - widest / 2.0f; + float opts_y = by + pad + fs_title + inner_gap; + for (int o = 0; o < d->option_count; o++) { + char num[2] = {(char)('1' + o), '\0'}; + DrawTextEx(font_value, num, + (Vector2){left, opts_y + o * line_h}, fs_option, 0.5f, + theme->prompt_key); + DrawTextEx(font_label, d->options[o], + (Vector2){left + key_num_w + gap, opts_y + o * line_h}, + fs_option, 0.5f, theme->prompt_text); + } + + char hint[32]; + if (d->option_count == 3) snprintf(hint, sizeof(hint), "Press 1, 2, or 3"); + else if (d->option_count == 2) snprintf(hint, sizeof(hint), "Press 1 or 2"); + else snprintf(hint, sizeof(hint), "Press 1"); + Vector2 hw = MeasureTextEx(font_label, hint, fs_hint, 0.5f); + DrawTextEx(font_label, hint, + (Vector2){cx - hw.x / 2.0f, by + box_h - pad * 0.5f - fs_hint}, + fs_hint, 0.5f, theme->prompt_hint); + + if (choice > 0) d->active = false; + return choice; +} diff --git a/wasm/wasm_dialog.h b/wasm/wasm_dialog.h new file mode 100644 index 0000000..90269fb --- /dev/null +++ b/wasm/wasm_dialog.h @@ -0,0 +1,42 @@ +// Non-blocking prompt dialog for the WASM build. Native's draw_prompt_dialog +// (src/replay_conflict.c:54) spins in its own while(!WindowShouldClose()) loop, +// which cannot work under emscripten_set_main_loop. This module keeps the same +// visual design (scrim + rounded box + numbered options + hint) but drives the +// state machine one frame at a time. +// +// Usage: +// wasm_dialog_begin(&d, "TITLE", " - subtitle", {"A","B","C"}, 3); +// ...each frame, after HUD, before EndDrawing: +// int choice = wasm_dialog_draw_and_poll(&d, theme, font_label, font_value); +// if (choice > 0) { ...apply choice; dialog is auto-closed... } +// +// While d.active is true, the frame callback must suppress other input. + +#ifndef WASM_DIALOG_H +#define WASM_DIALOG_H + +#include "raylib.h" +#include "theme.h" + +#define WASM_DIALOG_MAX_OPTIONS 3 +#define WASM_DIALOG_STR_MAX 64 + +typedef struct { + bool active; + char title[WASM_DIALOG_STR_MAX]; + char subtitle[WASM_DIALOG_STR_MAX]; + char options[WASM_DIALOG_MAX_OPTIONS][WASM_DIALOG_STR_MAX]; + int option_count; +} wasm_dialog_t; + +void wasm_dialog_begin(wasm_dialog_t *d, + const char *title, const char *subtitle, + const char **options, int count); + +// Returns 0 if still pending, 1..N when an option is pressed (dialog is then +// marked inactive automatically). Caller must draw this inside a BeginDrawing +// block (does not wrap its own). +int wasm_dialog_draw_and_poll(wasm_dialog_t *d, const theme_t *theme, + Font font_label, Font font_value); + +#endif diff --git a/wasm/wasm_main.c b/wasm/wasm_main.c new file mode 100644 index 0000000..89b3f56 --- /dev/null +++ b/wasm/wasm_main.c @@ -0,0 +1,1747 @@ +// Hawkeye WASM entry point. Line-ordered port of src/main.c with the following +// platform adaptations — every other call in main.c has a matching line here +// in the same order. +// +// native WASM +// ----------------------------------- -------------------------------------- +// argv CLI parsing fixed defaults (replay-only) +// while(!WindowShouldClose()) loop emscripten_set_main_loop_arg(frame) +// data_source_ulog_create(path) wasm_replay_ctx_t + data_source shim +// IsFileDropped() for .mvt themes hawkeye_add_theme_bytes() JS API +// draw_prompt_dialog (blocking loop) in-canvas dialog state machine (Phase 2) +// shader GLSL #version 330 SetLoadFileTextCallback patch to GLES +// +// See WASM_PORT_FAILURES.md for the history behind these adaptations. + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#include +#include +#include + +#include "raylib.h" +#include "raymath.h" + +#include "wasm_api.h" +#include "wasm_dialog.h" +#include "wasm/wasm_replay.h" + +#include "data_source.h" +#include "vehicle.h" +#include "scene.h" +#include "hud.h" +#include "tactical_hud.h" +#include "ui_logic.h" +#include "debug_panel.h" +#include "ortho_panel.h" +#include "theme.h" +#include "asset_path.h" +#include "replay_trail.h" +#include "replay_markers.h" +#include "replay_conflict.h" +#include "ui_marker_input.h" +#include "correlation.h" + +static void log_heap(const char *label) { + struct mallinfo mi = mallinfo(); + printf("[heap] %-24s live=%5zu KB arena=%5zu KB free=%5zu KB\n", + label, + (size_t)mi.uordblks / 1024, + (size_t)mi.arena / 1024, + (size_t)mi.fordblks / 1024); +} + +#define MAX_VEHICLES 16 +#define EARTH_RADIUS 6371000.0 +#define CHORD_TIMEOUT_S 0.3 + +// --------------------------------------------------------------------------- +// GLSL 330 → GLES 300 es shader patcher. SetLoadFileTextCallback routes every +// LoadFileText() call through here so Raylib's bundled #version 330 shaders +// compile under WebGL2. +// --------------------------------------------------------------------------- + +static char *read_whole_file(const char *path, int *out_size) { + FILE *fp = fopen(path, "rb"); + if (!fp) return NULL; + fseek(fp, 0, SEEK_END); + long sz = ftell(fp); + fseek(fp, 0, SEEK_SET); + if (sz < 0) { fclose(fp); return NULL; } + char *buf = (char *)malloc((size_t)sz + 1); + if (!buf) { fclose(fp); return NULL; } + size_t n = fread(buf, 1, (size_t)sz, fp); + fclose(fp); + buf[n] = '\0'; + if (out_size) *out_size = (int)n; + return buf; +} + +static char *patch_shader_text(char *src) { + const char *needle = "#version 330"; + if (strncmp(src, needle, strlen(needle)) != 0) return src; + const char *eol = strchr(src, '\n'); + if (!eol) return src; + const char *header = "#version 300 es\nprecision highp float;\nprecision highp int;\n"; + size_t header_len = strlen(header); + size_t tail_len = strlen(eol + 1); + char *out = (char *)malloc(header_len + tail_len + 1); + if (!out) return src; + memcpy(out, header, header_len); + memcpy(out + header_len, eol + 1, tail_len); + out[header_len + tail_len] = '\0'; + free(src); + return out; +} + +static char *hawkeye_load_file_text(const char *filename) { + int size = 0; + char *src = read_whole_file(filename, &size); + if (!src) return NULL; + return patch_shader_text(src); +} + +// --------------------------------------------------------------------------- +// Edge indicator chevrons. Ported verbatim from main.c:62-145 with the single +// call-site substitution of g.vehicles / g.selected etc. +// --------------------------------------------------------------------------- + +static void draw_edge_indicators(const vehicle_t *vehicles, int vehicle_count, + int selected, Camera3D camera, Font font, + float scale) +{ + (void)scale; + int ei_sw = GetScreenWidth(); + int ei_sh = GetScreenHeight(); + float ei_margin = 40.0f; + float ei_scale = powf(ei_sh / 720.0f, 0.7f); + if (ei_scale < 1.0f) ei_scale = 1.0f; + Vector3 cam_fwd = Vector3Normalize(Vector3Subtract( + camera.target, camera.position)); + + for (int i = 0; i < vehicle_count; i++) { + if (i == selected || !vehicles[i].active) continue; + + Vector3 to_drone = Vector3Subtract(vehicles[i].position, + camera.position); + float dot = to_drone.x * cam_fwd.x + to_drone.y * cam_fwd.y + + to_drone.z * cam_fwd.z; + + Vector2 sp = GetWorldToScreen(vehicles[i].position, camera); + + if (sp.x >= ei_margin && sp.x <= ei_sw - ei_margin && + sp.y >= ei_margin && sp.y <= ei_sh - ei_margin) continue; + + float ei_cx = ei_sw / 2.0f; + float ei_cy = ei_sh / 2.0f; + float ei_dx = sp.x - ei_cx; + float ei_dy = sp.y - ei_cy; + + if (dot < 0.5f) { + Vector3 cam_right = Vector3Normalize( + Vector3CrossProduct(cam_fwd, (Vector3){0, 1, 0})); + Vector3 cam_up_approx = Vector3CrossProduct(cam_right, cam_fwd); + ei_dx = Vector3DotProduct(to_drone, cam_right); + ei_dy = -Vector3DotProduct(to_drone, cam_up_approx); + float len = sqrtf(ei_dx * ei_dx + ei_dy * ei_dy); + if (len > 0.01f) { ei_dx /= len; ei_dy /= len; } + ei_dx *= ei_sw; + ei_dy *= ei_sh; + } + + float sx = (ei_dx != 0) + ? ((ei_dx > 0 ? ei_sw - ei_margin : ei_margin) - ei_cx) / ei_dx + : 1e9f; + float sy = (ei_dy != 0) + ? ((ei_dy > 0 ? ei_sh - ei_margin : ei_margin) - ei_cy) / ei_dy + : 1e9f; + float se = fminf(fabsf(sx), fabsf(sy)); + float ex = ei_cx + ei_dx * se; + float ey = ei_cy + ei_dy * se; + if (ex < ei_margin) ex = ei_margin; + if (ex > ei_sw - ei_margin) ex = ei_sw - ei_margin; + if (ey < ei_margin) ey = ei_margin; + if (ey > ei_sh - ei_margin) ey = ei_sh - ei_margin; + + Color col = vehicles[i].color; + col.a = 220; + float angle = atan2f(ei_dy, ei_dx); + float sz = 14.0f * ei_scale; + + float chev_len = sz * 1.2f; + float chev_spread = 0.5f; + Vector2 tip = { ex + cosf(angle) * chev_len, + ey + sinf(angle) * chev_len }; + Vector2 cl = { ex + cosf(angle + chev_spread) * sz * 0.6f, + ey + sinf(angle + chev_spread) * sz * 0.6f }; + Vector2 cr = { ex + cosf(angle - chev_spread) * sz * 0.6f, + ey + sinf(angle - chev_spread) * sz * 0.6f }; + DrawLineEx(tip, cl, 2.5f * ei_scale, col); + DrawLineEx(tip, cr, 2.5f * ei_scale, col); + + char num[4]; + snprintf(num, sizeof(num), "%d", i + 1); + float lfs = 18.0f * ei_scale; + Vector2 tw = MeasureTextEx(font, num, lfs, 0.5f); + float lx = ex - cosf(angle) * (sz * 0.3f) - tw.x / 2; + float ly = ey - sinf(angle) * (sz * 0.3f) - tw.y / 2; + DrawTextEx(font, num, (Vector2){ lx, ly }, lfs, 0.5f, col); + } +} + +// Thin wrapper mirroring main.c:54-58 +static void apply_vehicle_selection_hud(hud_t *hud, int idx, bool pin, + int *selected, int vehicle_count) { + apply_vehicle_selection(hud->pinned, &hud->pinned_count, + idx, pin, selected, vehicle_count); +} + +// --------------------------------------------------------------------------- +// Global state. Mirrors the local variables declared in main()'s body, scoped +// to module lifetime so the frame callback can access them. +// --------------------------------------------------------------------------- + +static const data_source_ops_t wasm_ds_ops; // forward decl + +static struct hawkeye_state { + bool initialized; + bool log_loaded; + int width, height; + char canvas_id[64]; + + // Rendering spine + scene_t scene; + vehicle_t vehicles[MAX_VEHICLES]; + data_source_t sources[MAX_VEHICLES]; + int vehicle_count; + + // HUD + hud_t hud; + debug_panel_t dbg_panel; + ortho_panel_t ortho; + bool show_hud; + float saved_chase_distance; + float tactical_chase_target; + + // Selection + chord + int selected; + int prev_selected; + int chord_first; + double chord_time; + bool chord_shift; + + // Per-vehicle tracking + bool was_connected[MAX_VEHICLES]; + Vector3 last_pos[MAX_VEHICLES]; + float prev_playback_pos[MAX_VEHICLES]; + bool insufficient_data[MAX_VEHICLES]; + int vehicle_tier[MAX_VEHICLES]; + int insufficient_check_frames; + bool insufficient_toasted; + int last_selected_for_insuf; + + // Display toggles + int trail_mode; + bool show_ground_track; + bool classic_colors; + bool show_edge_indicators; + int corr_mode; + bool show_corr_labels; + bool show_axes; + bool show_marker_labels; + + // Multi-drone state + bool ghost_mode; + bool ghost_mode_grid; + bool conflict_detected; + bool conflict_far; + bool has_tier3; + double ref_lat_rad, ref_lon_rad, min_alt; + bool takeoff_aligned; + corr_state_t corr[MAX_VEHICLES]; + + // Markers + user_markers_t markers[MAX_VEHICLES]; + sys_markers_t sys_markers[MAX_VEHICLES]; + precomp_trail_t precomp[MAX_VEHICLES]; + marker_input_t marker_input; + + // Replay contexts (one per loaded drone) + wasm_replay_ctx_t replays[MAX_VEHICLES]; + + // Non-blocking prompt dialog (replaces native's draw_prompt_dialog) + wasm_dialog_t dialog; + int dialog_context; // 0 = initial conflict, 1 = P-key mode switcher +} g; + +// --------------------------------------------------------------------------- +// data_source shim. The WASM build substitutes this vtable for native's +// data_source_ulog implementation. poll() drives wasm_replay_advance, which +// pulls events from the pre-extracted flat arrays. The static fields +// (mode_changes, takeoff_*, statustext ring pointer, vehicle_type) are +// populated once at finalize time by ds_init_static and never rewritten, +// so user-toggleable fields (speed, looping, paused, interpolation, +// time_offset_s) survive frame-to-frame. +// --------------------------------------------------------------------------- + +static void ds_init_static(data_source_t *ds, wasm_replay_ctx_t *r) { + ds->ops = &wasm_ds_ops; + ds->impl = r; + ds->state = r->state; + ds->home = r->home; + // Match native data_source_ulog_create:97 — connected is true as soon as + // the replay is opened, independent of state.valid. The main loop uses + // connected to unlock the grid_offset placeholder draw (main.c:531-534), + // so tying connected to state.valid would hide the drone until takeoff. + ds->connected = true; + ds->mav_type = r->vehicle_type; + ds->ref_rejected = r->ref_rejected; + + ds->playback.speed = 1.0f; + ds->playback.looping = true; + ds->playback.interpolation = true; + ds->playback.paused = false; + ds->playback.progress = 0.0f; + ds->playback.position_s = 0.0f; + ds->playback.time_offset_s = (float)r->time_offset_s; + ds->playback.current_nav_state = r->current_nav_state; + ds->playback.mode_changes = (const playback_mode_change_t *)r->mode_changes; + ds->playback.mode_change_count = r->mode_change_count; + ds->playback.takeoff_conf = r->takeoff_conf; + ds->playback.takeoff_time_s = r->takeoff_time_s; + ds->playback.takeoff_detected = r->takeoff_detected; + ds->playback.home_from_topic = r->home_from_topic; + ds->playback.statustext = &r->statustext; + ds->playback.correlation = NAN; + ds->playback.rmse = NAN; + + uint64_t range = wasm_replay_duration_us(r); + ds->playback.duration_s = range > 0 ? (float)((double)range / 1e6) : 0.0f; +} + +static void wasm_ds_poll(data_source_t *ds, float dt) { + wasm_replay_ctx_t *r = (wasm_replay_ctx_t *)ds->impl; + if (!r) return; + + if (!ds->playback.paused) { + bool still_playing = wasm_replay_advance(r, dt, ds->playback.speed, + ds->playback.looping, + ds->playback.interpolation); + // Native semantics (data_source_ulog.c:15): connected stays true as + // long as more events remain or the replay is looping. State validity + // is tracked separately on state.valid. + ds->connected = still_playing || ds->playback.looping; + } + + ds->state = r->state; + ds->home = r->home; + ds->playback.current_nav_state = r->current_nav_state; + + uint64_t range = wasm_replay_duration_us(r); + if (range > 0) { + ds->playback.duration_s = (float)((double)range / 1e6); + ds->playback.position_s = (float)(r->wall_accum - r->time_offset_s); + if (ds->playback.position_s < 0.0f) ds->playback.position_s = 0.0f; + if (ds->playback.position_s > ds->playback.duration_s) + ds->playback.position_s = ds->playback.duration_s; + ds->playback.progress = ds->playback.position_s / ds->playback.duration_s; + } +} + +static void wasm_ds_seek(data_source_t *ds, float target_s) { + wasm_replay_ctx_t *r = (wasm_replay_ctx_t *)ds->impl; + if (!r) return; + wasm_replay_seek(r, target_s); + ds->state = r->state; + ds->home = r->home; + ds->playback.current_nav_state = r->current_nav_state; + uint64_t range = wasm_replay_duration_us(r); + if (range > 0) { + ds->playback.position_s = (float)(r->wall_accum - r->time_offset_s); + if (ds->playback.position_s < 0.0f) ds->playback.position_s = 0.0f; + ds->playback.progress = ds->playback.position_s / ds->playback.duration_s; + } +} + +static void wasm_ds_set_time_offset(data_source_t *ds, double offset_s) { + wasm_replay_ctx_t *r = (wasm_replay_ctx_t *)ds->impl; + if (r) r->time_offset_s = offset_s; + ds->playback.time_offset_s = (float)offset_s; +} + +static void wasm_ds_close(data_source_t *ds) { (void)ds; } + +static const data_source_ops_t wasm_ds_ops = { + .poll = wasm_ds_poll, + .seek = wasm_ds_seek, + .set_time_offset = wasm_ds_set_time_offset, + .close = wasm_ds_close, +}; + +// --------------------------------------------------------------------------- +// Replay mode helpers. apply_replay_mode consolidates the per-mode vehicle +// reconfiguration that main.c spreads across the initial conflict block +// (339-409) and the P-key handler (682-792). +// +// rebase_to_home matters because native behaves differently between the two +// paths: +// • Initial (main.c:362-380): leaves origin_set=false and lets vehicle_update +// set origin from the first received state. This works because the drone +// hasn't been playing yet — first state == takeoff position. +// • P key (main.c:705-783): explicitly forces origin = home so the drone +// jumps back to home in the new coordinate system rather than continuing +// from its last position. +// Using rebase_to_home=true on the initial path shifts drones away from +// first-state-position by whatever delta exists between that point and +// home.lat/lon/alt — which in multi-country ULogs can be large. +// +// Modes: +// 1 = formation (shared origin, full alpha) +// 2 = ghost (own origin, non-primary alpha 0.35) +// 3 = grid (own origin + X offset per drone) +// 4 = cancel (P-key cancel: own origin, full alpha, no grid offset) +// --------------------------------------------------------------------------- + +static void apply_replay_mode(int mode, bool rebase_to_home) { + int count = g.vehicle_count; + if (count < 1) return; + + for (int i = 0; i < count; i++) { + g.vehicles[i].grid_offset = (Vector3){0, 0, 0}; + vehicle_set_ghost_alpha(&g.vehicles[i], 1.0f); + g.vehicles[i].origin_wait_count = 0; + vehicle_reset_trail(&g.vehicles[i]); + if (rebase_to_home) g.vehicles[i].origin_set = false; + } + + g.ghost_mode = (mode == 2); + g.ghost_mode_grid = (mode == 3); + + if (mode == 1 && count > 1) { + // Formation: shared NED origin. Native always sets this explicitly + // (main.c:372-378) — no first-state fallback — so we do the same. + for (int i = 0; i < count; i++) { + if (g.sources[i].home.valid) { + g.vehicles[i].lat0 = g.ref_lat_rad; + g.vehicles[i].lon0 = g.ref_lon_rad; + g.vehicles[i].alt0 = g.min_alt; + g.vehicles[i].origin_set = true; + } + } + } else if (mode == 2 || mode == 3 || mode == 4) { + if (rebase_to_home) { + for (int i = 0; i < count; i++) { + if (g.sources[i].home.valid) { + g.vehicles[i].lat0 = g.sources[i].home.lat * 1e-7 * (M_PI / 180.0); + g.vehicles[i].lon0 = g.sources[i].home.lon * 1e-7 * (M_PI / 180.0); + g.vehicles[i].alt0 = g.sources[i].home.alt * 1e-3; + g.vehicles[i].origin_set = true; + } + } + } + // else: vehicle_update will set origin from first state (native initial + // path behavior, main.c:362-380 — note the "Don't set origin_set" + // comment there). + + if (mode == 2 && count > 1) { + vehicle_set_ghost_alpha(&g.vehicles[0], 1.0f); + for (int i = 1; i < count; i++) + vehicle_set_ghost_alpha(&g.vehicles[i], 0.35f); + } + if (mode == 3) { + for (int i = 1; i < count; i++) + g.vehicles[i].grid_offset = (Vector3){ i * 5.0f, 0.0f, 0.0f }; + } + } + + memset(g.corr, 0, sizeof(g.corr)); + for (int i = 0; i < count; i++) { + g.sources[i].playback.correlation = NAN; + g.sources[i].playback.rmse = NAN; + } + for (int i = 0; i < count; i++) + g.sys_markers[i].resolved = false; +} + +// Reset to the pre-load single-placeholder state (used by initial conflict +// dialog's "Cancel & reupload" choice). +static void hawkeye_unload_internal(void) { + if (g.log_loaded) { + for (int i = 0; i < g.vehicle_count; i++) { + wasm_replay_close(&g.replays[i]); + vehicle_cleanup(&g.vehicles[i]); + precomp_trail_cleanup(&g.precomp[i]); + } + } + g.log_loaded = false; + g.vehicle_count = 1; + g.selected = 0; + g.prev_selected = 0; + + vehicle_init_ex(&g.vehicles[0], MODEL_QUADROTOR, g.scene.lighting_shader, 36000); + g.vehicles[0].color = g.scene.theme->drone_palette[0]; + precomp_trail_init(&g.precomp[0]); + + memset(&g.sources[0], 0, sizeof(g.sources[0])); + g.sources[0].ops = &wasm_ds_ops; + g.sources[0].playback.speed = 1.0f; + g.sources[0].playback.looping = true; + g.sources[0].playback.interpolation = true; + + memset(g.markers, 0, sizeof(g.markers)); + memset(g.sys_markers, 0, sizeof(g.sys_markers)); + g.markers[0].current = -1; + g.markers[0].last_drop_idx = -1; + g.sys_markers[0].current = -1; + + g.conflict_detected = false; + g.conflict_far = false; + g.has_tier3 = false; + g.ghost_mode = false; + g.ghost_mode_grid = false; + g.takeoff_aligned = false; +} + +// --------------------------------------------------------------------------- +// Frame callback. Line-ordered port of main.c:468-1428. +// Cross-references tagged "(main.c:NNN)" point at the native line being ported. +// --------------------------------------------------------------------------- + +static void frame(void *arg) { + (void)arg; + if (!g.initialized) return; + + float frame_dt = GetFrameTime(); + + if (g.vehicle_count <= 0) return; // main.c:471 + + // ── Poll all data sources + update vehicles (main.c:473-544) ──────────── + // Freeze the entire update pipeline while the modal dialog is open. + // Native achieves this naturally because draw_prompt_dialog blocks in its + // own render loop; we do it explicitly here. Without this, playback keeps + // advancing behind the dialog and the drones fly off to wherever they + // happened to be when the user finally clicked a mode, making the chosen + // grid/ghost offset look visually wrong. + if (g.log_loaded && !g.dialog.active) { + for (int i = 0; i < g.vehicle_count; i++) { + data_source_poll(&g.sources[i], frame_dt); // main.c:475 + + if (g.sources[i].playback.statustext) // main.c:478 + hud_feed_statustext(&g.hud, g.sources[i].playback.statustext, i); + + if (!g.sources[i].playback.paused) { // main.c:482 + float cur_pos = g.sources[i].playback.position_s; + float prev_pos = g.prev_playback_pos[i]; + for (int m = 0; m < g.markers[i].count; m++) { // main.c:486 + if (g.markers[i].times[m] > prev_pos && g.markers[i].times[m] <= cur_pos) { + annunc_trigger_tab_fade(&g.hud.annunciators, i); + annunc_trigger_radar_wave(&g.hud.annunciators, i); + if (i != g.selected) { + for (int p = 0; p < g.hud.pinned_count; p++) + if (g.hud.pinned[p] == i) + annunc_trigger_ring_bounce(&g.hud.annunciators, i); + } + break; + } + } + for (int m = 0; m < g.sys_markers[i].count; m++) { // main.c:499 + if (g.sys_markers[i].times[m] > prev_pos && g.sys_markers[i].times[m] <= cur_pos) { + annunc_trigger_tab_fade(&g.hud.annunciators, i); + annunc_trigger_radar_wave(&g.hud.annunciators, i); + if (i != g.selected) { + for (int p = 0; p < g.hud.pinned_count; p++) + if (g.hud.pinned[p] == i) + annunc_trigger_ring_bounce(&g.hud.annunciators, i); + } + break; + } + } + g.prev_playback_pos[i] = cur_pos; + } + + if (g.sources[i].connected && !g.was_connected[i]) { // main.c:515 + vehicle_reset_trail(&g.vehicles[i]); + if (g.sources[i].mav_type != 0) + vehicle_set_type(&g.vehicles[i], g.sources[i].mav_type); + } + g.was_connected[i] = g.sources[i].connected; + + g.vehicles[i].current_time = g.sources[i].playback.position_s; // main.c:526 + vehicle_update(&g.vehicles[i], &g.sources[i].state, &g.sources[i].home); + g.vehicles[i].sysid = g.sources[i].sysid; + + if (g.sources[i].connected && !g.vehicles[i].active) { // main.c:531 + g.vehicles[i].active = true; + g.vehicles[i].position = g.vehicles[i].grid_offset; + } + + if (g.vehicles[i].active && g.vehicles[i].trail_count > 0) { // main.c:537 + Vector3 delta = Vector3Subtract(g.vehicles[i].position, g.last_pos[i]); + if (Vector3Length(delta) > 50.0f) + vehicle_reset_trail(&g.vehicles[i]); + } + g.last_pos[i] = g.vehicles[i].position; + } + + // Lazy-resolve system markers (main.c:547-553). Precomp trail build walks + // the replay via poll() — save/restore the cursor state so the main + // playback cursor doesn't jump when precomp consumes the whole timeline. + for (int i = 0; i < g.vehicle_count; i++) { + if (!g.sys_markers[i].resolved && g.sys_markers[i].count > 0 + && g.vehicles[i].origin_set) { + wasm_replay_ctx_t saved = g.replays[i]; + replay_resolve_and_build_trail(&g.sys_markers[i], &g.precomp[i], + &g.sources[i], &g.vehicles[i]); + g.replays[i] = saved; + } + } + + // Insufficient data detection (main.c:556-569) + if (!g.insufficient_toasted) { + g.insufficient_check_frames++; + if (g.insufficient_check_frames > 120) { + for (int i = 0; i < g.vehicle_count; i++) { + if (g.sources[i].connected && !g.sources[i].state.valid) { + g.insufficient_data[i] = true; + char msg[64]; + snprintf(msg, sizeof(msg), "DRONE %d: INSUFFICIENT DATA", i + 1); + hud_toast_color(&g.hud, msg, 4.0f, g.vehicles[i].color); + } + } + g.insufficient_toasted = true; + } + } + + // Correlation accumulators (main.c:575-619) + if (g.vehicle_count > 1) { + if (g.selected != g.prev_selected) { + memset(g.corr, 0, sizeof(g.corr)); + for (int i = 0; i < g.vehicle_count; i++) { + g.sources[i].playback.correlation = NAN; + g.sources[i].playback.rmse = NAN; + } + g.prev_selected = g.selected; + } + + bool playing = g.sources[g.selected].connected && !g.sources[g.selected].playback.paused; + if (playing && g.vehicles[g.selected].origin_set) { + const vehicle_t *ref = &g.vehicles[g.selected]; + double rx[CORR_CHANNELS] = { ref->position.z, ref->position.x, ref->position.y }; + for (int i = 0; i < g.vehicle_count; i++) { + if (i == g.selected || !g.vehicles[i].origin_set) continue; + const vehicle_t *v = &g.vehicles[i]; + double vx[CORR_CHANNELS] = { v->position.z, v->position.x, v->position.y }; + for (int c = 0; c < CORR_CHANNELS; c++) { + g.corr[i].ch[c].sum_x += rx[c]; + g.corr[i].ch[c].sum_y += vx[c]; + g.corr[i].ch[c].sum_xy += rx[c] * vx[c]; + g.corr[i].ch[c].sum_x2 += rx[c] * rx[c]; + g.corr[i].ch[c].sum_y2 += vx[c] * vx[c]; + } + double dx = (v->position.x - v->grid_offset.x) - (ref->position.x - ref->grid_offset.x); + double dy = (v->position.y - v->grid_offset.y) - (ref->position.y - ref->grid_offset.y); + double dz = (v->position.z - v->grid_offset.z) - (ref->position.z - ref->grid_offset.z); + g.corr[i].sum_sq_dist += dx*dx + dy*dy + dz*dz; + g.corr[i].n++; + g.sources[i].playback.correlation = corr_compute(&g.corr[i]); + g.sources[i].playback.rmse = (g.corr[i].n >= CORR_MIN_SAMPLES) + ? (float)sqrt(g.corr[i].sum_sq_dist / g.corr[i].n) : NAN; + } + g.sources[g.selected].playback.correlation = 1.0f; + g.sources[g.selected].playback.rmse = 0.0f; + } + } + } + + bool any_connected = false; // main.c:622 + for (int i = 0; i < g.vehicle_count; i++) { + if (g.sources[i].connected) { any_connected = true; break; } + } + (void)any_connected; + + hud_update(&g.hud, g.sources[g.selected].state.time_usec, // main.c:628 + g.sources[g.selected].connected, frame_dt); + + // File-drop theme handling (main.c:632-671) — on WASM, JS calls + // hawkeye_add_theme_bytes() directly. See that function below. + + // ── Input handling (main.c:674-1171) ──────────────────────────────────── + // Dialog, when active, captures all keyboard input. Mirrors native's + // blocking draw_prompt_dialog semantics without blocking the frame loop. + if (!g.marker_input.active && !g.dialog.active) { + scene_handle_input(&g.scene); + + // P key mode switcher (main.c:682-792) + if (IsKeyPressed(KEY_P) && g.log_loaded && g.vehicle_count > 1) { + const char *p_grid_label = g.conflict_far ? "Narrow grid" : "Grid offset"; + const char *pl[3]; + char p_title[32]; + char p_subtitle[64]; + if (g.conflict_detected) { + snprintf(p_title, sizeof(p_title), "POSITION CONFLICT"); + pl[0] = "Cancel"; + pl[1] = "Ghost mode"; + pl[2] = p_grid_label; + if (g.conflict_far) + snprintf(p_subtitle, sizeof(p_subtitle), + " - %d drones too far apart", g.vehicle_count); + else + snprintf(p_subtitle, sizeof(p_subtitle), + " - %d drones overlap", g.vehicle_count); + } else { + snprintf(p_title, sizeof(p_title), "REPLAY MODE"); + pl[0] = "Formation"; + pl[1] = "Ghost"; + pl[2] = "Grid offset"; + snprintf(p_subtitle, sizeof(p_subtitle), + " - %d drones", g.vehicle_count); + } + wasm_dialog_begin(&g.dialog, p_title, p_subtitle, pl, 3); + g.dialog_context = 1; + } + + // Per-drone color refresh on theme change (main.c:796-798) + for (int i = 0; i < g.vehicle_count; i++) + g.vehicles[i].color = g.scene.theme->drone_palette[i]; + + // ? help overlay (main.c:801) + if (IsKeyPressed(KEY_SLASH) && + (IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT))) + g.hud.show_help = !g.hud.show_help; + + // H HUD mode cycle (main.c:806) + if (IsKeyPressed(KEY_H)) { + hud_mode_t prev_mode = g.hud.mode; + g.hud.mode = (g.hud.mode + 1) % HUD_MODE_COUNT; + const char *mode_names[] = { "Console HUD", "Tactical HUD", "HUD Off" }; + hud_toast(&g.hud, mode_names[g.hud.mode], 2.0f); + g.show_hud = (g.hud.mode != HUD_OFF); + if (g.hud.mode == HUD_TACTICAL) { + g.saved_chase_distance = g.scene.chase_distance; + g.scene.chase_distance = g.tactical_chase_target; + } else if (prev_mode == HUD_TACTICAL) { + g.scene.chase_distance = g.saved_chase_distance; + } + } + + // Shift+T correlation cycle (main.c:822) + if (IsKeyPressed(KEY_T) && + (IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT)) + && g.log_loaded && g.vehicle_count > 1) { + g.corr_mode = (g.corr_mode + 1) % 3; + const char *names[] = { "Correlation Off", "Correlation Line", "Correlation Curtain" }; + hud_toast(&g.hud, names[g.corr_mode], 2.0f); + } + // T trail cycle (main.c:829) + else if (IsKeyPressed(KEY_T)) { + int max_modes = (g.vehicle_count > 1) ? 4 : 3; + g.trail_mode = (g.trail_mode + 1) % max_modes; + const char *trail_names[] = { "Trails Off", "Direction Trails", "Speed Ribbons", "ID Trails" }; + hud_toast(&g.hud, trail_names[g.trail_mode], 2.0f); + } + + // K classic colors (main.c:837) + if (IsKeyPressed(KEY_K)) + g.classic_colors = !g.classic_colors; + + // G ground track (main.c:842) + if (IsKeyPressed(KEY_G)) + g.show_ground_track = !g.show_ground_track; + + // Ctrl+D debug panel (main.c:847) + if ((IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL)) && IsKeyPressed(KEY_D)) + g.dbg_panel.visible = !g.dbg_panel.visible; + + // Ctrl+L edge indicators + correlation labels (main.c:852) + if ((IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL)) && IsKeyPressed(KEY_L)) { + g.show_edge_indicators = !g.show_edge_indicators; + g.show_corr_labels = !g.show_corr_labels; + } + + // Z axis gizmo (main.c:858) + if (IsKeyPressed(KEY_Z) && !IsKeyDown(KEY_LEFT_SHIFT) && !IsKeyDown(KEY_RIGHT_SHIFT) + && !IsKeyDown(KEY_LEFT_CONTROL) && !IsKeyDown(KEY_RIGHT_CONTROL)) + g.show_axes = !g.show_axes; + + // O ortho panel (main.c:864) + if (IsKeyPressed(KEY_O)) + g.ortho.visible = !g.ortho.visible; + + // M / Shift+M model cycle (main.c:870) + if (IsKeyPressed(KEY_M)) { + if (IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT)) { + int next = (g.vehicles[g.selected].model_idx + 1) % vehicle_model_count; + vehicle_load_model(&g.vehicles[g.selected], next); + } else { + vehicle_cycle_model(&g.vehicles[g.selected]); + } + } + + // Vehicle selection (main.c:880-956) + if (g.vehicle_count > 1) { + if (IsKeyPressed(KEY_TAB)) { + for (int j = 1; j <= g.vehicle_count; j++) { + int next = (g.selected + j) % g.vehicle_count; + if (g.sources[next].connected) { g.selected = next; break; } + } + g.hud.pinned_count = 0; + memset(g.hud.pinned, -1, sizeof(g.hud.pinned)); + g.chord_first = -1; + } + // Number-key drone selection with two-digit chord for 10-16 + { + int digit = -1; + for (int k = KEY_ZERO; k <= KEY_NINE; k++) { + if (IsKeyPressed(k)) { digit = k - KEY_ZERO; break; } + } + + bool shift_held = IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT); + bool ctrl_held = IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL); + bool alt_held = IsKeyDown(KEY_LEFT_ALT) || IsKeyDown(KEY_RIGHT_ALT); + + if (g.chord_first >= 0 && GetTime() - g.chord_time > CHORD_TIMEOUT_S) { + int idx = g.chord_first - 1; + if (idx >= 0 && idx < g.vehicle_count) + apply_vehicle_selection_hud(&g.hud, idx, g.chord_shift, &g.selected, g.vehicle_count); + g.chord_first = -1; + } + + if (digit >= 0 && !ctrl_held && !alt_held) { + if (g.chord_first >= 0) { + int two_digit = g.chord_first * 10 + digit; + int idx = two_digit - 1; + g.chord_first = -1; + if (idx >= 0 && idx < g.vehicle_count) + apply_vehicle_selection_hud(&g.hud, idx, g.chord_shift, &g.selected, g.vehicle_count); + } else if (digit >= 1 && digit <= 9) { + if (g.vehicle_count > 9) { + g.chord_first = digit; + g.chord_time = GetTime(); + g.chord_shift = shift_held; + } else { + int idx = digit - 1; + if (idx < g.vehicle_count) + apply_vehicle_selection_hud(&g.hud, idx, shift_held, &g.selected, g.vehicle_count); + } + } + } + } + } + } // end !marker_input.active block + + // Marker label text input (main.c:960-963) + if (g.marker_input.active) { + int di = g.marker_input.drone_idx; + marker_input_update(&g.marker_input, &g.markers[di], &g.sources[di], &g.vehicles[di]); + } + + // Re-toast insufficient data on selection change (main.c:966-974) + if (g.log_loaded && g.selected != g.last_selected_for_insuf && g.insufficient_data[g.selected]) { + char msg[64]; + snprintf(msg, sizeof(msg), "DRONE %d: INSUFFICIENT DATA", g.selected + 1); + hud_toast_color(&g.hud, msg, 3.0f, g.vehicles[g.selected].color); + } + g.last_selected_for_insuf = g.selected; + + // ── Replay playback controls (main.c:977-1171) ────────────────────────── + if (g.log_loaded && !g.marker_input.active && !g.dialog.active) { + bool shift = IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT); + bool ctrl = IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL); + int nrf = g.vehicle_count; + + // Space play/pause + restart-from-end (main.c:981) + if (IsKeyPressed(KEY_SPACE)) { + if (!g.sources[g.selected].connected) { + for (int i = 0; i < nrf; i++) { + data_source_seek(&g.sources[i], 0.0f); + g.sources[i].connected = true; + g.sources[i].playback.paused = false; + vehicle_reset_trail(&g.vehicles[i]); + } + memset(g.corr, 0, sizeof(g.corr)); + } else { + bool p = !g.sources[g.selected].playback.paused; + for (int i = 0; i < nrf; i++) + g.sources[i].playback.paused = p; + } + } + + // Shift+L looping (main.c:996) + if (IsKeyPressed(KEY_L) && !ctrl && shift) { + bool l = !g.sources[g.selected].playback.looping; + for (int i = 0; i < nrf; i++) + g.sources[i].playback.looping = l; + } + + // Y yaw/heading (main.c:1001) + if (IsKeyPressed(KEY_Y)) + g.hud.show_yaw = !g.hud.show_yaw; + + // N notifications (main.c:1004) + if (IsKeyPressed(KEY_N)) { + g.hud.show_notifications = !g.hud.show_notifications; + hud_toast(&g.hud, g.hud.show_notifications ? "Notifications On" : "Notifications Off", 2.0f); + } + + // L marker labels toggle (main.c:1008) — guarded against B→L chord + if (IsKeyPressed(KEY_L) && !ctrl && !shift + && (g.markers[g.selected].last_drop_idx < 0 + || GetTime() - g.markers[g.selected].last_drop_time >= 0.5)) { + g.show_marker_labels = !g.show_marker_labels; + } + + // I interpolation (main.c:1011) + if (IsKeyPressed(KEY_I)) { + bool interp = !g.sources[g.selected].playback.interpolation; + for (int i = 0; i < nrf; i++) + g.sources[i].playback.interpolation = interp; + hud_toast(&g.hud, interp ? "Interpolation On" : "Interpolation Off", 2.0f); + } + + // +/= speed up (main.c:1017) + if (IsKeyPressed(KEY_EQUAL)) { + float spd = g.sources[g.selected].playback.speed; + if (spd < 0.5f) spd = 0.5f; + else if (spd < 1.0f) spd = 1.0f; + else if (spd < 2.0f) spd = 2.0f; + else if (spd < 4.0f) spd = 4.0f; + else if (spd < 8.0f) spd = 8.0f; + else spd = 16.0f; + for (int i = 0; i < nrf; i++) + g.sources[i].playback.speed = spd; + } + + // - speed down (main.c:1028) + if (IsKeyPressed(KEY_MINUS)) { + float spd = g.sources[g.selected].playback.speed; + if (spd > 8.0f) spd = 8.0f; + else if (spd > 4.0f) spd = 4.0f; + else if (spd > 2.0f) spd = 2.0f; + else if (spd > 1.0f) spd = 1.0f; + else if (spd > 0.5f) spd = 0.5f; + else spd = 0.25f; + for (int i = 0; i < nrf; i++) + g.sources[i].playback.speed = spd; + } + + // R reset all drones (main.c:1039) + if (IsKeyPressed(KEY_R)) { + for (int i = 0; i < nrf; i++) { + data_source_seek(&g.sources[i], 0.0f); + g.sources[i].connected = true; + g.sources[i].playback.paused = false; + vehicle_reset_trail(&g.vehicles[i]); + } + memset(g.corr, 0, sizeof(g.corr)); + for (int i = 0; i < g.vehicle_count; i++) { + g.markers[i].count = 0; + g.markers[i].current = -1; + } + } + + // A takeoff alignment (main.c:1053) + if (IsKeyPressed(KEY_A) && g.vehicle_count > 1) { + g.takeoff_aligned = !g.takeoff_aligned; + const float takeoff_buffer = 5.0f; + for (int i = 0; i < nrf; i++) { + if (g.takeoff_aligned) { + float skip = g.sources[i].playback.takeoff_detected + ? g.sources[i].playback.takeoff_time_s - takeoff_buffer : 0.0f; + if (skip < 0.0f) skip = 0.0f; + data_source_set_time_offset(&g.sources[i], (double)skip); + } else { + data_source_set_time_offset(&g.sources[i], 0.0); + } + data_source_seek(&g.sources[i], 0.0f); + g.sources[i].connected = true; + g.sources[i].playback.paused = false; + vehicle_reset_trail(&g.vehicles[i]); + } + memset(g.corr, 0, sizeof(g.corr)); + for (int i = 0; i < nrf; i++) { + g.sources[i].playback.correlation = NAN; + g.sources[i].playback.rmse = NAN; + } + for (int i = 0; i < nrf; i++) + g.sys_markers[i].resolved = false; + hud_toast(&g.hud, g.takeoff_aligned ? "Auto Align On" : "Auto Align Off", 2.0f); + } + + // Arrow keys seek all drones with 3 granularity levels (main.c:1083) + if (IsKeyPressed(KEY_RIGHT)) { + float step; + if (shift && ctrl) step = 1.0f; + else if (shift) { step = 0.02f; g.sources[g.selected].playback.paused = true; } + else step = 5.0f; + float seek_target = g.sources[g.selected].playback.position_s + step; + for (int i = 0; i < nrf; i++) { + data_source_seek(&g.sources[i], seek_target); + vehicle_reset_trail(&g.vehicles[i]); + } + memset(g.corr, 0, sizeof(g.corr)); + } + if (IsKeyPressed(KEY_LEFT)) { + float step; + if (shift && ctrl) step = 1.0f; + else if (shift) { step = 0.02f; g.sources[g.selected].playback.paused = true; } + else step = 5.0f; + float target = g.sources[g.selected].playback.position_s - step; + if (target < 0.0f) target = 0.0f; + for (int i = 0; i < nrf; i++) { + data_source_seek(&g.sources[i], target); + vehicle_reset_trail(&g.vehicles[i]); + } + memset(g.corr, 0, sizeof(g.corr)); + } + + // B drop / Shift+B delete (main.c:1110) + if (IsKeyPressed(KEY_B) && g.vehicles[g.selected].active && !g.marker_input.active) { + if (shift) { + marker_delete(&g.markers[g.selected]); + } else if (g.markers[g.selected].count < REPLAY_MAX_MARKERS) { + marker_drop(&g.markers[g.selected], g.sources[g.selected].playback.position_s, + g.vehicles[g.selected].position, &g.vehicles[g.selected], + &g.sys_markers[g.selected]); + } + } + + // B→L chord label input (main.c:1121) + if (IsKeyPressed(KEY_L) && !g.marker_input.active + && g.markers[g.selected].last_drop_idx >= 0) { + double elapsed = GetTime() - g.markers[g.selected].last_drop_time; + if (elapsed < 0.5) { + marker_input_begin(&g.marker_input, g.markers[g.selected].last_drop_idx, + &g.sources[g.selected]); + g.marker_input.drone_idx = g.selected; + g.markers[g.selected].last_drop_idx = -1; + for (int i = 0; i < g.vehicle_count; i++) + g.sources[i].playback.paused = true; + } + } + + // [/] marker cycling with Ctrl+[/] global (main.c:1136) + if (IsKeyPressed(KEY_LEFT_BRACKET) || IsKeyPressed(KEY_RIGHT_BRACKET)) { + int dir = IsKeyPressed(KEY_LEFT_BRACKET) ? -1 : 1; + if (ctrl) { + marker_cycle_result_t r = marker_cycle_global( + g.markers, g.sys_markers, g.vehicle_count, g.selected, dir, + g.sources, g.vehicles, g.precomp, &g.scene, g.last_pos); + if (r.jumped && r.drone_idx != g.selected) + g.selected = r.drone_idx; + } else if (g.markers[g.selected].count > 0 || g.sys_markers[g.selected].count > 0) { + marker_cycle(&g.markers[g.selected], &g.sys_markers[g.selected], dir, shift, + &g.sources[g.selected], &g.vehicles[g.selected], + &g.precomp[g.selected], &g.scene, &g.last_pos[g.selected]); + } + if (!shift) { + annunc_trigger_tab_fade(&g.hud.annunciators, g.selected); + annunc_trigger_radar_wave(&g.hud.annunciators, g.selected); + for (int p = 0; p < g.hud.pinned_count; p++) { + int pidx = g.hud.pinned[p]; + if (pidx >= 0 && pidx < g.vehicle_count) + annunc_trigger_ring_bounce(&g.hud.annunciators, pidx); + } + } + // Sync all drones to the same playback time after marker seek (main.c:1162) + if (!shift && g.vehicle_count > 1) { + float sync_time = g.sources[g.selected].playback.position_s; + for (int i = 0; i < g.vehicle_count; i++) { + if (i == g.selected) continue; + data_source_seek(&g.sources[i], sync_time); + vehicle_reset_trail(&g.vehicles[i]); + } + } + } + } + + // Debug panel update (main.c:1174) + debug_panel_update(&g.dbg_panel, frame_dt); + + // Camera update (main.c:1177) + { + Vector3 cam_target = g.vehicles[g.selected].position; + if (g.hud.mode == HUD_TACTICAL) + cam_target.y += g.scene.chase_distance * 0.10f; + scene_update_camera(&g.scene, cam_target, g.vehicles[g.selected].rotation); + } + + // Ortho panel render (main.c:1186) + if (g.ortho.visible || g.hud.mode == HUD_TACTICAL) { + ortho_panel_update(&g.ortho, g.vehicles[g.selected].position); + ortho_panel_render(&g.ortho, g.vehicles, g.vehicle_count, + g.selected, g.scene.theme, + g.corr_mode, g.hud.pinned, g.hud.pinned_count); + } + + int sw = GetScreenWidth(); + int sh = GetScreenHeight(); + + // ── Render (main.c:1194-1428) ────────────────────────────────────────── + BeginDrawing(); + + scene_draw_sky(&g.scene); + + bool fs_ortho = (g.scene.ortho_mode != ORTHO_NONE); + int tm_3d = fs_ortho ? 0 : g.trail_mode; + + BeginMode3D(g.scene.camera); + scene_draw(&g.scene); + for (int i = 0; i < g.vehicle_count; i++) { + if (g.vehicles[i].active || g.vehicle_count == 1) { + vehicle_draw(&g.vehicles[i], g.scene.theme, i == g.selected, + tm_3d, g.show_ground_track, g.scene.camera.position, + g.classic_colors); + } + } + + // 3D markers for all drones (main.c:1213) + for (int i = 0; i < g.vehicle_count; i++) { + if (g.log_loaded && g.markers[i].count > 0) { + int cur = (i == g.selected && !g.sys_markers[i].selected) + ? g.markers[i].current : -1; + vehicle_draw_markers(g.markers[i].positions, g.markers[i].labels, + g.markers[i].count, cur, + g.scene.camera.position, g.scene.camera, + g.markers[i].roll, g.markers[i].pitch, + g.markers[i].vert, g.markers[i].speed, + g.vehicles[i].trail_speed_max, g.scene.theme, + g.trail_mode, MARKER_USER, + g.vehicles[i].color); + } + if (g.log_loaded && g.sys_markers[i].count > 0) { + int cur = (i == g.selected && g.sys_markers[i].selected) + ? g.sys_markers[i].current : -1; + vehicle_draw_markers(g.sys_markers[i].positions, g.sys_markers[i].labels, + g.sys_markers[i].count, cur, + g.scene.camera.position, g.scene.camera, + g.sys_markers[i].roll, g.sys_markers[i].pitch, + g.sys_markers[i].vert, g.sys_markers[i].speed, + g.vehicles[i].trail_speed_max, g.scene.theme, + g.trail_mode, MARKER_SYSTEM, + g.vehicles[i].color); + } + } + + // Home position markers in formation mode (main.c:1241) + if (g.vehicle_count > 1 && !g.ghost_mode && !g.ghost_mode_grid) { + for (int i = 0; i < g.vehicle_count; i++) { + if (!g.sources[i].home.valid) continue; + double lat = g.sources[i].home.lat * 1e-7 * (M_PI / 180.0); + double lon = g.sources[i].home.lon * 1e-7 * (M_PI / 180.0); + double alt = g.sources[i].home.alt * 1e-3; + float hx = (float)(EARTH_RADIUS * (lon - g.ref_lon_rad) * cos(g.ref_lat_rad)) + g.vehicles[i].grid_offset.x; + float hy = (float)(alt - g.min_alt) + 0.02f; + float hz = (float)(-(EARTH_RADIUS * (lat - g.ref_lat_rad))) + g.vehicles[i].grid_offset.z; + float half = 0.333f; + Color fill = g.vehicles[i].color; fill.a = 70; + Color border = g.vehicles[i].color; border.a = 180; + DrawPlane((Vector3){hx, hy, hz}, (Vector2){half * 2, half * 2}, fill); + DrawLine3D((Vector3){hx - half, hy, hz - half}, (Vector3){hx + half, hy, hz - half}, border); + DrawLine3D((Vector3){hx + half, hy, hz - half}, (Vector3){hx + half, hy, hz + half}, border); + DrawLine3D((Vector3){hx + half, hy, hz + half}, (Vector3){hx - half, hy, hz + half}, border); + DrawLine3D((Vector3){hx - half, hy, hz + half}, (Vector3){hx - half, hy, hz - half}, border); + } + } + + // Axis gizmo (main.c:1264) + if (g.show_axes && g.vehicles[g.selected].active) { + Vector3 com = g.vehicles[g.selected].position; + com.y += g.vehicles[g.selected].model_scale * 0.15f; + draw_axis_gizmo_3d(com, g.vehicles[g.selected].model_scale * 0.5f, + g.vehicles[g.selected].rotation); + } + + // Correlation overlay (main.c:1274) + if (g.corr_mode > 0 && g.hud.pinned_count > 0) { + for (int p = 0; p < g.hud.pinned_count; p++) { + int pidx = g.hud.pinned[p]; + if (pidx >= 0 && pidx < g.vehicle_count && g.vehicles[pidx].active + && pidx != g.selected) { + if (g.corr_mode == 1 && !fs_ortho) { + vehicle_draw_correlation_line( + &g.vehicles[g.selected], &g.vehicles[pidx]); + } else if (g.corr_mode == 2) { + vehicle_draw_correlation_curtain( + &g.vehicles[g.selected], &g.vehicles[pidx], + g.scene.theme, g.scene.camera.position); + } + } + } + } + EndMode3D(); + + scene_draw_ortho_ground(&g.scene, sw, sh); + + // Edge indicators (main.c:1296) + if (g.vehicle_count > 1 && g.show_edge_indicators) { + float ei_scale = powf(sh / 720.0f, 0.7f); + if (ei_scale < 1.0f) ei_scale = 1.0f; + draw_edge_indicators(g.vehicles, g.vehicle_count, g.selected, + g.scene.camera, g.hud.font_value, ei_scale); + } + + // 2D marker labels (main.c:1304) + if (g.log_loaded && g.show_marker_labels) { + for (int i = 0; i < g.vehicle_count; i++) { + if (g.markers[i].count > 0) { + int cur = (i == g.selected && !g.sys_markers[i].selected) + ? g.markers[i].current : -1; + vehicle_draw_marker_labels(g.markers[i].positions, g.markers[i].labels, + g.markers[i].count, cur, + g.scene.camera.position, g.scene.camera, + g.hud.font_label, g.hud.font_value, + g.markers[i].roll, g.markers[i].pitch, + g.markers[i].vert, g.markers[i].speed, + g.vehicles[i].trail_speed_max, g.scene.theme, + g.trail_mode, MARKER_USER, + g.vehicles[i].color); + } + if (g.sys_markers[i].count > 0) { + int cur = (i == g.selected && g.sys_markers[i].selected) + ? g.sys_markers[i].current : -1; + vehicle_draw_marker_labels(g.sys_markers[i].positions, g.sys_markers[i].labels, + g.sys_markers[i].count, cur, + g.scene.camera.position, g.scene.camera, + g.hud.font_label, g.hud.font_value, + g.sys_markers[i].roll, g.sys_markers[i].pitch, + g.sys_markers[i].vert, g.sys_markers[i].speed, + g.vehicles[i].trail_speed_max, g.scene.theme, + g.trail_mode, MARKER_SYSTEM, + g.vehicles[i].color); + } + } + } + + // Fullscreen ortho 2D overlays (main.c:1336) + ortho_draw_fullscreen_2d(&g.scene, g.vehicles, g.vehicle_count, + g.selected, g.trail_mode, + g.corr_mode, g.hud.pinned, g.hud.pinned_count, + sw, sh, g.hud.font_label, g.show_corr_labels); + + // HUD (main.c:1343) + if (g.show_hud) { + bool has_awaiting_gps = g.vehicles[g.selected].active && + !g.vehicles[g.selected].origin_set && g.sources[g.selected].home.valid; + hud_marker_data_t all_user_md[MAX_VEHICLES] = {0}; + hud_marker_data_t all_sys_md[MAX_VEHICLES] = {0}; + for (int i = 0; i < g.vehicle_count; i++) { + all_user_md[i] = (hud_marker_data_t){ + .times = g.markers[i].times, + .labels = g.markers[i].labels, + .roll = g.markers[i].roll, + .pitch = g.markers[i].pitch, + .vert = g.markers[i].vert, + .speed = g.markers[i].speed, + .speed_max = g.markers[i].speed_max, + .count = g.markers[i].count, + .current = g.markers[i].current, + .selected = true, + .color = g.vehicles[i].color, + }; + all_sys_md[i] = (hud_marker_data_t){ + .times = g.sys_markers[i].times, + .labels = g.sys_markers[i].labels, + .roll = g.sys_markers[i].roll, + .pitch = g.sys_markers[i].pitch, + .vert = g.sys_markers[i].vert, + .speed = g.sys_markers[i].speed, + .speed_max = g.markers[i].speed_max, + .count = g.sys_markers[i].count, + .current = g.sys_markers[i].current, + .selected = g.sys_markers[i].selected, + .color = g.vehicles[i].color, + }; + } + if (g.hud.mode == HUD_CONSOLE) { + hud_draw(&g.hud, g.vehicles, g.sources, g.vehicle_count, + g.selected, sw, sh, g.scene.theme, g.trail_mode, + all_user_md, all_sys_md, g.vehicle_count, + g.ghost_mode, g.has_tier3, has_awaiting_gps); + } else if (g.hud.mode == HUD_TACTICAL) { + tactical_hud_draw(&g.hud, g.vehicles, g.sources, g.vehicle_count, + g.selected, sw, sh, g.scene.theme, g.ghost_mode, + g.has_tier3, has_awaiting_gps, + &g.ortho, g.trail_mode, g.corr_mode, + all_user_md, all_sys_md, g.vehicle_count); + } + } + + // Debug panel (main.c:1392) + { + int active_count = 0; + int total_trail = 0; + for (int i = 0; i < g.vehicle_count; i++) { + if (g.vehicles[i].active) active_count++; + total_trail += g.vehicles[i].trail_count; + } + debug_panel_draw(&g.dbg_panel, sw, sh, g.scene.theme, g.hud.font_label, + g.vehicle_count, active_count, total_trail, + g.vehicles[g.selected].position, + g.sources[g.selected].ref_rejected, + g.vehicle_tier[g.selected]); + } + + // Ortho panel (main.c:1409) + if (g.hud.mode != HUD_TACTICAL) { + int bar_h = g.show_hud ? hud_bar_height(&g.hud, sh) : 0; + ortho_panel_draw(&g.ortho, sh, bar_h, g.scene.theme, g.hud.font_label, + g.vehicles, g.vehicle_count, g.selected, g.trail_mode, + g.corr_mode, g.hud.pinned, g.hud.pinned_count, + g.show_axes); + } + + // Fullscreen ortho label (main.c:1418) + ortho_panel_draw_fullscreen_label(sw, sh, g.scene.ortho_mode, g.scene.ortho_span, + g.scene.theme, g.hud.font_label, g.show_axes); + + // Marker label input overlay (main.c:1423) + if (g.marker_input.active) { + marker_input_draw(&g.marker_input, g.hud.font_label, g.hud.font_value, + g.scene.theme, sw, sh); + } + + // Non-blocking prompt dialog. Renders on top of everything else and + // captures 1..N keys. Replaces native's blocking draw_prompt_dialog. + if (g.dialog.active) { + int choice = wasm_dialog_draw_and_poll(&g.dialog, g.scene.theme, + g.hud.font_label, g.hud.font_value); + if (choice > 0) { + if (g.dialog_context == 0) { + // Initial conflict dialog. Playback was frozen while the + // dialog was up, so first-state still establishes origin. + // 1 = Cancel & reupload (unload) + // 2 = Ghost mode + // 3 = Grid / Narrow grid + if (choice == 1) { + hawkeye_unload_internal(); + } else if (choice == 2) { + apply_replay_mode(2, false); + } else if (choice == 3) { + apply_replay_mode(3, false); + } + } else { + // P-key dialog. User is switching mid-flight — rebase + // origin to home so the chosen coordinate frame applies + // immediately (native main.c:705-783). + if (g.conflict_detected) { + if (choice == 1) apply_replay_mode(4, true); + else if (choice == 2) apply_replay_mode(2, true); + else if (choice == 3) apply_replay_mode(3, true); + } else { + apply_replay_mode(choice, true); + } + } + } + } + + EndDrawing(); +} + +// --------------------------------------------------------------------------- +// Exported API +// --------------------------------------------------------------------------- + +EMSCRIPTEN_KEEPALIVE +int hawkeye_init(const char *canvas_id, int width, int height) { + if (g.initialized) return -1; + if (!canvas_id || width <= 0 || height <= 0) return -1; + + log_heap("init:entry"); + + memset(&g, 0, sizeof(g)); + g.width = width; + g.height = height; + strncpy(g.canvas_id, canvas_id, sizeof(g.canvas_id) - 1); + + SetLoadFileTextCallback(hawkeye_load_file_text); + asset_path_init(); + log_heap("init:after_asset_path"); + + // DPI strategy (see rcore_web.c analysis in WASM_PORT_FAILURES.md): + // - Raylib's web backend has no DPI awareness (GetWindowScaleDPI() + // stub returns {1,1}; FLAG_WINDOW_HIGHDPI is ignored). + // - glfwCreateWindow(buf_w, buf_h) routes through emscripten's + // Browser.setCanvasSize, which sets both the GL buffer size AND + // canvas.style.* to the same DPR-scaled pixel count. On a Retina + // display that stretches the CSS size to 2x intended, making the + // canvas overflow the page. + // - We compensate by calling emscripten_set_element_css_size right + // after InitWindow to force the CSS size back to logical pixels. + // Browser then downscales the oversized GL buffer into the logical + // CSS box for crisp Retina rendering. + double dpr = emscripten_get_device_pixel_ratio(); + int buf_w = (int)(width * dpr); + int buf_h = (int)(height * dpr); + + SetConfigFlags(FLAG_MSAA_4X_HINT); + InitWindow(buf_w, buf_h, "Hawkeye"); + if (!IsWindowReady()) return -1; + SetTargetFPS(60); + log_heap("init:after_InitWindow"); + + emscripten_set_element_css_size(canvas_id, (double)width, (double)height); + + scene_init(&g.scene); + log_heap("init:after_scene_init"); + theme_registry_init(&g.scene.theme_reg); + log_heap("init:after_theme_reg"); + g.scene.theme_index = 0; + g.scene.theme = g.scene.theme_reg.themes[0]; + + // Placeholder vehicle so the frame loop has something to render until the + // first ULog loads. Replaced during finalize_multi_load. + vehicle_init_ex(&g.vehicles[0], MODEL_QUADROTOR, g.scene.lighting_shader, 36000); + log_heap("init:after_vehicle_init"); + g.vehicles[0].color = g.scene.theme->drone_palette[0]; + g.vehicle_count = 1; + + hud_init(&g.hud); + log_heap("init:after_hud_init"); + g.hud.is_replay = true; + g.show_hud = true; + debug_panel_init(&g.dbg_panel); + ortho_panel_init(&g.ortho); + log_heap("init:after_panels"); + + // Match main.c:421-465 defaults + g.selected = 0; + g.prev_selected = 0; + g.chord_first = -1; + g.trail_mode = 1; + g.show_ground_track = false; + g.classic_colors = false; + g.show_edge_indicators = true; + g.corr_mode = 0; + g.show_corr_labels = true; + g.show_axes = false; + g.show_marker_labels = true; + g.tactical_chase_target = 1.6f; + g.last_selected_for_insuf = -1; + + g.markers[0].current = -1; + g.markers[0].last_drop_idx = -1; + g.sys_markers[0].current = -1; + precomp_trail_init(&g.precomp[0]); + g.marker_input.target = -1; + + // Data-source shim starts disconnected with ops bound + memset(&g.sources[0], 0, sizeof(g.sources[0])); + g.sources[0].ops = &wasm_ds_ops; + g.sources[0].playback.speed = 1.0f; + g.sources[0].playback.looping = true; + g.sources[0].playback.interpolation = true; + + g.initialized = true; + emscripten_set_main_loop_arg(frame, &g, 0, 0); + printf("hawkeye_init: %dx%d canvas=%s theme=%s\n", + width, height, canvas_id, g.scene.theme ? g.scene.theme->name : "(none)"); + log_heap("init:done"); + return 0; +} + +// Forward declarations for the staged multi-load API +EMSCRIPTEN_KEEPALIVE int hawkeye_begin_multi_load(int count); +EMSCRIPTEN_KEEPALIVE int hawkeye_stage_ulog(int index, const uint8_t *buf, size_t len); +EMSCRIPTEN_KEEPALIVE int hawkeye_finalize_multi_load(int mode); + +EMSCRIPTEN_KEEPALIVE +int hawkeye_load_ulog_bytes(const uint8_t *buf, size_t len) { + if (hawkeye_begin_multi_load(1) != 0) return -1; + if (hawkeye_stage_ulog(0, buf, len) != 0) return -1; + return hawkeye_finalize_multi_load(1); +} + +EMSCRIPTEN_KEEPALIVE +int hawkeye_begin_multi_load(int count) { + if (!g.initialized || count < 1 || count > MAX_VEHICLES) return -1; + log_heap("begin_multi_load:entry"); + + // Pre-grow the WASM heap in one shot to fit `count` drones plus the + // transient raw-ULog staging buffer. Measured on 16 × 50 MB ULogs: + // - baseline after init: ~3 MB + // - raw-file staging peak: ~55 MB (one file at a time) + // - per-drone timeline + state: ~3 MB + // One emscripten_resize_heap call avoids the cascading grow-and-copy + // transients that hit N× peak during load. + size_t target = (size_t)(8 + 60 + count * 4) * 1024 * 1024; + emscripten_resize_heap(target); + + if (g.log_loaded) { + for (int i = 0; i < g.vehicle_count; i++) { + wasm_replay_close(&g.replays[i]); + vehicle_cleanup(&g.vehicles[i]); + precomp_trail_cleanup(&g.precomp[i]); + } + g.log_loaded = false; + } + g.vehicle_count = count; + g.selected = 0; + g.prev_selected = 0; + printf("hawkeye_begin_multi_load: preparing for %d drones\n", count); + return 0; +} + +EMSCRIPTEN_KEEPALIVE +int hawkeye_stage_ulog(int index, const uint8_t *buf, size_t len) { + if (!g.initialized || index < 0 || index >= g.vehicle_count || !buf || len == 0) + return -1; + + log_heap("stage_ulog:entry"); + if (wasm_replay_init_from_bytes(&g.replays[index], buf, len) != 0) { + fprintf(stderr, "hawkeye_stage_ulog: extraction failed for index %d\n", index); + return -1; + } + const ulog_timeline_t *tl = (const ulog_timeline_t *)g.replays[index].timeline; + printf("hawkeye_stage_ulog[%d]: %zu bytes, %d att, %d lpos\n", + index, len, tl->att_count, tl->lpos_count); + log_heap("stage_ulog:after_extract"); + return 0; +} + +// Finalize the multi-load. `mode` picks the layout: +// 0 = auto (formation if no conflict, defaults to formation — conflict +// dialog is Phase 2; callers wanting non-default behavior +// should pass 1/2/3 explicitly) +// 1 = formation +// 2 = ghost +// 3 = grid +// Mirrors main.c:294-409. +EMSCRIPTEN_KEEPALIVE +int hawkeye_finalize_multi_load(int mode) { + if (!g.initialized) return -1; + int count = g.vehicle_count; + + g.log_loaded = true; + + // Vehicles (main.c:251-254) + for (int i = 0; i < count; i++) { + vehicle_init_ex(&g.vehicles[i], MODEL_QUADROTOR, g.scene.lighting_shader, 36000); + g.vehicles[i].color = g.scene.theme->drone_palette[i % 16]; + vehicle_set_type(&g.vehicles[i], g.replays[i].vehicle_type); + g.vehicles[i].origin_set = false; + g.vehicles[i].origin_wait_count = 0; + } + + // Initialize data_source shims (one per replay ctx) + for (int i = 0; i < count; i++) { + memset(&g.sources[i], 0, sizeof(g.sources[i])); + ds_init_static(&g.sources[i], &g.replays[i]); + } + + // Multi-file alignment seed (main.c:286-292) + if (count > 1) { + for (int i = 0; i < count; i++) + g.sources[i].playback.time_offset_s = 0.0f; + } + + // Conflict detection (main.c:294-337) + g.conflict_detected = false; + g.conflict_far = false; + if (count > 1) { + conflict_result_t cr = replay_detect_conflict(g.sources, count); + g.conflict_detected = cr.conflict_detected; + g.conflict_far = cr.conflict_far; + } + + // Shared NED reference (main.c:339-385). Computed up-front so formation + // mode can snap to it without further arithmetic. + g.ref_lat_rad = 0.0; + g.ref_lon_rad = 0.0; + g.min_alt = 0.0; + if (count > 1) { + for (int i = 0; i < count; i++) { + if (g.sources[i].home.valid) { + g.ref_lat_rad = g.sources[i].home.lat / 1e7 * (M_PI / 180.0); + g.ref_lon_rad = g.sources[i].home.lon / 1e7 * (M_PI / 180.0); + break; + } + } + g.min_alt = 1e9; + for (int i = 0; i < count; i++) { + if (g.sources[i].home.valid) { + double a = g.sources[i].home.alt * 1e-3; + if (a < g.min_alt) g.min_alt = a; + } + } + if (g.min_alt > 1e8) g.min_alt = 0.0; + printf("Multi-drone origin: lat=%.6f lon=%.6f alt=%.1f\n", + g.ref_lat_rad * (180.0 / M_PI), g.ref_lon_rad * (180.0 / M_PI), g.min_alt); + } + + // Position tiers (main.c:388-395) + memset(g.vehicle_tier, 0, sizeof(g.vehicle_tier)); + for (int i = 0; i < count; i++) { + if (g.sources[i].playback.home_from_topic) g.vehicle_tier[i] = 1; + else if (g.sources[i].home.valid) g.vehicle_tier[i] = 2; + else g.vehicle_tier[i] = 3; + } + g.has_tier3 = false; + for (int i = 0; i < count; i++) { + if (!g.sources[i].home.valid) { g.has_tier3 = true; break; } + } + + // Markers (main.c:449-465). Must be initialized before the dialog opens so + // precomp structures exist even while the user is choosing a mode. + for (int i = 0; i < count; i++) { + memset(&g.markers[i], 0, sizeof(g.markers[i])); + g.markers[i].current = -1; + g.markers[i].last_drop_idx = -1; + memset(&g.sys_markers[i], 0, sizeof(g.sys_markers[i])); + g.sys_markers[i].current = -1; + precomp_trail_init(&g.precomp[i]); + replay_init_sys_markers(&g.sys_markers[i], &g.sources[i]); + } + + g.trail_mode = (count > 1) ? 3 : 1; + + memset(g.was_connected, 0, sizeof(g.was_connected)); + memset(g.prev_playback_pos, 0, sizeof(g.prev_playback_pos)); + memset(g.last_pos, 0, sizeof(g.last_pos)); + memset(g.insufficient_data, 0, sizeof(g.insufficient_data)); + g.insufficient_check_frames = 0; + g.insufficient_toasted = false; + memset(g.corr, 0, sizeof(g.corr)); + g.takeoff_aligned = false; + + // Resolve initial mode. mode 0 = auto, where the default depends on + // conflict state: no conflict → formation; near conflict → ghost; far + // conflict → grid. These defaults match what the dialog will propose, + // so the view never renders drones "in the wrong country" even for the + // one frame before the user picks. + int initial_mode = mode; + if (initial_mode == 0) { + if (count < 2 || !g.conflict_detected) initial_mode = 1; + else if (g.conflict_far) initial_mode = 3; + else initial_mode = 2; + } + apply_replay_mode(initial_mode, false); + + // Open the conflict dialog so the user can override the auto pick. + // Matches main.c:304-336 — only fires in replay multi-file mode with a + // detected conflict, and only when the caller asked for auto (mode == 0). + if (count > 1 && g.conflict_detected && mode == 0) { + const char *grid_label = g.conflict_far ? "Narrow grid offset" : "Grid offset"; + char subtitle[64]; + if (g.conflict_far) + snprintf(subtitle, sizeof(subtitle), + " - %d drones too far apart", count); + else + snprintf(subtitle, sizeof(subtitle), + " - %d drones overlap", count); + const char *labels[3] = { "Cancel & reupload", "Ghost mode", grid_label }; + wasm_dialog_begin(&g.dialog, "POSITION CONFLICT", subtitle, labels, 3); + g.dialog_context = 0; + } + + printf("hawkeye_finalize_multi_load: %d drones, mode=%d (requested=%d), conflict=%d/far=%d\n", + count, initial_mode, mode, g.conflict_detected, g.conflict_far); + log_heap("finalize:done"); + return 0; +} + +// Query conflict result so the host page can decide mode before finalizing. +// Bits: bit0 = conflict_detected, bit1 = conflict_far. Returns 0 if no log or +// single drone. Intended for Phase 2 dialog wiring. +EMSCRIPTEN_KEEPALIVE +int hawkeye_get_conflict_flags(void) { + if (!g.initialized || g.vehicle_count < 2) return 0; + conflict_result_t cr = replay_detect_conflict(g.sources, g.vehicle_count); + int flags = 0; + if (cr.conflict_detected) flags |= 1; + if (cr.conflict_far) flags |= 2; + return flags; +} + +// Accept a .mvt theme file as raw bytes (JS drag-drop replacement for native's +// IsFileDropped() block at main.c:632-671). Writes the bytes into Emscripten's +// MEMFS at /tmp/ so theme_load_mvt's fopen works unchanged, then calls +// theme_registry_add on that path. Returns 0 on success, -1 on failure. +EMSCRIPTEN_KEEPALIVE +int hawkeye_add_theme_bytes(const uint8_t *buf, size_t len, const char *name) { + if (!g.initialized || !buf || len == 0 || !name) return -1; + char path[256]; + snprintf(path, sizeof(path), "/tmp/%s", name); + FILE *f = fopen(path, "wb"); + if (!f) { fprintf(stderr, "hawkeye_add_theme_bytes: fopen(%s) failed\n", path); return -1; } + size_t w = fwrite(buf, 1, len, f); + fclose(f); + if (w != len) return -1; + if (!theme_registry_add(&g.scene.theme_reg, path)) return -1; + int last = g.scene.theme_reg.user_count - 1; + if (last >= 0) { + char msg[80]; + snprintf(msg, sizeof(msg), "Theme: %s", g.scene.theme_reg.name_bufs[last]); + hud_toast(&g.hud, msg, 3.0f); + } + return 0; +} + +EMSCRIPTEN_KEEPALIVE +void hawkeye_resize(int width, int height) { + if (!g.initialized || width <= 0 || height <= 0) return; + g.width = width; + g.height = height; + double dpr = emscripten_get_device_pixel_ratio(); + SetWindowSize((int)(width * dpr), (int)(height * dpr)); + // SetWindowSize re-stretches canvas.style via emscripten's Browser + // helper; undo that with the same CSS-size override we use at init. + emscripten_set_element_css_size(g.canvas_id, (double)width, (double)height); +} + +EMSCRIPTEN_KEEPALIVE +void hawkeye_destroy(void) { + if (!g.initialized) return; + emscripten_cancel_main_loop(); + for (int i = 0; i < g.vehicle_count; i++) { + if (g.log_loaded) wasm_replay_close(&g.replays[i]); + vehicle_cleanup(&g.vehicles[i]); + precomp_trail_cleanup(&g.precomp[i]); + } + ortho_panel_cleanup(&g.ortho); + hud_cleanup(&g.hud); + scene_cleanup(&g.scene); + CloseWindow(); + memset(&g, 0, sizeof(g)); +} + +EMSCRIPTEN_KEEPALIVE +void hawkeye_set_playing(int playing) { + if (!g.initialized || !g.log_loaded) return; + for (int i = 0; i < g.vehicle_count; i++) + g.sources[i].playback.paused = (playing == 0); +} + +EMSCRIPTEN_KEEPALIVE +void hawkeye_seek(double seconds) { + if (!g.initialized || !g.log_loaded) return; + if (seconds < 0.0) seconds = 0.0; + for (int i = 0; i < g.vehicle_count; i++) { + data_source_seek(&g.sources[i], (float)seconds); + vehicle_reset_trail(&g.vehicles[i]); + } + memset(g.corr, 0, sizeof(g.corr)); +} + +EMSCRIPTEN_KEEPALIVE +double hawkeye_get_duration(void) { + if (!g.log_loaded) return 0.0; + uint64_t range = wasm_replay_duration_us(&g.replays[0]); + return range == 0 ? 0.0 : (double)range / 1e6; +} + +EMSCRIPTEN_KEEPALIVE +double hawkeye_get_position(void) { + if (!g.log_loaded) return 0.0; + return (double)g.sources[g.selected].playback.position_s; +}