diff --git a/README.md b/README.md index 4acc4d5..9575eed 100644 --- a/README.md +++ b/README.md @@ -183,6 +183,33 @@ docker run --rm -p 8642:8642 -p 9002-9202:9002-9202/udp \ -v $(pwd)/build/city/tiles:/world:ro -e SKYSIM_TILES=/world skysim ``` +`docker-entrypoint.sh` turns `SKYSIM_*` variables into flags: `SKYSIM_API_BIND`, +`SKYSIM_API_PORT`, `SKYSIM_VEHICLES`, `SKYSIM_TILES`, `SKYSIM_STREAM_RADIUS`, +`SKYSIM_STREAM_MAX`, `SKYSIM_DT`, `SKYSIM_TIME_MODE`, `SKYSIM_SPAWN_HOME`, and +`SKYSIM_EXTRA_ARGS` for anything not listed. + +**Set `SKYSIM_TIME_MODE=interactive` on a long-lived server.** The default is `strict`, +which is what determinism and CI replays need — a barrier every tick, and an abort when +a vehicle misses it. A server that vehicles join and leave wants the abort replaced by +eviction, or one lagging aircraft takes the fleet down with it. Both modes barrier on +every connected vehicle's frame; what interactive adds is wall-clock pacing and a way +out, freezing and then despawning a vehicle that has gone quiet for `--straggler-timeout` +(default 2 s). Match `SKYSIM_DT` to the scheduler rate the vehicles are launched with, +too: in lockstep, physics faster than the autopilot's loop means most ticks miss their +deadline, and while that no longer corrupts the flight it does cost wall-clock pace. + +**`SKYSIM_CAMERA_FPS` is the camera switch.** Without it no frames are rendered and +the camera endpoints serve nothing, whatever else is set. With it, `SKYSIM_CAMERA_SIZE` +(default `256x144`), `SKYSIM_CAMERA_QUALITY`, `SKYSIM_CAMERA_THREADS`, +`SKYSIM_CAMERA_FOV`, `SKYSIM_CAMERA_PITCH` and `SKYSIM_CAMERA_RANGE` apply: + +```bash +docker run --rm -p 8642:8642 -p 9002-9202:9002-9202/udp \ + -v $(pwd)/build/city/tiles:/world:ro -e SKYSIM_TILES=/world \ + -e SKYSIM_CAMERA_FPS=10 -e SKYSIM_CAMERA_SIZE=256x144 skysim +# http://localhost:8642/instances/0/camera.mjpg +``` + --- ## Control plane (`--api-port`) @@ -191,7 +218,7 @@ docker run --rm -p 8642:8642 -p 9002-9202:9002-9202/udp \ |----------|--------| | `POST /vehicles` `{"instance":N, "launch_process":true}` (both fields optional) | reserve a requested or next-free ArduPilot instance (optionally forks arducopter) → `{id, instance, json_port, mavlink_tcp}` | | `DELETE /vehicles/{id}` | despawn, release instance, kill managed process | -| `GET /vehicles` | per-vehicle: connected, frozen, held_ticks, pos_ned, `midair_collisions`, `building_contacts` | +| `GET /vehicles` | per-vehicle: connected, frozen, silent_ticks, pos_ned, `midair_collisions`, `building_contacts` | | `GET /metrics` | tick p50/p99 µs, straggler_events, freezes, resident_tiles | | `POST /mission/check` `{"waypoints_ned":[[n,e,d],...], "clearance_m":1.0}` | sweep a planned route through loaded building geometry → `{checked, clear, hits}` | @@ -206,6 +233,7 @@ the dashboard. | `--truth-log out.csv` | Ground truth per tick — judge the physics with this, not the EKF | | `--record-servo` / `--replay-servo` | Deterministic input tapes for reproducible runs | | `--time-mode strict` | Lockstep barrier on every vehicle; aborts on a straggler (CI) | +| `--straggler-timeout ` | interactive: silence before a vehicle is frozen out of the barrier (2 s) | | `--stream-radius` / `--stream-max` | City tile residency around each vehicle | | `--physics-threads` | Opt a single huge world into Jolt's thread pool (only worth it past ~1000 bodies) | | `--io-threads` | Fan the reply path across cores (helps past ~48 vehicles) | diff --git a/docker-entrypoint.sh b/docker-entrypoint.sh index 6035a59..e52c0af 100755 --- a/docker-entrypoint.sh +++ b/docker-entrypoint.sh @@ -8,6 +8,16 @@ set -euo pipefail args=(--api-bind "${SKYSIM_API_BIND:-0.0.0.0}" --api-port "${SKYSIM_API_PORT:-8642}") +# One mapping, used by everything below: set the variable, get the flag; leave it +# unset and the binary's own default stands, so a default lives in one place +# rather than being restated here. +add_if_set() { # env-var-name flag + local value="${!1:-}" + if [[ -n "$value" ]]; then + args+=("$2" "$value") + fi +} + # Vehicles are normally spawned on demand over the control plane, so default to # starting with none rather than skysim's built-in default of one. args+=(--vehicles "${SKYSIM_VEHICLES:-0}") @@ -24,12 +34,31 @@ if [[ -n "${SKYSIM_TILES:-}" ]]; then fi fi -if [[ -n "${SKYSIM_DT:-}" ]]; then - args+=(--dt "${SKYSIM_DT}") -fi +add_if_set SKYSIM_DT --dt + +# Time mode, which a deployment could not set at all before. +# +# skysim defaults to strict because that is what determinism and CI replays need: +# a barrier every tick, and an abort when a vehicle misses it. A long-lived server +# wants the opposite — vehicles join and leave, some of them lag, and none of that +# should take the fleet down. Without this the only way to say so was +# SKYSIM_EXTRA_ARGS, so every deployment quietly ran strict. +add_if_set SKYSIM_TIME_MODE --time-mode + +add_if_set SKYSIM_SPAWN_HOME --spawn-home -if [[ -n "${SKYSIM_SPAWN_HOME:-}" ]]; then - args+=(--spawn-home "${SKYSIM_SPAWN_HOME}") +# Camera. SKYSIM_CAMERA_FPS is the switch — without --camera-fps the render +# service is never built and every other camera setting is inert, which is +# exactly how a deployment ends up serving a control plane with no pictures on +# it. The rest only apply once it is on. +if [[ -n "${SKYSIM_CAMERA_FPS:-}" ]]; then + args+=(--camera-fps "${SKYSIM_CAMERA_FPS}") + add_if_set SKYSIM_CAMERA_SIZE --camera-size + add_if_set SKYSIM_CAMERA_QUALITY --camera-quality + add_if_set SKYSIM_CAMERA_THREADS --camera-threads + add_if_set SKYSIM_CAMERA_FOV --camera-fov + add_if_set SKYSIM_CAMERA_PITCH --camera-pitch + add_if_set SKYSIM_CAMERA_RANGE --camera-range fi if [[ -n "${SKYSIM_EXTRA_ARGS:-}" ]]; then diff --git a/docs/DESIGN.md b/docs/DESIGN.md index effb454..09f8a27 100644 --- a/docs/DESIGN.md +++ b/docs/DESIGN.md @@ -33,12 +33,22 @@ run as separate `skysim` processes (sharding) for near-linear scaling. World advances at fixed `dt` (default 1/400 s; per-deployment). Modes: -- **strict** (CI, replays): barrier — tick when *every* registered vehicle's next input frame - has arrived. Deterministic; one stalled SITL stalls the world. Timeout ⇒ abort with report. -- **interactive** (SkyHub operators in the loop): tick on schedule. A vehicle that missed the - deadline gets its last PWM held for up to `k` ticks (default 3); beyond that it is **frozen** +- **strict** (CI, replays): barrier — tick when the next input frame of every *connected* + vehicle has arrived. Deterministic; one stalled SITL stalls the world. Timeout ⇒ abort with + report. The timeout starts once something has connected: a world still waiting for its first + SITL is not stalled, but a fleet that has all gone silent is, and aborts rather than hanging. +- **interactive** (SkyHub operators in the loop): the same barrier, paced to the wall clock, + with an exit from it instead of an abort. A vehicle that missed the deadline stalls its own + tick; once it has been silent for `--straggler-timeout` (default 2 s) it is **frozen** (kinematic hold, flagged in the API) until frames resume, and auto-despawned after a grace - period. Replies are sent immediately after the tick that consumed each vehicle's input. + period. Freezing takes it out of the barrier entirely — the fleet stops both blocking on it + and waiting out its share of the frame grace — while it stays polled so it can thaw. + Replies are sent immediately after the tick that consumed each vehicle's input. + + A late vehicle is never stepped on its previous tick's PWM. ArduPilot derives its scheduler + rate from the timestamp we return (`adjust_frame_time(1.0 / deltat)` in SIM_JSON.cpp), so a + step it did not contribute to re-rates its control loop and hands it motion it never + commanded — it crashed every takeoff. Lateness may cost wall-clock pace, never fidelity. Spawn/despawn only ever happens at a tick boundary (step 1), so mid-step body creation never occurs. diff --git a/src/api/control_server.cpp b/src/api/control_server.cpp index 2d3050a..3323119 100644 --- a/src/api/control_server.cpp +++ b/src/api/control_server.cpp @@ -21,6 +21,30 @@ namespace { // plane, which must stay answerable while they run. constexpr size_t kWorkerThreads = 64; +// How often an MJPEG stream looks for a newly drawn frame. Short relative to any render rate +// worth watching, because it bounds how long a finished frame waits before it goes out; it does +// not set the rate, since a frame is only written once (see write_mjpeg_part callers). +constexpr auto kFramePoll = std::chrono::milliseconds(5); + +// One multipart part: headers plus the JPEG. +// +// Only ever called for a frame the caller has not sent before. Re-sending whatever is in the +// slot on a fixed cadence — which is what this did — puts duplicates on the wire at the poll +// rate rather than the render rate: measured at 24.7 frames/s carrying 10 frames/s of content, +// 59% repeats and 37 KB/s of them. Worse than the waste, it feeds a consumer faster than the +// content arrives, and the excess lands in socket and queue buffers as latency the viewer sees +// as lag. FrameStore drops rather than queues for exactly this reason; this route was undoing it. +void write_mjpeg_part(httplib::DataSink &sink, const std::vector &jpeg) { + char head[128]; + const int n = std::snprintf(head, sizeof(head), + "--skysimframe\r\nContent-Type: image/jpeg\r\n" + "Content-Length: %zu\r\n\r\n", + jpeg.size()); + sink.write(head, static_cast(n)); + sink.write(reinterpret_cast(jpeg.data()), jpeg.size()); + sink.write("\r\n", 2); +} + // {"launch_process":true} — absent key means false. bool parse_launch_process(const std::string &body) { const char *p = std::strstr(body.c_str(), "\"launch_process\""); @@ -138,11 +162,11 @@ std::string vehicle_json(const VehicleInfo &v) { char buf[512]; std::snprintf(buf, sizeof(buf), "{\"id\":%u,\"instance\":%d,\"json_port\":%d,\"mavlink_tcp\":%d," - "\"connected\":%s,\"frozen\":%s,\"held_ticks\":%llu," + "\"connected\":%s,\"frozen\":%s,\"silent_ticks\":%llu," "\"midair_collisions\":%llu,\"static_contacts\":%llu," "\"building_contacts\":%llu,\"pos_ned\":[%.3f,%.3f,%.3f]}", v.id, v.instance, v.json_port, v.mavlink_tcp_port, v.connected ? "true" : "false", - v.frozen ? "true" : "false", static_cast(v.held_ticks), + v.frozen ? "true" : "false", static_cast(v.silent_ticks), static_cast(v.midair_collisions), static_cast(v.static_contacts), static_cast(v.building_contacts), v.pos_ned[0], v.pos_ned[1], @@ -305,7 +329,7 @@ ControlServer::ControlServer(const std::string &bind_addr, int port, CommandQueu res.set_content("{\"error\":\"no such instance\"}", "application/json"); return; } - std::vector jpeg = snapshots.camera_frame(id); + std::vector jpeg = snapshots.camera_frame(id).jpeg; if (jpeg.empty()) { res.status = 404; res.set_content("{\"error\":\"no frame\"}", "application/json"); @@ -324,22 +348,17 @@ ControlServer::ControlServer(const std::string &bind_addr, int port, CommandQueu } res.set_chunked_content_provider( "multipart/x-mixed-replace; boundary=skysimframe", - [snapshots, resolve_instance, instance](size_t, httplib::DataSink &sink) { + [snapshots, resolve_instance, instance, last_sent = -1.0](size_t, + httplib::DataSink &sink) mutable { // Re-resolved per frame, so a skysim restart mid-recording picks // the vehicle back up instead of streaming nothing forever. const uint32_t id = resolve_instance(instance); - std::vector jpeg = id != 0 ? snapshots.camera_frame(id) : std::vector{}; - if (!jpeg.empty()) { - char head[128]; - const int n = std::snprintf(head, sizeof(head), - "--skysimframe\r\nContent-Type: image/jpeg\r\n" - "Content-Length: %zu\r\n\r\n", - jpeg.size()); - sink.write(head, static_cast(n)); - sink.write(reinterpret_cast(jpeg.data()), jpeg.size()); - sink.write("\r\n", 2); + auto frame = id != 0 ? snapshots.camera_frame(id) : skysim::render::FrameStore::Frame{}; + if (!frame.jpeg.empty() && frame.sim_time_s != last_sent) { + last_sent = frame.sim_time_s; + write_mjpeg_part(sink, frame.jpeg); } - std::this_thread::sleep_for(std::chrono::milliseconds(40)); + std::this_thread::sleep_for(kFramePoll); return true; }); }); @@ -354,8 +373,9 @@ ControlServer::ControlServer(const std::string &bind_addr, int port, CommandQueu // strtoul, not stoul: the route regex accepts any number of digits, and // stoul throws on a value too big for the type. Saturating is the same // answer as "no such vehicle" without the exception. - std::vector jpeg = snapshots.camera_frame( - static_cast(std::strtoul(req.matches[1].str().c_str(), nullptr, 10))); + std::vector jpeg = + snapshots.camera_frame(static_cast(std::strtoul(req.matches[1].str().c_str(), nullptr, 10))) + .jpeg; if (jpeg.empty()) { res.status = 404; res.set_content("{\"error\":\"no frame\"}", "application/json"); @@ -378,21 +398,13 @@ ControlServer::ControlServer(const std::string &bind_addr, int port, CommandQueu const auto id = static_cast(std::strtoul(req.matches[1].str().c_str(), nullptr, 10)); res.set_chunked_content_provider( "multipart/x-mixed-replace; boundary=skysimframe", - [snapshots, id](size_t /*offset*/, httplib::DataSink &sink) { - std::vector jpeg = snapshots.camera_frame(id); - if (!jpeg.empty()) { - char head[128]; - const int n = std::snprintf(head, sizeof(head), - "--skysimframe\r\nContent-Type: image/jpeg\r\n" - "Content-Length: %zu\r\n\r\n", - jpeg.size()); - sink.write(head, static_cast(n)); - sink.write(reinterpret_cast(jpeg.data()), jpeg.size()); - sink.write("\r\n", 2); + [snapshots, id, last_sent = -1.0](size_t /*offset*/, httplib::DataSink &sink) mutable { + auto frame = snapshots.camera_frame(id); + if (!frame.jpeg.empty() && frame.sim_time_s != last_sent) { + last_sent = frame.sim_time_s; + write_mjpeg_part(sink, frame.jpeg); } - // Poll a little faster than frames are produced, so the stream - // tracks the render rate instead of setting its own. - std::this_thread::sleep_for(std::chrono::milliseconds(40)); + std::this_thread::sleep_for(kFramePoll); return true; }); }); diff --git a/src/api/control_server.h b/src/api/control_server.h index d4e9299..6ff836c 100644 --- a/src/api/control_server.h +++ b/src/api/control_server.h @@ -12,6 +12,7 @@ #include #include "core/world.h" +#include "render/frame_store.h" #include "vehicle/manager.h" namespace skysim::api { @@ -23,7 +24,7 @@ struct VehicleInfo { int mavlink_tcp_port = 0; bool connected = false; // has ever delivered a servo frame bool frozen = false; // straggler policy engaged (kinematic hold) - uint64_t held_ticks = 0; + uint64_t silent_ticks = 0; uint64_t midair_collisions = 0; // vehicle-vehicle contact events (crash telemetry) uint64_t static_contacts = 0; // any static touch, landings included uint64_t building_contacts = 0; // building-tile strikes only — the "hit a building" signal @@ -103,10 +104,11 @@ class ControlServer { struct Snapshots { std::function()> vehicles; std::function metrics; - // Latest camera frame for a vehicle as encoded JPEG; empty when the - // camera is off or that vehicle has not been rendered yet. Published by - // the tick thread like everything else here. - std::function(uint32_t)> camera_frame; + // Latest camera frame for a vehicle as encoded JPEG, with the sim time it was drawn + // at; empty when the camera is off or that vehicle has not been rendered yet. The + // timestamp is what lets the MJPEG routes send a frame once instead of re-sending + // whatever is in the slot. Published by the tick thread like everything else here. + std::function camera_frame; }; // Binds bind_addr:port and serves on its own thread. Throws on bind failure (startup diff --git a/src/core/clock.h b/src/core/clock.h index d6c6e17..143d59d 100644 --- a/src/core/clock.h +++ b/src/core/clock.h @@ -20,7 +20,11 @@ class PhysicsClock { uint64_t tick_ = 0; }; -// TODO(M1/M4): TickGate — strict: barrier on all vehicles' next frame (with abort timeout); -// interactive: deadline pacing + straggler policy (hold-k, freeze, grace despawn). +// TODO(M1/M4): TickGate — one barrier on the next frame of every *connected* vehicle, with +// the straggler policy injected: strict aborts on timeout, interactive adds deadline pacing +// and evicts (freeze, grace despawn). The barrier population is not the whole fleet: a slot +// that has never connected has nothing to wait for, and interactive freezing takes a vehicle +// out of the barrier rather than stepping it. A late vehicle that is still in the barrier +// must never be stepped without its frame in either mode — see run_interactive for why. } // namespace skysim::core diff --git a/src/main.cpp b/src/main.cpp index 0f53d7c..4d16d47 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,10 +1,11 @@ // skysim entry point: config -> World -> UdpEndpoints/ControlServer -> tick loop. -// Time policy (docs/DESIGN.md): strict = barrier on every vehicle's next frame (abort on -// timeout); interactive = wall-clock paced, stragglers hold->freeze->grace-despawn. +// Time policy (docs/DESIGN.md): both modes barrier on every connected vehicle's next frame; +// strict aborts on a straggler, interactive is wall-clock paced and evicts one (freeze->despawn). // --canned keeps the M1 kinematic reply path; --replay-servo re-runs a recorded input tape. #include #include #include +#include #include #include #include @@ -59,7 +60,13 @@ struct Options { // M4: control plane + straggler policy + managed SITL processes. int api_port = 0; // 0 = control plane disabled std::string api_bind = "127.0.0.1"; // 0.0.0.0 when the gateway calls from a bridge net - int hold_ticks = 3; // interactive: reuse last PWM for up to k missed deadlines + // interactive: how long a connected vehicle may go without sending a frame before it is + // declared gone and dropped out of the barrier. A liveness timeout, not a smoothing knob — + // a late vehicle stalls its own tick rather than flying on the last tick's PWM (see + // run_interactive). In seconds, not ticks, so its meaning does not quietly change with --dt: + // the tick-count knob this replaced defaulted to 3, which at 800 Hz wrote a vehicle off + // after 3.75 ms and froze SITLs that were merely busy booting. + double straggler_timeout_s = 2.0; // Fraction of a tick a frame may be late before it counts as a straggler. double frame_grace = 0.7; @@ -164,8 +171,18 @@ Options parse_args(int argc, char **argv) { o.camera_threads = std::atoi(need_value("--camera-threads")); } else if (std::strcmp(argv[i], "--frame-grace") == 0) { o.frame_grace = std::atof(need_value("--frame-grace")); - } else if (std::strcmp(argv[i], "--hold-ticks") == 0) { - o.hold_ticks = std::atoi(need_value("--hold-ticks")); + } else if (std::strcmp(argv[i], "--straggler-timeout") == 0) { + // Checked here rather than where it is used: run_interactive turns this into a + // tick count by dividing by dt, and atof happily returns nan or inf for "abc" or + // "1e400". Converting either to int is undefined behaviour, so the freeze + // threshold would be whatever that produced — silently, on a flag about how long + // to tolerate a silent vehicle. + const double v = std::atof(need_value("--straggler-timeout")); + if (!std::isfinite(v) || v < 0.0 || v > 86400.0) { + std::fprintf(stderr, "skysim: --straggler-timeout must be between 0 and 86400 seconds\n"); + std::exit(2); + } + o.straggler_timeout_s = v; } else if (std::strcmp(argv[i], "--grace") == 0) { o.grace_s = std::atof(need_value("--grace")); } else if (std::strcmp(argv[i], "--strict-timeout") == 0) { @@ -224,7 +241,7 @@ struct VehicleSlot { bool has_pending_input = false; // fresh frame consumed, reply owed after this tick skysim::protocol::ParsedServos pending{}; bool connected = false; // ever received a frame - int hold_ticks = 0; // consecutive deadline misses (interactive) + int silent_ticks = 0; // consecutive deadline misses (interactive) bool frozen = false; uint64_t frozen_at_tick = 0; uint64_t gaps = 0, reboots = 0, bad = 0, ticks = 0; @@ -297,7 +314,12 @@ void step_battery(VehicleSlot &v, const std::array &pwm, double dt } // Apply wrenches for every active vehicle and advance the world one tick. -// A slot participates with its `pending` PWM whether fresh (has_pending_input) or held. +// +// Invariant, and both loops are built to hold it: a participating slot's `pending` PWM is +// always a frame the vehicle actually sent for this tick. Slots that are frozen or have never +// connected sit out (gravity + contacts only); nobody is ever stepped on a replay of last +// tick's PWM. The autopilot reads its own loop rate off the timestamps we return, so a step it +// did not contribute to is not an approximation — it desynchronises its controller. void step_world(skysim::core::World &world, Fleet &fleet, double dt_s, FILE *truth_log, FILE *record_log) { const auto wind = world.wind_ned(); for (size_t i = 0; i < fleet.size(); ++i) { @@ -741,7 +763,7 @@ struct App { info.mavlink_tcp_port = 5760 + 10 * v->instance; info.connected = v->connected; info.frozen = v->frozen; - info.held_ticks = static_cast(v->hold_ticks); + info.silent_ticks = static_cast(v->silent_ticks); info.midair_collisions = v->state.midair_collisions; info.static_contacts = v->state.static_contacts; info.building_contacts = v->state.building_contacts; @@ -766,6 +788,20 @@ struct App { } }; +// Per-tick work that is neither physics nor waiting, timed so it can be charged to the tick. +// +// It spends the same budget the vehicle's round trip needs, and leaving it out of the metric is +// how a box reporting a 73 us p99 could be missing a hundred deadlines a minute — the cost was +// real, it was just all in tile streaming. Measured fresh per iteration rather than accumulated: +// an iteration that does not go on to step is time spent waiting, and charging it to whichever +// tick eventually lands would put a whole barrier stall into one sample. +double timed_housekeeping(App &app) { + const auto t0 = std::chrono::steady_clock::now(); + app.drain_commands(); + app.stream_tiles(); + return std::chrono::duration(std::chrono::steady_clock::now() - t0).count(); +} + // Strict: barrier on every CONNECTED vehicle's next frame; not-yet-connected vehicles don't // block (they sit on the ground until their SITL shows up). Abort on stalled barrier. int run_strict(App &app, FILE *truth_log, FILE *record_log) { @@ -773,16 +809,19 @@ int run_strict(App &app, FILE *truth_log, FILE *record_log) { auto barrier_stalled_since = clock::now(); bool barrier_was_complete = true; while (!g_stop.load(std::memory_order_relaxed)) { - app.drain_commands(); - app.stream_tiles(); + const double house_us = timed_housekeeping(app); bool all_fresh = true; bool any_fresh = false; + bool any_connected = false; for (auto &v : app.fleet) { const bool fresh = poll_endpoint(*v); any_fresh = any_fresh || fresh; - if (v->connected && !fresh) { - all_fresh = false; + if (v->connected) { + any_connected = true; + if (!fresh) { + all_fresh = false; + } } } const bool ready = all_fresh && any_fresh; @@ -790,8 +829,12 @@ int run_strict(App &app, FILE *truth_log, FILE *record_log) { if (barrier_was_complete) { barrier_was_complete = false; barrier_stalled_since = clock::now(); - } else if (app.opt.strict_timeout_s > 0.0 && any_fresh) { - // Someone is waiting on someone else: that's a genuine straggler stall. + } else if (app.opt.strict_timeout_s > 0.0 && any_connected) { + // Connected, not fresh: a genuine stall. Gated on anyone being connected + // rather than on anyone being fresh, so that the case this abort exists for — + // every SITL in the fleet going silent at once — is not the one case it sat + // through in silence. Waiting indefinitely is still right before the first + // vehicle arrives; a world with nothing in it is not stalled. const double stalled = std::chrono::duration(clock::now() - barrier_stalled_since).count(); if (stalled > app.opt.strict_timeout_s) { @@ -812,7 +855,7 @@ int run_strict(App &app, FILE *truth_log, FILE *record_log) { const auto t0 = clock::now(); step_world(*app.world, app.fleet, app.opt.dt_s, truth_log, record_log); - app.metrics.record_us(std::chrono::duration(clock::now() - t0).count()); + app.metrics.record_us(house_us + std::chrono::duration(clock::now() - t0).count()); if (const int rc = send_replies(app.world.get(), app.fleet, app.opt, app.world->now(), app.io_pool.get())) { return rc; @@ -831,7 +874,11 @@ void wait_for_frames(Fleet &fleet, std::chrono::steady_clock::time_point deadlin for (;;) { bool all_ready = true; for (auto &v : fleet) { - if (v->connected && !poll_endpoint(*v)) { + // Frozen slots stay `connected` — they are still ours, and still polled below so + // they can thaw. But waiting on one here spends the whole grace period every + // single tick on a vehicle the barrier has already agreed to step without, which + // is the fleet-wide stall that freezing it was supposed to end. + if (v->connected && !v->frozen && !poll_endpoint(*v)) { all_ready = false; } } @@ -842,13 +889,34 @@ void wait_for_frames(Fleet &fleet, std::chrono::steady_clock::time_point deadlin } } -// Interactive: tick on schedule; a vehicle missing its deadline gets its last PWM held for -// up to k ticks, then freezes (kinematic hold, flagged), then despawns after the grace. +// Interactive: pace to the wall clock, but never advance a vehicle without its frame. +// +// The tick is a barrier here exactly as it is in strict mode. What interactive adds is an +// exit from that barrier: a vehicle that stops answering for a real length of time is +// declared gone, frozen, and despawned after the grace, instead of stalling the fleet. That +// is what a long-lived server needs, where aircraft join and leave and one that dies must +// not take everybody else down with it. +// +// It used to add a second thing, and that second thing was the bug. A vehicle which missed its +// deadline had its last PWM replayed for up to k ticks so the world could keep to the wall +// clock. ArduPilot's JSON backend takes its entire scheduler rate from the timestamp we send it +// — SIM_JSON.cpp runs `adjust_frame_time(1.0 / deltat)` on every frame — so a tick stepped +// without the vehicle comes back as a doubled deltat and silently re-rates its control loop, +// describing motion it never commanded. It crashed every takeoff. Freezing a vehicle that was +// merely late is the same lie told louder. +// +// So a late vehicle now stalls its own tick. Lateness costs wall-clock pacing, which is +// recoverable and shows up in the metrics; it must never cost flight fidelity. int run_interactive(App &app, FILE *truth_log, FILE *record_log) { using clock = std::chrono::steady_clock; const auto dt = std::chrono::duration_cast(std::chrono::duration(app.opt.dt_s)); auto next_tick = clock::now() + dt; const uint64_t grace_ticks = static_cast(app.opt.grace_s / app.opt.dt_s); + // Ticks of silence before a vehicle is written off as gone. + const int freeze_after = std::max(1, static_cast(app.opt.straggler_timeout_s / app.opt.dt_s)); + // How often the API snapshot is refreshed while the world is held waiting for a straggler. + constexpr auto kStallPublishPeriod = std::chrono::milliseconds(50); + auto last_stall_publish = clock::now(); // How much of a tick a late frame may use before it counts as late. Kept // under the period so a persistently slow vehicle still shows up as one // rather than silently stretching every tick. @@ -862,8 +930,7 @@ int run_interactive(App &app, FILE *truth_log, FILE *record_log) { next_tick = clock::now() + dt; // fell far behind (debugger, suspend): resync } - app.drain_commands(); - app.stream_tiles(); + const double house_us = timed_housekeeping(app); // Give a late frame a bounded moment to land before calling it late. // @@ -879,32 +946,53 @@ int run_interactive(App &app, FILE *truth_log, FILE *record_log) { // stragglers into ordinary on-time ticks. wait_for_frames(app.fleet, clock::now() + frame_grace); + // A vehicle that has never connected, or that has already been written off, does not + // block the tick. Anyone else we are still waiting on does. + bool all_ready = true; for (auto &v : app.fleet) { - const bool fresh = poll_endpoint(*v); - if (fresh) { + if (poll_endpoint(*v)) { if (v->frozen) { std::printf("skysim: vehicle %u thawed (frames resumed)\n", v->vehicle_id); v->frozen = false; app.world->set_frozen(v->body_id, false); } - v->hold_ticks = 0; + v->silent_ticks = 0; } else if (v->connected && !v->frozen) { - ++v->hold_ticks; + ++v->silent_ticks; ++app.metrics.straggler_events; - if (v->hold_ticks > app.opt.hold_ticks) { - std::printf("skysim: vehicle %u FROZEN after %d held ticks\n", v->vehicle_id, - v->hold_ticks); + if (v->silent_ticks > freeze_after) { + std::printf("skysim: vehicle %u FROZEN after %.2f s without a frame\n", + v->vehicle_id, v->silent_ticks * app.opt.dt_s); v->frozen = true; v->frozen_at_tick = app.world->tick_index(); ++app.metrics.freezes; app.world->set_frozen(v->body_id, true); + } else { + all_ready = false; // still ours: wait for it rather than step without it } } } + if (!all_ready) { + // Hold the entire world at this tick. Time is shared — one Jolt world, one clock — + // so there is no advancing the rest of the fleet without also advancing the vehicle + // we are waiting on, and advancing that one without its frame is the thing that + // crashes it. + // + // Keep publishing so /vehicles and /metrics stay live through the stall, but at a + // polling rate rather than a tick rate: publish_snapshot sorts the whole tick-time + // ring to get its quantiles, and nothing it reports moves while the world is held. + // At the default 2 s timeout and 800 Hz that would be 1600 identical sorts. + if (clock::now() - last_stall_publish > kStallPublishPeriod) { + last_stall_publish = clock::now(); + app.publish_snapshot(); + } + continue; + } + const auto t0 = clock::now(); step_world(*app.world, app.fleet, app.opt.dt_s, truth_log, record_log); - app.metrics.record_us(std::chrono::duration(clock::now() - t0).count()); + app.metrics.record_us(house_us + std::chrono::duration(clock::now() - t0).count()); if (const int rc = send_replies(app.world.get(), app.fleet, app.opt, app.world->now(), app.io_pool.get())) { return rc; @@ -1090,7 +1178,7 @@ int main(int argc, char **argv) { return app.metrics_snapshot; }; snaps.camera_frame = [&app](uint32_t id) { - return app.render_service ? app.render_service->frame(id) : std::vector{}; + return app.render_service ? app.render_service->frame(id) : skysim::render::FrameStore::Frame{}; }; api = std::make_unique(opt.api_bind, opt.api_port, app.queue, std::move(snaps)); diff --git a/src/render/render_service.h b/src/render/render_service.h index 56e8668..889a198 100644 --- a/src/render/render_service.h +++ b/src/render/render_service.h @@ -61,8 +61,9 @@ class RenderService { // doubles under a mutex and returns. void publish_poses(std::vector poses); - // Latest encoded frame for a vehicle; empty until one has been drawn. - std::vector frame(uint32_t vehicle_id) const { return frames_.get(vehicle_id).jpeg; } + // Latest encoded frame for a vehicle, with the sim time it was drawn at so a consumer can + // tell a new picture from the one it already has. Empty until one has been drawn. + FrameStore::Frame frame(uint32_t vehicle_id) const { return frames_.get(vehicle_id); } size_t tiles_loaded() const { return tiles_loaded_; } size_t triangle_count() const { return scene_.triangle_count(); } diff --git a/tests/test_api.cpp b/tests/test_api.cpp index 67bda21..c983cac 100644 --- a/tests/test_api.cpp +++ b/tests/test_api.cpp @@ -106,7 +106,7 @@ int main() { v.mavlink_tcp_port = 6060; v.connected = true; v.frozen = true; - v.held_ticks = 4; + v.silent_ticks = 4; v.pos_ned[0] = 1.5; return std::vector{v}; }; @@ -230,7 +230,7 @@ int main() { auto list = client.Get("/vehicles"); CHECK(list && list->status == 200); CHECK(list && list->body.find("\"frozen\":true") != std::string::npos); - CHECK(list && list->body.find("\"held_ticks\":4") != std::string::npos); + CHECK(list && list->body.find("\"silent_ticks\":4") != std::string::npos); auto metrics = client.Get("/metrics"); CHECK(metrics && metrics->status == 200); @@ -267,7 +267,12 @@ int main() { const std::vector kCanned{0xFF, 0xD8, 'p', 'i', 'x', 0xFF, 0xD9}; ControlServer::Snapshots with_camera = snaps; with_camera.camera_frame = [kCanned](uint32_t id) { - return id == 1 ? kCanned : std::vector{}; + // sim_time_s advances per call so the MJPEG routes see each poll as a new frame; + // they now send a frame once and skip repeats. + static double t = 0.0; + t += 0.1; + return id == 1 ? skysim::render::FrameStore::Frame{kCanned, t} + : skysim::render::FrameStore::Frame{}; }; ControlServer server("127.0.0.1", kCameraPort, queue, with_camera); diff --git a/tests/test_render.cpp b/tests/test_render.cpp index a4efaae..34a3df3 100644 --- a/tests/test_render.cpp +++ b/tests/test_render.cpp @@ -261,7 +261,7 @@ int main() { skysim::render::RenderService service(scfg); CHECK(service.tiles_loaded() == 1); CHECK(service.triangle_count() > 0); - CHECK(service.frame(1).empty()); // nothing published yet, so nothing to draw + CHECK(service.frame(1).jpeg.empty()); // nothing published yet, so nothing to draw skysim::render::RenderService::Pose pose; pose.vehicle_id = 1; @@ -273,11 +273,11 @@ int main() { std::vector frame; for (int i = 0; i < 200 && frame.empty(); ++i) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); - frame = service.frame(1); + frame = service.frame(1).jpeg; } CHECK(!frame.empty()); CHECK(frame.size() > 2 && frame[0] == 0xFF && frame[1] == 0xD8); // a real JPEG - CHECK(service.frame(2).empty()); // only the posed vehicle + CHECK(service.frame(2).jpeg.empty()); // only the posed vehicle // Despawn it: the next pass must drop the frame rather than keep serving // a picture of a vehicle that is gone. @@ -285,7 +285,7 @@ int main() { bool cleared = false; for (int i = 0; i < 200 && !cleared; ++i) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); - cleared = service.frame(1).empty(); + cleared = service.frame(1).jpeg.empty(); } CHECK(cleared); diff --git a/tools/harness/params/skysim.parm b/tools/harness/params/skysim.parm index 19913b8..1dd7b7c 100644 --- a/tools/harness/params/skysim.parm +++ b/tools/harness/params/skysim.parm @@ -3,3 +3,9 @@ # check requires >= 1.8x SCHED_LOOP_RATE (400 Hz on copter), so 400 Hz sim rate trips # "PreArm: Gyro 0 rate ... < loop rate*1.8" while 800 Hz clears it (observed at M1). SIM_RATE_HZ 800 + +# X quad, matching src/vehicle/quad.cpp. copter.parm defaults to PLUS, which points the mixer +# 45 degrees away from our props and crashes the aircraft seconds after takeoff. Every harness +# here inherits copter.parm, so without this line they all flew the wrong airframe and could +# never have caught it. Same reasoning, at length, in skyhub_core sitl/scripts/sitl.sh. +FRAME_TYPE 1 diff --git a/tools/harness/straggler.py b/tools/harness/straggler.py index 4c5ef2f..ffb1cc9 100644 --- a/tools/harness/straggler.py +++ b/tools/harness/straggler.py @@ -38,7 +38,7 @@ def interactive_case(args) -> bool: print("=== interactive: SIGSTOP -> freeze+flag, SIGCONT -> thaw ===") sim = subprocess.Popen([args.sim, "--vehicles", "2", "--base-instance", str(args.base_instance), "--time-mode", "interactive", "--dt", "0.00125", - "--api-port", str(args.api_port), "--hold-ticks", "3", + "--api-port", str(args.api_port), "--straggler-timeout", "0.01", "--grace", "120", "--spacing", "30"]) time.sleep(0.5) vehicles = [Vehicle(args.base_instance + i,