From 239d33bf643f61bb1c2367e4b9918d3907723806 Mon Sep 17 00:00:00 2001 From: Kristof Roomp Date: Fri, 17 Jul 2026 12:30:24 +0200 Subject: [PATCH 1/3] Fix arduloop anti-windup fidelity gap vs real ArduPilot - arduloop's rate-PID anti-windup 'saturated' gating was architecturally present but never actually populated by any caller (always False), silently disabling anti-windup across all axes in simtests. - HeliRateController now self-computes per-axis saturation each tick, mirroring AP_MotorsHeli_Single::move_actuators: combined roll/pitch cyclic magnitude vs new HeliParams.CYC_MAX_cd (from H_CYC_MAX param, default 2500 centi-degrees), plus yaw output-limit saturation. Stored as self._prev_saturated and fed into the NEXT tick's update_all(limit=...) calls, replicating AP's one-cycle-delayed anti-windup feedback. - Removed the now-dead external 'saturated' parameter from HeliRateController.update() and GuidedAttitudeController.update() (no caller ever supplied it; clean cutover per repo convention). - Fixed a bug in AC_PID debug recording (d.SRate referenced a nonexistent self._output_slew_rate; now uses self._slew_limiter.get_slew_rate()). - Added simulation/analysis/arduloop_pid_replay.py: replays real SITL PID_TUNING desired/achieved samples through arduloop's own AC_PID to isolate controller-math divergence from physics/EKF drift. Confirmed P/D/FF terms already matched real AP almost exactly; I-term diverged (pitch especially) due to the anti-windup gap fixed here. - Extended telemetry schema (telemetry_columns.py, telemetry_csv.py, mock_ardupilot.py, mediator.py) and compare_pumping_sitl_simtest.py with PDmod/SRate columns end-to-end to support this diagnosis. Validated: full unit suite (467 passed), targeted arduloop/guided/heli/pid tests (55 passed). Re-ran SITL test_lua_flight_steady_sitl after the fix: still fails on max_tension (1202N vs 1000N limit), confirming this fix does not touch the real firmware's tension-spike behavior (that remains a separate, open issue) -- as expected since arduloop is simtest-only. --- .claude/commands/pytest.md | 63 ------ AGENTS.md | 37 ++++ design/flight_stack.md | 24 ++- simulation/analysis/analyze_ang_error.py | 10 +- .../analysis/analyze_yaw_oscillation.py | 92 +++++++- simulation/analysis/arduloop_pid_replay.py | 203 ++++++++++++++++++ .../analysis/compare_pumping_sitl_simtest.py | 6 + simulation/arduloop/README.md | 11 +- simulation/arduloop/attitude_heli.py | 31 ++- simulation/arduloop/guided.py | 4 - simulation/arduloop/params.py | 7 + simulation/arduloop/pid.py | 6 + simulation/config.py | 6 + simulation/mediator.py | 65 ++++-- simulation/physics_core.py | 25 --- simulation/scripts/rawes.lua | 30 ++- simulation/telemetry_columns.py | 27 +++ simulation/telemetry_csv.py | 18 ++ simulation/tests/common/mock_ardupilot.py | 70 +++++- simulation/tests/sitl/flight/conftest.py | 14 ++ .../tests/sitl/rawes_common_defaults.parm | 11 +- .../tests/sitl/rawes_sitl_defaults.parm | 6 + simulation/tests/sitl/stack_infra.py | 13 +- 23 files changed, 634 insertions(+), 145 deletions(-) delete mode 100644 .claude/commands/pytest.md create mode 100644 simulation/analysis/arduloop_pid_replay.py diff --git a/.claude/commands/pytest.md b/.claude/commands/pytest.md deleted file mode 100644 index b3accdf..0000000 --- a/.claude/commands/pytest.md +++ /dev/null @@ -1,63 +0,0 @@ -Run pytest via `simulation/run_tests.py`, streaming filtered output and saving structured logs. - -**Script:** `simulation/run_tests.py` -**Logs directory:** `simulation/logs/` - -## Output files (all written on every run) - -| File | Content | -|------|---------| -| `simulation/logs/pytest_last_run.log` | Full raw pytest output | -| `simulation/logs/pytest_last_run_passed.log` | One test ID per line for passing tests | -| `simulation/logs/pytest_last_run_failed.log` | One test ID per line for failing tests | -| `simulation/logs/pytest_last_run_summary.json` | **Machine-readable JSON** — counts, file paths, failed test list | - -## JSON summary format (for parsing failures programmatically) -```json -{ - "timestamp": "2025-01-01 12:00:00", - "elapsed_s": 19.4, - "exit_code": 1, - "passed": 266, - "failed": 2, - "errors": 0, - "skipped": 5, - "result": "failed", - "log": "/abs/path/pytest_last_run.log", - "passed_log": "/abs/path/pytest_last_run_passed.log", - "failed_log": "/abs/path/pytest_last_run_failed.log", - "failed_tests": ["simulation/tests/unit/test_foo.py::test_bar"], - "cmd": ["python", "-m", "pytest", ...] -} -``` - -## Filter modes -- `--filter summary` *(default)* — PASSED/FAILED lines + failure details + final result -- `--filter all` — stream every line (verbose) -- `--filter failures` — only failure/error sections + final result - -## Override log path -`--log ` — companion files (_passed, _failed, _summary) are written alongside it. - ---- - -## Execution - -``` -.venv/Scripts/python.exe simulation/run_tests.py $ARGUMENTS -``` - -Default when `$ARGUMENTS` is empty: -``` -.venv/Scripts/python.exe simulation/run_tests.py simulation/tests/unit -m "not simtest" -q -``` - -## After the run -1. Report pass/fail status and counts from the final printed summary. -2. For failures: read `simulation/logs/pytest_last_run_summary.json` to get the list of failed test IDs — this is the fastest way to know which tests need fixing. -3. For failure details: read `simulation/logs/pytest_last_run.log`. - -**CRITICAL: Always use this skill — never run `python -m pytest ...` directly.** -Direct pytest bypasses logging, loses failure details, and makes post-run diagnosis impossible. - -Note: do not pipe the command through `tail`, `head`, or any other filter. diff --git a/AGENTS.md b/AGENTS.md index 759a98f..d03b1c5 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. @@ -185,6 +193,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 +220,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 @@ -249,6 +278,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/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..5f83428 100644 --- a/simulation/analysis/compare_pumping_sitl_simtest.py +++ b/simulation/analysis/compare_pumping_sitl_simtest.py @@ -50,6 +50,12 @@ "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", 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/arduloop/attitude_heli.py b/simulation/arduloop/attitude_heli.py index 1557667..b33100c 100644 --- a/simulation/arduloop/attitude_heli.py +++ b/simulation/arduloop/attitude_heli.py @@ -47,6 +47,12 @@ def __init__(self, params: HeliParams): self.pid_yaw = AC_PID(params.yaw, fs) self.swash = SwashH3(params.H_SW_H3_PHANG) self.out = HeliRateOutput() + # Per-axis output-saturation flags from the PREVIOUS tick's + # move_actuators-equivalent below — fed into THIS tick's update_all() + # anti-windup `limit` argument, exactly mirroring how real AP feeds + # `_motors.limit.roll/pitch/yaw` (set once per cycle in move_actuators) + # back into `_pid_rate_*.update_all()` on the NEXT cycle. + self._prev_saturated: tuple[bool, bool, bool] = (False, False, False) def reload_params(self) -> None: self.pid_roll.reload_params() @@ -58,13 +64,13 @@ def reset(self) -> None: self.pid_roll.reset() self.pid_pitch.reset() self.pid_yaw.reset() + self._prev_saturated = (False, False, False) def update(self, rate_target_rads: tuple[float, float, float], gyro_rate_rads: tuple[float, float, float], dt: float, collective_norm: float = 0.0, - saturated: tuple[bool, bool, bool] = (False, False, False), sim_time_s: float = 0.0, ) -> HeliRateOutput: """Run one tick of the rate loop. @@ -75,12 +81,11 @@ def update(self, gyro_rate_rads : measured body rates (already gyro-filtered), rad/s. dt : tick duration, seconds. collective_norm : current collective in [-1, 1]; used by hover-roll-trim. - saturated : per-axis output saturation flags for anti-windup. sim_time_s : simulation time (s) for slew-rate limiter timestamps. """ tr, tp, ty = rate_target_rads gr, gp, gy = gyro_rate_rads - sr, sp, sy = saturated + sr, sp, sy = self._prev_saturated p = self.params # Leaky integrator: decay I toward ±ILMI before PID update. @@ -116,6 +121,26 @@ def update(self, trim_rad = math.radians(p.HOVR_ROL_TRM_cd * 0.01) roll_out += trim_rad * max(0.0, collective_norm) + # ------------------------------------------------------------------ + # Cyclic-magnitude rescale + saturation detection — AP + # `AP_MotorsHeli_Single::move_actuators`: when the combined roll/pitch + # cyclic magnitude exceeds `H_CYC_MAX` (centi-degrees, out of the + # ±4500 cd full range), both axes are rescaled down proportionally + # and `_motors.limit.roll/pitch` are set — fed back into next tick's + # rate-PID anti-windup above. Yaw saturates independently at ±1 + # (`move_yaw`). + # ------------------------------------------------------------------ + cyc_max_norm = p.CYC_MAX_cd / 4500.0 + total_out = math.hypot(pitch_out, roll_out) + sat_roll = sat_pitch = False + if cyc_max_norm > 0.0 and total_out > cyc_max_norm: + ratio = cyc_max_norm / total_out + roll_out *= ratio + pitch_out *= ratio + sat_roll = sat_pitch = True + sat_yaw = abs(yaw_out) > p.output_limit + self._prev_saturated = (sat_roll, sat_pitch, sat_yaw) + # ------------------------------------------------------------------ # Swash phase rotation — `H_SW_H3_PHANG` # ------------------------------------------------------------------ diff --git a/simulation/arduloop/guided.py b/simulation/arduloop/guided.py index d12c18b..176bcc6 100644 --- a/simulation/arduloop/guided.py +++ b/simulation/arduloop/guided.py @@ -723,7 +723,6 @@ def update( dt: float, collective_norm: float = 0.0, sim_time: float = 0.0, - saturated: tuple[bool, bool, bool] = (False, False, False), pos_z_up_m: float | None = None, vel_z_up_mps: float | None = None, ) -> HeliRateOutput: @@ -736,7 +735,6 @@ def update( dt : tick duration (s). collective_norm: current collective in [-1,1] for hover-roll-trim. sim_time : current sim time (s) for timeout detection. - saturated : per-axis PID saturation flags for anti-windup. """ q_body = np.asarray(q_body_ned, dtype=float) gyro = np.asarray(gyro_body_rads, dtype=float) @@ -830,7 +828,6 @@ def update( gyro_rate_rads=tuple(gyro), dt=dt, collective_norm=collective_norm, - saturated=saturated, sim_time_s=sim_time, ) out.collective_norm_cmd = self._apply_throttle_out( @@ -930,7 +927,6 @@ def update( gyro_rate_rads=tuple(gyro), dt=dt, collective_norm=collective_norm, - saturated=saturated, sim_time_s=sim_time, ) diff --git a/simulation/arduloop/params.py b/simulation/arduloop/params.py index 154983c..e187021 100644 --- a/simulation/arduloop/params.py +++ b/simulation/arduloop/params.py @@ -108,6 +108,12 @@ class HeliParams: # Output limit per axis [-1, 1] like motor mixer normalised cyclic. output_limit: float = 1.0 + # `H_CYC_MAX` (centi-degrees) — combined roll/pitch cyclic magnitude above + # which AP_MotorsHeli_Single::move_actuators rescales roll/pitch down and + # sets `_motors.limit.roll/pitch` (anti-windup feedback for next tick). + # AP default AP_MOTORS_HELI_SWASH_CYCLIC_MAX = 2500 centi-degrees. + CYC_MAX_cd: float = 2500.0 + # Attitude (outer) P-loop gains — `ATC_ANG_RLL_P / PIT_P / YAW_P`. # 0.0 = no outer-loop correction (safe default for ACRO-only use). ATC_ANG_RLL_P: float = 0.0 @@ -154,5 +160,6 @@ def _f(key: str, default: float = 0.0) -> float: ATC_RATE_P_MAX = _f("ATC_RATE_P_MAX"), ATC_RATE_Y_MAX = _f("ATC_RATE_Y_MAX"), ATC_INPUT_TC = _f("ATC_INPUT_TC"), + CYC_MAX_cd = _f("H_CYC_MAX", 2500.0), **overrides, ) diff --git a/simulation/arduloop/pid.py b/simulation/arduloop/pid.py index 8e1ab35..3cb336a 100644 --- a/simulation/arduloop/pid.py +++ b/simulation/arduloop/pid.py @@ -173,6 +173,9 @@ class PIDDebug: FF: float = 0.0 DFF: float = 0.0 out: float = 0.0 + # Mirror MAVLink PID_TUNING fields for direct comparison against real SITL logs. + PDmod: float = 1.0 # P/D derating factor from the slew-rate limiter (1.0 = no limiting) + SRate: float = 0.0 # output slew rate [units/s] from the slew-rate limiter class AC_PID: @@ -314,6 +317,7 @@ def update_all(self, target: float, measurement: float, dt: float, # Slew rate limiter: reduce P+D when output changes faster than SMAX. # Matches AC_PID::update_all line: Dmod = _slew_limiter.modifier(P+D, dt) + Dmod = 1.0 if p.SMAX > 0.0: Dmod = self._slew_limiter.modifier( (P_out + D_out) * self._slew_limit_scale, dt, sim_time_s) @@ -343,6 +347,8 @@ def update_all(self, target: float, measurement: float, dt: float, d.FF = FF_out d.DFF = DFF_out d.out = out + d.PDmod = Dmod + d.SRate = self._slew_limiter.get_slew_rate() if p.SMAX > 0.0 else 0.0 return out # ------------------------------------------------------------------ diff --git a/simulation/config.py b/simulation/config.py index 970c179..1546001 100644 --- a/simulation/config.py +++ b/simulation/config.py @@ -156,6 +156,12 @@ "mavlink_attitude_hz": 100.0, # Requested SERVO_OUTPUT_RAW message rate [Hz] over the dedicated link. "mavlink_servo_output_raw_hz": 100.0, + # Requested PID_TUNING message rate [Hz] over the dedicated link. + # 0 = disabled (default). PID_TUNING is noisy (4 axes, high rate) and + # ArduPilot only emits it when GCS_PID_MASK is nonzero -- SITL test fixtures + # set both this and GCS_PID_MASK; leave disabled for real hardware/radio + # links to avoid saturating the link. + "mavlink_pid_tuning_hz": 0.0, # Periodic mediator-side diagnostics log interval for dedicated MAVLink rx. "mavlink_log_diag_interval_s": 5.0, diff --git a/simulation/mediator.py b/simulation/mediator.py index 40f51ad..f9604f4 100644 --- a/simulation/mediator.py +++ b/simulation/mediator.py @@ -256,6 +256,7 @@ def _mavlink_logger_worker(): att_target_hz = float(cfg.get("mavlink_att_target_hz", 10.0)) attitude_hz = float(cfg.get("mavlink_attitude_hz", 50.0)) servo_hz = float(cfg.get("mavlink_servo_output_raw_hz", 50.0)) + pid_tuning_hz = float(cfg.get("mavlink_pid_tuning_hz", 0.0)) def _request_interval(message_name: str, message_id: int, rate_hz: float) -> None: interval_us = max(1, int(round(1_000_000.0 / max(rate_hz, 1.0)))) @@ -298,6 +299,16 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non mavutil.mavlink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, servo_hz, ) + # PID_TUNING: SITL-only (noisy, 4 axes; ArduPilot also gates + # emission on GCS_PID_MASK, set in rawes_sitl_defaults.parm). + # 0 Hz (the default) skips the request entirely so this never + # adds traffic on a real telemetry radio link. + if pid_tuning_hz > 0.0: + _request_interval( + "PID_TUNING", + mavutil.mavlink.MAVLINK_MSG_ID_PID_TUNING, + pid_tuning_hz, + ) except Exception as exc: log.warning("Failed requesting dedicated MAVLink intervals: %s", exc) @@ -355,7 +366,7 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non if _tu: fields["mav_time_usec"] = float(_tu) - if mtype not in ("ATTITUDE", "ATTITUDE_TARGET", "SERVO_OUTPUT_RAW", "NAMED_VALUE_FLOAT", "LOCAL_POSITION_NED"): + if mtype not in ("ATTITUDE", "ATTITUDE_TARGET", "SERVO_OUTPUT_RAW", "NAMED_VALUE_FLOAT", "LOCAL_POSITION_NED", "PID_TUNING"): if fields: update_async_mavlink(fields) continue @@ -396,6 +407,18 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non fields["ekf_pos_x"] = float(getattr(msg, "x", float("nan"))) fields["ekf_pos_y"] = float(getattr(msg, "y", float("nan"))) fields["ekf_pos_z"] = float(getattr(msg, "z", float("nan"))) + elif mtype == "PID_TUNING": + # ArduPilot emits axis as 1=roll, 2=pitch, 3=yaw, 4=accelz. + # Only roll/pitch/yaw have telemetry columns (ap_rate_pid_terms). + axis = int(getattr(msg, "axis", -1)) + prefix = {1: "rate_roll", 2: "rate_pitch", 3: "rate_yaw"}.get(axis) + if prefix is not None: + fields[f"{prefix}_p_contrib"] = float(getattr(msg, "P", float("nan"))) + fields[f"{prefix}_i_contrib"] = float(getattr(msg, "I", float("nan"))) + fields[f"{prefix}_d_contrib"] = float(getattr(msg, "D", float("nan"))) + fields[f"{prefix}_ff_contrib"] = float(getattr(msg, "FF", float("nan"))) + fields[f"{prefix}_pdmod"] = float(getattr(msg, "PDmod", float("nan"))) + fields[f"{prefix}_srate"] = float(getattr(msg, "SRate", float("nan"))) if fields: update_async_mavlink(fields) @@ -806,17 +829,17 @@ def step_fn(servos, t_sim): # kinematic ends, the tether is pre-tensioned correctly without huge spike. core.tether.rest_length = _winch_node.rest_length - # Yaw authority: NOT wired for the real SITL/flight-test path. The - # torque_model.HubState ESC-governor ODE + real-PWM readback was tried - # here, but reproduces the known-unresolved mediator_torque closed-loop - # yaw coupling issue (see repo memory sitl-param-verify-and-yaw-ff.md, - # "Root cause 2") even for the plain steady-flight test -- the hub - # spins up unbounded (SPIN WARN, auto-disarm) because there's no trim - # observer counterpart in this path (unlike GUIDED-mode simtests via - # mock_ardupilot.step_physics(), which does have YawTrimObserver and - # passes). Left as pure damp-to-zero here (yaw_throttle=None) pending - # a dedicated fix to the mediator_torque closed-loop coupling. - result = core.step(_dt, collective_rad, _tilt_lon, _tilt_lat) + # Yaw authority: real GB4008 Motor4/SERVO9 PWM readback drives the + # torque_model.HubState ESC-governor ODE (same ODE mediator_torque.py + # uses). rawes.lua's run_yaw_trim() is the REAL ArduPilot Lua script + # here (not a Python stand-in) -- it reads this same SERVO9 PWM via + # SRV_Channels:get_output_pwm() every tick, armed or not, in both + # MODE_PASSIVE (kinematic hold) and MODE_STEADY. See repo memory + # sitl-param-verify-and-yaw-ff.md for the earlier failed attempt at + # this (combined with removing rawes.lua's CH4 override -- CH4 is an + # unrelated, orthogonal mechanism and must stay intact). + _yaw_throttle = float(np.clip((sitl.last_pwm_raw[8] - 1000.0) / 1000.0, 0.0, 1.0)) + result = core.step(_dt, collective_rad, _tilt_lon, _tilt_lat, yaw_throttle=_yaw_throttle) _damp_alpha = float(result.get("damp_alpha", 0.0)) # ── Unpack physics results ──────────────────────────────────────── @@ -979,6 +1002,24 @@ def step_fn(servos, t_sim): "mav_nvf_yff_trim": _mav_async.get("mav_nvf_yff_trim", float("nan")), "mav_nvf_yff_u": _mav_async.get("mav_nvf_yff_u", float("nan")), "mav_nvf_yff_gz": _mav_async.get("mav_nvf_yff_gz", float("nan")), + "rate_roll_p_contrib": _mav_async.get("rate_roll_p_contrib", float("nan")), + "rate_roll_i_contrib": _mav_async.get("rate_roll_i_contrib", float("nan")), + "rate_roll_d_contrib": _mav_async.get("rate_roll_d_contrib", float("nan")), + "rate_roll_ff_contrib": _mav_async.get("rate_roll_ff_contrib", float("nan")), + "rate_pitch_p_contrib": _mav_async.get("rate_pitch_p_contrib", float("nan")), + "rate_pitch_i_contrib": _mav_async.get("rate_pitch_i_contrib", float("nan")), + "rate_pitch_d_contrib": _mav_async.get("rate_pitch_d_contrib", float("nan")), + "rate_pitch_ff_contrib": _mav_async.get("rate_pitch_ff_contrib", float("nan")), + "rate_yaw_p_contrib": _mav_async.get("rate_yaw_p_contrib", float("nan")), + "rate_yaw_i_contrib": _mav_async.get("rate_yaw_i_contrib", float("nan")), + "rate_yaw_d_contrib": _mav_async.get("rate_yaw_d_contrib", float("nan")), + "rate_yaw_ff_contrib": _mav_async.get("rate_yaw_ff_contrib", float("nan")), + "rate_roll_pdmod": _mav_async.get("rate_roll_pdmod", float("nan")), + "rate_roll_srate": _mav_async.get("rate_roll_srate", float("nan")), + "rate_pitch_pdmod": _mav_async.get("rate_pitch_pdmod", float("nan")), + "rate_pitch_srate": _mav_async.get("rate_pitch_srate", float("nan")), + "rate_yaw_pdmod": _mav_async.get("rate_yaw_pdmod", float("nan")), + "rate_yaw_srate": _mav_async.get("rate_yaw_srate", float("nan")), "ekf_pos_x": _mav_async.get("ekf_pos_x", float("nan")), "ekf_pos_y": _mav_async.get("ekf_pos_y", float("nan")), "ekf_pos_z": _mav_async.get("ekf_pos_z", float("nan")), diff --git a/simulation/physics_core.py b/simulation/physics_core.py index f37e782..3c59242 100644 --- a/simulation/physics_core.py +++ b/simulation/physics_core.py @@ -232,31 +232,6 @@ def aero(self): def tether(self): return self._tether - def hub_observe(self) -> dict: - """ - Raw observable state — only what real hardware sensors can measure. - - Returns a plain dict so physics_core.py stays free of dataclass - dependencies. PhysicsRunner.observe() wraps this in SensorReading. - - Keys - ---- - R ndarray (3,3) — body→world rotation matrix (AHRS) - pos ndarray (3,) — NED position [m] (GPS/EKF) - vel ndarray (3,) — NED velocity [m/s] (GPS/EKF) - omega_world ndarray (3,) — world-frame angular velocity [rad/s] (IMU input; - sensor computes gyro_body = R.T @ omega_world) - omega_spin float — rotor spin rate [rad/s] (ESC telemetry) - """ - hub = self._dyn.state - return { - "R": hub["R"], - "pos": hub["pos"], - "vel": hub["vel"], - "omega_world": hub["omega"], - "omega_spin": self._omega_rad_s, - } - def hub_observe(self) -> HubObservation: """Return current observable hub state as a HubObservation.""" hub = self._dyn.state diff --git a/simulation/scripts/rawes.lua b/simulation/scripts/rawes.lua index 174d010..a3361c2 100644 --- a/simulation/scripts/rawes.lua +++ b/simulation/scripts/rawes.lua @@ -261,6 +261,7 @@ local YAW_MOTOR_FUNC = 36 -- ArduPilot servo function for Motor4 (SERVO9) local TEL_HZ = 2.0 -- diagnostic NVF emission rate [Hz] (RAWES_TEL_HZ) local _yaw_ff_trim = 0.0 -- current H_YAW_TRIM value [0, YFF_MAX] +_yaw_ff_seed = nil -- ground-provided equilibrium trim seed [0, YFF_MAX] (RAWES_YFF) local _nvf_last_ms = nil -- shared timer for all outer-rate NVF diagnostic emissions -- ── Helpers ─────────────────────────────────────────────────────────────────── -- Convert a millis() result to seconds (float). On real ArduPilot millis() @@ -872,8 +873,24 @@ local function yaw_trim_step(dt, u, psi_dot) return _yaw_ff_trim end -local function run_yaw_trim(now) +local function run_yaw_trim(now, is_passive) if not arming:is_armed() then return end + -- MODE_PASSIVE (kinematic hold): the body's gyro/PWM readback is a + -- simulation artifact (position/attitude is externally forced), so the + -- normal psi_dot feedback below would incorrectly converge the trim + -- toward whatever near-zero throttle currently holds a fake zero rate. + -- Instead, directly hold the ground-computed equilibrium seed (RAWES_YFF) + -- so the real SERVO9 PWM already matches the yaw-motor ODE's frozen IC + -- equilibrium by the time of kinematic release -- avoiding a step-input + -- torque mismatch that spins the hub. Falls through to the normal + -- feedback path once mode leaves PASSIVE (release), continuing smoothly + -- from whatever _yaw_ff_trim was left at. + if is_passive and _yaw_ff_seed ~= nil then + _yaw_ff_trim = math.max(0.0, math.min(YFF_MAX, _yaw_ff_seed)) + param:set("H_YAW_TRIM", _yaw_ff_trim) + _diag_set("YFF_T", _yaw_ff_trim) + return + end local gyro = ahrs:get_gyro() if not gyro then return end -- Read the total applied throttle from the SERVO9 output (H_YAW_TRIM + @@ -1025,6 +1042,13 @@ local function update() if _nv_floats["RAWES_THR"] then _ic_pending_thrust = _nv_floats["RAWES_THR"] end if _nv_floats["RAWES_RIC"] then _ic_pending_roll_deg = math.deg(_nv_floats["RAWES_RIC"]) end if _nv_floats["RAWES_PIC"] then _ic_pending_pitch_deg = math.deg(_nv_floats["RAWES_PIC"]) end + -- Equilibrium yaw-trim seed (ground-computed from IC rotor omega and the + -- GB4008 hub model -- see torque_model.equilibrium_throttle()). Held + -- constant for the whole MODE_PASSIVE kinematic hold; consumed by + -- run_yaw_trim() below instead of its normal psi_dot feedback, which is + -- meaningless while the body is kinematically locked (see AGENTS.md / + -- design/flight_stack.md "Yaw observer in passive mode"). + if _nv_floats["RAWES_YFF"] then _yaw_ff_seed = _nv_floats["RAWES_YFF"] end -- Optional fixed yaw target for MODE_PASSIVE. When present, PASSIVE holds -- this absolute yaw instead of capturing (and holding) the spinning AHRS yaw. -- Sending RAWES_YIC_CAPTURE_SENTINEL instead captures roll/pitch/yaw all @@ -1131,13 +1155,13 @@ local function update() if mode == MODE_PASSIVE then run_passive_mode(now) - run_yaw_trim(now) + run_yaw_trim(now, true) _diag_emit(now) return update, BASE_PERIOD_MS end if mode == MODE_STEADY then - run_yaw_trim(now) + run_yaw_trim(now, false) if now - _last_flight_ms >= FLIGHT_PERIOD_MS then _last_flight_ms = now run_flight() diff --git a/simulation/telemetry_columns.py b/simulation/telemetry_columns.py index 79d9a11..80b9bf2 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,22 @@ "mav_nvf_yff_trim", "mav_nvf_yff_u", "mav_nvf_yff_gz", + "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..bfb501b 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] @@ -475,6 +481,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"), @@ -718,6 +730,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, diff --git a/simulation/tests/common/mock_ardupilot.py b/simulation/tests/common/mock_ardupilot.py index 1e072cd..4d36a07 100644 --- a/simulation/tests/common/mock_ardupilot.py +++ b/simulation/tests/common/mock_ardupilot.py @@ -92,6 +92,12 @@ def _feed_obs(sim, obs, accel_body: "np.ndarray | None" = None) -> None: sim.accel = accel_body.tolist() +# ArduPilot SRV_Channels function number for Motor4/SERVO9 -- must match +# rawes.lua's YAW_MOTOR_FUNC constant exactly (real run_yaw_trim() reads +# this same servo function via SRV_Channels:get_output_pwm()). +_YAW_MOTOR_FUNC = 36 + + class _MockArdupilotBase: """Common adapter utilities shared by lua/python backends.""" @@ -106,13 +112,11 @@ def __init__(self, *, wind: "np.ndarray", dt: float, initial_thrust: float = 0.2 self._telemetry: list = [] self._log_step = 0 self._guided_ctrl: "GuidedAttitudeController | None" = None - # Anti-rotation feedforward trim (Python port of rawes.lua's - # yaw_trim_step/run_yaw_trim -- see controller.YawTrimObserver). - # Applied uniformly for both Lua- and Python-backed simtests since - # arduloop.HeliRateController has no equivalent of ArduPilot's native - # H_YAW_TRIM mixer addition, and the Lua-side observer's output is - # never fed back into physics in the Lua backend (no real SITL - # firmware in the loop to consume H_YAW_TRIM). + # Anti-rotation feedforward trim. Default: Python port of rawes.lua's + # yaw_trim_step/run_yaw_trim (see controller.YawTrimObserver), used by + # the pure-Python backend which has no real Lua VM to run the actual + # function. _LuaBackend overrides _yaw_apply() below to route through + # the REAL rawes.lua run_yaw_trim() instead of this Python duplicate. self._yaw_trim = YawTrimObserver() def enable_guided(self, heli_params: "HeliParams | None" = None) -> None: @@ -146,11 +150,21 @@ def step_physics(self, runner, dt: float, *, rest_length: "float | None" = None) # the SERVO9_TRIM=SERVO9_MIN one-directional-motor convention (see # mediator.py's yaw_throttle derivation). u_raw = float(np.clip(heli_out.yaw_cmd, 0.0, 1.0)) - u_applied = float(np.clip(u_raw + self._yaw_trim.trim, 0.0, 1.0)) - self._yaw_trim.step(dt, u_applied, float(gyro[2])) + u_applied = self._yaw_apply(u_raw, gyro, dt) return runner.step_guided(dt, self._last_thrust, heli_out, rest_length=rest_length, yaw_throttle=u_applied) + def _yaw_apply(self, u_raw: float, gyro: np.ndarray, dt: float) -> float: + """Add yaw trim to the raw rate-controller output u_raw [0, 1]. + + Default implementation: Python port (YawTrimObserver). Used by the + pure-Python backend, which has no real ArduPilot/Lua firmware in the + loop to run the actual rawes.lua run_yaw_trim(). + """ + u_applied = float(np.clip(u_raw + self._yaw_trim.trim, 0.0, 1.0)) + self._yaw_trim.step(dt, u_applied, float(gyro[2])) + return u_applied + def log(self, runner, sr: dict) -> None: if self.tel_fn is None or self._tel_every is None: return @@ -194,6 +208,12 @@ def log(self, runner, sr: dict) -> None: "rate_yaw_i_contrib": float("nan"), "rate_yaw_d_contrib": float("nan"), "rate_yaw_ff_contrib": float("nan"), + "rate_roll_pdmod": float("nan"), + "rate_roll_srate": float("nan"), + "rate_pitch_pdmod": float("nan"), + "rate_pitch_srate": float("nan"), + "rate_yaw_pdmod": float("nan"), + "rate_yaw_srate": float("nan"), } if self._guided_ctrl is not None: _rate_ctrl = getattr(self._guided_ctrl, "_rate_ctrl", None) @@ -206,16 +226,22 @@ def log(self, runner, sr: dict) -> None: rate_terms["rate_roll_i_contrib"] = float(_pid_roll.debug.I) rate_terms["rate_roll_d_contrib"] = float(_pid_roll.debug.D) rate_terms["rate_roll_ff_contrib"] = float(_pid_roll.debug.FF + _pid_roll.debug.DFF) + rate_terms["rate_roll_pdmod"] = float(_pid_roll.debug.PDmod) + rate_terms["rate_roll_srate"] = float(_pid_roll.debug.SRate) if _pid_pitch is not None: rate_terms["rate_pitch_p_contrib"] = float(_pid_pitch.debug.P) rate_terms["rate_pitch_i_contrib"] = float(_pid_pitch.debug.I) rate_terms["rate_pitch_d_contrib"] = float(_pid_pitch.debug.D) rate_terms["rate_pitch_ff_contrib"] = float(_pid_pitch.debug.FF + _pid_pitch.debug.DFF) + rate_terms["rate_pitch_pdmod"] = float(_pid_pitch.debug.PDmod) + rate_terms["rate_pitch_srate"] = float(_pid_pitch.debug.SRate) if _pid_yaw is not None: rate_terms["rate_yaw_p_contrib"] = float(_pid_yaw.debug.P) rate_terms["rate_yaw_i_contrib"] = float(_pid_yaw.debug.I) rate_terms["rate_yaw_d_contrib"] = float(_pid_yaw.debug.D) rate_terms["rate_yaw_ff_contrib"] = float(_pid_yaw.debug.FF + _pid_yaw.debug.DFF) + rate_terms["rate_yaw_pdmod"] = float(_pid_yaw.debug.PDmod) + rate_terms["rate_yaw_srate"] = float(_pid_yaw.debug.SRate) extra_kwargs = self.tel_fn(runner, sr) if self.tel_fn else {} extra_kwargs.update( @@ -271,6 +297,30 @@ def __init__(self, sim, *, initial_thrust: float = 0.263, wind: "np.ndarray", dt if initial_thrust > 0.0: sim._lua.execute(f"_ic_thrust = {float(initial_thrust)}") + # ArduPilot heli mixer SERVO9 (Motor4) PWM range -- matches SERVO9_MIN/MAX + # in simulation/tests/sitl/rawes_common_defaults.parm and rawes.lua's + # run_yaw_trim() `p("SERVO9_MIN", 1000)` / `p("SERVO9_MAX", 2000)` fallback. + _SERVO9_MIN_US = 1000.0 + _SERVO9_MAX_US = 2000.0 + + def _yaw_apply(self, u_raw: float, gyro: np.ndarray, dt: float) -> float: + """Route yaw trim through the REAL rawes.lua run_yaw_trim(), not a + Python duplicate. + + Writes this tick's applied Motor4/SERVO9 PWM (rate-controller output + u_raw + the H_YAW_TRIM the PREVIOUS Lua tick computed) into the mock's + SRV_Channels output register -- exactly what ArduPilot's real heli + mixer would produce. The next time tick() runs sim._update_fn(), + rawes.lua's run_yaw_trim() reads this same PWM back via + SRV_Channels:get_output_pwm() and updates H_YAW_TRIM for real (no + Python-side observer involved for this backend). + """ + trim_prev = self._sim.get_param("H_YAW_TRIM") + u_applied = float(np.clip(u_raw + trim_prev, 0.0, 1.0)) + pwm = self._SERVO9_MIN_US + u_applied * (self._SERVO9_MAX_US - self._SERVO9_MIN_US) + self._sim.set_srv_out(_YAW_MOTOR_FUNC, pwm) + return u_applied + def tick(self, t_sim: float, runner, *, inject=None, accel_ned: "np.ndarray | None" = None) -> None: self._sim._mock.millis_val = int(t_sim * 1000) obs = runner.observe() @@ -314,8 +364,8 @@ def tick(self, t_sim: float, runner, *, inject=None, accel_ned: "np.ndarray | No ) def step(self, runner, dt: float, *, rest_length: "float | None" = None) -> dict: - obs = runner.observe() if not self._ctrl._target_set: + obs = runner.observe() self._ctrl.set_target_rotation(obs.R, sim_time=runner.t_sim) return self.step_physics(runner, dt, rest_length=rest_length) diff --git a/simulation/tests/sitl/flight/conftest.py b/simulation/tests/sitl/flight/conftest.py index c08b0b1..a4365c6 100644 --- a/simulation/tests/sitl/flight/conftest.py +++ b/simulation/tests/sitl/flight/conftest.py @@ -27,6 +27,7 @@ _STARTUP_DAMP_S, ) from ic import load_ic +from torque_model import HubParams, equilibrium_throttle # --------------------------------------------------------------------------- @@ -377,6 +378,19 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch ctx.gcs.send_named_float("RAWES_ALT", _alt_ic) ctx.log.info("IC equilibrium tension: %.0f N target altitude: %.1f m", _tension_eq, _alt_ic) + + # Seed the yaw-motor trim equilibrium for the IC/release rotor spin + # rate (same torque_model.equilibrium_throttle() calc physics_core.py + # uses to initialize the frozen hub ODE state). rawes.lua holds this + # directly in H_YAW_TRIM throughout MODE_PASSIVE instead of trying to + # derive it from a (kinematically-locked, hence meaningless) psi_dot + # readback -- so the real SERVO9 PWM already matches the yaw-motor + # ODE's equilibrium by the time of kinematic release, avoiding a + # step-input torque mismatch that spins the hub. See design/flight_stack.md + # "Yaw observer in passive mode" and repo memory sitl-param-verify-and-yaw-ff.md. + _yff_seed = equilibrium_throttle(float(_ic["omega_spin"]), HubParams()) + ctx.gcs.send_named_float("RAWES_YFF", _yff_seed) + ctx.log.info("IC yaw-trim equilibrium seed: %.3f", _yff_seed) ctx.wait_drain(timeout=0.5, label="post-col") # Wait for GPS fusion before yielding. diff --git a/simulation/tests/sitl/rawes_common_defaults.parm b/simulation/tests/sitl/rawes_common_defaults.parm index f8cbcd6..744eaa2 100644 --- a/simulation/tests/sitl/rawes_common_defaults.parm +++ b/simulation/tests/sitl/rawes_common_defaults.parm @@ -52,12 +52,15 @@ H_RSC_MODE 1 # passthrough: no main rotor motor H_TAIL_TYPE 3 H_COL2YAW 0.0 -# Small AP yaw PID assist: P handles fast disturbances and a tiny I removes -# residual bias. rawes.lua still owns the DC trim through H_YAW_TRIM. -ATC_RAT_YAW_P 0.015 +# AP yaw PID assist: P/I sized to match ArduCopter-Heli's stock default +# (ATC_RAT_YAW_P~0.18) so the rate loop has enough authority to keep heading +# error under the hardcoded 45 deg heading-error-max ceiling (see +# design/flight_stack.md yaw-runaway diagnosis) -- rawes.lua still owns the +# DC trim through H_YAW_TRIM. +ATC_RAT_YAW_P 0.18 ATC_RAT_YAW_D 0.0 ATC_RAT_YAW_FLTD 10.0 -ATC_RAT_YAW_I 0.0015 +ATC_RAT_YAW_I 0.018 # RAWES_* script-generated parameters (registered by rawes.lua via param:add_table). # These are script-generated (RAWES_*) params visible in GCS, registered by rawes.lua via param:add_table. diff --git a/simulation/tests/sitl/rawes_sitl_defaults.parm b/simulation/tests/sitl/rawes_sitl_defaults.parm index 4b1b45a..0ed6f86 100644 --- a/simulation/tests/sitl/rawes_sitl_defaults.parm +++ b/simulation/tests/sitl/rawes_sitl_defaults.parm @@ -28,6 +28,12 @@ GPS2_DELAY_MS 220 COMPASS_USE 0 # disable compass yaw source SCR_ENABLE 1 # enable Lua scripting +# Emit PID_TUNING (roll+pitch+yaw+accelZ) for telemetry logging. See +# copter-heli.parm for the bitmask reference; this is SITL-only (over the +# dedicated mavlink_log_connection UDP/TCP link, not a real telemetry radio) +# because PID_TUNING at high rate is too noisy for a real MAVLink radio link. +GCS_PID_MASK 15 + # Relax GPS checks / innovation gates for SITL boot determinism. EK3_GPS_CHECK 0 # skip HDOP/speed-accuracy/min-sat checks in SITL JSON GPS EK3_POS_I_GATE 50.0 # widened position innovation gate (sigma) diff --git a/simulation/tests/sitl/stack_infra.py b/simulation/tests/sitl/stack_infra.py index 59deed9..13ec9fa 100644 --- a/simulation/tests/sitl/stack_infra.py +++ b/simulation/tests/sitl/stack_infra.py @@ -751,6 +751,7 @@ def _acro_stack(tmp_path, *, extra_config=None, _med_extra.setdefault("mavlink_att_target_hz", 100.0) _med_extra.setdefault("mavlink_attitude_hz", 100.0) _med_extra.setdefault("mavlink_servo_output_raw_hz", 100.0) + _med_extra.setdefault("mavlink_pid_tuning_hz", 100.0) with _sitl_stack( tmp_path, @@ -1767,11 +1768,13 @@ def _wait_params_ready(gcs, log, timeout: float = 15.0) -> None: # (SCR_ENABLE=0, or Lua in MODE_NONE). Those tests keep a nonzero AP yaw P here. # # NOTE: this set NO LONGER mirrors rawes_common_defaults.parm. The flight/vanilla -# parm chain now zeroes the AP yaw PID (ATC_RAT_YAW_P/I/D = 0) because yaw there is -# regulated by the Lua manual PID (MODE_PASSIVE/STEADY) writing H_YAW_TRIM; a -# nonzero AP yaw gain would fight the Lua loop and drive a limit cycle. This -# override set is intentionally different: it drives the AP DDFP loop directly, so -# it must retain P>0. Rationale for P=0.02: with the inertial motor model the +# parm chain uses ATC_RAT_YAW_P/I = 0.18/0.018 (D=0 -- see rawes_common_defaults.parm) +# sized to match ArduCopter-Heli's stock default so the rate loop has enough +# authority to keep heading error under the 45 deg heading-error-max ceiling; +# H_YAW_TRIM (written by the Lua manual PID in MODE_PASSIVE/STEADY) carries the +# DC hold. This override set is intentionally different: it drives +# the AP DDFP loop directly as the SOLE yaw actuator, so it must retain a much +# larger P. Rationale for P=0.02 here: with the inertial motor model the # high-authority GB4008 (~58 rad/s per throttle) is stable at P=0.02 with a # filtered-but-off D; H_YAW_TRIM carries the DC hold so I=0. # Fixtures overlay this with EKF/compass/mode/script params only -- never with a From 3a423096c69c1b4887f5173d3f21e33401990688 Mon Sep 17 00:00:00 2001 From: Kristof Date: Fri, 17 Jul 2026 17:15:10 +0200 Subject: [PATCH 2/3] work in progress --- AGENTS.md | 7 + design/sitl_flight_timeline.md | 97 +++++ design/sitl_testing.md | 6 + .../analysis/compare_pumping_sitl_simtest.py | 2 +- simulation/analysis/compare_runs.py | 342 +++++++++++++++++- simulation/mediator.py | 6 + simulation/physics_core.py | 10 + simulation/scripts/rawes_test_surface.lua | 4 + simulation/telemetry_columns.py | 3 + simulation/telemetry_csv.py | 30 +- simulation/tests/common/mock_ardupilot.py | 26 ++ test.sh | 13 +- 12 files changed, 532 insertions(+), 14 deletions(-) create mode 100644 design/sitl_flight_timeline.md diff --git a/AGENTS.md b/AGENTS.md index d03b1c5..357cdfa 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -86,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` | @@ -248,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 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/compare_pumping_sitl_simtest.py b/simulation/analysis/compare_pumping_sitl_simtest.py index 5f83428..f577135 100644 --- a/simulation/analysis/compare_pumping_sitl_simtest.py +++ b/simulation/analysis/compare_pumping_sitl_simtest.py @@ -59,7 +59,7 @@ "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/mediator.py b/simulation/mediator.py index f9604f4..f2fd174 100644 --- a/simulation/mediator.py +++ b/simulation/mediator.py @@ -256,6 +256,7 @@ def _mavlink_logger_worker(): att_target_hz = float(cfg.get("mavlink_att_target_hz", 10.0)) attitude_hz = float(cfg.get("mavlink_attitude_hz", 50.0)) servo_hz = float(cfg.get("mavlink_servo_output_raw_hz", 50.0)) + local_position_ned_hz = float(cfg.get("mavlink_local_position_ned_hz", 10.0)) pid_tuning_hz = float(cfg.get("mavlink_pid_tuning_hz", 0.0)) def _request_interval(message_name: str, message_id: int, rate_hz: float) -> None: @@ -299,6 +300,11 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non mavutil.mavlink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, servo_hz, ) + _request_interval( + "LOCAL_POSITION_NED", + mavutil.mavlink.MAVLINK_MSG_ID_LOCAL_POSITION_NED, + local_position_ned_hz, + ) # PID_TUNING: SITL-only (noisy, 4 axes; ArduPilot also gates # emission on GCS_PID_MASK, set in rawes_sitl_defaults.parm). # 0 Hz (the default) skips the request entirely so this never diff --git a/simulation/physics_core.py b/simulation/physics_core.py index 3c59242..35cabbf 100644 --- a/simulation/physics_core.py +++ b/simulation/physics_core.py @@ -301,6 +301,7 @@ def step(self, dt: float, collective_rad: float, rotor/counter-rotation determines hub spin identically in both paths (see _integrate). Default None reproduces the previous pure damping-to-zero behavior exactly (no hub-motor ODE advance). + """ return self._integrate(dt, collective_rad, tilt_lon, tilt_lat, rest_length, yaw_throttle) @@ -415,6 +416,11 @@ def _integrate(self, dt: float, collective_rad: float, # Kinematic hold: bypass all physics contributions. # No tether, no aero, no gravity-driven dynamics step, and no rotor spin update. self._omega_rad_s = self._kinematic_omega_target(self._t_sim) + # Keep hub-motor state aligned with the kinematic omega profile. + # This prevents a motor/rotor speed mismatch at handoff from + # creating an artificial yaw-rate transient. + self._hub_state.omega_motor = self._omega_rad_s * self._hub_params.gear_ratio + self._hub_state.psi_dot = 0.0 result = SimpleNamespace( F_world=np.zeros(3), m_hub_world=np.zeros(3), @@ -454,6 +460,10 @@ def _integrate(self, dt: float, collective_rad: float, # can pull omega away from the kinematic target before the # mediator logs kinematic_exit. self._omega_rad_s = max(omega_min, float(self._omega_release_target_rad_s)) + # Use the same kinematic->IC handoff for hub motor state to + # avoid a mismatch-driven yaw kick at release. + self._hub_state.omega_motor = self._omega_rad_s * self._hub_params.gear_ratio + self._hub_state.psi_dot = 0.0 else: new_omega, new_spin = euler_step_omega( self._omega_rad_s, self._spin_angle_rad, diff --git a/simulation/scripts/rawes_test_surface.lua b/simulation/scripts/rawes_test_surface.lua index 561f7c2..17b05cf 100644 --- a/simulation/scripts/rawes_test_surface.lua +++ b/simulation/scripts/rawes_test_surface.lua @@ -97,4 +97,8 @@ _rawes_fns = { col_trim = function() return _col_trim end, alt_i = function() return _alt_i end, + -- Latest diagnostics emitted via _diag_set / _diag_emit. + -- Returns nil when a key has not been set yet. + diag_nvf = function(name) return _diag_nvf[name] end, + } diff --git a/simulation/telemetry_columns.py b/simulation/telemetry_columns.py index 80b9bf2..8e17a9c 100644 --- a/simulation/telemetry_columns.py +++ b/simulation/telemetry_columns.py @@ -320,6 +320,9 @@ "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", diff --git a/simulation/telemetry_csv.py b/simulation/telemetry_csv.py index bfb501b..98c8040 100644 --- a/simulation/telemetry_csv.py +++ b/simulation/telemetry_csv.py @@ -469,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"), @@ -596,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 @@ -637,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), @@ -693,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, @@ -715,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), diff --git a/simulation/tests/common/mock_ardupilot.py b/simulation/tests/common/mock_ardupilot.py index 4d36a07..e296aca 100644 --- a/simulation/tests/common/mock_ardupilot.py +++ b/simulation/tests/common/mock_ardupilot.py @@ -165,6 +165,10 @@ def _yaw_apply(self, u_raw: float, gyro: np.ndarray, dt: float) -> float: self._yaw_trim.step(dt, u_applied, float(gyro[2])) return u_applied + def _telemetry_overrides(self, obs: HubObservation) -> dict: # noqa: ARG002 + """Backend-specific telemetry field overrides.""" + return {} + def log(self, runner, sr: dict) -> None: if self.tel_fn is None or self._tel_every is None: return @@ -262,6 +266,7 @@ def log(self, runner, sr: dict) -> None: yaw_sp_rads=mav_att_target_yaw_rate_rads, ) extra_kwargs.update(rate_terms) + extra_kwargs.update(self._telemetry_overrides(obs)) collective_rad = self._col_min + self._last_thrust * (self._col_max - self._col_min) self._telemetry.append( @@ -303,6 +308,27 @@ def __init__(self, sim, *, initial_thrust: float = 0.263, wind: "np.ndarray", dt _SERVO9_MIN_US = 1000.0 _SERVO9_MAX_US = 2000.0 + def _lua_diag(self, key: str) -> float: + v = self._sim.fns.diag_nvf(key) + return float(v) if v is not None else float("nan") + + def _telemetry_overrides(self, obs: HubObservation) -> dict: # noqa: ARG002 + # In SITL, these fields come from Lua NAMED_VALUE_FLOAT diagnostics. + # Keep simtest Lua backend semantics identical: Lua-only source, no fallback. + return { + "roll_sp_rads": self._lua_diag("OL_RSP"), + "pitch_sp_rads": self._lua_diag("OL_PSP"), + "yaw_sp_rads": self._lua_diag("OL_YSP"), + "roll_rate_err_rads": self._lua_diag("OL_RER"), + "pitch_rate_err_rads": self._lua_diag("OL_PER"), + "yaw_rate_err_rads": self._lua_diag("OL_YER"), + "lua_ol_alt_p_contrib": self._lua_diag("OL_AP"), + "lua_ol_alt_i_contrib": self._lua_diag("OL_AI"), + "lua_ol_alt_d_contrib": self._lua_diag("OL_AD"), + "lua_ol_thrust_cmd": self._lua_diag("OL_COL"), + "lua_ol_tension_n": self._lua_diag("OL_TEN"), + } + def _yaw_apply(self, u_raw: float, gyro: np.ndarray, dt: float) -> float: """Route yaw trim through the REAL rawes.lua run_yaw_trim(), not a Python duplicate. diff --git a/test.sh b/test.sh index 747e1be..57e37a9 100644 --- a/test.sh +++ b/test.sh @@ -21,14 +21,11 @@ # Suppress path mangling in MSYS/Git-for-Windows; harmless elsewhere. [[ -n "${MSYSTEM:-}" ]] && export MSYS_NO_PATHCONV=1 -# Docker Desktop's Windows named-pipe backend is not reachable from an -# MSYS/Git-Bash shell on this box (fails with -# "failed to connect to the docker API at npipe:////./pipe/dockerDesktopLinuxEngine"). -# Docker works natively inside WSL2 instead, so when we detect we're running -# under Git-for-Windows bash (MSYSTEM set, not already inside WSL), reinvoke -# ourselves inside WSL with the repo path translated from /c/... (mingw) to -# /mnt/c/... (WSL), forwarding all original args. -if [[ -n "${MSYSTEM:-}" && -z "${WSL_DISTRO_NAME:-}" ]] && command -v wsl.exe >/dev/null 2>&1; then +# Prefer the current shell if Docker is already reachable here. On this box, +# Docker Desktop is available from Git Bash, so there is no need to reinvoke +# inside WSL. Only fall back to WSL when Docker is not usable locally and a +# WSL launch path is available. +if [[ -n "${MSYSTEM:-}" && -z "${WSL_DISTRO_NAME:-}" ]] && ! docker info >/dev/null 2>&1 && command -v wsl.exe >/dev/null 2>&1; then _script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" _drive="${_script_dir:1:1}" _wsl_dir="/mnt/${_drive,,}${_script_dir:2}" From 4f221eabc0761c0ffaad3f851d6ca2913c0317d9 Mon Sep 17 00:00:00 2001 From: Kristof Date: Fri, 17 Jul 2026 17:29:48 +0200 Subject: [PATCH 3/3] Temporarily restore damping-only SITL yaw --- simulation/mediator.py | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/simulation/mediator.py b/simulation/mediator.py index f2fd174..bc98310 100644 --- a/simulation/mediator.py +++ b/simulation/mediator.py @@ -835,17 +835,11 @@ def step_fn(servos, t_sim): # kinematic ends, the tether is pre-tensioned correctly without huge spike. core.tether.rest_length = _winch_node.rest_length - # Yaw authority: real GB4008 Motor4/SERVO9 PWM readback drives the - # torque_model.HubState ESC-governor ODE (same ODE mediator_torque.py - # uses). rawes.lua's run_yaw_trim() is the REAL ArduPilot Lua script - # here (not a Python stand-in) -- it reads this same SERVO9 PWM via - # SRV_Channels:get_output_pwm() every tick, armed or not, in both - # MODE_PASSIVE (kinematic hold) and MODE_STEADY. See repo memory - # sitl-param-verify-and-yaw-ff.md for the earlier failed attempt at - # this (combined with removing rawes.lua's CH4 override -- CH4 is an - # unrelated, orthogonal mechanism and must stay intact). - _yaw_throttle = float(np.clip((sitl.last_pwm_raw[8] - 1000.0) / 1000.0, 0.0, 1.0)) - result = core.step(_dt, collective_rad, _tilt_lon, _tilt_lat, yaw_throttle=_yaw_throttle) + # SITL temporarily uses damping-only yaw again. We still log SERVO9, + # but do not feed the tail-motor throttle into the shared physics core. + # This keeps the release behavior simple while we focus on logging and + # documentation work. + result = core.step(_dt, collective_rad, _tilt_lon, _tilt_lat, yaw_throttle=None) _damp_alpha = float(result.get("damp_alpha", 0.0)) # ── Unpack physics results ────────────────────────────────────────