diff --git a/AGENTS.md b/AGENTS.md index 759a98f..357cdfa 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -69,6 +69,14 @@ When to still use grep/`grep_search`: matching exact substrings/regex in prose, uniformly (e.g. searching for a TODO string or a parameter name that may appear in comments). +Searching OUTSIDE this workspace (e.g. a separate `C:\repos\ardupilot` checkout): +the `grep_search`/`file_search`/`semantic_search` tools are scoped to this workspace +folder and silently return "No matches found" (a generic VS Code search-exclusion +message) for paths outside it — this does NOT mean the pattern is genuinely absent, +it means the tool couldn't search there at all. For any path outside the current +workspace folder, go straight to a terminal command (`grep`/`sed`/`rg` via +`run_in_terminal`) instead of retrying the workspace-scoped search tools. + ## Documentation Ownership (Single Source of Truth) Use the primary doc for each topic. Other docs should link, not restate. @@ -78,6 +86,7 @@ Use the primary doc for each topic. Other docs should link, not restate. | Flight architecture, mode ownership, AP/Lua boundaries | `design/flight_stack.md` | `design/tension_collective_control_loop.md`, `design/GUIDED_CONTROL_LOOPS.md` | | Simulation internals (physics, sensors, controller plumbing, module map) | `design/simulation.md` | `simulation/README.md`, code docstrings | | SITL stack workflow, lockstep, diagnosis procedure | `design/sitl_testing.md` | `simulation/analysis/diagnose_sitl.py` usage text | +| SITL IC-start timeline and event anchors | `design/sitl_flight_timeline.md` | `design/sitl_testing.md`, `simulation/tests/sitl/flight/conftest.py` | | Aero interfaces and conventions | `design/aero_conventions.md` | `design/aero.md` | | EKF gating and GPS yaw bring-up | `design/EKF_GATING.md` | `design/ekf_const_pos_mode.md` | | ArduPilot heli control-loop behavior | `design/GUIDED_CONTROL_LOOPS.md` | `design/flight_stack.md` | @@ -185,6 +194,10 @@ SITL behavior: ## Workflow Rules +- Do not use `wsl` to run anything directly. `test.sh` is the only script that uses WSL, and it + already contains the logic to re-invoke itself inside WSL when needed (for Docker access). + Run `bash test.sh ...` (or `./test.sh ...`) from Git Bash directly — do not wrap it in + `wsl -e bash -lc "..."` or run other commands (pytest, analysis scripts, git, etc.) via `wsl`. - Do not use git history (`git log`, `git show`, `git blame`) for diagnosis unless user asks. - Do not preserve backward-compatibility parameters, fields, aliases, or shims when making code changes. - Assume no external callers: prefer a clean cutover and remove legacy paths in the same change to avoid debt. @@ -208,6 +221,23 @@ SITL behavior: exist only to work around circular imports are a sign of bad architecture — fix the circular dependency by refactoring (e.g. extract a shared module, invert the dependency) rather than papering over it with a local import. +- `/tmp` is NOT one shared filesystem on this box — Git Bash (mingw/MSYS2, the default terminal) + and WSL2 (used for `docker`/`test.sh stack`) each have their own separate `/tmp`: + - A bare `> /tmp/foo.log` redirection in a Git Bash command writes to Git Bash's own `/tmp` + (really `C:\Users\\AppData\Local\Temp\foo.log` — check with `cygpath -w /tmp/foo.log`). + - A command run via `wsl -e bash -lc "... > /tmp/foo.log"` writes inside WSL's `/tmp` + (`\\wsl$\...\tmp\foo.log` from Windows, or `/tmp/foo.log` from *inside* another `wsl -e` call) + — NOT reachable from a later plain Git Bash `cat /tmp/foo.log`. + - If the redirection is written OUTSIDE the `wsl -e bash -lc "..."` quoted string (e.g. + `wsl -e bash -lc "cmd" > /tmp/foo.log`), it's the OUTER (Git Bash) shell that owns the + redirect, not WSL — easy to mix up. + - A native Windows executable (e.g. `.venv/Scripts/python.exe`) invoked from Git Bash does NOT + understand `/tmp/...` paths passed as arguments (it's a Windows process, not MSYS2-aware) — + convert with `cygpath -w /tmp/foo.log` first, or it'll fail with `FileNotFoundError` even + though `ls /tmp/foo.log` (from Git Bash) shows the file existing. + - Rule of thumb: know which shell environment (Git Bash vs WSL) is actually creating/reading + a `/tmp` path before assuming a file exists or is missing; don't conclude "no output was + produced" just because a naive `cat`/`find` from the wrong shell doesn't see it. ## Test Entry Points @@ -219,6 +249,12 @@ There are three tiers, each with a different scope and runtime: | Simtest | `.venv/Scripts/python.exe -m pytest simulation/tests/simtests` | `simtest` | Python physics loop; seconds–minutes | | Stack | `bash test.sh stack [-n N]` | `sitl` | ArduPilot SITL in Docker | +SITL IC-start timeline rule (agent-critical): +- For SITL flight diagnosis, use one shared timeline anchored at the IC-start flow. +- Treat `t_sim` with the `kinematic_exit` event as the canonical phase boundary for + release-to-flight comparisons across steady/passive/pumping/landing stack tests. +- Canonical definition and per-phase markers live in `design/sitl_flight_timeline.md`. + Stack-test execution rule (agent-critical): - For any test under `simulation/tests/sitl/**`, ALWAYS use `bash test.sh stack -n 4 ...`. - DO NOT run SITL tests with host-side pytest commands like @@ -249,6 +285,14 @@ Long-running test commands (agent-critical, efficiency): - Once a command has been moved to background, do not repeatedly call `get_terminal_output` in a tight loop — it will not return new content until the process actually produces more output or exits. Wait for the automatic completion notification instead of polling. +- Do NOT call `get_terminal_output` immediately after a command moves to background "just to + check progress". It returns a byte-limited tail of the WHOLE terminal scrollback, not just + the new command's output — if the new command has only printed a little so far, the tail + can still be dominated by leftover output from earlier unrelated commands in the same + terminal, which looks like stale/wrong output but is really just "not enough new output yet + to push the old stuff out of the tail window". End the turn and wait for the automatic + completion notification instead; only poll if genuinely unsure whether the process is hung + after a long silence. - Prefer `bash test.sh stack -n 1 -k ` (single test) while iterating; only widen to `-n 4`/full suite once the targeted test is confirmed passing, to keep turnaround short. diff --git a/design/flight_stack.md b/design/flight_stack.md index 3c4af8d..d01278e 100644 --- a/design/flight_stack.md +++ b/design/flight_stack.md @@ -620,8 +620,12 @@ jumps (the old zero-inertia algebraic model did, which drove a yaw limit cycle). **Yaw control — servo-readback trim observer (rawes.lua):** -ArduPilot's yaw rate loop is kept **small but nonzero** (P=0.015, I=0.0015, D=0). -rawes.lua still runs a +ArduPilot's yaw rate loop uses gains sized to ArduCopter-Heli's stock default +(P=0.18, I=0.018, D=0) so it has enough authority to keep heading error under +the hardcoded 45 deg heading-error-max ceiling (`AC_AttitudeControl::thrust_heading_rotation_angles`) +instead of falling into a "target follows spin" mode where the attitude target +gets rewritten onto the current (spinning) body every cycle and the rate loop +sees near-zero error while the vehicle keeps rotating. rawes.lua still runs a model-based trim observer (`run_yaw_trim`) that writes `H_YAW_TRIM` every 10 ms tick: ``` @@ -633,19 +637,19 @@ param:set("H_YAW_TRIM", trim) This drives `H_YAW_TRIM` toward the equilibrium throttle `u_eq = omega_rotor × GEAR_RATIO / RPM_SCALE` (see `torque_model.py` for constants) at which `psi_dot = 0`. `YFF_A = RAWES_YAW_SLP × SERVO9_SPAN_US × 2π/60` (default ≈ 52.8 rad/s per -throttle unit; RAWES_YAW_SLP=0 uses bench value 0.504 RPM/µs). The AP yaw P-term handles -fast transients; the tiny I-term cleans up residual drift; the observer carries the -bulk DC trim so the AP integrator can stay small. +throttle unit; RAWES_YAW_SLP=0 uses bench value 0.504 RPM/µs). The AP yaw P/I-term handles +fast transients and residual drift; the observer carries the +bulk DC trim so AP's rate loop mainly acts as a fast disturbance-rejection assist. ### 5.3 Key Parameters | Parameter | Value | Purpose | |---|---|---| | H_TAIL_TYPE | 3 (DDFP CW) | No sign flip: positive yaw error → positive motor throttle | -| ATC_RAT_YAW_P | 0.015 | Small AP yaw P-term for fast disturbance rejection | -| ATC_RAT_YAW_I | 0.0015 | Tiny I-term for residual drift cleanup | +| ATC_RAT_YAW_P | 0.18 | AP yaw P-term, sized to ArduCopter-Heli's stock default so the rate loop has enough authority to keep heading error under the 45 deg heading-error-max ceiling (see AC_AttitudeControl::thrust_heading_rotation_angles) | +| ATC_RAT_YAW_I | 0.018 | I-term for residual drift cleanup, scaled with P | | ATC_RAT_YAW_D | 0.0 | Off | -| ATC_RAT_YAW_IMAX | 0.1 | Clamp (safety; integrator is zero) | +| ATC_RAT_YAW_IMAX | 0.1 | Clamp (safety) | | RAWES_YAW_SLP | 0 | Yaw motor slope override [RPM/µs]; 0 = bench default 0.504 | --- @@ -732,8 +736,8 @@ Current hardware: GB4008 + 10:1 spur gear. See §5.2 and [components.md](compone | SERVO9_FUNCTION | 36 (Motor4) | Anti-rotation motor ESC on output 9 (AUX 1) | | SERVO9_MIN | 1000 µs | ESC disarm | | SERVO9_MAX | 2000 µs | ESC maximum | -| ATC_RAT_YAW_P | 0.015 | Small AP yaw P-term for fast disturbance rejection | -| ATC_RAT_YAW_I | 0.0015 | Tiny I-term for residual drift cleanup | +| ATC_RAT_YAW_P | 0.18 | AP yaw P-term, sized to ArduCopter-Heli's stock default so the rate loop has enough authority to keep heading error under the 45 deg heading-error-max ceiling | +| ATC_RAT_YAW_I | 0.018 | I-term for residual drift cleanup, scaled with P | | ATC_RAT_YAW_D | 0.0 | Start at zero | --- diff --git a/design/sitl_flight_timeline.md b/design/sitl_flight_timeline.md new file mode 100644 index 0000000..120b5f9 --- /dev/null +++ b/design/sitl_flight_timeline.md @@ -0,0 +1,97 @@ +# SITL Flight Timeline (IC-Start Canonical) + +This document defines the canonical timeline used for SITL flight analysis when +stacks start from the shared IC flow. + +Scope: +- SITL stack flight tests under `simulation/tests/sitl/flight/`. +- Steady, passive, pumping, and landing variants that use IC-start fixtures. + +Out of scope: +- Windows-native unit/simtests. +- Non-flight SITL infrastructure checks. + +## Why this exists + +All SITL flight comparisons must use the same event anchors. Without one +canonical timeline, we can mis-attribute divergence to controller behavior when +it is actually a phase-alignment artifact. + +## Canonical time anchors + +Use these anchors in this order: + +1. Absolute simulation time: `t_sim` from telemetry CSV. +2. Kinematic release boundary: row/event where `note == "kinematic_exit"`. +3. Relative flight time: `t_rel = t_sim - t_kin_exit`. + +Rules: +- Use `t_rel` for any release-to-flight comparison (simtest vs SITL, run-vs-run, + first-divergence windows, reaction windows). +- Do not use wall clock, container uptime, or MAVLink receive order as primary + comparison axes. +- If `kinematic_exit` is missing, treat the run as invalid for flight-phase + root-cause analysis until telemetry/event logging is fixed. + +## Standard phase timeline (IC-start stacks) + +This is the expected sequence for IC-start SITL flight stacks. + +1. `t_sim = 0`: mediator start; kinematic startup begins. +2. Kinematic hold window: EKF alignment and IC approach while physics hand-off is + gated. +3. `note == "kinematic_exit"`: one-shot release marker; this is `t_rel = 0`. +4. Post-release guided flight: controller/physics behavior under test. + +Notes: +- Exact hold duration is fixture-configured and may vary by test. +- Phase duration differences do not change the anchor rule: always compare using + `t_rel` based on `kinematic_exit`. + +## Pre-release event matrix (major columns) + +Use this table format for steady SITL timeline analysis up to kinematic exit. +Each row is a milestone, and each major subsystem has its own column. + +| Time marker | Kinematic / trajectory | GPS | EKF | Vehicle mode / arm | RAWES / Lua | Analysis note | +|---|---|---|---|---|---|---| +| `t_sim = 0`, `t_rel < 0` | Kinematic startup begins | No guaranteed fix yet | Startup alignment state | Disarmed | `RAWES_MODE=0` expected | Run initialization | +| Early hold (`t_rel << 0`) | Trapezoid accel/cruise/decel progressing | `GPS_RAW_INT` transitions 0 -> 1 -> 6 | `EKF_STATUS` transitions toward aiding-ready flags | GUIDED_NOGPS setup starts, then arm pending | Passive pipeline preparing IC seed | Confirm bring-up order | +| Mid hold (`t_rel < 0`) | Kinematic still active | Fix 6 expected to be stable | Origin set, then GPS aiding active | Armed, GUIDED_NOGPS stable | `RAWES_MODE=3` (passive), IC ready status | Verify pre-release stability | +| Late hold (`t_rel -> 0-`) | Near release target state (at IC pose/vel) | Fix remains stable | Aiding-ready flags stable | Armed/mode unchanged | Passive hold commands continue | Ensure no last-second state jumps | +| `note == kinematic_exit`, `t_rel = 0` | Kinematic handoff ends | Should still be locked | Should still be aiding | Armed/mode continuous across handoff | Steady logic eligible to take control | Start post-release comparisons | + +Minimum fields to populate per row: +- Time: `t_sim`, `t_rel`. +- GPS: `fix_type`, `satellites_visible`. +- EKF: `EKF_STATUS_REPORT.flags` and key STATUSTEXT milestones. +- Mode/arm: HEARTBEAT `custom_mode` and armed bit. +- RAWES/Lua: mode transitions and passive/steady status text. + +## Required analysis convention + +For SITL flight diagnostics, report timing in both forms: +- `t_sim` for raw traceability. +- `t_rel` for behavioral comparison. + +When reporting first divergence, include: +- first-divergence `t_rel` bucket, +- pre-window and post-window definitions in `t_rel`, +- whether events are before or after `kinematic_exit`. + +## Data sources and marker ownership + +Canonical marker producers/consumers: +- Producer: `simulation/mediator.py` writes `note = "kinematic_exit"` and event log entry. +- Shared fixture path: `simulation/tests/sitl/flight/conftest.py` (`_ic_trapezoid_stack`). +- Primary diagnosis workflow: `design/sitl_testing.md` and `simulation/analysis/diagnose_sitl.py`. + +## Validation checklist (after timeline-related changes) + +1. Run one IC-start SITL flight test. +2. Confirm telemetry has a single `kinematic_exit` marker. +3. Confirm analysis scripts compute finite `t_rel` values post-release. +4. Confirm first-divergence output references `t_rel` (not only absolute time). + +Suggested command: +- `bash test.sh stack -n 1 -k test_lua_flight_steady_sitl` diff --git a/design/sitl_testing.md b/design/sitl_testing.md index bd70e48..f1641df 100644 --- a/design/sitl_testing.md +++ b/design/sitl_testing.md @@ -115,6 +115,12 @@ running during the wait. Full reference: ## Kinematic hold timeline +Canonical timeline note: +- The repository-level canonical timeline definition for IC-start SITL flight + analysis is `design/sitl_flight_timeline.md`. +- Use this section for stack-specific execution details; use the canonical doc + for shared event anchors (`t_sim`, `kinematic_exit`, `t_rel`). + The **kinematic hold** (a.k.a. kinematic startup phase) is the artificial, physics-free trajectory that brings the hub to the IC operating point and holds it there while the EKF aligns on GPS. It exists for one reason: to leave the EKF diff --git a/simulation/analysis/analyze_ang_error.py b/simulation/analysis/analyze_ang_error.py index 6d7de01..60815d1 100644 --- a/simulation/analysis/analyze_ang_error.py +++ b/simulation/analysis/analyze_ang_error.py @@ -206,9 +206,15 @@ def _heading_error_max(rat_yaw_p: float, ang_yaw_p: float, acc_y_degss: float) - # ── log loading ─────────────────────────────────────────────────────────────── def _dynamics_start_tsim(log_dir: Path) -> float | None: + """t_sim [s] of the dynamics-start reference for t_dyn=0. + + Prefers `dynamics_start` (torque-test convention); falls back to + `kinematic_exit` (flight-stack convention: t_dyn=0 at kinematic release). + """ p = log_dir / "events.jsonl" if not p.exists(): return None + fallback: float | None = None for line in p.read_text(encoding="utf-8", errors="replace").splitlines(): line = line.strip() if not line: @@ -219,7 +225,9 @@ def _dynamics_start_tsim(log_dir: Path) -> float | None: continue if e.get("event") == "dynamics_start": return float(e["t_sim"]) - return None + if e.get("event") == "kinematic_exit": + fallback = float(e["t_sim"]) + return fallback def _load_dataflash(bin_path: Path, t_dyn_start_s: float) -> dict: diff --git a/simulation/analysis/analyze_yaw_oscillation.py b/simulation/analysis/analyze_yaw_oscillation.py index d7bba27..52c423f 100644 --- a/simulation/analysis/analyze_yaw_oscillation.py +++ b/simulation/analysis/analyze_yaw_oscillation.py @@ -52,6 +52,9 @@ def load(path: str) -> dict: nvf: dict[str, list] = {k: [] for k in ("YFF_T", "YFF_U", "YFF_GZ", "YFF_A", "YFF_KD")} nvf_t: dict[str, list] = {k: [] for k in nvf} + rpm_t, rpm1 = [], [] + pidy_t: list = [] + pidy_p, pidy_i, pidy_d, pidy_ff = [], [], [], [] with open(path, encoding="utf-8") as f: for line in f: @@ -75,6 +78,17 @@ def load(path: str) -> dict: elif mt == "SERVO_OUTPUT_RAW": srv_t.append(t) srv_pwm.append(float(d.get("servo9_raw", math.nan))) + elif mt == "RPM": + rpm_t.append(t) + rpm1.append(float(d.get("rpm1", math.nan))) + elif mt == "PID_TUNING": + # ArduPilot axis: 1=roll, 2=pitch, 3=yaw, 4=accelz. + if int(d.get("axis", -1)) == 3: + pidy_t.append(t) + pidy_p.append(float(d.get("P", math.nan))) + pidy_i.append(float(d.get("I", math.nan))) + pidy_d.append(float(d.get("D", math.nan))) + pidy_ff.append(float(d.get("FF", math.nan))) elif mt == "NAMED_VALUE_FLOAT": nm = d.get("name") if nm in nvf: @@ -87,9 +101,23 @@ def load(path: str) -> dict: "srv_t": np.array(srv_t), "srv_pwm": np.array(srv_pwm), "nvf": {k: np.array(v) for k, v in nvf.items()}, "nvf_t": {k: np.array(v) for k, v in nvf_t.items()}, + "rpm_t": np.array(rpm_t), "rpm1": np.array(rpm1), + "pidy_t": np.array(pidy_t), + "pidy_p": np.array(pidy_p), + "pidy_i": np.array(pidy_i), + "pidy_d": np.array(pidy_d), + "pidy_ff": np.array(pidy_ff), } +def _window_filter(t_ms: np.ndarray, t0_ms: float, window: "tuple[float, float] | None"): + """Boolean mask selecting samples with (t_ms - t0_ms)/1000 in [window[0], window[1]].""" + if window is None or len(t_ms) == 0: + return np.ones(len(t_ms), dtype=bool) + rel_s = (t_ms - t0_ms) / 1000.0 + return (rel_s >= window[0]) & (rel_s <= window[1]) + + # --------------------------------------------------------------------------- # Signal analysis # --------------------------------------------------------------------------- @@ -255,7 +283,7 @@ def correction_analysis(t_srv_ms, pwm, t_att_ms, rate): # Report # --------------------------------------------------------------------------- -def analyze(path: str, do_plot: bool = False) -> None: +def analyze(path: str, do_plot: bool = False, window: "tuple[float, float] | None" = None) -> None: D = load(path) att_t, yaw, rate = D["att_t"], D["att_yaw"], D["att_rate"] srv_t, pwm = D["srv_t"], D["srv_pwm"] @@ -267,6 +295,26 @@ def analyze(path: str, do_plot: bool = False) -> None: return t0 = min(att_t[0], srv_t[0]) + + if window is not None: + print(f" window = [{window[0]:.1f}, {window[1]:.1f}] s relative to first ATTITUDE/SERVO sample") + m_att = _window_filter(att_t, t0, window) + att_t, yaw, rate = att_t[m_att], yaw[m_att], rate[m_att] + m_srv = _window_filter(srv_t, t0, window) + srv_t, pwm = srv_t[m_srv], pwm[m_srv] + for k in nvf: + m = _window_filter(nvf_t[k], t0, window) + nvf_t[k], nvf[k] = nvf_t[k][m], nvf[k][m] + m_rpm = _window_filter(D["rpm_t"], t0, window) + D["rpm_t"], D["rpm1"] = D["rpm_t"][m_rpm], D["rpm1"][m_rpm] + m_pidy = _window_filter(D["pidy_t"], t0, window) + for k in ("pidy_t", "pidy_p", "pidy_i", "pidy_d", "pidy_ff"): + D[k] = D[k][m_pidy] + + if len(att_t) < 4 or len(srv_t) < 4: + print(" Not enough ATTITUDE/SERVO samples inside window to analyze.") + return + dur = (max(att_t[-1], srv_t[-1]) - t0) / 1000.0 print(f" duration = {dur:.1f} s ATTITUDE N={len(att_t)} " f"({len(att_t)/dur:.1f} Hz) SERVO N={len(srv_t)} ({len(srv_t)/dur:.1f} Hz)") @@ -341,6 +389,39 @@ def analyze(path: str, do_plot: bool = False) -> None: print(f" YFF_GZ (Lua psi_dot) : mean={_fmt(np.mean(gz),3)} " f"rms={_fmt(np.std(gz),3)} rad/s (ATTITUDE rms={_fmt(np.std(r_ac),3)})") + # -- Rotor speed (real ESC/DShot telemetry, RPM.rpm1) ------------------ + rpm_t, rpm1 = D["rpm_t"], D["rpm1"] + print("\n--- Yaw motor RPM (RPM.rpm1, real ESC/DShot telemetry) ---") + if len(rpm1) >= 2: + ts = (rpm_t - rpm_t[0]) / 1000.0 + growth = np.polyfit(ts, rpm1, 1)[0] + print(f" N={len(rpm1)} start={_fmt(rpm1[0],0)} rpm end={_fmt(rpm1[-1],0)} rpm " + f"mean={_fmt(np.mean(rpm1),0)} growth={_fmt(growth,2)} rpm/s") + if abs(growth) > 5.0: + print(f" -> rotor speed is {'RISING' if growth > 0 else 'FALLING'} " + f"over this window, not steady.") + else: + print(" not streamed (RPM message absent)") + + # -- ArduPilot yaw rate-PID contribution (PID_TUNING axis=3) ---------- + pidy_t, pidy_p, pidy_i, pidy_d, pidy_ff = ( + D["pidy_t"], D["pidy_p"], D["pidy_i"], D["pidy_d"], D["pidy_ff"]) + print("\n--- ArduPilot yaw rate-PID contribution (PID_TUNING axis=yaw) ---") + if len(pidy_t) >= 2: + print(f" N={len(pidy_t)} ({len(pidy_t)/max(dur,1e-6):.1f} Hz)") + for lab, arr in (("P", pidy_p), ("I", pidy_i), ("D", pidy_d), ("FF", pidy_ff)): + print(f" {lab:2s}: mean={_fmt(np.mean(arr),5)} std={_fmt(np.std(arr),5)} " + f"range=[{_fmt(np.min(arr),5)}, {_fmt(np.max(arr),5)}]") + trim_mean = np.mean(nvf["YFF_T"]) if len(nvf["YFF_T"]) else float("nan") + pi_mag = float(np.mean(np.abs(pidy_p)) + np.mean(np.abs(pidy_i))) + if not math.isnan(trim_mean) and trim_mean > 0: + print(f" AP P+I magnitude vs Lua trim (H_YAW_TRIM~{_fmt(trim_mean,3)}): " + f"{_fmt(pi_mag,5)} ({_fmt(100.0*pi_mag/trim_mean,1)}% of trim)") + print(" -> ArduPilot's own rate-PID assist is this small; it cannot arrest a") + print(" divergence that the Lua trim (H_YAW_TRIM, low-passed at YFF_TAU) misses.") + else: + print(" not streamed (PID_TUNING axis=yaw absent -- check GCS_PID_MASK)") + # -- Interpretation --------------------------------------------------- print("\n--- Interpretation ---") kd_now = kd[-1] if len(kd) else 0.0 @@ -387,10 +468,15 @@ def _plot(D, t0): def main(): ap = argparse.ArgumentParser(description="Analyze yaw oscillation + Lua trim interaction") - ap.add_argument("log", help="path to a .mavlink.jsonl calibrate log") + ap.add_argument("log", help="path to a .mavlink.jsonl calibrate/SITL log") ap.add_argument("--plot", action="store_true", help="show time-series plots") + ap.add_argument("--window", nargs=2, type=float, metavar=("START_S", "END_S"), + help="restrict analysis to [START_S, END_S] relative to the " + "first ATTITUDE/SERVO sample in the file (e.g. to zoom " + "into the post-release divergence window)") args = ap.parse_args() - analyze(args.log, do_plot=args.plot) + window = tuple(args.window) if args.window else None + analyze(args.log, do_plot=args.plot, window=window) if __name__ == "__main__": diff --git a/simulation/analysis/arduloop_pid_replay.py b/simulation/analysis/arduloop_pid_replay.py new file mode 100644 index 0000000..92f4e91 --- /dev/null +++ b/simulation/analysis/arduloop_pid_replay.py @@ -0,0 +1,203 @@ +"""arduloop_pid_replay.py -- replay real SITL PID_TUNING through arduloop's AC_PID port. + +Purpose +------- +Isolate whether arduloop's Python port of ArduPilot's rate-PID (`arduloop.pid.AC_PID`) +computes the SAME P/I/D/FF contributions as the real firmware, GIVEN THE EXACT SAME +target/measurement stream the real SITL saw. This is different from +`compare_pumping_sitl_simtest.py`, which diffs two INDEPENDENT simulations (real SITL +physics vs mock-Python physics) that inevitably drift apart over time for reasons +unrelated to the controller math itself (different physics, EKF, timing). + +Here there is no independent simulation and no time-alignment ambiguity: for each +real `PID_TUNING` sample (per axis) logged by the SITL run, we feed the REAL +`desired` (target) and `achieved` (measurement) straight into a same-gain +`arduloop.pid.AC_PID` instance and record what our port would have computed. +Any divergence from the real `P`/`I`/`D`/`FF` values is a genuine modeling gap in +arduloop's port (or a params/gain mismatch), not a physics/EKF difference. + +Usage: + python simulation/analysis/arduloop_pid_replay.py [--window START_S END_S] + +Example: + python simulation/analysis/arduloop_pid_replay.py simulation/logs/test_lua_flight_steady_sitl \\ + --window 60 75 +""" +from __future__ import annotations + +import argparse +import json +import math +import sys +from pathlib import Path + +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from arduloop.params import RateAxisParams +from arduloop.pid import AC_PID + +AXIS_NAMES = {1: "roll", 2: "pitch", 3: "yaw"} +AXIS_TO_AP = {1: "RLL", 2: "PIT", 3: "YAW"} + + +def _load_pid_tuning(mavlink_jsonl: Path) -> dict[int, list[dict]]: + """Return {axis: [rows sorted by time_boot_ms]} for PID_TUNING messages.""" + rows: dict[int, list[dict]] = {1: [], 2: [], 3: []} + with mavlink_jsonl.open(encoding="utf-8") as f: + for line in f: + line = line.strip() + if not line: + continue + try: + d = json.loads(line) + except Exception: + continue + if d.get("_dir") != "rx" or d.get("mavpackettype") != "PID_TUNING": + continue + axis = int(d.get("axis", -1)) + if axis not in rows: + continue + if d.get("time_boot_ms") is None or d.get("_t_wall") is None: + continue + rows[axis].append(d) + for axis in rows: + rows[axis].sort(key=lambda d: d["time_boot_ms"]) + return rows + + +def _startup_run_id(log_dir: Path) -> float | None: + """Wall-clock epoch at t_sim=0, from events.jsonl's `startup` event (if present).""" + events = log_dir / "events.jsonl" + if not events.exists(): + return None + with events.open(encoding="utf-8") as f: + for line in f: + try: + d = json.loads(line) + except Exception: + continue + if d.get("event") == "startup" and "run_id" in d: + return float(d["run_id"]) + return None + + +def replay_axis(axis: int, real_rows: list[dict], params: dict[str, float], + run_id: float | None) -> dict: + """Feed real desired/achieved through arduloop's AC_PID; return arrays for diffing.""" + ap_axis = AXIS_TO_AP[axis] + rp = RateAxisParams.from_ap_dict(params, ap_axis) + pid = AC_PID(rp, sample_hz=400.0) + + n = len(real_rows) + t_rel = np.zeros(n) # t_sim [s] if run_id known, else boot_ms/1000 relative to first sample + ard_P = np.zeros(n) + ard_I = np.zeros(n) + ard_D = np.zeros(n) + ard_FF = np.zeros(n) + real_P = np.zeros(n) + real_I = np.zeros(n) + real_D = np.zeros(n) + real_FF = np.zeros(n) + + t0_ms = real_rows[0]["time_boot_ms"] + t_prev = None + for i, d in enumerate(real_rows): + t_ms = d["time_boot_ms"] + if run_id is not None: + t_rel[i] = float(d["_t_wall"]) - run_id + else: + t_rel[i] = (t_ms - t0_ms) / 1000.0 + dt = 0.0 if t_prev is None else max(0.0, (t_ms - t_prev) / 1000.0) + t_prev = t_ms + + target = float(d.get("desired", math.nan)) + measurement = float(d.get("achieved", math.nan)) + + if i == 0: + pid.reset(target=target, measurement=measurement) + else: + pid.update_all(target=target, measurement=measurement, dt=dt, + sim_time_s=t_ms / 1000.0) + + ard_P[i] = pid.debug.P + ard_I[i] = pid.debug.I + ard_D[i] = pid.debug.D + ard_FF[i] = pid.debug.FF + real_P[i] = float(d.get("P", math.nan)) + real_I[i] = float(d.get("I", math.nan)) + real_D[i] = float(d.get("D", math.nan)) + real_FF[i] = float(d.get("FF", math.nan)) + + return dict(t_rel=t_rel, ard_P=ard_P, ard_I=ard_I, ard_D=ard_D, ard_FF=ard_FF, + real_P=real_P, real_I=real_I, real_D=real_D, real_FF=real_FF) + + +def _window_mask(t_rel: np.ndarray, window: tuple[float, float] | None) -> np.ndarray: + if window is None or len(t_rel) == 0: + return np.ones(len(t_rel), dtype=bool) + return (t_rel >= window[0]) & (t_rel <= window[1]) + + +def report(axis: int, res: dict, window: tuple[float, float] | None) -> None: + mask = _window_mask(res["t_rel"], window) + n = int(mask.sum()) + name = AXIS_NAMES[axis] + print(f"\n=== axis={axis} ({name}) : n={n} samples" + + (f" window=[{window[0]:.1f},{window[1]:.1f}]s" if window else "") + " ===") + if n == 0: + print(" (no samples in window)") + return + + for term in ("P", "I", "D", "FF"): + ard = res[f"ard_{term}"][mask] + real = res[f"real_{term}"][mask] + diff = ard - real + mean_abs = float(np.nanmean(np.abs(diff))) + max_abs = float(np.nanmax(np.abs(diff))) if n else float("nan") + idx = int(np.nanargmax(np.abs(diff))) if n else -1 + t_at_max = float(res["t_rel"][mask][idx]) if idx >= 0 else float("nan") + print(f" {term:>2} mean|diff|={mean_abs:.6f} max|diff|={max_abs:.6f}" + f" (at t_rel={t_at_max:.2f}s: arduloop={ard[idx]:.6f} real={real[idx]:.6f})") + + +def main() -> None: + ap = argparse.ArgumentParser(description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("log_dir", help="SITL test log dir, e.g. simulation/logs/test_lua_flight_steady_sitl") + ap.add_argument("--window", nargs=2, type=float, metavar=("START_S", "END_S"), + help="restrict to t_rel in [START_S, END_S] (relative to first PID_TUNING sample)") + args = ap.parse_args() + + log_dir = Path(args.log_dir) + mavlink_jsonl = log_dir / "mavlink.jsonl" + params_json = log_dir / "params.json" + if not mavlink_jsonl.exists(): + raise SystemExit(f"missing {mavlink_jsonl}") + if not params_json.exists(): + raise SystemExit(f"missing {params_json}") + + params = json.loads(params_json.read_text()) + pid_rows = _load_pid_tuning(mavlink_jsonl) + run_id = _startup_run_id(log_dir) + + window = tuple(args.window) if args.window else None + + print(f"arduloop PID replay vs real SITL PID_TUNING : {log_dir}") + print(f"Time base: {'t_sim (run_id-aligned)' if run_id is not None else 'boot_ms relative to first PID_TUNING sample (no run_id found)'}") + print(f"Gains: ATC_RAT_RLL_P={params.get('ATC_RAT_RLL_P')} " + f"ATC_RAT_PIT_P={params.get('ATC_RAT_PIT_P')} " + f"ATC_RAT_YAW_P={params.get('ATC_RAT_YAW_P')}") + + for axis in (1, 2, 3): + rows = pid_rows[axis] + if not rows: + print(f"\n=== axis={axis} ({AXIS_NAMES[axis]}) : no PID_TUNING samples ===") + continue + res = replay_axis(axis, rows, params, run_id) + report(axis, res, window) + + +if __name__ == "__main__": + main() diff --git a/simulation/analysis/compare_pumping_sitl_simtest.py b/simulation/analysis/compare_pumping_sitl_simtest.py index 04a2f15..f577135 100644 --- a/simulation/analysis/compare_pumping_sitl_simtest.py +++ b/simulation/analysis/compare_pumping_sitl_simtest.py @@ -50,10 +50,16 @@ "rate_yaw_i_contrib", "rate_yaw_d_contrib", "rate_yaw_ff_contrib", + "rate_roll_pdmod", + "rate_roll_srate", + "rate_pitch_pdmod", + "rate_pitch_srate", + "rate_yaw_pdmod", + "rate_yaw_srate", "lua_ol_alt_p_contrib", "lua_ol_alt_i_contrib", "lua_ol_alt_d_contrib", - "lua_ol_col_cmd_rad", + "lua_ol_thrust_cmd", ] PARAM_PREFIX_FOCUS = ( diff --git a/simulation/analysis/compare_runs.py b/simulation/analysis/compare_runs.py index c1c4d88..7fc5e24 100644 --- a/simulation/analysis/compare_runs.py +++ b/simulation/analysis/compare_runs.py @@ -56,6 +56,8 @@ _ORB_DIFF_WARN = 5.0 # m _YAW_DIFF_WARN = 15.0 # deg _COL_DIFF_WARN = 0.02 # rad +_REACTION_PRE_S = 1.0 +_REACTION_HORIZON_S = 3.0 # --------------------------------------------------------------------------- @@ -88,6 +90,21 @@ def _mean(vals: list[float]) -> Optional[float]: return sum(vals) / len(vals) if vals else None +def _mean_finite(vals: list[float]) -> Optional[float]: + good = [v for v in vals if math.isfinite(v)] + return (sum(good) / len(good)) if good else None + + +def _sgn(v: Optional[float], eps: float = 1e-6) -> int: + if v is None or not math.isfinite(v): + return 0 + if v > eps: + return 1 + if v < -eps: + return -1 + return 0 + + def _stats(rows: list[TelRow]) -> dict: """Compute mean key metrics for a list of rows.""" if not rows: @@ -121,12 +138,313 @@ def _diff_flag(a: Optional[float], b: Optional[float], return f"{d:+{fmt}}{flag}" +def _bucket_control(rows: list[TelRow], t_ref: float, window_s: float) -> dict[int, dict]: + """Bucket rate-loop diagnostics after t_ref. + + Returns per-bucket means for error and PID contributions on each axis. + """ + out: dict[int, dict] = {} + by_key: dict[int, list[TelRow]] = {} + for r in rows: + if r.damp_alpha != 0.0: + continue + dt = r.t_sim - t_ref + if dt < 0.0: + continue + key = int(dt // window_s) + by_key.setdefault(key, []).append(r) + + axes = [ + ("roll", "rate_roll_p_contrib", "rate_roll_i_contrib", "rate_roll_d_contrib", "rate_roll_ff_contrib"), + ("pitch", "rate_pitch_p_contrib", "rate_pitch_i_contrib", "rate_pitch_d_contrib", "rate_pitch_ff_contrib"), + ("yaw", "rate_yaw_p_contrib", "rate_yaw_i_contrib", "rate_yaw_d_contrib", "rate_yaw_ff_contrib"), + ] + + def _axis_error(r: TelRow, axis: str) -> float: + if axis == "roll": + return float(r.roll_sp_rads - r.omega_x) + if axis == "pitch": + return float(r.pitch_sp_rads - r.omega_y) + return float(r.yaw_sp_rads - r.omega_z) + + for key, group in by_key.items(): + d: dict = {"n": len(group)} + for name, p_f, i_f, d_f, ff_f in axes: + err = _mean_finite([_axis_error(r, name) for r in group]) + p = _mean_finite([getattr(r, p_f) for r in group]) + i = _mean_finite([getattr(r, i_f) for r in group]) + dd = _mean_finite([getattr(r, d_f) for r in group]) + ff = _mean_finite([getattr(r, ff_f) for r in group]) + total = None + parts = [x for x in [p, i, dd, ff] if x is not None and math.isfinite(x)] + if parts: + total = sum(parts) + d[name] = { + "err": err, + "p": p, + "i": i, + "d": dd, + "ff": ff, + "total": total, + } + out[key] = d + return out + + +def _print_pid_diagnostics( + rows_a: list[TelRow], rows_b: list[TelRow], + label_a: str, label_b: str, + t_kin_a: float, t_kin_b: float, + window_s: float, +) -> None: + """Print whether both runs are correcting the same rate error with similar commands.""" + ca = _bucket_control(rows_a, t_kin_a, window_s) + cb = _bucket_control(rows_b, t_kin_b, window_s) + common = sorted(set(ca) & set(cb)) + if not common: + print("\nPID diagnostics: no overlapping free-flight control buckets.") + return + + print("\nPID diagnostics (rate loop) -- are both runs correcting the same error?") + print(f" Bucket size: {window_s:.1f}s, overlapping buckets: {len(common)}") + + axes = ["roll", "pitch", "yaw"] + for axis in axes: + err_sign_match = 0 + cmd_sign_match = 0 + both_correct = 0 + valid_err = 0 + valid_cmd = 0 + valid_correct = 0 + + disagree_rows: list[tuple[float, dict, dict]] = [] + abs_means_a = {"p": [], "i": [], "d": [], "ff": []} + abs_means_b = {"p": [], "i": [], "d": [], "ff": []} + + for key in common: + a = ca[key][axis] + b = cb[key][axis] + t_rel = key * window_s + + for term in ["p", "i", "d", "ff"]: + va = a.get(term) + vb = b.get(term) + if va is not None and math.isfinite(va): + abs_means_a[term].append(abs(va)) + if vb is not None and math.isfinite(vb): + abs_means_b[term].append(abs(vb)) + + se_a = _sgn(a.get("err")) + se_b = _sgn(b.get("err")) + if se_a != 0 and se_b != 0: + valid_err += 1 + if se_a == se_b: + err_sign_match += 1 + + sc_a = _sgn(a.get("total")) + sc_b = _sgn(b.get("total")) + if sc_a != 0 and sc_b != 0: + valid_cmd += 1 + if sc_a == sc_b: + cmd_sign_match += 1 + + corr_a = (se_a != 0 and sc_a != 0 and se_a == sc_a) + corr_b = (se_b != 0 and sc_b != 0 and se_b == sc_b) + if (se_a != 0 and sc_a != 0 and se_b != 0 and sc_b != 0): + valid_correct += 1 + if corr_a and corr_b: + both_correct += 1 + if (se_a != se_b) or (sc_a != sc_b) or (corr_a != corr_b): + disagree_rows.append((t_rel, a, b)) + + def _pct(num: int, den: int) -> float: + return (100.0 * num / den) if den else float("nan") + + dom_a = max(abs_means_a.items(), key=lambda kv: _mean(kv[1]) or -1.0)[0] + dom_b = max(abs_means_b.items(), key=lambda kv: _mean(kv[1]) or -1.0)[0] + + print(f"\nAxis: {axis}") + print( + f" Error sign match ({label_a} vs {label_b}): " + f"{err_sign_match}/{valid_err} ({_pct(err_sign_match, valid_err):.1f}%)" + ) + print( + f" Command sign match ({label_a} vs {label_b}): " + f"{cmd_sign_match}/{valid_cmd} ({_pct(cmd_sign_match, valid_cmd):.1f}%)" + ) + print( + f" Both correcting own error (sign(cmd)=sign(err)): " + f"{both_correct}/{valid_correct} ({_pct(both_correct, valid_correct):.1f}%)" + ) + print( + f" Dominant mean |term|: {label_a}={dom_a.upper()} {label_b}={dom_b.upper()}" + ) + + if disagree_rows: + print(" First disagreement buckets (t_rel s):") + for t_rel, a, b in disagree_rows[:5]: + print( + f" t={t_rel:6.1f} " + f"err(A,B)=({a.get('err', float('nan')):+.3f},{b.get('err', float('nan')):+.3f}) " + f"cmd(A,B)=({a.get('total', float('nan')):+.3f},{b.get('total', float('nan')):+.3f})" + ) + + print("\nNote: 'correcting own error' uses sign(cmd_total)=sign(rate_error).") + + +def _rows_in_rel_window(rows: list[TelRow], t_kin: float, t0: float, t1: float) -> list[TelRow]: + return [r for r in rows if r.damp_alpha == 0.0 and t0 <= (r.t_sim - t_kin) < t1] + + +def _mean_attr(rows: list[TelRow], attr: str) -> Optional[float]: + vals = [getattr(r, attr) for r in rows] + return _mean_finite(vals) + + +def _dominant_pid_term(rows: list[TelRow], axis: str) -> str: + if axis == "roll": + fields = { + "P": "rate_roll_p_contrib", + "I": "rate_roll_i_contrib", + "D": "rate_roll_d_contrib", + "FF": "rate_roll_ff_contrib", + } + elif axis == "pitch": + fields = { + "P": "rate_pitch_p_contrib", + "I": "rate_pitch_i_contrib", + "D": "rate_pitch_d_contrib", + "FF": "rate_pitch_ff_contrib", + } + else: + fields = { + "P": "rate_yaw_p_contrib", + "I": "rate_yaw_i_contrib", + "D": "rate_yaw_d_contrib", + "FF": "rate_yaw_ff_contrib", + } + scored: list[tuple[str, float]] = [] + for name, field in fields.items(): + v = _mean_finite([abs(getattr(r, field)) for r in rows]) + scored.append((name, v if v is not None else -1.0)) + scored.sort(key=lambda x: x[1], reverse=True) + return scored[0][0] + + +def _axis_err_cmd(rows: list[TelRow], axis: str) -> tuple[Optional[float], Optional[float]]: + if axis == "roll": + e = _mean_finite([r.roll_sp_rads - r.omega_x for r in rows]) + p = _mean_attr(rows, "rate_roll_p_contrib") + i = _mean_attr(rows, "rate_roll_i_contrib") + d = _mean_attr(rows, "rate_roll_d_contrib") + ff = _mean_attr(rows, "rate_roll_ff_contrib") + elif axis == "pitch": + e = _mean_finite([r.pitch_sp_rads - r.omega_y for r in rows]) + p = _mean_attr(rows, "rate_pitch_p_contrib") + i = _mean_attr(rows, "rate_pitch_i_contrib") + d = _mean_attr(rows, "rate_pitch_d_contrib") + ff = _mean_attr(rows, "rate_pitch_ff_contrib") + else: + e = _mean_finite([r.yaw_sp_rads - r.omega_z for r in rows]) + p = _mean_attr(rows, "rate_yaw_p_contrib") + i = _mean_attr(rows, "rate_yaw_i_contrib") + d = _mean_attr(rows, "rate_yaw_d_contrib") + ff = _mean_attr(rows, "rate_yaw_ff_contrib") + parts = [x for x in [p, i, d, ff] if x is not None and math.isfinite(x)] + cmd = sum(parts) if parts else None + return e, cmd + + +def _print_first_divergence_reactions( + rows_a: list[TelRow], rows_b: list[TelRow], + label_a: str, label_b: str, + t_kin_a: float, t_kin_b: float, + first_diverge: Optional[float], + reaction_pre_s: float, + reaction_horizon_s: float, +) -> None: + if first_diverge is None: + print("\nFirst-divergence reactions: not available (no divergence found).") + return + + t0 = first_diverge + pre0 = max(0.0, t0 - reaction_pre_s) + post1 = t0 + reaction_horizon_s + + a_pre = _rows_in_rel_window(rows_a, t_kin_a, pre0, t0) + b_pre = _rows_in_rel_window(rows_b, t_kin_b, pre0, t0) + a_post = _rows_in_rel_window(rows_a, t_kin_a, t0, post1) + b_post = _rows_in_rel_window(rows_b, t_kin_b, t0, post1) + + print("\nFirst-divergence reactions (pre-noise window)") + print(f" First divergence t_rel: {t0:.2f}s") + print(f" Window: pre=[{pre0:.2f},{t0:.2f}) post=[{t0:.2f},{post1:.2f})") + print(f" Samples: {label_a} pre={len(a_pre)} post={len(a_post)}; {label_b} pre={len(b_pre)} post={len(b_post)}") + + def _safe_delta(post: Optional[float], pre: Optional[float]) -> Optional[float]: + if post is None or pre is None: + return None + return post - pre + + def _fmt(v: Optional[float], fmt: str = "+.3f") -> str: + if v is None or not math.isfinite(v): + return " n/a" + return format(v, fmt) + + # Command-level reactions likely linked to tension divergence. + def _cyc_mag(rows: list[TelRow]) -> Optional[float]: + vals = [math.hypot(r.roll_sp_rads, r.pitch_sp_rads) for r in rows] + return _mean_finite(vals) + + a_col_pre = _mean_attr(a_pre, "collective_rad") + a_col_post = _mean_attr(a_post, "collective_rad") + b_col_pre = _mean_attr(b_pre, "collective_rad") + b_col_post = _mean_attr(b_post, "collective_rad") + + a_cyc_pre = _cyc_mag(a_pre) + a_cyc_post = _cyc_mag(a_post) + b_cyc_pre = _cyc_mag(b_pre) + b_cyc_post = _cyc_mag(b_post) + + a_ten_pre = _mean_attr(a_pre, "tether_tension") + a_ten_post = _mean_attr(a_post, "tether_tension") + b_ten_pre = _mean_attr(b_pre, "tether_tension") + b_ten_post = _mean_attr(b_post, "tether_tension") + + print("\n Command/tension reaction summary (post - pre):") + print(f" collective_rad {label_a}: {_fmt(_safe_delta(a_col_post, a_col_pre))} {label_b}: {_fmt(_safe_delta(b_col_post, b_col_pre))}") + print(f" cyclic_sp_mag {label_a}: {_fmt(_safe_delta(a_cyc_post, a_cyc_pre))} {label_b}: {_fmt(_safe_delta(b_cyc_post, b_cyc_pre))}") + print(f" tether_tension {label_a}: {_fmt(_safe_delta(a_ten_post, a_ten_pre), '+.1f')} {label_b}: {_fmt(_safe_delta(b_ten_post, b_ten_pre), '+.1f')}") + + # Axis-wise: are both runs correcting the same error in this early reaction window? + print("\n PID reaction at first divergence (post window means):") + for axis in ["roll", "pitch", "yaw"]: + e_a, c_a = _axis_err_cmd(a_post, axis) + e_b, c_b = _axis_err_cmd(b_post, axis) + corr_a = (_sgn(e_a) != 0 and _sgn(c_a) != 0 and _sgn(e_a) == _sgn(c_a)) + corr_b = (_sgn(e_b) != 0 and _sgn(c_b) != 0 and _sgn(e_b) == _sgn(c_b)) + same_err = (_sgn(e_a) != 0 and _sgn(e_b) != 0 and _sgn(e_a) == _sgn(e_b)) + same_cmd = (_sgn(c_a) != 0 and _sgn(c_b) != 0 and _sgn(c_a) == _sgn(c_b)) + dom_a = _dominant_pid_term(a_post, axis) if a_post else "n/a" + dom_b = _dominant_pid_term(b_post, axis) if b_post else "n/a" + print( + f" {axis:>5}: " + f"err(A,B)=({_fmt(e_a)},{_fmt(e_b)}) " + f"cmd(A,B)=({_fmt(c_a)},{_fmt(c_b)}) " + f"same_err={same_err} same_cmd={same_cmd} " + f"correct(A,B)=({corr_a},{corr_b}) " + f"dom(A,B)=({dom_a},{dom_b})" + ) + + # --------------------------------------------------------------------------- # Main report # --------------------------------------------------------------------------- def compare(name_a: str, name_b: str, window_s: float = 5.0, - do_plot: bool = False) -> None: + do_plot: bool = False, + reaction_pre_s: float = _REACTION_PRE_S, + reaction_horizon_s: float = _REACTION_HORIZON_S) -> None: dir_a = _LOG_DIR / name_a dir_b = _LOG_DIR / name_b @@ -250,6 +568,15 @@ def _run_summary(rows: list[TelRow], label: str, t_kin: float) -> None: _run_summary(rows_a, f"Run A [{name_a}]", t_kin_a) _run_summary(rows_b, f"Run B [{name_b}]", t_kin_b) + _print_pid_diagnostics(rows_a, rows_b, f"A[{name_a}]", f"B[{name_b}]", t_kin_a, t_kin_b, window_s) + _print_first_divergence_reactions( + rows_a, rows_b, + f"A[{name_a}]", f"B[{name_b}]", + t_kin_a, t_kin_b, + first_diverge, + reaction_pre_s, + reaction_horizon_s, + ) # ── Optional plot ────────────────────────────────────────────────────────── if do_plot: @@ -353,6 +680,17 @@ def _get(rows: list[TelRow], attr: str) -> list[float]: parser.add_argument("run_b", help="Second test log directory name (in simulation/logs/)") parser.add_argument("--window", type=float, default=5.0, help="Averaging window in seconds (default: 5.0)") + parser.add_argument("--reaction-pre", type=float, default=_REACTION_PRE_S, + help="Seconds before first divergence for reaction baseline (default: 1.0)") + parser.add_argument("--reaction-horizon", type=float, default=_REACTION_HORIZON_S, + help="Seconds after first divergence to analyze reactions (default: 3.0)") parser.add_argument("--plot", action="store_true", help="Save comparison plot") args = parser.parse_args() - compare(args.run_a, args.run_b, window_s=args.window, do_plot=args.plot) + compare( + args.run_a, + args.run_b, + window_s=args.window, + do_plot=args.plot, + reaction_pre_s=args.reaction_pre, + reaction_horizon_s=args.reaction_horizon, + ) diff --git a/simulation/arduloop/README.md b/simulation/arduloop/README.md index 8f9cf01..697e610 100644 --- a/simulation/arduloop/README.md +++ b/simulation/arduloop/README.md @@ -225,11 +225,18 @@ out = ctrl.update( rate_target_rads=(roll_rate, pitch_rate, yaw_rate), gyro_rate_rads=(gr, gp, gy), dt=dt, - collective_norm=0.5, - saturated=(False, False, False)) + collective_norm=0.5) # out.roll_cyclic, out.pitch_cyclic, out.yaw_cmd ``` +Anti-windup saturation flags (AP `_motors.limit.roll/pitch/yaw`) are computed +internally each tick from the combined roll/pitch cyclic magnitude vs +`H_CYC_MAX` (and yaw vs the output limit), then fed into the NEXT tick's +`update_all(limit=...)` — mirroring AP's one-cycle-delayed feedback from +`AP_MotorsHeli_Single::move_actuators`. There is no external `saturated` +parameter to pass in. + + Signal path per axis (`pid.py` / `AC_PID::update_all`): ``` diff --git a/simulation/telemetry_columns.py b/simulation/telemetry_columns.py index 79d9a11..8e17a9c 100644 --- a/simulation/telemetry_columns.py +++ b/simulation/telemetry_columns.py @@ -127,6 +127,15 @@ "rate_yaw_i_contrib", "rate_yaw_d_contrib", "rate_yaw_ff_contrib", + # PD-sum derating factor (1.0=no slew-rate limiting) and the + # slew-limiter's tracked output slew rate, mirroring MAVLink + # PID_TUNING's PDmod/SRate fields for direct SITL comparison. + "rate_roll_pdmod", + "rate_roll_srate", + "rate_pitch_pdmod", + "rate_pitch_srate", + "rate_yaw_pdmod", + "rate_yaw_srate", ), ), # Net force/moment assembled in PhysicsCore before state integration. @@ -311,4 +320,25 @@ "mav_nvf_yff_trim", "mav_nvf_yff_u", "mav_nvf_yff_gz", + "ekf_pos_x", + "ekf_pos_y", + "ekf_pos_z", + "rate_roll_p_contrib", + "rate_roll_i_contrib", + "rate_roll_d_contrib", + "rate_roll_ff_contrib", + "rate_pitch_p_contrib", + "rate_pitch_i_contrib", + "rate_pitch_d_contrib", + "rate_pitch_ff_contrib", + "rate_yaw_p_contrib", + "rate_yaw_i_contrib", + "rate_yaw_d_contrib", + "rate_yaw_ff_contrib", + "rate_roll_pdmod", + "rate_roll_srate", + "rate_pitch_pdmod", + "rate_pitch_srate", + "rate_yaw_pdmod", + "rate_yaw_srate", ) diff --git a/simulation/telemetry_csv.py b/simulation/telemetry_csv.py index c0d5472..98c8040 100644 --- a/simulation/telemetry_csv.py +++ b/simulation/telemetry_csv.py @@ -223,6 +223,12 @@ class TelRow: rate_yaw_i_contrib: float = float("nan") rate_yaw_d_contrib: float = float("nan") rate_yaw_ff_contrib: float = float("nan") + rate_roll_pdmod: float = float("nan") + rate_roll_srate: float = float("nan") + rate_pitch_pdmod: float = float("nan") + rate_pitch_srate: float = float("nan") + rate_yaw_pdmod: float = float("nan") + rate_yaw_srate: float = float("nan") vel_radial_mps: float = 0.0 # actual hub radial velocity (outward +) [m/s] orbit_radius_m: float = 0.0 # horizontal radius from anchor [m] orbit_azimuth_rad: float = 0.0 # atan2(E, N) horizontal azimuth [rad] @@ -463,6 +469,9 @@ def from_physics( roll_sp_rads: float = 0.0, pitch_sp_rads: float = 0.0, yaw_sp_rads: float = 0.0, + roll_rate_err_rads: float = float("nan"), + pitch_rate_err_rads: float = float("nan"), + yaw_rate_err_rads: float = float("nan"), rate_roll_p_contrib: float = float("nan"), rate_roll_i_contrib: float = float("nan"), rate_roll_d_contrib: float = float("nan"), @@ -475,6 +484,12 @@ def from_physics( rate_yaw_i_contrib: float = float("nan"), rate_yaw_d_contrib: float = float("nan"), rate_yaw_ff_contrib: float = float("nan"), + rate_roll_pdmod: float = float("nan"), + rate_roll_srate: float = float("nan"), + rate_pitch_pdmod: float = float("nan"), + rate_pitch_srate: float = float("nan"), + rate_yaw_pdmod: float = float("nan"), + rate_yaw_srate: float = float("nan"), vel_radial_mps: float = 0.0, net_moment=None, mav_att_roll_deg: float = float("nan"), @@ -584,6 +599,7 @@ def from_physics( pos_h = pos[:2] vel_h = vel[:2] + v_horiz = float(np.linalg.norm(vel_h)) orbit_radius = float(np.linalg.norm(pos_h)) if orbit_radius > 1e-9: er_h = pos_h / orbit_radius @@ -625,6 +641,16 @@ def from_physics( body_z_target_az = 0.0 body_z_az_gap = 0.0 + vel_heading_deg = float(np.degrees(np.arctan2(vel[1], vel[0]))) + heading_gap_deg = float((np.degrees(yaw) - vel_heading_deg + 180.0) % 360.0 - 180.0) + + if not math.isfinite(roll_rate_err_rads): + roll_rate_err_rads = float(roll_sp_rads - om[0]) + if not math.isfinite(pitch_rate_err_rads): + pitch_rate_err_rads = float(pitch_sp_rads - om[1]) + if not math.isfinite(yaw_rate_err_rads): + yaw_rate_err_rads = float(yaw_sp_rads - om[2]) + return cls( t_sim = float(t_sim), sitl_time = float(t_sim), @@ -681,6 +707,16 @@ def from_physics( sens_accel_x = float(acc_body[0]), sens_accel_y = float(acc_body[1]), sens_accel_z = float(acc_body[2]), + orb_yaw_rad = yaw, + v_horiz_ms = v_horiz, + sens_vel_n = float(vel[0]), + sens_vel_e = float(vel[1]), + sens_vel_d = float(vel[2]), + sens_gyro_x = float(om[0]), + sens_gyro_y = float(om[1]), + sens_gyro_z = float(om[2]), + vel_heading_deg = vel_heading_deg, + heading_gap_deg = heading_gap_deg, rpy_roll = roll, rpy_pitch = pitch, rpy_yaw = yaw, @@ -703,9 +739,9 @@ def from_physics( roll_sp_rads = float(roll_sp_rads), pitch_sp_rads = float(pitch_sp_rads), yaw_sp_rads = float(yaw_sp_rads), - roll_rate_err_rads = float(roll_sp_rads - om[0]), - pitch_rate_err_rads = float(pitch_sp_rads - om[1]), - yaw_rate_err_rads = float(yaw_sp_rads - om[2]), + roll_rate_err_rads = float(roll_rate_err_rads), + pitch_rate_err_rads = float(pitch_rate_err_rads), + yaw_rate_err_rads = float(yaw_rate_err_rads), rate_roll_p_contrib = float(rate_roll_p_contrib), rate_roll_i_contrib = float(rate_roll_i_contrib), rate_roll_d_contrib = float(rate_roll_d_contrib), @@ -718,6 +754,12 @@ def from_physics( rate_yaw_i_contrib = float(rate_yaw_i_contrib), rate_yaw_d_contrib = float(rate_yaw_d_contrib), rate_yaw_ff_contrib = float(rate_yaw_ff_contrib), + rate_roll_pdmod = float(rate_roll_pdmod), + rate_roll_srate = float(rate_roll_srate), + rate_pitch_pdmod = float(rate_pitch_pdmod), + rate_pitch_srate = float(rate_pitch_srate), + rate_yaw_pdmod = float(rate_yaw_pdmod), + rate_yaw_srate = float(rate_yaw_srate), vel_radial_mps = float(vel_radial_mps if vel_radial_mps != 0.0 else vel_radial_auto), orbit_radius_m = orbit_radius, orbit_azimuth_rad = orbit_azimuth,