From a813dd6dd9ac266bb20865e690d1e2e4dfca1433 Mon Sep 17 00:00:00 2001 From: Kristof Date: Sun, 19 Jul 2026 12:10:04 +0200 Subject: [PATCH 1/2] fix: clear SITL typing warnings --- tests/sitl/stack_infra.py | 60 +++++++++++++++------------------------ tests/sitl/stack_utils.py | 9 +++--- 2 files changed, 28 insertions(+), 41 deletions(-) diff --git a/tests/sitl/stack_infra.py b/tests/sitl/stack_infra.py index 0f3f95b..1b3e9be 100644 --- a/tests/sitl/stack_infra.py +++ b/tests/sitl/stack_infra.py @@ -101,7 +101,6 @@ def _project_default_rotor(): SetAttitudeTarget, ) from simulation.mediator_events import MediatorEventLog -from simulation.controller import make_hold_controller from simulation.ic import load_ic_dict, IC_JSON_PATH _STARTING_STATE = IC_JSON_PATH @@ -328,7 +327,6 @@ class StackContext: all_statustext : all STATUSTEXT messages seen during setup setup_samples : list of dicts — EKF/ATTITUDE samples captured during setup sim_dir : simulation/ directory (for writing outputs) - controller : PhysicalHoldController instance last_local_position_ned : most recent (x, y, z, vx, vy, vz) LOCAL_POSITION_NED sample seen by wait_drain(), or None @@ -338,9 +336,9 @@ class StackContext: """ # ── required for all stack tests ────────────────────────────────────────── gcs: RawesGCS - mediator_proc: object - sitl_proc: object - mediator_log: Path + mediator_proc: subprocess.Popen | None + sitl_proc: subprocess.Popen | None + mediator_log: Path | None sitl_log: Path gcs_log: Path events_log: MediatorEventLog @@ -353,9 +351,8 @@ class StackContext: flight_events: dict = dataclasses.field(default_factory=dict) all_statustext: list = dataclasses.field(default_factory=list) setup_samples: list = dataclasses.field(default_factory=list) - sim_dir: Path | None = None - controller: object = None - test_log_dir: Path | None = None + sim_dir: Path = dataclasses.field(default_factory=Path) + test_log_dir: Path = dataclasses.field(default_factory=Path) last_local_position_ned: tuple | None = None # ── pumping socket (default 0 = disabled) ──────────────────────────────── winch_cmd_port: int = 0 @@ -432,7 +429,7 @@ def _check_liveness() -> None: ("mediator", self.mediator_proc, self.mediator_log), ("SITL", self.sitl_proc, self.sitl_log), ]: - if proc.poll() is not None: + if proc is not None and proc.poll() is not None: txt = lp.read_text(encoding="utf-8", errors="replace") if lp.exists() else "(no log)" pytest.fail( f"{name} exited during {label} " @@ -505,10 +502,10 @@ class SitlContext: repo_root: Path log: logging.Logger test_log_dir: Path - boot_setup: object # ParamSetup + boot_setup: ParamSetup | None # Set by _static_stack; None when using _sitl_stack directly - mediator_proc: object = None # subprocess.Popen | None - mediator_log: "Path | None" = None + mediator_proc: subprocess.Popen | None = None + mediator_log: Path | None = None @contextlib.contextmanager @@ -576,8 +573,12 @@ def _sitl_stack( # project YAML carries 545 deg/s / 100 deg ≈ 5.45. try: _rotor = _project_default_rotor() - _servo_speed = (_rotor.control.servo_slew_rate_deg_s - / _rotor.control.servo_travel_deg) + _control = getattr(_rotor, "control", None) + _servo_slew = getattr(_control, "servo_slew_rate_deg_s", None) + _servo_travel = getattr(_control, "servo_travel_deg", None) + if _servo_slew is None or _servo_travel is None or _servo_travel == 0: + raise ValueError("missing rotor control rates") + _servo_speed = float(_servo_slew) / float(_servo_travel) except Exception as _exc: # Keep arm/connectivity stack tests runnable even if dynbem is # unavailable in the container (e.g. Rust toolchain mismatch). @@ -769,17 +770,13 @@ def _acro_stack(tmp_path, *, extra_config=None, initial_state = load_ic_dict() home_alt_m = -float(initial_state["pos"][2]) - # ── Hold controller ──────────────────────────────────────────────────────── + # ── Anchor position ────────────────────────────────────────────────────── _anchor_ned = _np.array([0.0, 0.0, float(home_alt_m)]) - controller = make_hold_controller(anchor_ned=_anchor_ned) # ── Lua scripts ─────────────────────────────────────────────────────────── _install_lua_scripts("rawes.lua") - # ── Merge controller params + test extras into boot params ───────────────── - _extra: dict = dict(controller.extra_params) - if extra_boot_params: - _extra.update(extra_boot_params) + _extra: dict = dict(extra_boot_params) if extra_boot_params else {} _med_extra = dict(extra_config or {}) _med_extra.setdefault("mavlink_log_connection", "tcp:127.0.0.1:5762") @@ -794,7 +791,6 @@ def _acro_stack(tmp_path, *, extra_config=None, extra_boot_params = _extra, ) as sitl_ctx: log = sitl_ctx.log - log.info("Hold controller: %s", type(controller).__name__) # ── Extra log paths (mediator-specific) ──────────────────────────────── mediator_log = tmp_path / "mediator.log" @@ -821,11 +817,11 @@ def _acro_stack(tmp_path, *, extra_config=None, mediator_proc = None def _procs_alive(): - _checks = [("SITL", sitl_ctx.sitl_proc, sitl_ctx.sitl_log)] + _checks: list[tuple[str, subprocess.Popen | None, Path]] = [("SITL", sitl_ctx.sitl_proc, sitl_ctx.sitl_log)] if with_mediator: _checks.insert(0, ("mediator", mediator_proc, mediator_log)) for name, proc, lp in _checks: - if proc.poll() is not None: + if proc is not None and proc.poll() is not None: txt = lp.read_text(encoding="utf-8", errors="replace") if lp.exists() else "(no log)" pytest.fail(f"{name} exited early (rc={proc.returncode}):\n{txt[-3000:]}") @@ -841,7 +837,6 @@ def _procs_alive(): initial_state=initial_state, home_alt_m=home_alt_m, flight_events={}, all_statustext=[], setup_samples=[], log=log, sim_dir=sitl_ctx.sim_dir, - controller=controller, test_log_dir=sitl_ctx.test_log_dir, winch_cmd_port=int(_med_extra.get("winch_cmd_port", 0)), ) @@ -1228,16 +1223,6 @@ def _run_acro_setup( log.info("[setup 4/6] Clean ATTITUDE — EKF attitude ready.") ekf_att = True t_ekf = t_now - # Capture equilibrium orientation for PhysicalHoldController. - # This is the tether equilibrium the hub holds during the 45 s - # kinematic startup (R locked to R0). The controller uses - # (roll - roll_eq, pitch - pitch_eq) as a yaw-independent error - # so corrections stay valid even when velocity-derived yaw jumps - # ~150° at kinematic end when the tether activates. - if hasattr(ctx.controller, "set_equilibrium"): - ctx.controller.set_equilibrium(decoded.roll, decoded.pitch) - log.info("[setup 4/6] Equilibrium set: roll_eq=%.2f° pitch_eq=%.2f°", - r, p) ekf_ok = ekf_att elif isinstance(decoded, EkfStatusReport): @@ -1484,8 +1469,9 @@ def analyze_startup_logs(ctx: StackContext) -> dict: } # ── Mediator log ────────────────────────────────────────────────────────── - if ctx.mediator_log.exists(): - lines = ctx.mediator_log.read_text(encoding="utf-8", errors="replace").splitlines() + mediator_log = ctx.mediator_log + if mediator_log is not None and mediator_log.exists(): + lines = mediator_log.read_text(encoding="utf-8", errors="replace").splitlines() result["mediator_tail"] = lines[-30:] for line in lines: if " ERROR " in line or " CRITICAL " in line: @@ -1732,7 +1718,7 @@ def assert_procs_alive(ctx, label: str = "observation") -> None: ("mediator", ctx.mediator_proc, ctx.mediator_log), ("SITL", ctx.sitl_proc, ctx.sitl_log), ]: - if proc.poll() is not None: + if proc is not None and proc.poll() is not None: txt = lp.read_text(encoding="utf-8", errors="replace") if lp.exists() else "(no log)" crash = get_arducopter_crash_info(ctx) pytest.fail( diff --git a/tests/sitl/stack_utils.py b/tests/sitl/stack_utils.py index 0da2630..8b4cc1b 100644 --- a/tests/sitl/stack_utils.py +++ b/tests/sitl/stack_utils.py @@ -12,6 +12,7 @@ import subprocess import sys import time +from collections.abc import Mapping from pathlib import Path import numpy as np @@ -100,7 +101,7 @@ class ParamSetup: extended = base_setup.update({"NEW_PARAM": 1.0}) """ - def __init__(self, params: "dict[str, int | float]"): + def __init__(self, params: "Mapping[str, int | float]"): self._params: "dict[str, int | float]" = { k: (int(v) if isinstance(v, int) else float(v)) for k, v in params.items() @@ -436,7 +437,7 @@ def _terminate_process(proc: subprocess.Popen) -> None: # os.killpg because processes may not be their own process group leader # (e.g. when start_new_session is not set). try: - os.kill(proc.pid, signal.SIGTERM) + os.kill(proc.pid, getattr(signal, "SIGTERM", 15)) except ProcessLookupError: return try: @@ -445,7 +446,7 @@ def _terminate_process(proc: subprocess.Popen) -> None: except subprocess.TimeoutExpired: pass try: - os.kill(proc.pid, signal.SIGKILL) + os.kill(proc.pid, getattr(signal, "SIGKILL", 9)) except ProcessLookupError: return proc.wait(timeout=5.0) @@ -502,7 +503,7 @@ def _kill_by_port(port: int, proto: str = "tcp") -> None: if os.readlink(fd_path) == target: pid = int(fd_path.split("/")[2]) try: - os.kill(pid, _signal.SIGKILL) + os.kill(pid, getattr(_signal, "SIGKILL", 9)) except (ProcessLookupError, PermissionError): pass except OSError: From d8dc39c692ef0dfc0e5db62e296e42d2ec23b453 Mon Sep 17 00:00:00 2001 From: Kristof Date: Sun, 19 Jul 2026 12:11:29 +0200 Subject: [PATCH 2/2] chore: finalize controller cleanup and SITL typing fixes --- analysis/analyse_descent.py | 439 ------------- analysis/analyse_pumping.py | 287 --------- analysis/autorotation_equilibrium.py | 134 ---- analysis/check_landing_forces.py | 86 --- analysis/compare_aero_beaupoil.py | 319 ---------- analysis/compare_aero_models.py | 302 --------- analysis/pump_envelope.py | 654 -------------------- design/flight_stack.md | 4 +- design/simulation.md | 39 +- design/sitl_flight_timeline.md | 5 +- envelope/CLAUDE.md | 33 +- envelope/analyse_envelope.py | 4 +- envelope/compute_map.py | 66 +- envelope/point_mass.py | 47 +- envelope/rotor_helpers.py | 20 + simulation/README.md | 2 +- simulation/controller.py | 587 +----------------- simulation/mediator.py | 8 +- simulation/physics_core.py | 15 +- simulation/requirements-docker.txt | 2 +- simulation/requirements.txt | 2 +- simulation/steady_state_starting.json | 22 +- simulation/telemetry_csv.py | 18 +- tests/envelope/test_collective_sign.py | 43 +- tests/envelope/test_tension_continuation.py | 37 +- tests/envelope/test_tension_ramp_30deg.py | 37 +- tests/simtests/test_generate_ic.py | 4 +- tests/simtests/test_ground_liftoff.py | 47 +- tests/unit/test_controller.py | 405 +----------- tests/unit/test_force_balance.py | 8 +- tests/unit/test_telemetry_aero_columns.py | 52 ++ viz3d/visualize_3d.py | 12 +- 32 files changed, 335 insertions(+), 3405 deletions(-) delete mode 100644 analysis/analyse_descent.py delete mode 100644 analysis/analyse_pumping.py delete mode 100644 analysis/autorotation_equilibrium.py delete mode 100644 analysis/check_landing_forces.py delete mode 100644 analysis/compare_aero_beaupoil.py delete mode 100644 analysis/compare_aero_models.py delete mode 100644 analysis/pump_envelope.py create mode 100644 envelope/rotor_helpers.py create mode 100644 tests/unit/test_telemetry_aero_columns.py diff --git a/analysis/analyse_descent.py b/analysis/analyse_descent.py deleted file mode 100644 index 3e83d51..0000000 --- a/analysis/analyse_descent.py +++ /dev/null @@ -1,439 +0,0 @@ -""" -analyse_descent.py -- Steady autorotation equilibrium for vertical powered descent. - -Simulates the hub as a point mass descending vertically with a horizontal disk. -Cyclic is controlled by a horizontal-velocity PID to maintain zero horizontal -drift. Collective and tether tension are fixed inputs. The simulation runs -until omega and descent rate converge (or timeout). - -Physics -------- -- Disk always horizontal: body_z = [0, 0, -1] (NED) -- Tether tension T_tether [N] acts purely downward (el_deg=90) -- extra gravity. -- Spin ODE: d(omega)/dt = Q_aero / I_spin -- Vertical: d(vel_z)/dt = (thrust - weight - T_tether) / mass -- Horizontal: d(vel_xy)/dt = F_horiz/mass, cyclic PID drives F_horiz -> 0 -- Peters-He inflow carried forward each step. - -Thrust sign ------------ -Upward thrust = dot(F_world, [0,0,-1]) = -F_world[2]. -NEVER negate disk_normal: dot(F_world, -disk_normal) gives DOWNWARD component. -Verified by test_hover_sign.py. - -Usage ------ - python analysis/analyse_descent.py - python analysis/analyse_descent.py --wind 0 --col 0.02 --tension 176 - python analysis/analyse_descent.py --wind 5 --col 0.02 --tension 200 -""" -from __future__ import annotations - -import argparse -import math -import sys -from pathlib import Path - -import numpy as np - - -from dynbem import rotor_definition as rd -from dynbem import create_aero -from simulation.frames import build_orb_frame - - -_BZ_VERT = np.array([0.0, 0.0, -1.0]) -_G = 9.81 - - -def simulate_descent( - col: float, - T_tether: float, - wind_speed: float = 0.0, - omega_init: float = 5.0, - vel_z_init: float = 0.5, - dt: float = 0.01, - t_end: float = 30.0, - kp_cyc: float = 0.5, - ki_cyc: float = 0.1, -) -> dict: - """ - Simulate descent at fixed collective and tether tension. - - A horizontal-velocity PID drives cyclic to keep vel_xy near zero. - Returns dict with time histories and final equilibrium values. - """ - rotor = rd.default() - aero = create_aero(rotor, model="peters_he") - dk = rotor.dynamics_kwargs() - mass = dk["mass"] - I_spin = dk["I_spin"] - weight = mass * _G - - R = build_orb_frame(_BZ_VERT) - wind = np.array([0.0, float(wind_speed), 0.0]) - - # State - omega = float(omega_init) - vel = np.array([0.0, 0.0, float(vel_z_init)]) - c_lon = 0.0 - c_lat = 0.0 - int_vx = 0.0 - int_vy = 0.0 - - history = [] - n_steps = int(t_end / dt) - - for i in range(n_steps): - t = i * dt - - # Aero forces with current state - hub_vel = vel.copy() - f = aero.compute_forces(col, c_lon, c_lat, R, hub_vel, omega, wind) - - thrust = float(np.dot(f.F_world, _BZ_VERT)) # upward = -F_world[2] - Q_net = float(np.dot(aero.last_M_spin, _BZ_VERT)) - F_horiz = f.F_world[:2].copy() # NED North, East - - # Spin ODE - d_omega = Q_net / I_spin - omega = max(0.0, omega + d_omega * dt) - - # Vertical dynamics: positive vel_z = downward (NED) - F_vert_net = thrust - weight - T_tether - vel[2] += (-F_vert_net / mass) * dt # NED: downward accel if F_vert_net < 0 - - # Horizontal dynamics - vel[:2] += (F_horiz / mass) * dt - - # Cyclic PID: drive horizontal velocity to zero - int_vx += vel[0] * dt - int_vy += vel[1] * dt - c_lon = -(kp_cyc * vel[0] + ki_cyc * int_vx) - c_lat = -(kp_cyc * vel[1] + ki_cyc * int_vy) - # Clamp to physically reasonable range - c_lon = float(np.clip(c_lon, -0.15, 0.15)) - c_lat = float(np.clip(c_lat, -0.15, 0.15)) - - if i % int(1.0 / dt) == 0: - history.append(dict( - t=t, omega=omega, vel_z=vel[2], vel_horiz=float(np.linalg.norm(vel[:2])), - thrust=thrust, Q_net=Q_net, F_vert_net=F_vert_net, - c_lon=math.degrees(c_lon), c_lat=math.degrees(c_lat), - )) - - # Final 5 s average as equilibrium estimate - final = [h for h in history if h["t"] >= t_end - 5.0] - if final: - eq = {k: float(np.mean([h[k] for h in final])) - for k in ("omega", "vel_z", "thrust", "Q_net", "F_vert_net", "c_lon", "c_lat")} - else: - eq = history[-1] if history else {} - - return dict(history=history, eq=eq, converged=len(final) > 0) - - -def analyse( - wind_speed: float = 0.0, - col: float = 0.02, - T_tether: float = 176.0, - omega_init: float = 5.0, -) -> None: - print(f"\nDescent simulation -- wind={wind_speed} m/s col={math.degrees(col):.1f} deg " - f"T_tether={T_tether:.0f} N") - - result = simulate_descent(col, T_tether, wind_speed=wind_speed, omega_init=omega_init) - history = result["history"] - eq = result["eq"] - - print(f"\n {'t':>5} {'omega':>7} {'vel_z':>7} {'thrust':>8} " - f"{'Q_net':>8} {'F_vert':>8} {'c_lon_deg':>9} {'c_lat_deg':>9}") - print(" " + "-" * 75) - for h in history: - print(f" {h['t']:>5.1f} {h['omega']:>7.2f} {h['vel_z']:>7.3f} " - f"{h['thrust']:>8.1f} {h['Q_net']:>8.3f} {h['F_vert_net']:>8.1f} " - f"{h['c_lon']:>9.2f} {h['c_lat']:>9.2f}") - - rotor = rd.default() - mass = rotor.dynamics_kwargs()["mass"] - weight = mass * _G - print(f"\n Equilibrium (last 5 s avg):") - print(f" omega = {eq.get('omega',0):.2f} rad/s") - print(f" vel_z = {eq.get('vel_z',0):.3f} m/s (NED, positive=down)") - print(f" thrust = {eq.get('thrust',0):.1f} N (weight={weight:.1f} N + tether={T_tether:.0f} N = {weight+T_tether:.1f} N)") - print(f" F_vert = {eq.get('F_vert_net',0):.2f} N (0 = perfect balance)") - print(f" c_lon = {eq.get('c_lon',0):.2f} deg") - print(f" c_lat = {eq.get('c_lat',0):.2f} deg") - - -def simulate_descent_bem_ode( - col: float, - T_tether: float, - wind_speed: float = 0.0, - omega_init: float = 5.0, - vel_z_init: float = 0.5, - dt: float = 0.01, - t_end: float = 30.0, - kp_cyc: float = 0.5, - ki_cyc: float = 0.1, -) -> dict: - """ - Same point-mass ODE as simulate_descent() but uses q_spin_from_aero + - step_spin_ode explicitly, with I_ode_kgm2 (not I_spin) — matching PhysicsCore. - """ - from simulation.physics_core import q_spin_from_aero, step_spin_ode - - rotor = rd.default() - dk = rotor.dynamics_kwargs() - mass = dk["mass"] - weight = mass * _G - I_ode = rotor.I_ode_kgm2 - omega_min = rotor.omega_min_rad_s - - aero = create_aero(rotor, model="peters_he") - R = build_orb_frame(_BZ_VERT) - wind = np.array([0.0, float(wind_speed), 0.0]) - - omega = float(omega_init) - vel = np.array([0.0, 0.0, float(vel_z_init)]) - c_lon = 0.0 - c_lat = 0.0 - int_vx = 0.0 - int_vy = 0.0 - - history = [] - n_steps = int(t_end / dt) - - for i in range(n_steps): - t = i * dt - - f = aero.compute_forces(col, c_lon, c_lat, R, vel.copy(), omega, wind) - - thrust = float(np.dot(f.F_world, _BZ_VERT)) - Q_spin = q_spin_from_aero(aero, R) - F_horiz = f.F_world[:2].copy() - F_vert_net = thrust - weight - T_tether - - omega = step_spin_ode(omega, Q_spin, I_ode, omega_min, dt) - vel[2] += (-F_vert_net / mass) * dt - vel[:2] += (F_horiz / mass) * dt - - int_vx += vel[0] * dt - int_vy += vel[1] * dt - c_lon = float(np.clip(-(kp_cyc * vel[0] + ki_cyc * int_vx), -0.15, 0.15)) - c_lat = float(np.clip(-(kp_cyc * vel[1] + ki_cyc * int_vy), -0.15, 0.15)) - - if i % int(1.0 / dt) == 0: - history.append(dict( - t=t, omega=omega, vel_z=vel[2], - thrust=thrust, Q_spin=Q_spin, F_vert_net=F_vert_net, - c_lon=math.degrees(c_lon), - )) - - final = [h for h in history if h["t"] >= t_end - 5.0] - eq = ({k: float(np.mean([h[k] for h in final])) - for k in ("omega", "vel_z", "thrust", "F_vert_net", "c_lon")} - if final else (history[-1] if history else {})) - return dict(history=history, eq=eq) - - -def simulate_descent_physics( - col: float, - T_tether: float, - wind_speed: float = 0.0, - omega_init: float = 5.0, - vel_z_init: float = 0.5, - altitude: float = 500.0, - dt: float = 1.0 / 400.0, - t_end: float = 30.0, - kp_cyc: float = 0.5, - ki_cyc: float = 0.1, -) -> dict: - """ - Same scenario as simulate_descent() but using PhysicsCore directly + Peters-He. - - PhysicsRunner is intentionally avoided — it bakes in HeliCyclicController (rate PID - + servo lag) which would process c_lon/c_lat as rate setpoints rather than tilt - angles, making comparison with the other two functions meaningless. - """ - from types import SimpleNamespace - from simulation.physics_core import PhysicsCore, q_spin_from_aero - from simulation.param_defaults import load_collective_phys_range as _lr - - rotor = rd.default() - bz_vert = np.array([0.0, 0.0, -1.0]) - R0 = build_orb_frame(bz_vert) - wind = np.array([0.0, float(wind_speed), 0.0]) - - _col_min, _col_max = _lr() - ic = SimpleNamespace( - pos = np.array([0.0, 0.0, -float(altitude)]), - vel = np.array([0.0, 0.0, float(vel_z_init)]), - R0 = R0, - rest_length = float(altitude), - eq_thrust = (float(col) - _col_min) / (_col_max - _col_min), - omega_spin = float(omega_init), - ) - - core = PhysicsCore(rotor, ic, wind, - aero_model="peters_he", - z_floor=1.0) - - # Replace tether with constant downward force (NED +Z = downward). - _F_tether = np.array([0.0, 0.0, float(T_tether)]) - _M_zero = np.zeros(3) - core._tether.compute = lambda pos, vel, R=None: (_F_tether, _M_zero) - core._tension_now = float(T_tether) - - c_lon = 0.0 - c_lat = 0.0 - int_vx = 0.0 - int_vy = 0.0 - - history = [] - log_every = max(1, int(1.0 / dt)) - - n_steps = int(t_end / dt) - for i in range(n_steps): - t = i * dt - - hub = core.hub_state - vel = hub["vel"] - omega = core.omega_spin - - # Cyclic PID on horizontal velocity (same logic as other two functions) - int_vx += vel[0] * dt - int_vy += vel[1] * dt - c_lon = float(np.clip(-(kp_cyc * vel[0] + ki_cyc * int_vx), -0.15, 0.15)) - c_lat = float(np.clip(-(kp_cyc * vel[1] + ki_cyc * int_vy), -0.15, 0.15)) - - sr = core.step(dt, col, c_lon, c_lat) - # tension_now is overwritten from _last_info after each step; restore it - core._tension_now = float(T_tether) - # Keep disk horizontal: reset R and zero orbital angular velocity after each step. - # This matches what simulate_descent and simulate_descent_bem_ode do (fixed R=R0). - core._dyn._R[:] = R0 - core._dyn._omega[:] = 0.0 - - if i % log_every == 0: - aero_res = sr.get("aero_result") - thrust_up = float(np.dot(aero_res.F_world, bz_vert)) if aero_res is not None else float(np.nan) - history.append(dict( - t=t, omega=omega, vel_z=float(vel[2]), - vel_horiz=float(np.linalg.norm(vel[:2])), - tension=float(T_tether), - thrust=thrust_up, - c_lon=math.degrees(c_lon), c_lat=math.degrees(c_lat), - )) - - final = [h for h in history if h["t"] >= t_end - 5.0] - if final: - eq = {k: float(np.mean([h[k] for h in final])) - for k in ("omega", "vel_z", "tension", "thrust", "c_lon", "c_lat")} - else: - eq = history[-1] if history else {} - - return dict(history=history, eq=eq) - - -def compare( - col: float = 0.02, - T_tether: float = 176.0, - wind_speed: float = 0.0, - omega_init: float = 5.0, -) -> None: - """Run both models side-by-side and compare equilibrium.""" - rotor = rd.default() - mass = rotor.dynamics_kwargs()["mass"] - weight = mass * _G - - print(f"\nComparison -- wind={wind_speed} m/s col={math.degrees(col):.1f} deg " - f"T_tether={T_tether:.0f} N (weight={weight:.1f} N, total={weight+T_tether:.1f} N)") - - r_simple = simulate_descent(col, T_tether, wind_speed=wind_speed, omega_init=omega_init) - r_bem = simulate_descent_bem_ode(col, T_tether, wind_speed=wind_speed, omega_init=omega_init) - r_physics = simulate_descent_physics(col, T_tether, wind_speed=wind_speed, omega_init=omega_init) - - eq_s = r_simple["eq"] - eq_b = r_bem["eq"] - eq_p = r_physics["eq"] - - print(f"\n {'':20} {'simple(I_spin)':>14} {'bem_ode(I_ode)':>14} {'PhysicsRunner':>14}") - print(f" {'':20} {'--------------':>14} {'--------------':>14} {'-------------':>14}") - print(f" {'omega [rad/s]':20} {eq_s.get('omega',0):>14.2f} {eq_b.get('omega',0):>14.2f} {eq_p.get('omega',0):>14.2f}") - print(f" {'vel_z [m/s]':20} {eq_s.get('vel_z',0):>14.3f} {eq_b.get('vel_z',0):>14.3f} {eq_p.get('vel_z',0):>14.3f}") - print(f" {'thrust [N]':20} {eq_s.get('thrust',0):>14.1f} {eq_b.get('thrust',0):>14.1f} {eq_p.get('thrust',0):>14.1f}") - print(f" {'c_lon [deg]':20} {eq_s.get('c_lon',0):>14.2f} {eq_b.get('c_lon',0):>14.2f} {eq_p.get('c_lon',0):>14.2f}") - - print(f"\n Time history (1 Hz, all three):") - print(f" {'t':>5} {'om_s':>6} {'vz_s':>6} | {'om_b':>6} {'vz_b':>6} | {'om_p':>6} {'vz_p':>6} {'ten_p':>7}") - print(" " + "-" * 70) - hs = r_simple["history"] - hb = r_bem["history"] - hp = r_physics["history"] - for i in range(min(len(hs), len(hb), len(hp))): - print(f" {hs[i]['t']:>5.1f}" - f" {hs[i]['omega']:>6.2f} {hs[i]['vel_z']:>6.3f}" - f" | {hb[i]['omega']:>6.2f} {hb[i]['vel_z']:>6.3f}" - f" | {hp[i]['omega']:>6.2f} {hp[i]['vel_z']:>6.3f}" - f" {hp[i]['tension']:>7.1f}") - - -def sweep( - wind_speed: float = 0.0, - v_desc_targets: "list[float]" = (0.5, 1.0, 1.5), - tension_list: "list[float]" = (0.0, 50.0, 100.0, 150.0, 200.0), - col_list: "list[float] | None" = None, -) -> None: - """Sweep (col, T_tether) and find best operating points.""" - if col_list is None: - col_list = list(np.linspace(-0.10, 0.20, 7)) - - rotor = rd.default() - mass = rotor.dynamics_kwargs()["mass"] - weight = mass * _G - - print(f"\nSweep -- wind={wind_speed} m/s weight={weight:.1f} N") - print(f" {'col_deg':>8} {'T_tether':>9} {'omega_eq':>8} {'vel_z_eq':>9} " - f"{'thrust_eq':>10} {'F_vert':>8} {'c_lon':>7} converged?") - print(" " + "-" * 85) - - for col in col_list: - for T_tether in tension_list: - r = simulate_descent(col, T_tether, wind_speed=wind_speed) - eq = r["eq"] - ok = "YES" if r["converged"] and abs(eq.get("F_vert_net", 999)) < 10.0 else "---" - print(f" {math.degrees(col):>8.1f} {T_tether:>9.0f} " - f"{eq.get('omega',0):>8.2f} {eq.get('vel_z',0):>9.3f} " - f"{eq.get('thrust',0):>10.1f} {eq.get('F_vert_net',0):>8.1f} " - f"{eq.get('c_lon',0):>7.2f} {ok}") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Descent equilibrium simulation") - parser.add_argument("--wind", type=float, default=0.0, - help="crosswind speed [m/s] (default 0)") - parser.add_argument("--col", type=float, default=None, - help="fixed collective [rad] — if given, run single simulation") - parser.add_argument("--tension", type=float, default=None, - help="tether tension [N] — if given with --col, run single sim") - parser.add_argument("--sweep", action="store_true", - help="sweep (col, tension) grid and print table") - parser.add_argument("--compare", action="store_true", - help="run simple ODE and PhysicsRunner side-by-side") - parser.add_argument("--omega0", type=float, default=5.0, - help="initial omega [rad/s] (default 5)") - args = parser.parse_args() - - if args.compare: - col = args.col if args.col is not None else 0.02 - tension = args.tension if args.tension is not None else 176.0 - compare(col=col, T_tether=tension, wind_speed=args.wind, omega_init=args.omega0) - elif args.col is not None and args.tension is not None: - analyse(wind_speed=args.wind, col=args.col, - T_tether=args.tension, omega_init=args.omega0) - elif args.sweep: - sweep(wind_speed=args.wind) - else: - # Default: show zero-wind equilibrium for the known good operating point - analyse(wind_speed=args.wind, col=0.02, T_tether=176.0, omega_init=5.0) diff --git a/analysis/analyse_pumping.py b/analysis/analyse_pumping.py deleted file mode 100644 index 6b037c5..0000000 --- a/analysis/analyse_pumping.py +++ /dev/null @@ -1,287 +0,0 @@ -""" -analyse_pumping.py -- Steady-state collective sweep for pumping cycle reel-out. - -Sweeps collective values. For each, runs a simple point-mass ODE with: - - Constant tether force vector (idealized, no elasticity) - - 3-axis velocity PI -> cyclic (lon/lat) to null horizontal drift - - Fixed disk orientation (body_z set from force-balance geometry at start) - - Peters-He aero (valid at all skew angles) - -Goal: find which collective produces zero net acceleration (vel stays near 0). - -Convention ----------- - Tether force : East (+Y in NED) + downward (+Z) component from elevation - Wind : West (-Y in NED) - NED : X=North, Y=East, Z=Down; gravity = [0, 0, +m*g] - -Usage ------ - .venv/Scripts/python.exe analysis/analyse_pumping.py - .venv/Scripts/python.exe analysis/analyse_pumping.py --wind 8 - .venv/Scripts/python.exe analysis/analyse_pumping.py --el 30 --tension 400 --wind 10 - .venv/Scripts/python.exe analysis/analyse_pumping.py --col -0.21 # single run, verbose -""" -from __future__ import annotations - -import argparse -import math -import sys -from pathlib import Path - -import numpy as np - - -from dynbem import rotor_definition as rd -from simulation.frames import build_orb_frame -from simulation.physics_core import PhysicsCore -from simulation.ic import load_ic - - -_G = 9.81 - - -# ── Geometry helpers ─────────────────────────────────────────────────────────── - -def _tether_force_ned(elevation_deg: float, tension_n: float) -> np.ndarray: - """ - Constant tether force on hub in NED. - - Hub is west of and above anchor. - Tether pulls hub eastward and downward: [0, +cos(el), +sin(el)] * T. - """ - el = math.radians(elevation_deg) - return tension_n * np.array([0.0, math.cos(el), math.sin(el)]) - - -def _balance_bz(F_teth: np.ndarray, mass: float) -> np.ndarray: - """ - Disk normal (body_z) that balances tether + gravity. - - The thrust must be equal and opposite to (tether + gravity) at equilibrium. - Thrust direction = body_z, so body_z = -normalize(F_teth + F_grav). - """ - F_grav = np.array([0.0, 0.0, mass * _G]) - F_load = F_teth + F_grav - return -F_load / np.linalg.norm(F_load) - - -# ── Single simulation run (PhysicsCore, evolving R) ─────────────────────────── - -def simulate_one( - col: float, - elevation_deg: float = 30.0, - tension_n: float = 400.0, - wind_speed: float = 10.0, - v_reel_in: float = 0.0, - omega_init: float | None = None, - dt: float = 1.0 / 400.0, - t_end: float = 40.0, - verbose: bool = False, -) -> dict: - """ - Run PhysicsCore at fixed collective with evolving disk orientation. - - v_reel_in > 0: hub starts moving along the tether toward the anchor - (models reel-in descent). Cyclic nulls velocity TRANSVERSE to the - tether axis only — along-tether velocity evolves freely, driven by - the force balance. Its equilibrium value is the steady reel-in speed. - - v_reel_in = 0: pure hover equilibrium sweep (reel-out). - """ - import json - from types import SimpleNamespace - - rotor = rd.default() - mass = rotor.dynamics_kwargs()["mass"] - - _ic = load_ic() - if omega_init is None: - omega_init = _ic.omega_spin - from simulation.param_defaults import thrust_to_coll_rad as _t2c # noqa: F811 - col_warm = _t2c(_ic.eq_thrust) - - el = math.radians(elevation_deg) - L = 100.0 - # Hub west of anchor, elevated: pos = [0, -L*cos(el), -L*sin(el)] - pos0 = np.array([0.0, -L * math.cos(el), -L * math.sin(el)]) - - # Tether unit vector hub -> anchor (East + Down in NED) - t_hat = np.array([0.0, math.cos(el), math.sin(el)]) - - F_teth = _tether_force_ned(elevation_deg, tension_n) # = tension_n * t_hat - bz0 = _balance_bz(F_teth, mass) - R0 = build_orb_frame(bz0) - wind = np.array([0.0, -wind_speed, 0.0]) # West in NED - - # Initial velocity: along tether toward anchor (reel-in descent) - vel0 = float(v_reel_in) * t_hat - - ic = SimpleNamespace( - pos = pos0, - vel = vel0.copy(), - R0 = R0, - rest_length = L, - eq_thrust = _ic.eq_thrust, - omega_spin = omega_init, - ) - - # NOTE: without an attitude controller the disk will tumble after ~0.1 s. - # This analysis script is only valid for short time windows or when R0 is - # already close to equilibrium and base_k_ang damps rotational divergence. - core = PhysicsCore(rotor, ic, wind, aero_model="peters_he", - z_floor=-500.0) - - M_zero = np.zeros(3) - core._tether.compute = lambda _p, _v, _R=None: (F_teth, M_zero) # type: ignore[assignment] - core._tension_now = float(tension_n) - - omega_floor = rotor.omega_min_rad_s - - history = [] - log_every = max(1, int(1.0 / dt)) - - n_steps = int(t_end / dt) - for i in range(n_steps): - t = i * dt - - hub = core.hub_state - vel = hub["vel"] - bz_now = hub["R"][:, 2] - - # Clamp spin before step to keep aero model valid - if core._omega_spin < omega_floor: - core._omega_spin = omega_floor - - # Disk stays near equilibrium for short runs due to base_k_ang damping; - # no cyclic needed for quasi-static descent analysis. - sr = core.step(dt, col, 0.0, 0.0) - core._tension_now = float(tension_n) - - if i % log_every == 0: - ar = sr.get("aero_result") - thrust = float(np.dot(ar.F_world, bz_now)) if ar is not None else float("nan") - v_along = float(np.dot(vel, t_hat)) - v_trans = float(np.linalg.norm(vel - v_along * t_hat)) - history.append(dict( - t = round(t, 3), - omega = round(core.omega_spin, 3), - v_along = round(v_along, 4), - v_trans = round(v_trans, 4), - vel_z = round(float(vel[2]), 4), - thrust = round(thrust, 2), - )) - - final = [h for h in history if h["t"] >= t_end - 8.0] - eq = ({k: round(float(np.mean([h[k] for h in final])), 5) - for k in ("omega", "v_along", "v_trans", "vel_z", "thrust")} - if final else (history[-1] if history else {})) - - if verbose: - print(f"\n col={math.degrees(col):.2f} deg v_reel_in_init={v_reel_in:.2f} m/s") - print(f" {'t':>5} {'omega':>7} {'v_along':>8} {'v_trans':>8} {'thrust':>8}") - print(" " + "-" * 55) - for h in history: - print(f" {h['t']:>5.1f} {h['omega']:>7.2f} {h['v_along']:>8.3f} " - f"{h['v_trans']:>8.3f} {h['thrust']:>8.1f}") - - return dict(history=history, eq=eq) - - -# ── Sweep ────────────────────────────────────────────────────────────────────── - -def sweep( - elevation_deg: float = 30.0, - tension_n: float = 400.0, - wind_speed: float = 10.0, - v_reel_in: float = 0.0, - col_list: list[float] | None = None, - t_end: float = 40.0, -) -> None: - rotor = rd.default() - mass = rotor.dynamics_kwargs()["mass"] - weight = mass * _G - - F_teth = _tether_force_ned(elevation_deg, tension_n) - bz0 = _balance_bz(F_teth, mass) - F_load = F_teth + np.array([0.0, 0.0, weight]) - - print() - print(f"Reel-out equilibrium sweep") - print(f" el={elevation_deg} deg T={tension_n:.0f} N wind={wind_speed} m/s West") - print(f" Tether force NED : [{F_teth[0]:+.1f}, {F_teth[1]:+.1f}, {F_teth[2]:+.1f}] N") - print(f" Total load NED : [{F_load[0]:+.1f}, {F_load[1]:+.1f}, {F_load[2]:+.1f}] N") - print(f" Balance body_z : [{bz0[0]:+.4f}, {bz0[1]:+.4f}, {bz0[2]:+.4f}]") - print(f" Disk tilt from vertical: " - f"{math.degrees(math.acos(max(-1.0, min(1.0, float(-bz0[2]))))):.1f} deg") - print() - - if col_list is None: - col_list = list(np.linspace(-0.28, 0.02, 16)) - - mode = "reel-in" if v_reel_in > 0 else "hover" - print(f" mode={mode} v_reel_in_init={v_reel_in:.2f} m/s") - print() - print(f" {'col_deg':>8} {'omega':>7} {'v_along':>8} {'v_trans':>8} {'thrust':>8} note") - print(" " + "-" * 65) - - best_col = None - best_metric = 1e9 - - for col in col_list: - r = simulate_one(col, elevation_deg, tension_n, wind_speed, - v_reel_in=v_reel_in, t_end=t_end) - eq = r["eq"] - # For reel-in: look for stable v_along (not exploding) and small v_trans - # For hover: look for small v_along - v_along = eq.get("v_along", float("nan")) - v_trans = eq.get("v_trans", float("nan")) - metric = abs(v_trans) + (abs(v_along - v_reel_in) if v_reel_in > 0 else abs(v_along)) - - note = "" - if metric < best_metric and math.isfinite(metric): - best_metric = metric - best_col = col - if v_trans < 0.5 and abs(v_along - v_reel_in) < 0.5: - note = "<-- stable" - elif v_along > 5: - note = "(accelerating in)" - elif v_along < -2: - note = "(climbing out)" - - print(f" {math.degrees(col):>8.2f} " - f"{eq.get('omega', 0):>7.2f} " - f"{v_along:>8.3f} " - f"{v_trans:>8.3f} " - f"{eq.get('thrust', 0):>8.1f} " - f"{note}") - - if best_col is not None: - print() - print(f" Best collective: {math.degrees(best_col):.2f} deg ({best_col:.4f} rad)") - - -# ── CLI ──────────────────────────────────────────────────────────────────────── - -if __name__ == "__main__": - p = argparse.ArgumentParser(description="Reel-out collective sweep") - p.add_argument("--el", type=float, default=30.0, help="elevation angle [deg]") - p.add_argument("--tension", type=float, default=400.0, help="tether tension [N]") - p.add_argument("--wind", type=float, default=10.0, help="wind speed [m/s]") - p.add_argument("--t-end", type=float, default=40.0, help="sim time [s]") - p.add_argument("--v-reel-in",type=float, default=0.0, - help="reel-in speed along tether [m/s] (0=hover/reel-out)") - p.add_argument("--col", type=float, default=None, - help="single collective [rad] — verbose time history") - args = p.parse_args() - - if args.col is not None: - r = simulate_one(args.col, args.el, args.tension, args.wind, - v_reel_in=args.v_reel_in, t_end=args.t_end, verbose=True) - eq = r["eq"] - print(f"\n Equilibrium (last 8 s avg):") - for k, v in eq.items(): - print(f" {k:<14} = {v}") - else: - sweep(args.el, args.tension, args.wind, - v_reel_in=args.v_reel_in, t_end=args.t_end) diff --git a/analysis/autorotation_equilibrium.py b/analysis/autorotation_equilibrium.py deleted file mode 100644 index 973a925..0000000 --- a/analysis/autorotation_equilibrium.py +++ /dev/null @@ -1,134 +0,0 @@ -""" -autorotation_equilibrium.py — Free-autorotation equilibrium RPM at 10 m/s wind. - -Finds the omega where Q_spin = 0 (no mechanical resistance): - - Wind: 10 m/s East in NED = [0, 10, 0] - - Collective: 0 rad - - Two disk orientations: horizontal (body_z up) and design tether angle - -Uses PetersHeBEMJit (production model). Settles inflow ODE for N_SETTLE steps -before sampling Q_spin, then uses scipy.optimize.brentq for exact root. - -Usage: - .venv/Scripts/python.exe analysis/autorotation_equilibrium.py -""" - -import sys -import math -from pathlib import Path - -import numpy as np -from scipy.optimize import brentq - - -from dynbem import rotor_definition as rd -from dynbem.aero_peters_he_jit import PetersHeBEMJit - -WIND = np.array([0.0, 10.0, 0.0]) # NED: East at 10 m/s -COL = 0.0 # rad: free autorotation, no blade pitch -N_SETTLE = 400 # time steps to let inflow converge -DT = 0.01 # s per step (4 s total settle time) - - -def make_R_hub(body_z: np.ndarray) -> np.ndarray: - """Build rotation matrix from disk normal (body_z = third column).""" - bz = body_z / np.linalg.norm(body_z) - # Reference direction: East; fall back to North if degenerate - ref = np.array([0.0, 1.0, 0.0]) - if abs(np.dot(ref, bz)) > 0.99: - ref = np.array([1.0, 0.0, 0.0]) - bx = ref - np.dot(ref, bz) * bz - bx /= np.linalg.norm(bx) - by = np.cross(bz, bx) - return np.column_stack([bx, by, bz]) - - -def q_spin_settled(rotor, omega: float, R_hub: np.ndarray) -> float: - """Run the ODE for N_SETTLE steps at fixed omega, return steady Q_spin.""" - model = PetersHeBEMJit(rotor) - t = 0.0 - q = 0.0 - for _ in range(N_SETTLE): - t += DT - result = model.compute_forces( - collective_rad=COL, - tilt_lon=0.0, tilt_lat=0.0, - R_hub=R_hub, - v_hub_world=np.zeros(3), - omega_rotor=omega, - wind_world=WIND, - ) - q = result.Q_spin - return q - - -def find_equilibrium(rotor, R_hub, label: str) -> float: - """Print a Q_spin sweep and use brentq to find the autorotation equilibrium.""" - print(f"\n--- {label} ---") - print(f" wind = {WIND} m/s (East, NED)") - disk_n = R_hub[:, 2] - v_ax = float(np.dot(WIND, disk_n)) - v_ip = float(np.linalg.norm(WIND - v_ax * disk_n)) - print(f" disk_normal = [{disk_n[0]:.3f}, {disk_n[1]:.3f}, {disk_n[2]:.3f}]") - print(f" v_axial = {v_ax:.2f} m/s, v_inplane = {v_ip:.2f} m/s") - print(f" collective = {math.degrees(COL):.1f} deg") - - print(f"\n {'omega rad/s':>12} {'RPM':>8} {'Q_spin N*m':>12}") - omegas = [1, 3, 5, 8, 10, 14, 18, 22, 26, 30, 40, 50, 65, 80, 100] - q_values = {} - for om in omegas: - q = q_spin_settled(rotor, float(om), R_hub) - q_values[om] = q - print(f" {om:>12.1f} {om*60/(2*math.pi):>8.1f} {q:>12.2f}") - - # Find bracket for brentq - # Find first sign change in sweep - bracket_lo, bracket_hi = None, None - sorted_oms = sorted(omegas) - for i in range(len(sorted_oms) - 1): - a, b = sorted_oms[i], sorted_oms[i + 1] - if q_values[a] * q_values[b] < 0: - bracket_lo, bracket_hi = a, b - break - - if bracket_lo is None: - print("\n WARNING: no sign change found in sweep range") - return float("nan") - - lo, hi = float(bracket_lo or 0), float(bracket_hi or 0) - - omega_eq = float(brentq( # type: ignore[call-overload] - lambda om: q_spin_settled(rotor, float(om), R_hub), - float(lo), float(hi), - xtol=0.01, maxiter=30, - )) - rpm_eq = omega_eq * 60.0 / (2.0 * math.pi) - tip_speed = omega_eq * rotor.radius_m - tsr = tip_speed / 10.0 # tip-speed ratio at 10 m/s wind - - print(f"\n EQUILIBRIUM: omega = {omega_eq:.2f} rad/s ({rpm_eq:.1f} RPM)") - print(f" tip speed = {tip_speed:.1f} m/s") - print(f" TSR (tip-speed ratio) = {tsr:.2f}") - return omega_eq - - -def main(): - rotor = rd.default() - print(f"Rotor: {rotor.name}") - print(f" R={rotor.radius_m} m, r_root={rotor.root_cutout_m} m, " - f"N={rotor.n_blades}, c={rotor.chord_m} m") - print(f" CL0={rotor.CL0:.3f}, CL_alpha={rotor.CL_alpha_per_rad:.3f}/rad, " - f"CD0={rotor.CD0:.4f}") - print(f" CL at col=0: {rotor.CL0 + rotor.CL_alpha_per_rad * COL:.3f}") - - # ── Pure axial flow: disk facing East, all wind is axial ───────────────── - # body_z = East = [0,1,0] in NED. Wind [0,10,0] is entirely axial. - # This is the cleanest case: classic windmill/autogyro in axial inflow. - body_z_axial = np.array([0.0, 1.0, 0.0]) - R_axial = make_R_hub(body_z_axial) - - find_equilibrium(rotor, R_axial, "Pure axial flow (disk facing East, v_axial=10 m/s, v_inplane=0)") - - -if __name__ == "__main__": - main() diff --git a/analysis/check_landing_forces.py b/analysis/check_landing_forces.py deleted file mode 100644 index a2357d9..0000000 --- a/analysis/check_landing_forces.py +++ /dev/null @@ -1,86 +0,0 @@ -""" -check_landing_forces.py -- Force balance sanity check for landing descent at xi=80 deg. - -At xi=80 deg, disk is nearly horizontal. For equilibrium: - - Thrust T along disk_normal must support weight + tether tension component - - Print thrust vs collective sweep to find the actual col_min for hover -""" -import sys, math -from pathlib import Path -import numpy as np - -from dynbem import rotor_definition as rd -from dynbem import create_aero -from simulation.controller import col_min_for_altitude_rad -from tests.simtests.simtest_ic import load_ic - -WIND = np.array([0.0, 10.0, 0.0]) -XI_DEG = 80.0 -OMEGA_SPIN = 20.3 # rad/s (from IC) -T_AERO = 50.0 # well past ramp -MASS_KG = rd.default().mass_kg -G = 9.81 - -rotor = rd.default() -aero = create_aero(rotor) -IC = load_ic() - -# Build R_hub at xi=80 deg with equilibrium yaw (body_x = East projected) -xi_rad = math.radians(XI_DEG) -# disk_normal at xi=80 from vertical: tilted 80 deg from [0,0,-1] -# pointing mostly South + slightly up: [sin(xi), 0, -cos(xi)] if tilt toward North -# Use same direction as IC but scaled to xi=80: -bz_unit = IC.R0[:, 2] / np.linalg.norm(IC.R0[:, 2]) -# Build disk_normal at xi=80 in the same azimuthal direction as IC -bz_horiz = bz_unit.copy(); bz_horiz[2] = 0.0 -bz_horiz_norm = np.linalg.norm(bz_horiz) -if bz_horiz_norm > 1e-6: - bz_horiz /= bz_horiz_norm -else: - bz_horiz = np.array([1.0, 0.0, 0.0]) -disk_normal = math.cos(xi_rad) * bz_horiz + math.sin(xi_rad) * np.array([0.0, 0.0, -1.0]) -disk_normal /= np.linalg.norm(disk_normal) - -# Build orbital frame around disk_normal -east = np.array([0.0, 1.0, 0.0]) -ep = east - np.dot(east, disk_normal) * disk_normal -bx = ep / np.linalg.norm(ep) -by = np.cross(disk_normal, bx) -R_hub = np.column_stack([bx, by, disk_normal]) - -v_hub = np.zeros(3) - -weight = MASS_KG * G -print(f"Hub mass: {MASS_KG:.2f} kg weight: {weight:.1f} N") -print(f"xi = {XI_DEG} deg disk_normal = {disk_normal.round(3)}") -print(f"Axial wind component: {np.dot(WIND, disk_normal):.2f} m/s") -print(f"In-plane wind: {np.linalg.norm(WIND - np.dot(WIND, disk_normal)*disk_normal):.2f} m/s") -print() - -# At xi=80, the tether pulls the hub toward anchor. -# For a hub at (pos_x, pos_y, alt), tether direction ~ -pos/|pos| -# Approximate tether force: along -disk_normal direction (for nearly-tangential orbit) -# Actually at xi=80 the tether is roughly along -disk_normal (tangent tethered kite config) -# Weight component along disk_normal: -w_axial = weight * abs(np.dot(np.array([0.0, 0.0, 1.0]), disk_normal)) -print(f"Weight component along disk_normal (axial load): {w_axial:.1f} N") -print() - -# Sweep collective -print(f"{'coll_rad':>10} {'T_axial':>9} {'F_z':>8} {'net_axial':>10} {'K_skew':>7} {'v_i':>6}") -print("-" * 60) -for coll in np.arange(-0.30, 0.15, 0.01): - r = aero.compute_forces( - collective_rad=float(coll), - tilt_lon=0.0, tilt_lat=0.0, - R_hub=R_hub, v_hub_world=v_hub, - omega_rotor=OMEGA_SPIN, wind_world=WIND, - ) - T_axial = float(np.dot(r.F_world, disk_normal)) # thrust along disk axis - net = T_axial - w_axial # net axial force - print(f"{coll:10.3f} {T_axial:9.1f} {float(r.F_world[2]):8.1f} {net:10.1f} " - f"{aero.last_K_skew:7.4f} {aero.last_v_i:6.3f}") - -print() -print(f"col_min_for_altitude_rad(xi=80): {col_min_for_altitude_rad(aero, XI_DEG, MASS_KG, omega=OMEGA_SPIN):.4f} rad") -print(f"COL_CRUISE hardcoded: 0.0790 rad") diff --git a/analysis/compare_aero_beaupoil.py b/analysis/compare_aero_beaupoil.py deleted file mode 100644 index 670dfe9..0000000 --- a/analysis/compare_aero_beaupoil.py +++ /dev/null @@ -1,319 +0,0 @@ -""" -compare_aero_beaupoil.py -- PetersHeBEM vs SkewedWakeBEM on the beaupoil_2026 rotor. - -Two figures: - Fig 1 -- Tilt sweep 0-90deg (autorotation -> hover) at V=10 m/s, omega=20.15 rad/s - 4 panels: Thrust, v_i, Glauert diagram, CT vs mu - RAWES regime bands: reel-out (xi=20-65deg), reel-in (xi=65-80deg), - landing (xi=80-90deg) - - Fig 2 -- Wind speed sweep 5-20 m/s at two fixed tilts: - xi=45deg (mid reel-out), xi=70deg (reel-in) - 2 panels per tilt: Thrust vs V, v_i vs V - -Run: - python analysis/compare_aero_beaupoil.py -""" - -import math -import sys -from pathlib import Path - -import numpy as np -import matplotlib -matplotlib.use("Agg") -import matplotlib.pyplot as plt -from matplotlib.lines import Line2D - -from dynbem import rotor_definition as rd -from simulation.frames import build_orb_frame -from dynbem import create_aero, PetersHeBEM - -# --------------------------------------------------------------------------- -# Beaupoil rotor parameters -# --------------------------------------------------------------------------- -rotor = rd.default() # beaupoil_2026 -R_TIP = rotor.radius_m # 2.5 m -R_ROOT = rotor.root_cutout_m # 0.5 m -RHO = rotor.rho_kg_m3 # 1.22 kg/m3 -A_DISK = math.pi * (R_TIP**2 - R_ROOT**2) - -OMEGA = 20.15 # rad/s (omega_eq at V=10 m/s) -TIP_SPEED = OMEGA * R_TIP # ~50.4 m/s -COL_RAD = -0.10 # moderate collective (reel-out operating point) -T_RUN = 10.0 # past ramp - -# --------------------------------------------------------------------------- -# Regime definitions -# --------------------------------------------------------------------------- -REGIMES = [ - (0, 20, "#f0f0f0", "free spin\n(no tether)"), - (20, 65, "#d4edda", "reel-out\n(xi=20-65deg)"), - (65, 80, "#fff3cd", "reel-in\n(xi=65-80deg)"), - (80, 90, "#f8d7da", "landing\n(xi=80-90deg)"), -] - -def shade_regimes(ax, show_labels=True): - for lo, hi, color, label in REGIMES: - ax.axvspan(lo, hi, alpha=0.35, color=color) - if show_labels: - mid = (lo + hi) / 2 - ax.text(mid, 0.97, label, transform=ax.get_xaxis_transform(), - ha="center", va="top", fontsize=6.5, color="#444", - multialignment="center") - - -# --------------------------------------------------------------------------- -# Sweep helpers -# --------------------------------------------------------------------------- -def _disk_normal(tilt_deg): - """Disk normal at tilt_deg from wind (East) toward Down.""" - t = math.radians(tilt_deg) - return np.array([0.0, math.cos(t), -math.sin(t)]) - - -def tilt_sweep(model_factory, tilt_arr, wind_speed, omega): - """Return (N,9) array: tilt, T, v_i, v_ax, v_inp, CT, mu_z, mu, lambda_i.""" - rows = [] - wind = np.array([0.0, wind_speed, 0.0]) - for td in tilt_arr: - bz = _disk_normal(float(td)) - R_hub = build_orb_frame(bz) - mdl = model_factory() - mdl.compute_forces(COL_RAD, 0.0, 0.0, R_hub, np.zeros(3), omega, wind, T_RUN) - T = mdl.last_T - v_i = mdl.last_v_i - v_ax = mdl.last_v_axial - v_inp = mdl.last_v_inplane - CT = T / (RHO * A_DISK * TIP_SPEED**2) if TIP_SPEED > 0 else 0.0 - mu_z = v_ax / TIP_SPEED - mu = v_inp / TIP_SPEED - lam_i = v_i / TIP_SPEED - rows.append((td, T, v_i, v_ax, v_inp, CT, mu_z, mu, lam_i)) - return np.array(rows) - - -def wind_sweep(model_factory, tilt_deg, wind_arr, omega): - """Return (N,3) array: V, T, v_i.""" - bz = _disk_normal(float(tilt_deg)) - R_hub = build_orb_frame(bz) - rows = [] - for V in wind_arr: - wind = np.array([0.0, V, 0.0]) - mdl = model_factory() - mdl.compute_forces(COL_RAD, 0.0, 0.0, R_hub, np.zeros(3), omega, wind, T_RUN) - rows.append((V, mdl.last_T, mdl.last_v_i)) - return np.array(rows) - - -# --------------------------------------------------------------------------- -# Run sweeps -# --------------------------------------------------------------------------- -TILT_DEG = np.arange(0, 91, 5) -WIND_DEG = np.linspace(5, 20, 16) -PH_fac = lambda: PetersHeBEM(rotor) -SW_fac = lambda: create_aero(rotor, model="skewed_wake_numpy") - -print("Fig 1 -- tilt sweep (PetersHe)...") -G = tilt_sweep(PH_fac, TILT_DEG, 10.0, OMEGA) -print("Fig 1 -- tilt sweep (SkewedWake)...") -S = tilt_sweep(SW_fac, TILT_DEG, 10.0, OMEGA) - -print("Fig 2 -- wind sweep xi=45deg (PetersHe)...") -G45 = wind_sweep(PH_fac, 45, WIND_DEG, OMEGA) -print("Fig 2 -- wind sweep xi=45deg (SkewedWake)...") -S45 = wind_sweep(SW_fac, 45, WIND_DEG, OMEGA) - -print("Fig 2 -- wind sweep xi=70deg (PetersHe)...") -G70 = wind_sweep(PH_fac, 70, WIND_DEG, OMEGA) -print("Fig 2 -- wind sweep xi=70deg (SkewedWake)...") -S70 = wind_sweep(SW_fac, 70, WIND_DEG, OMEGA) - -# Unpack tilt sweep -td = G[:, 0] -T_g = G[:, 1]; T_s = S[:, 1] -vi_g = G[:, 2]; vi_s = S[:, 2] -CT_g = G[:, 5]; CT_s = S[:, 5] -mu_z_g = G[:, 6]; mu_z_s = S[:, 6] -mu_g = G[:, 7]; mu_s = S[:, 7] -li_g = G[:, 8]; li_s = S[:, 8] - -ratio = np.where(T_s > 1.0, T_g / T_s, np.nan) - -C_PH = "#1f77b4" # PetersHe -- blue -C_SW = "#d62728" # SkewedWake -- red -C_RAT = "#2ca02c" # ratio -- green - - -# =========================================================================== -# Figure 1 -- Tilt sweep (4 panels) -# =========================================================================== -fig1, axes1 = plt.subplots(2, 2, figsize=(13, 10)) -fig1.suptitle( - f"PetersHeBEM vs SkewedWakeBEM -- beaupoil_2026\n" - f"col={COL_RAD:.2f} rad, omega={OMEGA:.1f} rad/s, V_wind=10 m/s East", - fontsize=12, fontweight="bold", -) - -# --- Panel 1: Thrust vs tilt --- -ax = axes1[0, 0] -shade_regimes(ax) -ax.plot(td, T_g, color=C_PH, lw=2, label="PetersHe") -ax.plot(td, T_s, color=C_SW, lw=2, label="SkewedWake") -ax2 = ax.twinx() -ax2.plot(td, ratio, color=C_RAT, lw=1.5, ls="--", label="ratio PH/SW") -ax2.axhline(1.0, color=C_RAT, lw=0.7, ls=":") -ax2.set_ylabel("PetersHe / SkewedWake", color=C_RAT, fontsize=9) -ax2.tick_params(axis="y", labelcolor=C_RAT) -ax2.set_ylim(0, max(np.nanmax(ratio) * 1.2, 2.0)) -ax.set_xlabel("Disk tilt from wind [deg]") -ax.set_ylabel("Thrust T [N]") -ax.set_title("1. Thrust vs tilt angle", fontweight="bold") -ax.set_xlim(0, 90) -ax.axhline(0, color="k", lw=0.5) -lines = [Line2D([0],[0], color=C_PH, lw=2), - Line2D([0],[0], color=C_SW, lw=2), - Line2D([0],[0], color=C_RAT, lw=1.5, ls="--")] -ax.legend(lines, ["PetersHe", "SkewedWake", "ratio PH/SW"], fontsize=8, loc="upper left") - -# --- Panel 2: v_i vs tilt --- -ax = axes1[0, 1] -shade_regimes(ax) -ax.plot(td, vi_g, color=C_PH, lw=2, label="PetersHe v_i") -ax.plot(td, vi_s, color=C_SW, lw=2, label="SkewedWake v_i0") -ax.set_xlabel("Disk tilt from wind [deg]") -ax.set_ylabel("Induced velocity v_i [m/s]") -ax.set_title("2. Induced velocity", fontweight="bold") -ax.set_xlim(0, 90) -ax.legend(fontsize=8) -ax.text(0.97, 0.50, - "Glauert: uniform (min power)\nColeman: non-uniform (higher v_i at tilt)", - transform=ax.transAxes, ha="right", va="center", fontsize=7.5, - bbox=dict(fc="white", ec="gray", alpha=0.7, pad=3)) - -# --- Panel 3: Glauert universal inflow diagram --- -ax = axes1[1, 0] -CT_ref = float(np.nanmean(CT_g[CT_g > 0])) -mu_z_line = np.linspace(-0.3, 0.5, 300) -lambda_theory = [] -vi0 = 0.05 -for mz in mu_z_line: - li = vi0 - for _ in range(80): - denom = math.sqrt(max(1e-9, (mz + li)**2)) - li = 0.4 * CT_ref / (2.0 * denom) + 0.6 * li - lambda_theory.append(li) -lambda_theory = np.array(lambda_theory) -ax.plot(mu_z_line, lambda_theory, color="gray", lw=1.2, ls="--", - label=f"Glauert curve (CT~{CT_ref:.4f})") -sc_g = ax.scatter(mu_z_g, li_g, c=td, cmap="Blues_r", s=45, zorder=5, - vmin=0, vmax=90, edgecolors=C_PH, lw=0.8, label="PetersHe") -sc_s = ax.scatter(mu_z_s, li_s, c=td, cmap="Reds_r", s=45, zorder=5, - vmin=0, vmax=90, edgecolors=C_SW, lw=0.8, marker="s", - label="SkewedWake") -for idx, lbl in [(0, "0deg"), (-1, "90deg")]: - ax.annotate(f"tilt={lbl}", xy=(mu_z_g[idx], li_g[idx]), - xytext=(8, 4), textcoords="offset points", fontsize=7.5, color=C_PH) -ax.set_xlabel("Axial advance ratio mu_z = v_axial / (Omega*R)") -ax.set_ylabel("Induced inflow ratio lambda_i = v_i / (Omega*R)") -ax.set_title("3. Glauert universal inflow diagram", fontweight="bold") -ax.axvline(0, color="k", lw=0.5) -ax.axhline(0, color="k", lw=0.5) -cb = fig1.colorbar(sc_g, ax=ax, shrink=0.7, pad=0.01) -cb.set_label("Tilt angle [deg]", fontsize=8) -ax.legend(fontsize=7.5, loc="upper right") - -# --- Panel 4: CT vs mu --- -ax = axes1[1, 1] -sc_g2 = ax.scatter(mu_g, CT_g, c=td, cmap="Blues_r", s=50, zorder=5, - vmin=0, vmax=90, edgecolors=C_PH, lw=0.8, label="PetersHe") -sc_s2 = ax.scatter(mu_s, CT_s, c=td, cmap="Reds_r", s=50, zorder=5, - vmin=0, vmax=90, edgecolors=C_SW, lw=0.8, marker="s", - label="SkewedWake") -for i in range(0, len(td), 3): - ax.annotate("", xy=(mu_g[i], CT_g[i]), xytext=(mu_s[i], CT_s[i]), - arrowprops=dict(arrowstyle="-", color="gray", lw=0.6, alpha=0.5)) -for idx, lbl in [(0, "tilt=0deg"), (-1, "tilt=90deg")]: - ax.annotate(lbl, xy=(mu_g[idx], CT_g[idx]), - xytext=(8, 4), textcoords="offset points", fontsize=7.5, color=C_PH) - ax.annotate(lbl, xy=(mu_s[idx], CT_s[idx]), - xytext=(8, -10), textcoords="offset points", fontsize=7.5, color=C_SW) -ax.set_xlabel("In-plane advance ratio mu = v_inplane / (Omega*R)") -ax.set_ylabel("Thrust coefficient CT") -ax.set_title("4. CT vs advance ratio", fontweight="bold") -ax.axhline(0, color="k", lw=0.5) -cb2 = fig1.colorbar(sc_g2, ax=ax, shrink=0.7, pad=0.01) -cb2.set_label("Tilt angle [deg]", fontsize=8) -ax.legend(fontsize=7.5, loc="upper left") - -# RAWES operating envelope annotation -ax.text(0.97, 0.08, - "RAWES operating envelope:\n" - "reel-out: tilt=20-65deg\n" - "reel-in: tilt=65-80deg", - transform=ax.transAxes, ha="right", va="bottom", fontsize=7, - bbox=dict(fc="white", ec="gray", alpha=0.7, pad=3)) - -plt.tight_layout() -out1 = Path(__file__).parent / "compare_aero_beaupoil_tilt.png" -fig1.savefig(out1, dpi=150, bbox_inches="tight") -print(f"Saved: {out1}") - - -# =========================================================================== -# Figure 2 -- Wind speed sweep at xi=45 and xi=70 -# =========================================================================== -fig2, axes2 = plt.subplots(2, 2, figsize=(12, 8)) -fig2.suptitle( - f"PetersHeBEM vs SkewedWakeBEM -- beaupoil_2026 Wind speed sweep\n" - f"col={COL_RAD:.2f} rad, omega={OMEGA:.1f} rad/s", - fontsize=12, fontweight="bold", -) - -for row, (tilt_deg, G_ws, S_ws, regime_label) in enumerate([ - (45, G45, S45, "reel-out (xi=45deg)"), - (70, G70, S70, "reel-in (xi=70deg)"), -]): - V = G_ws[:, 0] - Tg = G_ws[:, 1]; Ts = S_ws[:, 1] - vig = G_ws[:, 2]; vis = S_ws[:, 2] - - # Thrust vs V - ax = axes2[row, 0] - ax.plot(V, Tg, color=C_PH, lw=2, label="PetersHe") - ax.plot(V, Ts, color=C_SW, lw=2, label="SkewedWake") - ax.axhline(0, color="k", lw=0.5) - ax.axvline(10, color="gray", lw=0.8, ls=":", label="V=10 m/s (nominal)") - ax.set_xlabel("Wind speed V [m/s]") - ax.set_ylabel("Thrust T [N]") - ax.set_title(f"Thrust vs wind -- {regime_label}", fontweight="bold") - ax.legend(fontsize=8) - ax.text(0.03, 0.97, - f"tilt = {tilt_deg} deg from wind\nomega = {OMEGA:.1f} rad/s", - transform=ax.transAxes, ha="left", va="top", fontsize=8, - bbox=dict(fc="white", ec="gray", alpha=0.7, pad=3)) - - # v_i vs V - ax = axes2[row, 1] - ax.plot(V, vig, color=C_PH, lw=2, label="PetersHe v_i") - ax.plot(V, vis, color=C_SW, lw=2, label="SkewedWake v_i0") - ax.axhline(0, color="k", lw=0.5) - ax.axvline(10, color="gray", lw=0.8, ls=":", label="V=10 m/s (nominal)") - ax.set_xlabel("Wind speed V [m/s]") - ax.set_ylabel("Induced velocity v_i [m/s]") - ax.set_title(f"Induced velocity vs wind -- {regime_label}", fontweight="bold") - ax.legend(fontsize=8) - - # Difference annotation - diff_T = np.abs(Tg - Ts) - diff_vi = np.abs(vig - vis) - idx10 = np.argmin(np.abs(V - 10.0)) - ax.text(0.97, 0.97, - f"At V=10: |PH-SW|={diff_T[idx10]:.1f} N T, {diff_vi[idx10]:.2f} m/s v_i", - transform=axes2[row, 1].transAxes, ha="right", va="top", fontsize=7.5, - bbox=dict(fc="lightyellow", ec="gray", alpha=0.9, pad=3)) - -plt.tight_layout() -out2 = Path(__file__).parent / "compare_aero_beaupoil_wind.png" -fig2.savefig(out2, dpi=150, bbox_inches="tight") -print(f"Saved: {out2}") diff --git a/analysis/compare_aero_models.py b/analysis/compare_aero_models.py deleted file mode 100644 index 696f4bf..0000000 --- a/analysis/compare_aero_models.py +++ /dev/null @@ -1,302 +0,0 @@ -""" -compare_aero_models.py — PetersHeBEM vs SkewedWakeBEM across the full tilt envelope. - -Sweeps tilt angle from 0° (pure autorotation, axial inflow) to 90° (hover, -in-plane inflow) and plots four panels: - - 1. Thrust vs tilt — primary divergence between models - 2. Induced velocity v_i vs tilt — the root cause of the divergence - 3. Glauert universal inflow diagram — operating envelope on (μ_z, μ, λ_i) axes - 4. CT vs in-plane advance ratio μ — standard helicopter performance chart - -Run: - python analysis/compare_aero_models.py -""" - -import math -import sys -from pathlib import Path - -import numpy as np -import matplotlib -matplotlib.use("Agg") # headless; swap to TkAgg/Qt5Agg for interactive -import matplotlib.pyplot as plt -from matplotlib.lines import Line2D - -from dynbem import rotor_definition as rd -from simulation.frames import build_orb_frame -from dynbem import create_aero, PetersHeBEM - -# --------------------------------------------------------------------------- -# Sweep parameters -# --------------------------------------------------------------------------- -TILT_DEG = np.arange(0, 91, 5) # 0° (autorotation) → 90° (hover) -COL_RAD = -0.10 # moderate collective, positive thrust for both -OMEGA = 28.0 # rad/s — nominal RAWES spin rate -WIND_SPEED = 10.0 # m/s -WIND_NED = np.array([0.0, WIND_SPEED, 0.0]) # East -T_PAST_RAMP = 10.0 - - -def _body_z(tilt_deg: float) -> np.ndarray: - """Disk normal: rotated tilt_deg from wind direction (East) toward Down.""" - t = math.radians(tilt_deg) - return np.array([0.0, math.cos(t), -math.sin(t)]) - - -def _run_sweep(model_factory): - """Return arrays (tilt_deg, T, v_i, v_axial, v_inplane, CT, mu_z, mu, lambda_i).""" - rotor = rd.default() - R = rotor.radius_m - RHO = rotor.rho_kg_m3 - A = math.pi * R**2 - - results = [] - for td in TILT_DEG: - bz = _body_z(float(td)) - R_hub = build_orb_frame(bz) - model = model_factory(rotor) - model.compute_forces( - collective_rad=COL_RAD, - tilt_lon=0.0, tilt_lat=0.0, - R_hub=R_hub, - v_hub_world=np.zeros(3), - omega_rotor=OMEGA, - wind_world=WIND_NED, - ) - T = model.last_T - v_i = model.last_v_i - v_ax = model.last_v_axial - v_inp = model.last_v_inplane - - tip_speed = OMEGA * R - CT = T / (RHO * A * tip_speed**2) if T > 0 else 0.0 - mu_z = v_ax / tip_speed - mu = v_inp / tip_speed - lambda_i = v_i / tip_speed - - results.append((td, T, v_i, v_ax, v_inp, CT, mu_z, mu, lambda_i)) - - return np.array(results) - - -# --------------------------------------------------------------------------- -# Run both models -# --------------------------------------------------------------------------- -print("Running PetersHeBEM sweep...") -G = _run_sweep(lambda r: PetersHeBEM(r)) - -print("Running SkewedWakeBEM sweep...") -S = _run_sweep(lambda r: create_aero(r, model="skewed_wake_numpy")) - -# Unpack columns -td = G[:, 0] -T_g = G[:, 1]; T_s = S[:, 1] -vi_g = G[:, 2]; vi_s = S[:, 2] -CT_g = G[:, 5]; CT_s = S[:, 5] -mu_z_g = G[:, 6]; mu_z_s = S[:, 6] -mu_g = G[:, 7]; mu_s = S[:, 7] -li_g = G[:, 8]; li_s = S[:, 8] - -ratio = np.where(T_s > 1.0, T_g / T_s, np.nan) - -# --------------------------------------------------------------------------- -# Colours and regime shading helpers -# --------------------------------------------------------------------------- -C_GL = "#1f77b4" # Glauert — blue -C_SW = "#d62728" # Skewed — red -C_RAT = "#2ca02c" # ratio — green - -REGIME_AUTOROT = dict(color="#e8f4f8", label="autorotation\n(Coleman valid)") -REGIME_TRANSIT = dict(color="#fef9e7", label="transition") -REGIME_HOVER = dict(color="#f9ebea", label="hover\n(Glauert valid)") - -def shade_regimes(ax): - ax.axvspan( 0, 55, alpha=0.40, **{k: v for k, v in REGIME_AUTOROT.items() if k != "label"}) - ax.axvspan(55, 75, alpha=0.40, **{k: v for k, v in REGIME_TRANSIT.items() if k != "label"}) - ax.axvspan(75, 90, alpha=0.40, **{k: v for k, v in REGIME_HOVER.items() if k != "label"}) - - -# --------------------------------------------------------------------------- -# Figure — 2 × 2 -# --------------------------------------------------------------------------- -fig, axes = plt.subplots(2, 2, figsize=(13, 10)) -fig.suptitle( - f"PetersHeBEM vs SkewedWakeBEM | col={COL_RAD:.2f} rad, " - f"ω={OMEGA:.0f} rad/s, wind={WIND_SPEED:.0f} m/s East", - fontsize=13, fontweight="bold", -) - -# ── Panel 1: Thrust vs tilt ───────────────────────────────────────────────── -ax = axes[0, 0] -shade_regimes(ax) -ax.plot(td, T_g, color=C_GL, lw=2, label="PetersHeBEM") -ax.plot(td, T_s, color=C_SW, lw=2, label="SkewedWakeBEM") -ax2 = ax.twinx() -ax2.plot(td, ratio, color=C_RAT, lw=1.5, ls="--", label="ratio G/S") -ax2.axhline(1.0, color=C_RAT, lw=0.7, ls=":") -ax2.set_ylabel("Glauert / Skewed ratio", color=C_RAT, fontsize=9) -ax2.tick_params(axis="y", labelcolor=C_RAT) -ax2.set_ylim(0, max(np.nanmax(ratio) * 1.1, 2.0)) - -ax.set_xlabel("Disk tilt from wind [°]") -ax.set_ylabel("Axial thrust T [N]") -ax.set_title("1. Thrust vs tilt angle", fontweight="bold") -ax.set_xlim(0, 90) -ax.axhline(0, color="k", lw=0.5) -ax.axvline(55, color="gray", lw=0.8, ls=":") -ax.axvline(75, color="gray", lw=0.8, ls=":") -ax.text( 27, 0.03, "autorotation\n(Coleman valid)", - transform=ax.get_xaxis_transform(), ha="center", va="bottom", fontsize=7.5, color="#555") -ax.text( 65, 0.03, "transition", - transform=ax.get_xaxis_transform(), ha="center", va="bottom", fontsize=7.5, color="#555") -ax.text( 83, 0.03, "hover\n(Glauert valid)", - transform=ax.get_xaxis_transform(), ha="center", va="bottom", fontsize=7.5, color="#555") - -lines = [ - Line2D([0], [0], color=C_GL, lw=2), - Line2D([0], [0], color=C_SW, lw=2), - Line2D([0], [0], color=C_RAT, lw=1.5, ls="--"), -] -ax.legend(lines, ["PetersHeBEM", "SkewedWakeBEM", "ratio G/S"], fontsize=8, loc="lower left") - -# ── Panel 2: Induced velocity vs tilt ─────────────────────────────────────── -ax = axes[0, 1] -shade_regimes(ax) -ax.plot(td, vi_g, color=C_GL, lw=2, label="PetersHeBEM v_i") -ax.plot(td, vi_s, color=C_SW, lw=2, label="SkewedWakeBEM v_i₀") -ax.axvline(55, color="gray", lw=0.8, ls=":") -ax.axvline(75, color="gray", lw=0.8, ls=":") -ax.set_xlabel("Disk tilt from wind [°]") -ax.set_ylabel("Induced velocity v_i [m/s]") -ax.set_title("2. Induced velocity — root cause of divergence", fontweight="bold") -ax.set_xlim(0, 90) -ax.legend(fontsize=8) -ax.text(0.97, 0.95, - "Glauert: uniform (min power)\nColeman: non-uniform (higher v_i → less T)", - transform=ax.transAxes, ha="right", va="top", fontsize=7.5, - bbox=dict(fc="white", ec="gray", alpha=0.7, pad=3)) - -# ── Panel 3: Glauert universal inflow diagram ──────────────────────────────── -# x-axis: μ_z (axial advance ratio), y-axis: λ_i (induced inflow ratio) -# Overlay the theoretical Glauert curve and both models' operating points. -ax = axes[1, 0] - -# Theoretical Glauert curve at fixed CT (use mean of measured CT values) -CT_ref = float(np.nanmean(CT_g[CT_g > 0])) -mu_z_line = np.linspace(-0.3, 0.5, 300) -lambda_theory = [] -vi0 = 0.05 -for mz in mu_z_line: - li = vi0 - for _ in range(80): - denom = math.sqrt(max(1e-9, (mz + li)**2)) - li_new = CT_ref / (2.0 * denom) - li = 0.4 * li_new + 0.6 * li - lambda_theory.append(li) -lambda_theory = np.array(lambda_theory) - -ax.plot(mu_z_line, lambda_theory, color="gray", lw=1.2, ls="--", - label=f"Glauert curve (CT≈{CT_ref:.4f})") - -# Operating points coloured by tilt angle -sc_g = ax.scatter(mu_z_g, li_g, c=td, cmap="Blues_r", s=40, zorder=5, - vmin=0, vmax=90, edgecolors=C_GL, lw=0.8, label="PetersHeBEM pts") -sc_s = ax.scatter(mu_z_s, li_s, c=td, cmap="Reds_r", s=40, zorder=5, - vmin=0, vmax=90, edgecolors=C_SW, lw=0.8, marker="s", - label="SkewedWakeBEM pts") - -# Annotate tilt=0 and tilt=90 -for idx, label in [(0, "0°"), (-1, "90°")]: - ax.annotate(f"tilt={label}", xy=(mu_z_g[idx], li_g[idx]), - xytext=(8, 4), textcoords="offset points", fontsize=7.5, color=C_GL) - -ax.set_xlabel("Axial advance ratio μ_z = v_axial / (Ω·R)") -ax.set_ylabel("Induced inflow ratio λ_i = v_i / (Ω·R)") -ax.set_title("3. Glauert universal inflow diagram", fontweight="bold") -ax.axvline(0, color="k", lw=0.5) -ax.axhline(0, color="k", lw=0.5) -cb = fig.colorbar(sc_g, ax=ax, shrink=0.7, pad=0.01) -cb.set_label("Tilt angle [°]", fontsize=8) -ax.legend(fontsize=7.5, loc="upper right") -ax.text(0.02, 0.05, "← axial descent axial climb →", - transform=ax.transAxes, fontsize=7, color="gray", ha="left") - -# ── Panel 4: CT vs in-plane advance ratio μ ───────────────────────────────── -ax = axes[1, 1] - -sc_g2 = ax.scatter(mu_g, CT_g, c=td, cmap="Blues_r", s=50, zorder=5, - vmin=0, vmax=90, edgecolors=C_GL, lw=0.8, label="PetersHeBEM") -sc_s2 = ax.scatter(mu_s, CT_s, c=td, cmap="Reds_r", s=50, zorder=5, - vmin=0, vmax=90, edgecolors=C_SW, lw=0.8, marker="s", - label="SkewedWakeBEM") - -# Connect corresponding tilt angles with light arrows -for i in range(0, len(td), 3): - ax.annotate("", xy=(mu_g[i], CT_g[i]), xytext=(mu_s[i], CT_s[i]), - arrowprops=dict(arrowstyle="-", color="gray", lw=0.6, alpha=0.5)) - -# Annotate tilt=0 and tilt=90 -for idx, label in [(0, "tilt=0°"), (-1, "tilt=90°")]: - ax.annotate(label, xy=(mu_g[idx], CT_g[idx]), - xytext=(8, 4), textcoords="offset points", fontsize=7.5, color=C_GL) - ax.annotate(label, xy=(mu_s[idx], CT_s[idx]), - xytext=(8, -10), textcoords="offset points", fontsize=7.5, color=C_SW) - -ax.set_xlabel("In-plane advance ratio μ = v_inplane / (Ω·R)") -ax.set_ylabel("Thrust coefficient CT = T / (ρ·A·(Ω·R)²)") -ax.set_title("4. CT vs advance ratio (standard helicopter chart)", fontweight="bold") -ax.axhline(0, color="k", lw=0.5) - -cb2 = fig.colorbar(sc_g2, ax=ax, shrink=0.7, pad=0.01) -cb2.set_label("Tilt angle [°]", fontsize=8) -ax.legend(fontsize=7.5, loc="upper left") -ax.text(0.97, 0.05, - "Each point is one tilt angle.\n" - "Arrows connect same tilt between models.\n" - "Tilt 0° = axial (μ=0); tilt 90° = in-plane (μ max).", - transform=ax.transAxes, ha="right", va="bottom", fontsize=7, - bbox=dict(fc="white", ec="gray", alpha=0.7, pad=3)) - -# --------------------------------------------------------------------------- -# Save -# --------------------------------------------------------------------------- -plt.tight_layout() -out = Path(__file__).parent / "compare_aero_models.png" -fig.savefig(out, dpi=150, bbox_inches="tight") -print(f"Saved: {out}") - -# --------------------------------------------------------------------------- -# Per-operating-point divergence table (RAWES cruise collectives) -# PetersHeBEM assumes uniform (minimum-power) disk induction; SkewedWakeBEM -# uses Coleman non-uniform induction (higher v_i, less thrust for same collective). -# The two models diverge 2-5x in the autorotation regime (tilt 0-55 deg). -# SkewedWakeBEM is correct for tilt 0-55 deg; PetersHeBEM for hover (tilt >= 80 deg). -# --------------------------------------------------------------------------- -_CRUISE_COL = {0: -0.20, 15: -0.20, 30: -0.18, 45: -0.05, 60: 0.02} -_OMEGA_CMP = 28.0 -_WIND_CMP = np.array([0.0, 10.0, 0.0]) -_T_CMP = 10.0 - -print() -print("Per-operating-point thrust divergence (RAWES cruise collectives, 10 m/s East wind):") -print(f" {'tilt':>6} {'col':>6} {'T_glauert':>10} {'T_skewed':>10} {'ratio G/S':>10} note") -print(f" {'-'*6} {'-'*6} {'-'*10} {'-'*10} {'-'*10} ----") - -_rotor = rd.default() -_peters = PetersHeBEM(_rotor) -_skewed_cmp = create_aero(_rotor, model="skewed_wake_numpy") - -for _td, _col in sorted(_CRUISE_COL.items()): - _bz = _body_z(float(_td)) - _R_hub = build_orb_frame(_bz) - _kwargs = dict(collective_rad=_col, tilt_lon=0.0, tilt_lat=0.0, - R_hub=_R_hub, v_hub_world=np.zeros(3), - omega_rotor=_OMEGA_CMP, wind_world=_WIND_CMP) - _fp = _peters.compute_forces(**_kwargs) - _fs = _skewed_cmp.compute_forces(**_kwargs) - _T_g = float(np.dot(_fp[:3], _bz)) - _T_s = float(np.dot(_fs[:3], _bz)) - _ratio = _T_g / _T_s if abs(_T_s) > 0.1 else float("nan") - _note = "within 40%" if 0.6 < _ratio < 1.4 else "DIVERGE (expected in autorotation)" - print(f" {_td:>6} {_col:>6.2f} {_T_g:>10.1f} {_T_s:>10.1f} {_ratio:>10.2f} {_note}") diff --git a/analysis/pump_envelope.py b/analysis/pump_envelope.py deleted file mode 100644 index 7968649..0000000 --- a/analysis/pump_envelope.py +++ /dev/null @@ -1,654 +0,0 @@ -""" -pump_envelope.py — Performance envelope for the RAWES pumping cycle. - -Physics summary ---------------- -Two actuators: collective (thrust magnitude) and body_z tilt (cyclic direction). -Two outputs: tether tension T and hub altitude h. - -At static equilibrium, the altitude constraint fixes a relationship between -body_z tilt angle delta (measured from tether direction toward zenith) and thrust: - - sin(delta) = mg*cos(el) / T_thrust [altitude maintenance] - T = T_thrust*cos(delta) - mg*sin(el) [tether tension] - -Eliminating T_thrust: - - T = mg * cos(el + delta) / sin(delta) [combined formula] - -So tension is controlled by delta alone (for altitude-maintaining flight). -Given a target tension T*: - - delta* = arctan( mg*cos(el) / (T* + mg*sin(el)) ) - T_thrust* = mg*cos(el) / sin(delta*) - -Net energy per pumping cycle: - E_net = T_out * v_out * t_out - T_in * v_in * t_in - (positive means net generation; maximised by large T_out - T_in differential) - -Usage ------ - # Static envelope analysis only: - .venv/Scripts/python.exe analysis/pump_envelope.py - .venv/Scripts/python.exe analysis/pump_envelope.py --wind 8 12 15 - - # Static envelope + actual vs optimal from a telemetry CSV: - .venv/Scripts/python.exe analysis/pump_envelope.py \\ - --telemetry simulation/logs/test_pump_cycle/telemetry.csv -""" - -import argparse -import sys -import math -import json -from pathlib import Path - -import numpy as np - - -from dynbem import create_aero -from dynbem import rotor_definition as rd -from simulation.ic import load_ic - -# ── Current simtest parameters (shown as baseline in all tables) ────────────── -TENSION_OUT_NOW = 435.0 # N reel-out TensionPI setpoint -TENSION_IN_NOW = 226.0 # N reel-in TensionPI setpoint -V_REEL_OUT = 0.4 # m/s -V_REEL_IN = 0.4 # m/s -T_REEL_OUT = 30.0 # s -T_REEL_IN = 30.0 # s -XI_REEL_IN_NOW = 80.0 # deg (None = no tilt in Lua test) - -COL_MIN = -0.28 # rad -COL_MAX = 0.10 # rad (simtest override; TensionPI default is 0.00) -BREAK_LOAD_N = 620.0 # N -SAFETY_LOAD_N = 496.0 # N (80% break load) - -# ── Physics constants ───────────────────────────────────────────────────────── -rotor = rd.default() -MASS = rotor.mass_kg -G = 9.81 -MG = MASS * G - -_IC = load_ic() -IC_POS = _IC.pos -IC_R0 = _IC.R0 -OMEGA_SPIN = _IC.omega_spin - -IC_TLEN = float(np.linalg.norm(IC_POS)) -IC_EL = math.degrees(math.asin(max(-1.0, min(1.0, -IC_POS[2] / IC_TLEN)))) - - -# ── Core formulas ───────────────────────────────────────────────────────────── - -def delta_for_tension(el_rad: float, target_t: float) -> float: - return math.atan2(MG * math.cos(el_rad), target_t + MG * math.sin(el_rad)) - -def thrust_for_delta(el_rad: float, delta_rad: float) -> float: - s = math.sin(delta_rad) - return MG * math.cos(el_rad) / s if s > 1e-9 else float("inf") - -def tension_from_delta(el_rad: float, delta_rad: float, t_thrust: float) -> float: - return t_thrust * math.cos(delta_rad) - MG * math.sin(el_rad) - -def t_min_at_elevation(el_rad: float, t_min_aero: float) -> "float | None": - """Minimum achievable tether tension at given elevation and aero thrust floor.""" - if t_min_aero < MG * math.cos(el_rad): - return None # can't maintain altitude - d_opt = math.asin(MG * math.cos(el_rad) / t_min_aero) - return tension_from_delta(el_rad, d_opt, t_min_aero) - - -# ── Aero model queries ──────────────────────────────────────────────────────── - -def _make_aero(wind_ned: np.ndarray): - return create_aero(rotor), wind_ned - -def aero_thrust(col: float, wind_ned: np.ndarray, R_hub: np.ndarray = None) -> float: - aero, w = _make_aero(wind_ned) - R = IC_R0 if R_hub is None else R_hub - res = aero.compute_forces(col, 0.0, 0.0, R, np.zeros(3), OMEGA_SPIN, w) - return float(np.linalg.norm(res.F_world)) - -def col_for_thrust(target: float, wind_ned: np.ndarray, n: int = 40) -> "float | None": - t_lo = aero_thrust(COL_MIN, wind_ned) - t_hi = aero_thrust(COL_MAX, wind_ned) - if target < t_lo or target > t_hi: - return None - lo, hi = COL_MIN, COL_MAX - for _ in range(n): - mid = (lo + hi) / 2 - if aero_thrust(mid, wind_ned) < target: - lo = mid - else: - hi = mid - return (lo + hi) / 2 - - -# ── Section 1: formula validation ──────────────────────────────────────────── - -def validate_formula(wind_ned: np.ndarray) -> None: - t_min_aero = aero_thrust(COL_MIN, wind_ned) - t_max_aero = aero_thrust(COL_MAX, wind_ned) - el = math.radians(IC_EL) - - print("=" * 72) - print("FORMULA VALIDATION at IC conditions") - print(f" mass={MASS:.1f} kg mg={MG:.1f} N wind={np.linalg.norm(wind_ned):.1f} m/s") - print(f" IC: elevation={IC_EL:.1f} deg altitude={-IC_POS[2]:.1f} m tether={IC_TLEN:.1f} m") - print(f" Aero: T_thrust at COL_MIN={t_min_aero:.1f} N COL_MAX={t_max_aero:.1f} N") - print() - print(f" {'T_target_N':>12} {'delta_deg':>10} {'T_thrust_N':>12} {'T_check_N':>11}") - print(f" {'-'*12} {'-'*10} {'-'*12} {'-'*11}") - for t_star in [55, 100, 150, 200, 226, 250, 300, 350, 400, 435, 496]: - d = delta_for_tension(el, float(t_star)) - thr = thrust_for_delta(el, d) - chk = tension_from_delta(el, d, thr) - cur = " <- TENSION_OUT" if t_star == 435 else (" <- TENSION_IN (now)" if t_star == 226 else "") - print(f" {t_star:>12.0f} {math.degrees(d):>10.2f} {thr:>12.1f} {chk:>11.1f} {cur}") - print() - - -# ── Section 2: T_min across elevation range ─────────────────────────────────── - -def elevation_envelope(wind_ned: np.ndarray) -> None: - t_min_aero = aero_thrust(COL_MIN, wind_ned) - t_max_aero = aero_thrust(COL_MAX, wind_ned) - - print("=" * 72) - print("PERFORMANCE ENVELOPE: achievable tension range per elevation") - print(f" COL range [{COL_MIN:+.2f}, {COL_MAX:+.2f}] rad " - f"omega={OMEGA_SPIN:.1f} rad/s wind={np.linalg.norm(wind_ned):.1f} m/s") - print() - print(f" {'el_deg':>7} {'T_min_N':>8} {'T_max_N':>8} " - f"{'col@435N':>9} {'col@226N':>9} {'col@100N':>9} status") - print(f" {'-'*7} {'-'*8} {'-'*8} {'-'*9} {'-'*9} {'-'*9} {'-'*18}") - - for el_deg in [5, 10, 15, IC_EL, 20, 25, 30, 40, 50, 60, 70, 80]: - el = math.radians(el_deg) - t_min = t_min_at_elevation(el, t_min_aero) - if t_min is None: - print(f" {el_deg:>7.1f} {'---':>8} {'---':>8} altitude fail") - continue - - d_max = math.asin(min(1.0, MG * math.cos(el) / t_max_aero)) if t_max_aero >= MG * math.cos(el) else 0.0 - t_max = tension_from_delta(el, d_max, t_max_aero) - - def _col(t_target): - d = delta_for_tension(el, t_target) - thr = thrust_for_delta(el, d) - c = col_for_thrust(thr, wind_ned) - return f"{c:+.3f}" if c is not None else " infeas" - - status = ("BREAKS" if t_min > BREAK_LOAD_N - else "above safety" if t_min > SAFETY_LOAD_N - else "T>226 always" if t_min > 226.0 - else "T>100 always" if t_min > 100.0 - else "OK") - marker = " <-- IC" if abs(el_deg - IC_EL) < 0.2 else "" - print(f" {el_deg:>7.1f} {t_min:>8.1f} {t_max:>8.1f} " - f"{_col(435.0):>9} {_col(226.0):>9} {_col(100.0):>9} {status}{marker}") - print() - - -# ── Section 3: net energy sweep over TENSION_IN ─────────────────────────────── - -def net_energy_sweep(wind_ned: np.ndarray) -> None: - t_min_aero = aero_thrust(COL_MIN, wind_ned) - el = math.radians(IC_EL) - t_min_abs = t_min_at_elevation(el, t_min_aero) - - print("=" * 72) - print("NET ENERGY SWEEP: effect of lowering TENSION_IN") - print(f" TENSION_OUT={TENSION_OUT_NOW:.0f} N v_out={V_REEL_OUT} m/s t_out={T_REEL_OUT:.0f} s") - print(f" v_in={V_REEL_IN} m/s t_in={T_REEL_IN:.0f} s elevation={IC_EL:.1f} deg") - print(f" Minimum achievable tension at COL_MIN: {t_min_abs:.1f} N" if t_min_abs else " altitude fail") - print() - - e_out = TENSION_OUT_NOW * V_REEL_OUT * T_REEL_OUT - - print(f" {'T_in_N':>8} {'col_needed':>11} {'E_out_J':>8} {'E_in_J':>8} " - f"{'E_net_J':>8} {'vs_now':>8} note") - print(f" {'-'*8} {'-'*11} {'-'*8} {'-'*8} {'-'*8} {'-'*8} {'-'*14}") - - e_net_now = None - for t_in in [50, 75, 80, 90, 100, 110, 120, 150, 175, 200, 226, 250, 300]: - # Is this tension achievable? - if t_min_abs is not None and t_in < t_min_abs: - note = "below T_min" - d = delta_for_tension(el, t_in) - thr = thrust_for_delta(el, d) - c = col_for_thrust(thr, wind_ned) - col_s = f"{c:+.3f}" if c is not None else " infeas" - print(f" {t_in:>8.0f} {col_s:>11} {e_out:>8.0f} {'---':>8} {'---':>8} {'---':>8} {note}") - continue - - d = delta_for_tension(el, float(t_in)) - thr = thrust_for_delta(el, d) - c = col_for_thrust(thr, wind_ned) - col_s = f"{c:+.3f}" if c is not None else " infeas" - - if c is None: - print(f" {t_in:>8.0f} {col_s:>11} {e_out:>8.0f} {'---':>8} {'---':>8} {'---':>8} infeasible") - continue - - e_in = float(t_in) * V_REEL_IN * T_REEL_IN - e_net = e_out - e_in - if t_in == 226: - e_net_now = e_net - - vs = (f"{e_net - e_net_now:+.0f} J" if e_net_now is not None and t_in != 226 - else " baseline" if t_in == 226 else "") - note = "<-- current" if t_in == 226 else ("recommended" if t_in == 100 else "") - print(f" {t_in:>8.0f} {col_s:>11} {e_out:>8.0f} {e_in:>8.0f} " - f"{e_net:>8.0f} {vs:>8} {note}") - print() - - -# ── Section 4: wind speed sensitivity ──────────────────────────────────────── - -def wind_sensitivity(wind_speeds: "list[float]") -> None: - print("=" * 72) - print("WIND SENSITIVITY: T_min and optimal TENSION_IN vs wind speed") - print(f" (at IC elevation={IC_EL:.1f} deg, COL_MIN={COL_MIN:+.2f} rad)") - print() - print(f" {'wind_ms':>8} {'T_thrust_min':>13} {'T_tether_min':>13} " - f"{'rec_T_in':>10} {'E_net_J':>9} note") - print(f" {'-'*8} {'-'*13} {'-'*13} {'-'*10} {'-'*9} {'-'*16}") - - el = math.radians(IC_EL) - for ws in wind_speeds: - wind = np.array([0.0, ws, 0.0]) - t_thrust_min = aero_thrust(COL_MIN, wind) - t_min = t_min_at_elevation(el, t_thrust_min) - if t_min is None: - print(f" {ws:>8.1f} {t_thrust_min:>13.1f} {'altitude fail':>13}") - continue - - # Recommended TENSION_IN: 20 N above T_min (margin for TensionPI overshoot) - rec_t_in = math.ceil((t_min + 20.0) / 10) * 10.0 - - d = delta_for_tension(el, rec_t_in) - thr = thrust_for_delta(el, d) - c = col_for_thrust(thr, wind) - - e_out = TENSION_OUT_NOW * V_REEL_OUT * T_REEL_OUT - e_in = rec_t_in * V_REEL_IN * T_REEL_IN - e_net = e_out - e_in - - note = "<-- current sim" if abs(ws - 10.0) < 0.1 else "" - col_s = f"{c:+.3f}" if c is not None else "infeas" - print(f" {ws:>8.1f} {t_thrust_min:>13.1f} {t_min:>13.1f} " - f"{rec_t_in:>10.0f} {e_net:>9.0f} {note} col={col_s}") - print() - - -# ── Section 5: reel-in tilt cost/benefit ────────────────────────────────────── - -def tilt_analysis(wind_ned: np.ndarray) -> None: - t_min_aero = aero_thrust(COL_MIN, wind_ned) - slew_rate = rotor.body_z_slew_rate_rad_s # rad/s - - print("=" * 72) - print("REEL-IN TILT COST/BENEFIT: is tilting to high xi worth the transition time?") - print(f" Slew rate={slew_rate:.2f} rad/s v_reel_in={V_REEL_IN} m/s") - print(f" Baseline xi_start=30 deg (IC equilibrium disk angle)") - print() - print(f" {'xi_deg':>7} {'t_trans_s':>10} {'T_min_N':>8} {'DeltaT_vs_notilt':>18} " - f"{'tether_saved_m':>15} {'E_saved_J':>10} verdict") - print(f" {'-'*7} {'-'*10} {'-'*8} {'-'*18} {'-'*15} {'-'*10} {'-'*14}") - - xi_start_rad = math.radians(30.0) - el = math.radians(IC_EL) - t_min_notilt = t_min_at_elevation(el, t_min_aero) - - for xi_deg in [30, 40, 50, 60, 70, 80]: - xi_rad = math.radians(xi_deg) - t_trans = max(0.0, (xi_rad - xi_start_rad) / slew_rate) + 1.5 - t_min_xi = t_min_at_elevation(xi_rad, t_min_aero) - - if t_min_notilt is None or t_min_xi is None: - print(f" {xi_deg:>7.0f} altitude fail") - continue - - delta_t = t_min_notilt - t_min_xi # tension reduction from tilting - tether_saved = V_REEL_IN * t_trans # tether NOT reeled in during transition - # Energy saved by lower tension during T_REEL_IN after transition - e_saved = delta_t * V_REEL_IN * (T_REEL_IN - t_trans) - - verdict = ("current" if abs(xi_deg - XI_REEL_IN_NOW) < 1 - else "recommended" if xi_deg == 30 - else "") - marker = " <--" if abs(xi_deg - XI_REEL_IN_NOW) < 1 or xi_deg == 30 else "" - print(f" {xi_deg:>7.0f} {t_trans:>10.1f} {t_min_xi:>8.1f} " - f"{delta_t:>18.1f} {tether_saved:>15.1f} {e_saved:>10.0f} {verdict}{marker}") - print() - print(f" Conclusion: tilting from 30->80 deg saves only " - f"{(t_min_notilt or 0) - (t_min_at_elevation(math.radians(80), t_min_aero) or 0):.1f} N " - f"but costs {(math.radians(80)-xi_start_rad)/slew_rate+1.5:.0f} s of transition time.") - print(f" Removing the tilt (xi_reel_in_deg=None) is recommended for the new aero model.") - print() - - -# ── Section 6: pump cycle telemetry analysis ────────────────────────────────── - -def _parse_cycle_phase(phase_str: str) -> "tuple[int, str] | None": - """Parse 'cycle1_reel-out' -> (1, 'reel-out'). None for non-pump phases.""" - if not phase_str.startswith("cycle"): - return None - parts = phase_str.split("_", 1) - if len(parts) != 2: - return None - try: - cycle = int(parts[0][5:]) - except ValueError: - return None - return cycle, parts[1] - - -def pump_cycle_report(csv_path: str) -> None: - """ - Load a pump-cycle telemetry CSV and compare actual performance against the - optimal envelope. Outputs: - - Per-cycle phase summary (tension tracking, altitude hold, energy) - - Net energy per cycle vs theoretical maximum - - Gap to optimal TENSION_IN setpoint - - TensionPI saturation events - - Altitude hold quality - """ - from collections import defaultdict - from simulation.telemetry_csv import read_csv - - rows = read_csv(csv_path) - if not rows: - print(f" No rows in {csv_path}") - return - - groups: "dict[tuple[int,str], list]" = defaultdict(list) - for r in rows: - cp = _parse_cycle_phase(r.phase) - if cp is not None: - groups[cp].append(r) - - if not groups: - print(" No pump cycle phases found (expected 'cycleN_phase' labels).") - return - - n_cycles = max(cp[0] for cp in groups) - wind_ms = float(np.linalg.norm([rows[0].wind_x, rows[0].wind_y, rows[0].wind_z])) - pwind = np.array([0.0, wind_ms, 0.0]) - - t_min_aero = aero_thrust(COL_MIN, pwind) - el = math.radians(IC_EL) - t_min_tether = t_min_at_elevation(el, t_min_aero) - - print("=" * 72) - print(f"PUMP CYCLE TELEMETRY ANALYSIS: {Path(csv_path).name}") - print(f" {n_cycles} cycle(s) wind={wind_ms:.1f} m/s elevation={IC_EL:.1f} deg") - if t_min_tether is not None: - print(f" T_min at COL_MIN ({COL_MIN:+.2f} rad): {t_min_tether:.1f} N " - f"(aero floor {t_min_aero:.1f} N)") - print() - - # ── Per-cycle phase summary ──────────────────────────────────────────────── - print(f" {'cy':>2} {'phase':>10} {'t_start':>7} {'t_end':>6} " - f"{'T_mean':>7} {'T_sp':>6} {'T_err':>6} " - f"{'col':>7} {'alt_m':>6} {'tgt_m':>6} {'E_J':>7}") - print(f" {'--':>2} {'-'*10} {'-'*7} {'-'*6} " - f"{'-'*7} {'-'*6} {'-'*6} " - f"{'-'*7} {'-'*6} {'-'*6} {'-'*7}") - - cycle_e_out: "dict[int, float]" = {} - cycle_e_in: "dict[int, float]" = {} - pruned_groups: "dict[tuple[int,str], list]" = {} - - for cy in range(1, n_cycles + 1): - for ph in ("reel-out", "hold", "reel-in"): - grp = groups.get((cy, ph), []) - if not grp: - continue - - # Drop isolated rows (planner wrap-around artifacts at cycle boundaries). - # Keep only the largest contiguous block where consecutive time gaps - # are <= 2x the median step interval. - tms = np.array([r.t_sim for r in grp]) - if len(tms) > 2: - dt_steps = np.diff(tms) - med_dt = float(np.median(dt_steps)) - gaps = np.where(dt_steps > 2.0 * med_dt)[0] - if len(gaps): - blocks = np.split(np.arange(len(grp)), gaps + 1) - biggest = max(blocks, key=len) - grp = [grp[i] for i in biggest] - - pruned_groups[(cy, ph)] = grp - - tensions = np.array([r.tether_tension for r in grp]) - colls = np.array([r.collective_rad for r in grp]) - alts = np.array([-r.pos_z for r in grp]) - tsps = np.array([r.tension_feedforward_n for r in grp]) - tgts = np.array([r.gnd_alt_cmd_m for r in grp]) - - # Energy via rest_length trapezoidal integration - rls = np.array([r.tether_rest_length for r in grp]) - drls = np.abs(np.diff(rls)) - t_avg = 0.5 * (tensions[:-1] + tensions[1:]) - energy = float(np.sum(t_avg * drls)) - - if ph == "reel-out": - cycle_e_out[cy] = energy - elif ph == "reel-in": - cycle_e_in[cy] = energy - - t_mean = float(tensions.mean()) - valid_sp = tsps[tsps > 0] - sp_mean = float(valid_sp.mean()) if len(valid_sp) else 0.0 - t_err = t_mean - sp_mean if sp_mean > 0 else math.nan - col_m = float(colls.mean()) - alt_m = float(alts.mean()) - valid_tgt = tgts[tgts > 0] - tgt_m = float(valid_tgt.mean()) if len(valid_tgt) else math.nan - - t_err_s = f"{t_err:+.1f}" if not math.isnan(t_err) else " ---" - tgt_s = f"{tgt_m:.1f}" if not math.isnan(tgt_m) else " ---" - - print(f" {cy:>2} {ph:>10} {grp[0].t_sim:>7.1f} {grp[-1].t_sim:>6.1f} " - f"{t_mean:>7.1f} {sp_mean:>6.0f} {t_err_s:>6} " - f"{col_m:>+7.3f} {alt_m:>6.1f} {tgt_s:>6} {energy:>7.0f}") - print() - - # ── Net energy per cycle ─────────────────────────────────────────────────── - print(f" NET ENERGY PER CYCLE") - print(f" {'cycle':>6} {'E_out_J':>9} {'E_in_J':>8} {'E_net_J':>9}") - print(f" {'-'*6} {'-'*9} {'-'*8} {'-'*9}") - nets = [] - for cy in range(1, n_cycles + 1): - eo = cycle_e_out.get(cy, 0.0) - ei = cycle_e_in.get(cy, 0.0) - en = eo - ei - nets.append(en) - print(f" {cy:>6} {eo:>9.0f} {ei:>8.0f} {en:>9.0f}") - if nets: - avg_net = float(np.mean(nets)) - print(f" {'avg':>6} {'---':>9} {'---':>8} {avg_net:>9.0f}") - print() - - # ── vs optimal envelope ──────────────────────────────────────────────────── - if t_min_tether is not None and nets: - rec_t_in = math.ceil((t_min_tether + 20.0) / 10.0) * 10.0 - - all_tin_rows = [] - all_tout_rows = [] - for cy in range(1, n_cycles + 1): - all_tin_rows.extend([r.tether_tension for r in groups.get((cy, "reel-in"), [])]) - all_tout_rows.extend([r.tether_tension for r in groups.get((cy, "reel-out"), [])]) - actual_t_in = float(np.mean(all_tin_rows)) if all_tin_rows else math.nan - actual_t_out = float(np.mean(all_tout_rows)) if all_tout_rows else math.nan - - # Actual reel-in energy vs what it would be at rec_t_in - avg_e_in = float(np.mean([cycle_e_in.get(cy, 0.0) for cy in range(1, n_cycles + 1)])) - avg_e_out = float(np.mean([cycle_e_out.get(cy, 0.0) for cy in range(1, n_cycles + 1)])) - # Theoretical E_in at rec_t_in uses same reel-in duration as actual run - actual_rl_delta = sum( - abs(groups[(cy, "reel-in")][-1].tether_rest_length - - groups[(cy, "reel-in")][0].tether_rest_length) - for cy in range(1, n_cycles + 1) - if groups.get((cy, "reel-in")) - ) / max(1, sum(1 for cy in range(1, n_cycles + 1) if groups.get((cy, "reel-in")))) - e_in_opt = rec_t_in * actual_rl_delta - e_net_opt = avg_e_out - e_in_opt - - print(f" VS OPTIMAL ENVELOPE") - print(f" T_min at COL_MIN: {t_min_tether:.1f} N") - if not math.isnan(actual_t_out): - print(f" Actual mean TENSION_OUT: {actual_t_out:.1f} N") - if not math.isnan(actual_t_in): - print(f" Actual mean TENSION_IN: {actual_t_in:.1f} N " - f"(+{actual_t_in - t_min_tether:.1f} N above T_min)") - print(f" Recommended TENSION_IN: {rec_t_in:.0f} N (T_min + 20 N margin)") - print(f" Theoretical E_net @ T_in={rec_t_in:.0f} N: {e_net_opt:.0f} J " - f"(using actual reel-in delta={actual_rl_delta:.1f} m)") - gap = e_net_opt - avg_net - pct = 100.0 * gap / avg_net if avg_net > 0 else math.nan - pct_s = f" (+{pct:.0f}% improvement)" if not math.isnan(pct) else "" - print(f" Actual avg E_net: {avg_net:.0f} J") - print(f" Gap to optimal: +{gap:.0f} J{pct_s}") - print() - - # ── AP controller diagnostics (TensionApController) ─────────────────────── - print(f" AP CONTROLLER DIAGNOSTICS (TensionApController + HeliCyclicController)") - print(f" {'phase':>10} {'n_rows':>7} {'sat_%':>7} note") - print(f" {'-'*10} {'-'*7} {'-'*7} {'-'*36}") - for ph in ("reel-out", "transition", "reel-in"): - sat_vals = [] - for cy in range(1, n_cycles + 1): - sat_vals.extend([r.coll_saturated for r in groups.get((cy, ph), [])]) - if not sat_vals: - continue - n = len(sat_vals) - pct_sat = 100.0 * sum(sat_vals) / n - note = "" - if ph == "reel-in" and pct_sat > 50: - note = "collective pinned: T_in limited by aero floor" - elif ph == "reel-out" and pct_sat > 50: - note = "collective pinned: T_out limited by aero ceiling" - print(f" {ph:>10} {n:>7} {pct_sat:>7.1f} {note}") - print() - - # ── Altitude hold quality ───────────────────────────────────────────────── - print(f" ALTITUDE HOLD QUALITY") - print(f" {'phase':>10} {'mean_m':>7} {'target_m':>10} {'std_m':>7} {'bias_m':>7}") - print(f" {'-'*10} {'-'*7} {'-'*10} {'-'*7} {'-'*7}") - for ph in ("reel-out", "reel-in"): - alts = [] - tgts = [] - for cy in range(1, n_cycles + 1): - grp = pruned_groups.get((cy, ph), []) - alts.extend([-r.pos_z for r in grp]) - tgts.extend([r.gnd_alt_cmd_m for r in grp]) - if not alts: - continue - alts_arr = np.array(alts) - tgts_arr = np.array(tgts) - valid = tgts_arr[tgts_arr > 0] - tgt_m = float(valid.mean()) if len(valid) else math.nan - bias = alts_arr.mean() - tgt_m if not math.isnan(tgt_m) else math.nan - tgt_s = f"{tgt_m:.1f}" if not math.isnan(tgt_m) else "---" - bias_s = f"{bias:+.1f}" if not math.isnan(bias) else "---" - print(f" {ph:>10} {alts_arr.mean():>7.1f} {tgt_s:>10} " - f"{alts_arr.std():>7.2f} {bias_s:>7}") - print() - - # ── Orbital trajectory + elevation tracking ─────────────────────────────── - - def _r_horiz(r) -> float: - return math.sqrt(r.pos_x**2 + r.pos_y**2) - - def _el_actual_deg(r) -> float: - tlen = math.sqrt(r.pos_x**2 + r.pos_y**2 + r.pos_z**2) - return math.degrees(math.asin(max(-1.0, min(1.0, -r.pos_z / tlen)))) if tlen > 0.1 else 0.0 - - print(f" ORBITAL TRAJECTORY + ELEVATION TRACKING") - print(f" {'cy':>2} {'phase':>10} {'r_s':>6} {'r_e':>6} {'dr_m':>6} " - f"{'dr/dt':>6} {'el_s':>5} {'el_e':>5} {'el_tgt':>7} {'el_err':>7} note") - print(f" {'--':>2} {'-'*10} {'-'*6} {'-'*6} {'-'*6} " - f"{'-'*6} {'-'*5} {'-'*5} {'-'*7} {'-'*7} {'-'*22}") - - for cy in range(1, n_cycles + 1): - for ph in ("reel-out", "transition", "reel-in"): - grp = pruned_groups.get((cy, ph), []) - if len(grp) < 2: - continue - - r0, r1 = grp[0], grp[-1] - r_start = _r_horiz(r0) - r_end = _r_horiz(r1) - dr = r_end - r_start - duration = r1.t_sim - r0.t_sim - dr_dt = dr / duration if duration > 0.01 else math.nan - - el_s = _el_actual_deg(r0) - el_e = _el_actual_deg(r1) - - # AP elevation target (from TensionApController.elevation_rad) - el_tgt_vals = [math.degrees(r.elevation_rad) for r in grp if r.elevation_rad != 0.0] - el_tgt = float(np.mean(el_tgt_vals)) if el_tgt_vals else math.nan - el_err = float(np.mean([_el_actual_deg(r) - math.degrees(r.elevation_rad) - for r in grp if r.elevation_rad != 0.0])) if el_tgt_vals else math.nan - - note = "" - if ph == "reel-out" and dr > 0: - note = "OK: hub expands" - elif ph == "reel-out" and dr <= 0: - note = "WARN: hub not expanding" - elif ph == "reel-in" and dr < 0: - note = "OK: hub retracts" - elif ph == "reel-in" and dr >= 0: - note = "WARN: hub not retracting" - - dr_s = f"{dr:+.1f}" - drdt_s = f"{dr_dt:+.2f}" if not math.isnan(dr_dt) else " ---" - tgt_s = f"{el_tgt:.1f}" if not math.isnan(el_tgt) else " ---" - err_s = f"{el_err:+.2f}" if not math.isnan(el_err) else " ---" - - print(f" {cy:>2} {ph:>10} {r_start:>6.1f} {r_end:>6.1f} {dr_s:>6} " - f"{drdt_s:>6} {el_s:>5.1f} {el_e:>5.1f} {tgt_s:>7} {err_s:>7} {note}") - print() - - - -# ── Main ────────────────────────────────────────────────────────────────────── - -def main(): - p = argparse.ArgumentParser(description="RAWES pumping cycle performance envelope") - p.add_argument("--wind", type=float, nargs="+", default=[10.0], - metavar="W", help="Wind speed(s) in m/s for sensitivity analysis (default: 10)") - p.add_argument("--telemetry", type=str, default=None, - metavar="CSV", - help="Path to a pump-cycle telemetry.csv — adds actual vs optimal analysis") - args = p.parse_args() - - primary_wind = np.array([0.0, args.wind[0], 0.0]) - - print() - print("pump_envelope.py -- RAWES pumping cycle performance envelope") - print(f"IC: elevation={IC_EL:.1f} deg altitude={-IC_POS[2]:.1f} m " - f"tether={IC_TLEN:.1f} m omega={OMEGA_SPIN:.1f} rad/s") - print(f"Current simtest: TENSION_OUT={TENSION_OUT_NOW:.0f} N " - f"TENSION_IN={TENSION_IN_NOW:.0f} N xi_reel_in={XI_REEL_IN_NOW:.0f} deg") - print() - - validate_formula(primary_wind) - elevation_envelope(primary_wind) - net_energy_sweep(primary_wind) - tilt_analysis(primary_wind) - - all_speeds = sorted(set(args.wind) | {6.0, 8.0, 10.0, 12.0, 15.0}) - wind_sensitivity(all_speeds) - - if args.telemetry: - pump_cycle_report(args.telemetry) - - -if __name__ == "__main__": - main() diff --git a/design/flight_stack.md b/design/flight_stack.md index 82f2536..d210f8a 100644 --- a/design/flight_stack.md +++ b/design/flight_stack.md @@ -283,7 +283,7 @@ def estimate_wind(T_meas, anti_rot_pwm, body_z, theta_col, hub_pos_lpf): return V, wind_dir ``` -**Calibration.** The LUT is generated from PetersHeBEM at the current rotor definition — the same map `pump_envelope.py` already sweeps. No separate calibration pass. +**Calibration.** The LUT is generated by sweeping the current rotor definition's aero model directly. No separate calibration pass. --- @@ -1050,7 +1050,7 @@ level first. | `tests/sitl/torque/conftest.py` | Torque fixtures for DDFP and Lua PASSIVE stack paths | | `tests/sitl/stack_infra.py` | Shared infra: `_sitl_stack`, torque stack helpers, `StackContext`; `_arm_sequence(...)` for stack bring-up | | `calibrate/` (`python -m calibrate`) | Interactive calibration CLI for run/watch/motor/log/config workflows | -| `simulation/controller.py` | `compute_bz_altitude_hold`, `AltitudeHoldController`, `TensionPI`, `RatePID`, `compute_rate_cmd`, `OrbitTracker` | +| `simulation/controller.py` | `compute_bz_altitude_hold`, `AltitudeHoldController`, `TensionPI`, `RatePID`, `compute_rate_cmd` | | `simulation/ap_controller.py` | `TensionApController` (400 Hz AP side), `LandingApController` | | `groundstation/pumping_planner.py` | `TensionCommand`, `PumpingGroundController` (10 Hz phase state machine) | | `groundstation/unified_ground.py` | `NvComms`, `GcsComms` (production TensionCommand -> NAMED_VALUE_FLOAT adapter) | diff --git a/design/simulation.md b/design/simulation.md index 7f8dddb..9bb8ffe 100644 --- a/design/simulation.md +++ b/design/simulation.md @@ -24,8 +24,6 @@ Always initialise the hub with body Z along the tether, not upright. The `build_ - `gyro_body` = `R_hub.T @ omega_body` — full body angular velocity in electronics body frame. No stripping: GB4008 keeps electronics non-rotating via K_YAW damping in dynamics. - `accel_body` = `R_hub.T @ (accel_world_ned − [0,0,9.81])` — specific force in electronics body frame -**`PhysicalHoldController`** (`controller.py`): captures equilibrium roll/pitch at kinematic startup end. Calls `compute_rc_from_attitude(roll − roll_eq, pitch − pitch_eq, ...)` so the function receives the deviation from equilibrium, not the raw physical angle. - **Sensor consistency rules (must all agree or EKF triggers emergency yaw reset):** 1. `rpy[2]` = actual hub orientation yaw from `R_hub` (never overridden with velocity heading) 2. `gyro_body` = `R_hub.T @ omega_body` — no artificial stripping @@ -54,7 +52,6 @@ Always initialise the hub with body Z along the tether, not upright. The `build_ - **`AltitudeHoldController`** — wraps `compute_bz_altitude_hold` with an internal elevation rate-limiter. `from_pos(pos, slew_rate_rad_s)` initialises `_el_rad` from the IC elevation; `update(pos, target_alt_m, tension_n, mass_kg, dt)` rate-limits `_el_rad` and returns the corresponding `body_z_eq`. Here `tension_n` is the **commanded** tension (force-balance feedforward), never a measurement. Used by the AP pumping mode (`_PumpingPythonMode`), `LandingApController`, and hold tests. - **`ElevationHoldController`** — adds `compute_rate_cmd` on top of `AltitudeHoldController`, returning `(rate_roll_sp, rate_pitch_sp)` directly. Kept for compatibility; the GUIDED angle path is the primary AP path. - **`HeliCyclicController`** — inner rate loop + servo lag model. Baked into `PhysicsRunner.step()`. Accepts `(collective_rad, rate_roll, rate_pitch, omega_body, dt)` and returns `(tilt_lon, tilt_lat, col_actual)` after applying the `SwashplateServoModel` (25 ms servo lag). Maps to ArduPilot `ATC_RAT_RLL` / `ATC_RAT_PIT` PID on the stack side. -- **`compute_rc_from_attitude(roll, pitch, rollspeed, pitchspeed, yawspeed, ...)`** — MAVLink-side helper: takes ArduPilot ATTITUDE-message fields (which already encode tether-relative attitude error via `PhysicalSensor`) and returns RC PWM dict. Used by Python-side hold loops where the only feedback is a 10 Hz MAVLink stream. ### arduloop/ — ArduPilot GUIDED mode Python port @@ -69,7 +66,13 @@ Gain names are 1:1 with ArduPilot parameters. `GuidedAttitudeController.update(q ### Inactive controller helpers -`controller.py` includes helpers such as `orbit_tracked_body_z_eq`, `orbit_tracked_body_z_eq_3d`, `compute_swashplate_from_state`, `compute_rc_rates`, `OrbitTracker`, and `PhysicalHoldController` that are not on the current production control path. Do not add new callers; use `compute_bz_altitude_hold` + `slerp_body_z` for active paths. +The pre-`AltitudeHoldController` orbit-tracking design (`orbit_tracked_body_z_eq`, +`orbit_tracked_body_z_eq_3d`, `OrbitTracker`, `blend_body_z`), the truth-state/ +physical-attitude RC helpers it depended on (`compute_rc_rates`, +`compute_swashplate_from_state`, `compute_rc_from_physical_attitude`), and the +MAVLink-side ACRO RC-override path (`compute_rc_from_attitude`, `PhysicalHoldController`, +`make_hold_controller`) had no production callers and were all deleted from +`controller.py`. Use `compute_bz_altitude_hold` + `slerp_body_z` for active paths. ### `TensionPI` — collective PID (offline / winch only) @@ -154,11 +157,14 @@ M_orbital += disk_normal * T_GB4008 `K_yaw → large`: perfect damper (yaw rate ≈ 0, hardware nominal). `K_yaw` finite: realistic GB4008 with residual yaw drift, visible in `rpy_yaw` telemetry. -**Rotor spin** is maintained as a separate scalar `omega_spin`, updated each step via the BEM-derived spin torque: +**Rotor spin** is maintained as a separate scalar `omega_spin`, updated each step via `dynbem.mechanical.step_omega()` — the single canonical spin-ODE integrator (also used by `omega_derivative()` for direct-derivative callers such as unit tests). windpower never re-implements the Euler update itself: ``` -omega_spin += result.Q_spin / I_SPIN_KGMS2 × dt +new_omega, new_spin = step_omega( + omega, spin_angle, Q_spin, motor_torque_Nm, I_ode_kgm2, dt, + bearing_friction_Nm=BEARING_FRICTION_NM, +) ``` -`Q_spin` comes from `dynbem`'s `create_aero()` model (quasi_static BEM) which balances drive torque (from inflow) against profile drag torque. Gives a stable equilibrium — no empirical K_drive/K_drag constants needed. +`Q_spin` comes from `dynbem`'s `create_aero()` model (quasi_static BEM) which balances drive torque (from inflow) against profile drag torque. Gives a stable equilibrium — no empirical K_drive/K_drag constants needed. `BEARING_FRICTION_NM` is a windpower-owned constant in `physics_core.py` (not part of the aero rotor schema). **Gyroscopic coupling** is included in Euler's equations (`dynamics.py`): ``` @@ -244,7 +250,7 @@ These follow directly from the orbit physics and pin down key simulation invaria **Production model:** `quasi_static` BEM from the `dynbem` external package (`create_aero(rotor, model="quasi_static")`). -All simulation code, simtests, and the mediator use `dynbem.create_aero()` with `model="quasi_static"` (the default in `PhysicsCore` and `PhysicsRunner`). Dynamic inflow models (`oye`, `pitt_peters`, `jit`) are opt-in only and are not used in any production flight path. +All simulation code, simtests, and the mediator use `dynbem.create_aero()` with `model="quasi_static"` (the default in `PhysicsCore` and `PhysicsRunner`). Dynamic inflow models (`oye`, `pitt_peters`, `vpm`) are opt-in only and are not used in any production flight path. ### dynbem factory @@ -254,18 +260,19 @@ All simulation code, simtests, and the mediator use `dynbem.create_aero()` with from dynbem import create_aero, rotor_definition as rd aero = create_aero(rotor, model="quasi_static") # production default aero = create_aero(rotor, model="oye") # dynamic inflow (opt-in) -aero = create_aero(rotor, model="jit") # Peters-He JIT (opt-in) +aero = create_aero(rotor, model="pitt_peters") # dynamic inflow (opt-in) ``` ### Available model keys | Key | Description | |-----|-------------| -| `quasi_static` | **Production default** — quasi-static BEM, no inflow state; fastest and most numerically stable | -| `oye` | Øye dynamic wake model — first-order filter on induced velocity | -| `pitt_peters` | Pitt-Peters 3-state dynamic inflow | -| `jit` | Peters-He 3-state inflow with Numba `@njit` hot loop; stateful via `to_dict()` / `from_dict()` | -| `bem` | Minimal textbook BEM — sanity cross-check only | +| `quasi_static` (alias `bem`) | **Production default** — quasi-static BEM, no inflow state; fastest and most numerically stable | +| `oye` (alias `oye_bem`) | Øye dynamic wake model — 2-stage annular inflow filter | +| `pitt_peters` | Pitt-Peters 3-state dynamic inflow (L-matrix) | +| `vpm` (alias `vpm_rotor`/`free_wake`) | Forward-flight free-wake vortex particle method; no single-shot `compute_forces` -- advance with `step(inputs, state, dt)` | + +Note: the old Peters-He model (`model="jit"`/`"peters_he"`, 5-state dynamic inflow, valid through axial descent) no longer exists in `dynbem` -- it has not been re-implemented under the new state-based `step()` API. Code that relied on it for near-vertical/axial-descent validity (e.g. `envelope/`) currently uses `quasi_static` instead; see `envelope/CLAUDE.md` for the open caveat. ### AeroResult @@ -473,8 +480,7 @@ simulation/ │ col_min_for_altitude_rad/compute_bz_altitude_hold), AltitudeHoldController, │ ElevationHoldController, HeliCyclicController (rate PIDs + SwashplateServoModel │ 25 ms lag; baked into PhysicsRunner), TensionPI (collective PID at 400 Hz; -│ kd=0 default). Legacy dead code: compute_swashplate_from_state, -│ compute_rc_from_attitude, PhysicalHoldController, OrbitTracker. +│ kd=0 default). ├── mock_ardupilot.lua Minimal Lua stub of the ArduPilot API used by rawes.lua unit tests │ (runs via lupa in RawesLua harness). Provides Vector3f, ahrs, rc, │ SRV_Channels, param, gcs, arming, vehicle, mavlink stubs. State lives in @@ -554,7 +560,6 @@ analysis/ ├── analyse_run.py Post-run report: print_flight_report, compute_steady_metrics, │ validate_ekf_window; CLI: --bucket S. ├── analyse_landing.py Landing diagnosis (alt/vz/winch/tension/collective per bucket). -├── pump_envelope.py Pumping cycle envelope sweep (tension setpoints, wind, tilt). └── pump_diagnosis.py Per-bucket compact summary + CSV (osc, corr) to test log dir. viz3d/ diff --git a/design/sitl_flight_timeline.md b/design/sitl_flight_timeline.md index dcc899c..6d89654 100644 --- a/design/sitl_flight_timeline.md +++ b/design/sitl_flight_timeline.md @@ -105,8 +105,9 @@ Rotor state reference for this run: `kinematic_exit`; there is no distinct spin-start transition in this run. Tilt scheduling ownership note: -- The kinematic hold controller (`make_hold_controller`) is setup/parameter plumbing in - the stack harness; it is not the runtime source of tilt commands in this IC-start path. +- The kinematic hold controller setup in the stack harness (`tests/sitl/stack_infra.py`) + is parameter/anchor plumbing only; it is not the runtime source of tilt commands in + this IC-start path. - Runtime tilt scheduling during kinematic hold comes from `rawes.lua` in `MODE_PASSIVE` once the IC seed is complete (`RAWES_THR` + `RAWES_RIC` + `RAWES_PIC`) and `guided_ok` is true. diff --git a/envelope/CLAUDE.md b/envelope/CLAUDE.md index b055eea..7cd5de7 100644 --- a/envelope/CLAUDE.md +++ b/envelope/CLAUDE.md @@ -13,21 +13,34 @@ target tensions are reachable across the full reel-out/reel-in envelope. | File | Purpose | |------|---------| -| `point_mass.py` | Single-cell ODE integrator — generalised point-mass force balance at arbitrary elevation. `simulate_point(col, elevation_deg, tension_n, wind_speed, ...)` → `{eq, history}`. Uses `model="peters_he"` (PetersHeBEMJit). | +| `point_mass.py` | Single-cell ODE integrator — generalised point-mass force balance at arbitrary elevation. `simulate_point(col, elevation_deg, tension_n, wind_speed, ...)` → `{eq, history}`. Uses `model="quasi_static"` (the project's production aero model; see below for the open validity caveat). | | `compute_map.py` | Parallel grid computation. `_ramp_full_column()` sweeps tension from T_min→T_max on a single (wind, angle, v_target) column, sampling at each grid tension. `compute_grid()` runs all columns in a `ProcessPoolExecutor`. CLI: `--quick` / `--full` / `--load`. | | `analyse_envelope.py` | Post-processing: load `.npz` grid, compute net-energy contours, plot slices, export CSVs. | --- -## Peters-He Dependency - -All cells use `model="peters_he"` → `PetersHeBEMJit` (Numba JIT, 5-state dynamic inflow). -This is required because the envelope covers near-vertical angles (high elevation, xi ≳ 85°) -where the Coleman skewed-wake correction degenerates. Peters-He uses a momentum ODE -(`V_T = sqrt(v_inplane² + v_axial²)`) that is valid from hover through axial descent. - -`PetersHeBEM.is_valid()` guards on `abs(v0, v1c, v1s) ≤ INFLOW_MAX_MS=50.0` and -`isfinite(last_T)`. `point_mass.py` calls this at line 148 to detect diverged states. +## Aero Model — Peters-He Removed, Not Yet Replaced for High Elevation + +The envelope module used to run all cells with `model="peters_he"` (`PetersHeBEMJit`, +Numba JIT, 5-state dynamic inflow) because the envelope covers near-vertical angles +(high elevation, xi ≳ 85°) where the Coleman skewed-wake correction degenerates. +Peters-He used a momentum ODE (`V_T = sqrt(v_inplane² + v_axial²)`) valid from hover +through axial descent. + +**`dynbem` no longer exposes a Peters-He model at all** (no `"peters_he"`/`"jit"` key, +no `PetersHeBEM` class — see `design/simulation.md` "Aerodynamic Model" section for the +current model list: `quasi_static`/`bem`, `pitt_peters`, `oye`/`oye_bem`, `vpm`). The +module has been migrated to `model="quasi_static"` (the project's production model, +used everywhere else in windpower) purely to restore a working state-based API — this +has **not** been validated as accurate at high elevation/near-axial-descent angles the +way Peters-He was. Treat envelope results near xi ≳ 85° with caution until this is +revisited (e.g. by trying `pitt_peters`/`oye` or asking upstream for a Peters-He- +equivalent replacement in `dynbem`). + +The per-step `isfinite`/`OverflowError`/`ValueError` guards that used to call +`PetersHeBEM.is_valid()` now check `np.isfinite(f.F_world)` / `math.isfinite(f.Q_spin)` +directly on the `AeroResult` returned by `aero.step()` — there is no separate +`is_valid()` method in the current API. --- diff --git a/envelope/analyse_envelope.py b/envelope/analyse_envelope.py index 4cc628f..b23911e 100644 --- a/envelope/analyse_envelope.py +++ b/envelope/analyse_envelope.py @@ -55,8 +55,8 @@ # ── Geometry summary ─────────────────────────────────────────────────────────── def _print_geometry(elevation_deg: float, tension_n: float, wind_speed: float) -> None: - import rotor_definition as rd - mass = rd.default().dynamics_kwargs()["mass"] + from envelope.rotor_helpers import load_default_rotor + mass = float(load_default_rotor().inertia.mass_kg) weight = mass * _G t_hat = tether_hat(elevation_deg) F_teth = tension_n * t_hat diff --git a/envelope/compute_map.py b/envelope/compute_map.py index a7740bc..60f3e15 100644 --- a/envelope/compute_map.py +++ b/envelope/compute_map.py @@ -40,16 +40,18 @@ def _ramp_full_column(args: tuple) -> list: ti_unified is the index into tension_list_unified (the merged list of all tensions). """ vi, wi, ai, v_target, wind, angle, v_tension_list, dt, tension_list_unified = args - import rotor_definition as rd - from dynbem import create_aero + from dynbem import create_aero, RotorInputs, step_omega from simulation.frames import build_orb_frame + from simulation.physics_core import BEARING_FRICTION_NM from envelope.point_mass import tether_hat, balance_bz - - rotor = rd.default() - aero = create_aero(rotor, model="peters_he") - dk = rotor.dynamics_kwargs() - mass = dk["mass"] - I_spin = dk["I_spin"] + from envelope.rotor_helpers import load_default_rotor + + rotor = load_default_rotor() + aero = create_aero(rotor, model="quasi_static") + rotor_state = aero.initial_rotor_state() + mass = float(rotor.inertia.mass_kg) + I_ode = float(rotor.autorotation.I_ode_kgm2 or 10.0) + omega_min = float(rotor.autorotation.omega_min_rad_s or 0.5) weight = mass * 9.81 clamp = math.radians(8.6) wind_v = np.array([0.0, -float(wind), 0.0]) @@ -57,6 +59,7 @@ def _ramp_full_column(args: tuple) -> list: # Fresh start: omega=28 (operational), vel=0, col=0 omega = 28.0 + spin_angle = 0.0 vel = np.zeros(3) col_now = 0.0 c_lon = c_lat = 0.0 @@ -107,19 +110,27 @@ def _ramp_full_column(args: tuple) -> list: R = build_orb_frame(bz) F_teth = tension * t_hat + inputs = RotorInputs( + collective_rad = col_now, tilt_lon = c_lon, tilt_lat = c_lat, + R_hub = R, v_hub_world = vel.copy(), + wind_world = wind_v, omega_rad_s = omega, + rho_kg_m3 = 1.225, + ) try: - f = aero.compute_forces(col_now, c_lon, c_lat, R, vel.copy(), - omega, wind_v) + f, rotor_state = aero.step(inputs, rotor_state, dt) except (OverflowError, ValueError, FloatingPointError): break - if not aero.is_valid(): + if not np.all(np.isfinite(f.F_world)) or not math.isfinite(f.Q_spin): break thrust = float(np.dot(f.F_world, bz)) - Q_net = float(np.dot(aero.last_M_spin, bz)) F_net = f.F_world + F_teth + np.array([0.0, 0.0, weight]) - omega = max(0.0, omega + (Q_net / I_spin) * dt) + omega, spin_angle = step_omega( + omega, spin_angle, float(f.Q_spin), 0.0, I_ode, dt, + bearing_friction_Nm=BEARING_FRICTION_NM, + ) + omega = max(omega_min, omega) vel = vel + (F_net / mass) * dt if not np.all(np.isfinite(vel)): @@ -193,35 +204,6 @@ def _ramp_full_column(args: tuple) -> list: V_REEL_IN = 0.8 -# ── PID-based sweep ──────────────────────────────────────────────────────────── - -def _run_pass(tasks: list, tag: str, n_workers: int, - settled_col: np.ndarray, settled_v: np.ndarray, - col_sat: np.ndarray, settled_ic: np.ndarray) -> int: - """Submit tasks, collect results into arrays. Returns number of newly converged cells.""" - newly = 0 - total = len(tasks) - done = 0 - t0 = time.time() - with ProcessPoolExecutor(max_workers=n_workers) as pool: - futures = {pool.submit(_pid_cell, t[1:]): t[:4] for t in tasks} - for fut in as_completed(futures): - vi, wi, ai, ti = futures[fut][:4] - _, _, _, col, va, sat, ic_out = fut.result() - if np.isfinite(col): - settled_col[vi, wi, ai, ti] = col - settled_v [vi, wi, ai, ti] = va - col_sat [vi, wi, ai, ti] = sat - settled_ic [vi, wi, ai, ti] = ic_out - newly += 1 - done += 1 - elapsed = time.time() - t0 - eta = elapsed / done * (total - done) if done > 0 else 0 - print(f" [{tag}] {done:>4}/{total} eta={eta:.0f}s ", end="\r") - print() - return newly - - def compute_grid( wind_list: list[float], angle_list: list[float], diff --git a/envelope/point_mass.py b/envelope/point_mass.py index c249bf2..34f5dfe 100644 --- a/envelope/point_mass.py +++ b/envelope/point_mass.py @@ -1,8 +1,6 @@ """ point_mass.py -- Generalised point-mass equilibrium ODE for arbitrary flight conditions. -Extends the analyse_descent approach to arbitrary tether elevation angles. - Physics ------- - Disk orientation fixed at force-balance direction: bz = -norm(F_teth + F_grav) @@ -10,9 +8,8 @@ - Wind: [0, +wind_speed, 0] in NED (blowing East, i.e. from West) - Cyclic PI drives horizontal velocity (North, East) toward zero - Along-tether and vertical velocities evolve freely from force balance -- Spin ODE: d(omega)/dt = Q_spin / I_spin - -At el=90 (vertical) this is identical to simulate_descent() in analyse_descent.py. +- Spin ODE: integrated via dynbem.mechanical.step_omega() (the single canonical + spin-ODE integrator; see simulation/physics_core.py for the production usage) NED convention: X=North, Y=East, Z=Down. Gravity = [0, 0, +g*m]. Altitude = -pos[2]. Upward thrust = dot(F_world, body_z). @@ -26,9 +23,10 @@ import numpy as np -from dynbem import rotor_definition as rd -from dynbem import create_aero +from dynbem import create_aero, RotorInputs, step_omega from simulation.frames import build_orb_frame +from simulation.physics_core import BEARING_FRICTION_NM +from envelope.rotor_helpers import load_default_rotor _G = 9.81 @@ -99,11 +97,11 @@ def simulate_point( range over conv_window_s is below this conv_window_s : window duration [s] to check for convergence """ - rotor = rd.default() - dk = rotor.dynamics_kwargs() - mass = dk["mass"] - I_spin = dk["I_spin"] - weight = mass * gravity + rotor = load_default_rotor() + mass = float(rotor.inertia.mass_kg) + I_ode = float(rotor.autorotation.I_ode_kgm2 or 10.0) + omega_min = float(rotor.autorotation.omega_min_rad_s or 0.5) + weight = mass * gravity t_hat = tether_hat(elevation_deg) F_teth = tension_n * t_hat @@ -112,9 +110,10 @@ def simulate_point( wind = np.array([0.0, -float(wind_speed), 0.0]) # blowing West, away from anchor clamp = math.radians(cyc_clamp_deg) + aero = create_aero(rotor, model="quasi_static") + rotor_state = aero.initial_rotor_state() + if ic is not None: - aero = create_aero(rotor, model="peters_he", - state_dict=ic.get("aero_state")) omega = float(ic.get("omega", omega_init)) vel = np.array(ic.get("vel", float(v_along_init) * t_hat), dtype=float) col_now = float(ic.get("col", col)) @@ -124,7 +123,6 @@ def simulate_point( int_vy = float(ic.get("int_vy", 0.0)) int_vcol = float(ic.get("int_vcol", 0.0)) else: - aero = create_aero(rotor, model="peters_he") omega = float(omega_init) vel = float(v_along_init) * t_hat col_now = float(col) @@ -133,6 +131,7 @@ def simulate_point( int_vx = 0.0 int_vy = 0.0 int_vcol = 0.0 + spin_angle = 0.0 history = [] log_every = max(1, int(round(1.0 / dt))) @@ -140,18 +139,27 @@ def simulate_point( conv_steps = max(1, int(round(conv_window_s / dt))) for i in range(n_steps): + inputs = RotorInputs( + collective_rad = col_now, tilt_lon = c_lon, tilt_lat = c_lat, + R_hub = R, v_hub_world = vel.copy(), + wind_world = wind, omega_rad_s = omega, + rho_kg_m3 = 1.225, + ) try: - f = aero.compute_forces(col_now, c_lon, c_lat, R, vel.copy(), omega, wind) + f, rotor_state = aero.step(inputs, rotor_state, dt) except (OverflowError, ValueError, FloatingPointError): break - if not aero.is_valid(): + if not np.all(np.isfinite(f.F_world)) or not math.isfinite(f.Q_spin): break thrust = float(np.dot(f.F_world, bz)) - Q_net = float(np.dot(aero.last_M_spin, bz)) F_net = f.F_world + F_teth + np.array([0.0, 0.0, weight]) # weight = mass*gravity - omega = max(0.0, omega + (Q_net / I_spin) * dt) + omega, spin_angle = step_omega( + omega, spin_angle, float(f.Q_spin), 0.0, I_ode, dt, + bearing_friction_Nm=BEARING_FRICTION_NM, + ) + omega = max(omega_min, omega) vel = vel + (F_net / mass) * dt # Cyclic PI: null cross-tether velocity only (project out along-tether) @@ -241,7 +249,6 @@ def simulate_point( int_vx = int_vx, int_vy = int_vy, int_vcol = int_vcol, - aero_state = aero.to_dict(), ) return dict(history=history, eq=eq, cyc_saturated=cyc_sat, diff --git a/envelope/rotor_helpers.py b/envelope/rotor_helpers.py new file mode 100644 index 0000000..3be1ff1 --- /dev/null +++ b/envelope/rotor_helpers.py @@ -0,0 +1,20 @@ +"""Shared rotor-loading helper for the envelope package. + +The current (nested blade/airfoil/inertia/control/autorotation) +``dynbem.rotor_definition.RotorDefinition`` has no project ``default()`` +factory or ``dynamics_kwargs()`` method -- envelope scripts load the project +rotor explicitly from ``simulation/rotor_definitions/`` and read +``rotor.inertia.mass_kg`` / ``rotor.autorotation.I_ode_kgm2`` directly. +""" +from __future__ import annotations +from pathlib import Path + +import simulation +from dynbem import rotor_definition as _rd + +_ROTOR_DEFS = Path(simulation.__file__).resolve().parent / "rotor_definitions" + + +def load_default_rotor(): + """Load the project default rotor (beaupoil_2026).""" + return _rd.load(str(_ROTOR_DEFS / "beaupoil_2026.yaml")) diff --git a/simulation/README.md b/simulation/README.md index 117d4f8..8268b40 100644 --- a/simulation/README.md +++ b/simulation/README.md @@ -55,7 +55,7 @@ Canonical design details are maintained in: | `frames.py` | Coordinate-frame utilities (`build_orb_frame()`, transforms) | | `sensor.py` | `build_sitl_packet()` — NED truth state → ArduPilot JSON sensor packet | | `sitl_interface.py` | ArduPilot SITL UDP binary protocol (servo recv, state send) | -| `controller.py` | `compute_swashplate_from_state()` — truth-state tether-alignment controller; `compute_rc_from_attitude()` — legacy ACRO RC helper | +| `controller.py` | `compute_bz_altitude_hold` + `slerp_body_z` — active elevation-hold path | | `gcs.py` | MAVLink GCS client (arm, mode, params, named-float commands) | | `flight_report.py` | Multi-panel PNG flight report from position/attitude/servo history | diff --git a/simulation/controller.py b/simulation/controller.py index 022811d..da42f1e 100644 --- a/simulation/controller.py +++ b/simulation/controller.py @@ -9,7 +9,6 @@ import numpy as np from simulation.frames import build_orb_frame, cross3 # noqa: F401 — build_orb_frame re-exported for callers -from simulation.servo_pwm import SWASH_PWM_NEUTRAL, SWASH_PWM_RANGE from simulation.swashplate import SwashplateServoModel from dynbem import RotorInputs from simulation.param_defaults import load_ap_params as _load_ap_params @@ -17,406 +16,6 @@ from arduloop import HeliRateController, HeliParams, RateAxisParams -def compute_rc_rates( - hub_state: dict, - anchor_pos: np.ndarray, - vel_ned: np.ndarray, - kp: float = 0.5, - kd: float = 0.2, - rate_max_deg: float = 200.0, -) -> dict: - """ - Compute RC override channels to keep body_z aligned with the tether. - - Parameters - ---------- - hub_state : dict - Current rigid-body state with keys: - ``pos`` — NED position (3,) - ``R`` — rotation matrix body→world (3, 3) - ``omega`` — angular velocity in world NED frame (3,) - anchor_pos : array-like (3,) - Tether anchor position in NED world frame. - vel_ned : array-like (3,) - Hub velocity in NED frame (used to derive heading yaw for body-frame - alignment with ArduPilot's ACRO controller). - kp : float - Proportional gain on attitude error (rad/s per rad). - kd : float - Derivative gain on orbital angular rate (dimensionless). - rate_max_deg : float - Angular rate corresponding to full stick deflection (degrees/s). - - Returns - ------- - dict - RC channel overrides: {1: pwm, 2: pwm, 4: pwm}. - Channel 1 = roll rate, 2 = pitch rate, 4 = yaw rate. - Neutral = 1500, min = 1000, max = 2000. - """ - pos = np.asarray(hub_state["pos"], dtype=float) - R = np.asarray(hub_state["R"], dtype=float) - omega = np.asarray(hub_state["omega"], dtype=float) - anch = np.asarray(anchor_pos, dtype=float) - - # Safety: if hub is at/inside anchor, return neutral - tether = pos - anch - t_len = float(np.linalg.norm(tether)) - if t_len < 0.1: - return {1: SWASH_PWM_NEUTRAL, 2: SWASH_PWM_NEUTRAL, 4: SWASH_PWM_NEUTRAL} - - # FRD body_z points DOWN through the disk → hub→anchor in tethered hover. - body_z_eq = -tether / t_len - - # Actual body_z = third column of rotation matrix - body_z_cur = R[:, 2] - - # Attitude error in world NED frame (cross product → rotation axis toward target) - error_world = cross3(body_z_cur, body_z_eq) - - # Strip spin component from omega (spin is along body_z, not an orbital rate) - omega_spin = np.dot(omega, body_z_cur) * body_z_cur - omega_orbital = omega - omega_spin - - # Rate correction in NED world frame: P term + D term - omega_corr = kp * error_world - kd * omega_orbital - - # Rotate into ArduPilot's yaw-aligned body frame - vel_ned = np.asarray(vel_ned, dtype=float) - v_horiz = float(np.hypot(vel_ned[0], vel_ned[1])) - yaw = float(np.arctan2(vel_ned[1], vel_ned[0])) if v_horiz > 0.1 else 0.0 - cy, sy = np.cos(yaw), np.sin(yaw) - Rz_inv = np.array([[cy, sy, 0.], [-sy, cy, 0.], [0., 0., 1.]]) - omega_body = Rz_inv @ omega_corr - - # Map rad/s to PWM [1000, 2000], 1500 = zero - max_rate = np.radians(rate_max_deg) - - def _pwm(w: float) -> int: - return int(round(SWASH_PWM_NEUTRAL + SWASH_PWM_RANGE * float(np.clip(w / max_rate, -1.0, 1.0)))) - - return { - 1: _pwm(omega_body[0]), # roll rate - 2: _pwm(omega_body[1]), # pitch rate - 4: _pwm(omega_body[2]), # yaw rate - } - - -def compute_swashplate_from_state( - hub_state: dict, - anchor_pos: np.ndarray, - kp: float = 0.5, - kd: float = 0.2, - tilt_max_rad: float = 0.3, - body_z_eq: "np.ndarray | None" = None, - swashplate_phase_deg: float = 0.0, -) -> dict: - """ - Compute swashplate tilt commands directly from hub truth state. - - This bypasses ArduPilot entirely — the mediator can call this directly - and feed tilt_lon/tilt_lat into aero.compute_forces(). - - Parameters - ---------- - hub_state : dict with pos (NED), R (body→world), omega (world NED) - anchor_pos : tether anchor in NED [m] - kp : proportional gain on attitude error [rad/rad] - kd : derivative gain on orbital rate [rad·s/rad] - tilt_max_rad: maximum swashplate tilt [rad] (saturation limit) - body_z_eq : optional equilibrium body-z override (unit vector, NED). - When None (default), body_z_eq is computed as the tether - direction (pos - anchor) / |pos - anchor|. Pass the hub's - current R[:,2] to hold the present orientation with zero - error, or a blended vector to ramp in tether-alignment - gradually after a mode transition. - - Returns - ------- - dict: {collective_rad: float, tilt_lon: float, tilt_lat: float} - tilt_lon/tilt_lat in [-1, 1] (normalised swashplate coordinates) - """ - pos = np.asarray(hub_state["pos"], dtype=float) - R = np.asarray(hub_state["R"], dtype=float) - omega = np.asarray(hub_state["omega"], dtype=float) - anch = np.asarray(anchor_pos, dtype=float) - - # Safety: if hub is at/inside anchor, return neutral - tether = pos - anch - t_len = float(np.linalg.norm(tether)) - if t_len < 0.1: - return {"collective_rad": 0.0, "tilt_lon": 0.0, "tilt_lat": 0.0} - - # FRD body_z points DOWN through the disk → hub→anchor in tethered hover. - if body_z_eq is not None: - body_z_eq = np.asarray(body_z_eq, dtype=float) - n = float(np.linalg.norm(body_z_eq)) - body_z_eq = body_z_eq / n if n > 1e-6 else -tether / t_len - else: - body_z_eq = -tether / t_len - body_z_cur = R[:, 2] - - error_world = cross3(body_z_cur, body_z_eq) - - omega_spin = np.dot(omega, body_z_cur) * body_z_cur - omega_orbital = omega - omega_spin - - # Correction in world NED frame - corr_ned = kp * error_world - kd * omega_orbital - - # Project the correction into FRD body frame. Sign convention from - # the new aero (aero/cyclic.py): - # - # tilt_lat > 0 ⇒ roll right (+rotation about body_x) - # tilt_lon > 0 ⇒ nose-down disk (−rotation about body_y; positive pitch is nose-up) - # - # So a desired body-frame rate (roll, pitch, yaw) = R.T @ corr_ned maps to - # tilt_lat = +corr_body[0] (roll) - # tilt_lon = -corr_body[1] (negative pitch = nose-down) - corr_body = R.T @ corr_ned - tilt_lat = float(np.clip( corr_body[0] / tilt_max_rad, -1.0, 1.0)) - tilt_lon = float(np.clip(-corr_body[1] / tilt_max_rad, -1.0, 1.0)) - - # Gyroscopic phase compensation. - # A spinning rotor precesses 90° off-axis from an applied cyclic torque. - # Rotating the command by swashplate_phase_deg advances it so the aerodynamic - # response lands in the intended direction. 0° = no compensation (I_spin = 0). - if swashplate_phase_deg != 0.0: - phi = math.radians(swashplate_phase_deg) - c, s = math.cos(phi), math.sin(phi) - tilt_lon, tilt_lat = ( - c * tilt_lon - s * tilt_lat, - s * tilt_lon + c * tilt_lat, - ) - - return { - "collective_rad": 0.0, # collective held at zero; attitude via tilt only - "tilt_lon": tilt_lon, - "tilt_lat": tilt_lat, - } - - -def compute_rc_from_attitude( - roll: float, - pitch: float, - rollspeed: float, - pitchspeed: float, - yawspeed: float, - kp: float = 1.0, - kd: float = 0.3, - rate_max_deg: float = 200.0, -) -> dict: - """ - Map attitude error + body rates to ACRO RC PWM overrides. - - Generic error-to-PWM converter used by PhysicalHoldController, which - pre-computes roll/pitch as deviations from the captured equilibrium - (att["roll"] − roll_eq, att["pitch"] − pitch_eq) before calling here. - - Commands: - cmd_roll = -kp * roll - kd * rollspeed - cmd_pitch = -kp * pitch - kd * pitchspeed - cmd_yaw = - kd * yawspeed - - Parameters - ---------- - roll, pitch : attitude error from equilibrium [rad] - rollspeed, pitchspeed, yawspeed : body-frame angular rates [rad/s] - kp : attitude error gain [rad/s per rad] - kd : rate damping gain [dimensionless] - rate_max_deg : full-stick rate [deg/s] → maps to PWM 1000/2000 - - Returns - ------- - dict : {1: pwm, 2: pwm, 4: pwm} - """ - max_rate = np.radians(rate_max_deg) - - def _pwm(w: float) -> int: - return int(round(SWASH_PWM_NEUTRAL + SWASH_PWM_RANGE * float(np.clip(w / max_rate, -1.0, 1.0)))) - - cmd_roll = -kp * float(roll) - kd * float(rollspeed) - cmd_pitch = -kp * float(pitch) - kd * float(pitchspeed) - cmd_yaw = - kd * float(yawspeed) - - return { - 1: _pwm(cmd_roll), - 2: _pwm(cmd_pitch), - 4: _pwm(cmd_yaw), - } - - -def compute_rc_from_physical_attitude( - roll: float, - pitch: float, - yaw: float, - rollspeed: float, - pitchspeed: float, - yawspeed: float, - pos_ned: np.ndarray, # hub NED position [m] from LOCAL_POSITION_NED - anchor_ned: np.ndarray, # tether anchor NED position [m] (relative to same origin) - kp: float = 1.0, - kd: float = 0.3, - rate_max_deg: float = 200.0, -) -> dict: - """ - Compute RC override channels from physical attitude + position. - - Used with ``PhysicalSensor`` where ATTITUDE.roll/pitch are the actual NED - Euler angles of the orbital frame (~65° from vertical at tether - equilibrium), NOT tether-relative deviations. - - Derives the tether-alignment error directly from: - - The physical body_z direction (column 2 of R_body built from rpy) - - The tether equilibrium direction (pos_ned − anchor_ned, normalised) - - The PD correction is computed in the physical NED body frame and mapped - to PWM the same way as ``compute_rc_from_attitude``. - - Parameters - ---------- - roll, pitch, yaw : actual NED Euler angles [rad] from ATTITUDE message - rollspeed, pitchspeed, yawspeed : NED body-frame angular rates [rad/s] - pos_ned : hub position in NED [m] - anchor_ned : tether anchor in NED [m] (same LOCAL frame origin) - kp, kd, rate_max_deg : same meaning as compute_rc_from_attitude - - Returns - ------- - dict : {1: pwm, 2: pwm, 4: pwm} - """ - max_rate = np.radians(rate_max_deg) - - def _pwm(w: float) -> int: - return int(round(SWASH_PWM_NEUTRAL + SWASH_PWM_RANGE * float(np.clip(w / max_rate, -1.0, 1.0)))) - - # Body→NED rotation matrix from ZYX Euler angles. - # Column j = j-th body axis expressed in NED. - cr, sr = math.cos(roll), math.sin(roll) - cp, sp = math.cos(pitch), math.sin(pitch) - cy, sy = math.cos(yaw), math.sin(yaw) - R_body = np.array([ - [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr], - [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr], - [-sp, cp*sr, cp*cr ], - ]) - - # Actual rotor axle (body Z) in NED - body_z_ned = R_body[:, 2] - - # FRD body_z_eq points DOWN through the disk → hub→anchor direction. - tether_ned = np.asarray(anchor_ned, dtype=float) - np.asarray(pos_ned, dtype=float) - t_len = float(np.linalg.norm(tether_ned)) - if t_len < 0.1: - return {1: SWASH_PWM_NEUTRAL, 2: SWASH_PWM_NEUTRAL, 4: SWASH_PWM_NEUTRAL} - body_z_eq = tether_ned / t_len - - # Attitude error: rotation axis needed to align body_z with body_z_eq. - # cross(a, b) = |a||b|sin(θ) n̂ — small when already aligned. - error_ned = cross3(body_z_ned, body_z_eq) - - # Express error in NED body frame (NED→body = R_body.T) - error_body = R_body.T @ error_ned - - # Angular velocity in body frame from ATTITUDE message - omega_body = np.array([rollspeed, pitchspeed, yawspeed]) - - # Strip spin component along body_z (electronics don't spin) - omega_spin = np.dot(omega_body, body_z_ned) * body_z_ned - omega_orbital = omega_body - omega_spin - - # PD rate correction in body frame - omega_corr = kp * error_body - kd * omega_orbital - - return { - 1: _pwm(omega_corr[0]), # roll rate - 2: _pwm(omega_corr[1]), # pitch rate - 4: _pwm(omega_corr[2]), # yaw rate - } - - -# --------------------------------------------------------------------------- -# Hold controller abstraction — used by test_guided_flight.py -# --------------------------------------------------------------------------- - - - -class PhysicalHoldController: - """ - Legacy hold controller retained for compatibility tests. - - ATTITUDE.roll/pitch are actual NED Euler angles. Error is computed as - deviation from the tether equilibrium orientation (roll_eq, pitch_eq) - captured during the kinematic startup. Uses compute_rc_from_attitude so - the correction is yaw-independent — the velocity-derived yaw jumps ~150° - when the tether activates at kinematic end, making any yaw-dependent error - computation destabilising. - - Parameters - ---------- - anchor_ned : array (3,) - Tether anchor position in NED [m] (kept for divergence guard). - - Call ``set_equilibrium(roll_eq, pitch_eq)`` with values from the first - clean ATTITUDE message before the controller is used. - """ - - # Leave PID limits at ArduPilot defaults unless a fixture explicitly overrides them. - extra_params: dict = {} - - def __init__(self, anchor_ned: np.ndarray): - self._anchor_ned = np.asarray(anchor_ned, dtype=float) - self._roll_eq: float | None = None - self._pitch_eq: float | None = None - - def set_equilibrium(self, roll_eq: float, pitch_eq: float) -> None: - """ - Record the tether equilibrium roll and pitch [rad] from the first - clean ATTITUDE message during kinematic startup. Must be called - before send_correction is used. - """ - self._roll_eq = float(roll_eq) - self._pitch_eq = float(pitch_eq) - - def send_correction( - self, - att: dict, - pos_ned: tuple | None, - gcs, - ) -> dict: - """ - Compute and send a legacy RC correction dict. - - Error = (roll - roll_eq, pitch - pitch_eq) — yaw-independent deviation - from the tether equilibrium captured during kinematic startup. - - Returns neutral sticks when equilibrium has not been set yet. - """ - _neutral = {1: SWASH_PWM_NEUTRAL, 2: SWASH_PWM_NEUTRAL, - 3: SWASH_PWM_NEUTRAL, 4: SWASH_PWM_NEUTRAL} - - if self._roll_eq is None or self._pitch_eq is None: - return _neutral - - roll_dev = att["roll"] - self._roll_eq - pitch_dev = att["pitch"] - self._pitch_eq - - rc = compute_rc_from_attitude( - roll = roll_dev, - pitch = pitch_dev, - rollspeed = att["rollspeed"], - pitchspeed = att["pitchspeed"], - yawspeed = att["yawspeed"], - ) - rc[3] = SWASH_PWM_NEUTRAL # neutral collective - gcs.send_rc_override(rc) - return rc - - -# --------------------------------------------------------------------------- -# Pumping cycle helpers — used by planner (planner.py) and unit tests -# --------------------------------------------------------------------------- - class TensionPI: """ PID controller: adjusts collective_rad to maintain requested tether tension. @@ -464,47 +63,6 @@ def update(self, tension_actual: float, dt: float) -> float: return float(np.clip(raw, self.coll_min, self.coll_max)) -def orbit_tracked_body_z_eq( - cur_pos: np.ndarray, - tether_dir0: np.ndarray, - body_z_eq0: np.ndarray, -) -> np.ndarray: - """ - Rotate body_z_eq0 azimuthally to track the hub's orbital position. - - Projects both tether directions onto the horizontal plane and applies the - resulting azimuthal (vertical-axis only) rotation to body_z_eq0. The - vertical (Z) component of body_z_eq0 is preserved exactly. - - Used by the Python simulation loop, which calls this at 400 Hz without - rate limiting. The altitude-insensitive Z-preservation prevents the - positive-feedback altitude instability that would arise if the setpoint - fully tracked tether elevation changes at high bandwidth. - - For the Lua-equivalent 3D algorithm (used on hardware, where a rate-limited - slerp acts as the bandwidth limiter) see orbit_tracked_body_z_eq_3d(). - - At t=0 (cur_pos = initial pos) returns body_z_eq0 unchanged → zero error - and zero tilt at the natural equilibrium. - """ - cur_tdir = cur_pos / np.linalg.norm(cur_pos) - th0h = np.array([tether_dir0[0], tether_dir0[1], 0.0]) - thh = np.array([cur_tdir[0], cur_tdir[1], 0.0]) - n0h = np.linalg.norm(th0h); nhh = np.linalg.norm(thh) - if n0h < 0.01 or nhh < 0.01: - return body_z_eq0.copy() - th0h /= n0h; thh /= nhh - cos_phi = float(np.clip(np.dot(th0h, thh), -1.0, 1.0)) - sin_phi = float(th0h[0] * thh[1] - th0h[1] * thh[0]) - bz0 = body_z_eq0 - result = np.array([ - cos_phi * bz0[0] - sin_phi * bz0[1], - sin_phi * bz0[0] + cos_phi * bz0[1], - bz0[2], - ]) - return result / np.linalg.norm(result) - - def position_feedback_bz_eq( bz_eq: np.ndarray, pos: np.ndarray, @@ -836,16 +394,15 @@ class AltitudeHoldController: Converts a target altitude setpoint into a body_z_eq for the swashplate. The elevation angle is rate-limited so disk transitions are smooth. - Replaces OrbitTracker for altitude-holding flight — stateless geometry, - no orbit reference, no quaternion slerp. The planner sets target_alt_m; - the controller handles how fast to get there. + Stateless geometry — no orbit reference, no quaternion slerp. The planner + sets target_alt_m; the controller handles how fast to get there. Usage ----- ctrl = AltitudeHoldController.from_pos(ic.pos, rotor.body_z_slew_rate_rad_s) # each physics step: body_z_eq = ctrl.update(pos, target_alt_m, tension_n, mass_kg, dt) - # then: compute_swashplate_from_state(state, omega, body_z_eq) + # then feed body_z_eq into HeliCyclicController via compute_rate_cmd """ def __init__(self, initial_el_rad: float, @@ -988,131 +545,6 @@ def update( return float(rate_sp[0]), float(rate_sp[1]) -def orbit_tracked_body_z_eq_3d( - cur_pos: np.ndarray, - tether_dir0: np.ndarray, - body_z_eq0: np.ndarray, -) -> np.ndarray: - """ - Rotate body_z_eq0 to track the hub's orbital position via full 3D Rodrigues. - - Applies the minimal 3D rotation that maps tether_dir0 to the current - tether direction (cur_pos normalised). Matches rawes.lua - orbit_track() exactly. - - Use this function only when the output is fed through a rate-limited slerp - before reaching the attitude controller — otherwise the altitude-sensitive - Z component creates a positive-feedback instability (see orbit_tracked_body_z_eq - docstring). On hardware the Lua slerp (0.40 rad/s) provides this limiting. - - Frame-agnostic: pass NED or ENU consistently across all arguments. - """ - cur_pos = np.asarray(cur_pos, dtype=float) - tether_dir0 = np.asarray(tether_dir0, dtype=float) - body_z_eq0 = np.asarray(body_z_eq0, dtype=float) - - t_len = float(np.linalg.norm(cur_pos)) - if t_len < 0.01: - return body_z_eq0.copy() - bzt = cur_pos / t_len - - axis = np.cross(tether_dir0, bzt) - sinth = float(np.linalg.norm(axis)) - if sinth < 1e-6: - return body_z_eq0.copy() - costh = float(np.dot(tether_dir0, bzt)) - angle = float(np.arctan2(sinth, costh)) - axis_n = axis / sinth - - ca = float(np.cos(angle)) - sa = float(np.sin(angle)) - ad = float(np.dot(axis_n, body_z_eq0)) - return body_z_eq0 * ca + np.cross(axis_n, body_z_eq0) * sa + axis_n * (ad * (1.0 - ca)) - - -class OrbitTracker: - """ - Rate-limited orbit-tracking body_z setpoint. - - Mirrors the rawes.lua state machine exactly: - _bz_orbit = orbit_track_3d(bz_eq0, tdir0, pos/|pos|) each step - _bz_slerp = slerp(_bz_slerp, bz_target or _bz_orbit, slew_rate, dt) - - Parameters - ---------- - body_z_eq0 : body_z at equilibrium capture (NED unit vector) - tether_dir0 : tether direction at capture (NED unit vector) - slew_rate_rad_s : maximum setpoint rotation rate [rad/s] - Use rotor_definition.body_z_slew_rate_rad_s. - - Usage - ----- - tracker = OrbitTracker(ic_body_z_eq0, ic_tether_dir0, rd.body_z_slew_rate_rad_s) - # in loop — natural orbit: - body_z_eq = tracker.update(pos, dt) - # in loop — planner override (e.g. reel-in tilt): - body_z_eq = tracker.update(pos, dt, bz_target=quat_apply(aq, [0,0,-1])) - """ - - def __init__( - self, - body_z_eq0: np.ndarray, - tether_dir0: np.ndarray, - slew_rate_rad_s: float, - ) -> None: - self._bz_slerp = np.asarray(body_z_eq0, dtype=float).copy() - self._tether_dir0 = np.asarray(tether_dir0, dtype=float).copy() - self._body_z_eq0 = np.asarray(body_z_eq0, dtype=float).copy() - self._slew_rate = float(slew_rate_rad_s) - - def update( - self, - pos: np.ndarray, - dt: float, - bz_target: "np.ndarray | None" = None, - ) -> np.ndarray: - """ - Advance and return the rate-limited body_z setpoint. - - Parameters - ---------- - pos : hub position NED [m]; anchor assumed at origin - dt : timestep [s] - bz_target : planner override unit vector NED; None = follow natural orbit - - Returns - ------- - np.ndarray (3,) — rate-limited body_z setpoint (copy) - """ - bz_orbit = orbit_tracked_body_z_eq_3d(pos, self._tether_dir0, self._body_z_eq0) - goal = np.asarray(bz_target, dtype=float) if bz_target is not None else bz_orbit - self._bz_slerp = slerp_body_z(self._bz_slerp, goal, self._slew_rate, dt) - return self._bz_slerp.copy() - - @property - def bz_slerp(self) -> np.ndarray: - """Current rate-limited setpoint (read-only copy).""" - return self._bz_slerp.copy() - - -def blend_body_z( - alpha: float, - bz_start: np.ndarray, - bz_end: np.ndarray, -) -> np.ndarray: - """ - Linearly blend two unit vectors and renormalise. - - alpha=0 → bz_start, alpha=1 → bz_end. Not true SLERP but accurate - enough for smooth attitude transitions over several seconds. - """ - alpha = float(np.clip(alpha, 0.0, 1.0)) - blended = (1.0 - alpha) * np.asarray(bz_start, dtype=float) \ - + alpha * np.asarray(bz_end, dtype=float) - n = np.linalg.norm(blended) - return blended / n if n > 1e-6 else np.asarray(bz_end, dtype=float).copy() - - # --------------------------------------------------------------------------- # Heli cyclic controller — ArduPilot-compatible rate loop + servo model # --------------------------------------------------------------------------- @@ -1526,16 +958,3 @@ def compute_rate_cmd_sqrt( return R.T @ (proportional_world - damping_world) - -def make_hold_controller( - anchor_ned: "np.ndarray", -) -> "PhysicalHoldController": - """ - Return a PhysicalHoldController for the given anchor position. - - Parameters - ---------- - anchor_ned : tether anchor in NED [m] relative to LOCAL_POSITION_NED origin. - """ - return PhysicalHoldController(anchor_ned) - diff --git a/simulation/mediator.py b/simulation/mediator.py index a494d17..0c27d2e 100644 --- a/simulation/mediator.py +++ b/simulation/mediator.py @@ -70,8 +70,6 @@ TELEMETRY_HZ = 400.0 # telemetry CSV write rate [Hz] LOG_INTERVAL = 1.0 # position/attitude log interval [s] WEIGHT_N = 294.3 # rotor weight [N] = 30 kg * 9.81 m/s² -I_SPIN_KGMS2 = 10.0 # rotor spin-axis moment of inertia [kg·m²] -OMEGA_SPIN_MIN = 0.5 # minimum spin rate clamp [rad/s] # --------------------------------------------------------------------------- @@ -542,10 +540,6 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non elif issue.level == "WARNING": log.warning("Rotor validation: %s", issue) - # Shadow module-level constants with rotor-definition values - I_SPIN_KGMS2 = rotor.autorotation.I_ode_kgm2 - OMEGA_SPIN_MIN = rotor.autorotation.omega_min_rad_s - # Collective denormalisation range (must precede _ic which uses _col_min_rad) _col_min_rad, _col_max_rad = _load_col_range() @@ -914,6 +908,7 @@ def step_fn(servos, t_sim): _mav_async = get_async_mavlink_snapshot() _rpy = sensor_data["rpy"] _ti = core.tether._last_info + _aero_v_i = 0.0 _cur_phase = _phase_label if _cur_phase and _cur_phase != _prev_phase and not _tel_note: _tel_note = f"phase_{_cur_phase.replace('-', '_')}_start" @@ -950,6 +945,7 @@ def step_fn(servos, t_sim): "aero_fy": aero_result.F_world[1], "aero_fz": aero_result.F_world[2], "aero_T": float(np.linalg.norm(aero_result.F_world)), + "aero_v_i": _aero_v_i, "aero_mx": aero_result.m_hub_world[0], "aero_my": aero_result.m_hub_world[1], "aero_mz": aero_result.m_hub_world[2], diff --git a/simulation/physics_core.py b/simulation/physics_core.py index 166413f..fbda199 100644 --- a/simulation/physics_core.py +++ b/simulation/physics_core.py @@ -39,7 +39,7 @@ import os from simulation.dynamics import RigidBodyDynamics -from dynbem import create_aero, RotorInputs, euler_step_omega +from dynbem import create_aero, RotorInputs, step_omega from simulation.tether import TetherModel from simulation.rotor_physics import resolve_i_spin_kgm2 from simulation.param_defaults import thrust_to_coll_rad as _t2c @@ -48,6 +48,14 @@ equilibrium_throttle as _hub_equilibrium_throttle, ) +# Coulomb (dry) bearing friction on the rotor's own spin axle [N.m], fed into +# dynbem.mechanical.step_omega. This is a windpower simulation constant, not +# part of the aero rotor definition (rotor.autorotation carries only I_ode_kgm2 +# and omega_min_rad_s -- aerodynamic-adjacent inertia/clamp values owned by +# dynbem's rotor schema). 0.0 = frictionless; not yet characterised on the +# bench (see design/hardware.md). +BEARING_FRICTION_NM = 0.0 + @dataclass class HubObservation: @@ -448,7 +456,7 @@ def _integrate(self, dt: float, collective_rad: float, # RotorInputs field (not part of RotorState) so it stays external. result, self._rotor_state = self._aero.step(rotor_inputs, self._rotor_state, dt) - # Integrate omega externally using euler_step_omega + # Integrate omega externally using step_omega omega_min = (r.autorotation.omega_min_rad_s if r.autorotation.omega_min_rad_s is not None else 0.5) I_ode = (r.autorotation.I_ode_kgm2 @@ -464,9 +472,10 @@ def _integrate(self, dt: float, collective_rad: float, 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( + new_omega, new_spin = step_omega( self._omega_rad_s, self._spin_angle_rad, float(result.Q_spin), 0.0, I_ode, dt, + bearing_friction_Nm=BEARING_FRICTION_NM, ) self._omega_rad_s = max(omega_min, new_omega) self._spin_angle_rad = new_spin diff --git a/simulation/requirements-docker.txt b/simulation/requirements-docker.txt index caf0988..c6c240d 100644 --- a/simulation/requirements-docker.txt +++ b/simulation/requirements-docker.txt @@ -13,4 +13,4 @@ pymavlink pyserial pexpect empy==3.3.4 -dynbem==0.5.0 +dynbem==0.7.0 diff --git a/simulation/requirements.txt b/simulation/requirements.txt index 22560e6..96ec96a 100644 --- a/simulation/requirements.txt +++ b/simulation/requirements.txt @@ -12,4 +12,4 @@ imageio imageio-ffmpeg pymavlink pyserial -dynbem==0.5.0 +dynbem==0.7.0 diff --git a/simulation/steady_state_starting.json b/simulation/steady_state_starting.json index a61030f..074d03a 100644 --- a/simulation/steady_state_starting.json +++ b/simulation/steady_state_starting.json @@ -16,14 +16,14 @@ 0.0 ], [ - 0.564745644840231, + 0.5647451958696346, 0.0, - -0.8252650220589697 + -0.8252653292985039 ], [ - 0.8252650220589697, + 0.8252653292985039, 0.0, - 0.5647456448402308 + 0.5647451958696345 ] ], "R0_kinematic": [ @@ -43,12 +43,12 @@ 0.5548365980613125 ] ], - "omega_spin": 38.101537779188334, - "rest_length": 99.89403192581494, - "eq_thrust": 0.31174330324262606, - "coll_eq_rad": -0.16153754476780213, - "tension_eq_n": 298.0162978128262, - "trim_tilt_lon": -4.149781997942145e-17, - "trim_tilt_lat": 0.020712671418464716, + "omega_spin": 38.10162672796176, + "rest_length": 99.89403191709575, + "eq_thrust": 0.3117436161247757, + "coll_eq_rad": -0.16153742587258527, + "tension_eq_n": 298.01632233401403, + "trim_tilt_lon": -2.3718772521239913e-17, + "trim_tilt_lat": 0.02071259971186424, "home_z_ned": 0.0 } \ No newline at end of file diff --git a/simulation/telemetry_csv.py b/simulation/telemetry_csv.py index 8e9ea10..0c5d6d0 100644 --- a/simulation/telemetry_csv.py +++ b/simulation/telemetry_csv.py @@ -523,7 +523,6 @@ def from_physics( tension_now = runner.tension_now omega_spin = runner.omega_spin t_sim = runner.t_sim - aero_obj = runner.aero tilt_lon = float(step_result.get("tilt_lon", 0.0)) tilt_lat = float(step_result.get("tilt_lat", 0.0)) @@ -567,29 +566,19 @@ def from_physics( aero_fx = aero_fy = aero_fz = 0.0 aero_mx = aero_my = aero_mz = 0.0 - aero_T = aero_v_axial = aero_v_inplane = aero_v_i = 0.0 + aero_T = aero_v_axial = aero_v_inplane = aero_v_i = aero_Q_spin = 0.0 net_F = np.zeros(3) net_M = np.zeros(3) if aero_result is not None: F = np.asarray(aero_result.F_world, dtype=float) M = np.asarray(aero_result.m_hub_world, dtype=float) + aero_Q_spin = float(getattr(aero_result, "Q_spin", 0.0)) aero_fx, aero_fy, aero_fz = float(F[0]), float(F[1]), float(F[2]) aero_mx, aero_my, aero_mz = float(M[0]), float(M[1]), float(M[2]) + aero_T = float(np.linalg.norm(F)) net_F = F + tf if net_moment is not None: net_M = np.asarray(net_moment, dtype=float) - if aero_obj is not None: - # New aero package doesn't expose these as model attributes — - # they live in the per-step result/state. Telemetry-only, - # safe to default to 0 when unavailable. - aero_T = float(getattr(aero_obj, "last_T", 0.0)) - aero_v_axial = float(getattr(aero_obj, "last_v_axial", 0.0)) - aero_v_inplane = float(getattr(aero_obj, "last_v_inplane", 0.0)) - aero_v_i = float(getattr(aero_obj, "last_v_i", 0.0)) - # dynbem (PittPeters/Oye) doesn't expose last_T — fall back to - # the total aero force magnitude as a proxy for thrust. - if aero_T == 0.0: - aero_T = float(np.sqrt(aero_fx**2 + aero_fy**2 + aero_fz**2)) tm = np.asarray(tether_moment, dtype=float) if tether_moment is not None else np.zeros(3) @@ -698,6 +687,7 @@ def from_physics( aero_v_axial = aero_v_axial, aero_v_inplane = aero_v_inplane, aero_v_i = aero_v_i, + aero_Q_spin = aero_Q_spin, F_x = float(net_F[0]), F_y = float(net_F[1]), F_z = float(net_F[2]), diff --git a/tests/envelope/test_collective_sign.py b/tests/envelope/test_collective_sign.py index 84142dd..cc35f98 100644 --- a/tests/envelope/test_collective_sign.py +++ b/tests/envelope/test_collective_sign.py @@ -2,7 +2,8 @@ test_collective_sign.py -- Verify collective sign convention. Uses the same setup as test_hover_sign.py: - - Disk horizontal: bz = [0,0,-1] (up in NED) + - Disk horizontal: body_z = [0,0,+1] (down through disk, per project FRD + convention -- see design/aero_conventions.md) - Hub falling at 2 m/s downward - No wind - omega = 28 rad/s @@ -10,44 +11,35 @@ At this operating point, increasing collective should increase thrust (upward force = more negative F_world[2]). -Sign chain (from test_hover_sign.py / CLAUDE.md): - upward thrust = dot(F_world, disk_normal) = dot(F_world, [0,0,-1]) = -F_world[2] - So thrust > 0 means F_world[2] < 0. +Sign chain (from test_hover_sign.py / design/aero_conventions.md): + upward thrust = -F_world[2]. More collective -> more thrust -> F_world[2] more negative. """ -import sys -from pathlib import Path - import numpy as np +from simulation.frames import build_orb_frame +from tests.unit._aero_probe import load_rotor, make_probe, probe_steady -from dynbem import rotor_definition as rd -from dynbem import create_aero - -R_HORIZONTAL = np.array([[-1., 0., 0.], - [ 0., 1., 0.], - [ 0., 0., -1.]]) # bz = [0,0,-1] (up in NED) +R_HORIZONTAL = build_orb_frame(np.array([0., 0., 1.])) # FRD: body_z DOWN through disk V_FALL = np.array([0., 0., 2.0]) # hub falling 2 m/s downward NO_WIND = np.zeros(3) OMEGA = 28.0 T_STEADY = 20.0 +_AERO = make_probe(load_rotor("beaupoil_2026")) + def _thrust(col): - rotor = rd.default() - aero = create_aero(rotor, model="peters_he") - r = aero.compute_forces( + r = probe_steady( + _AERO, collective_rad = col, - tilt_lon = 0.0, - tilt_lat = 0.0, R_hub = R_HORIZONTAL, v_hub_world = V_FALL, omega_rotor = OMEGA, wind_world = NO_WIND, ) - bz = R_HORIZONTAL[:, 2] # [0, 0, -1] - return float(np.dot(r.F_world, bz)) # upward thrust > 0 + return -float(r.F_world[2]) # upward thrust > 0 def test_thrust_is_positive_at_zero_collective(): @@ -67,8 +59,15 @@ def test_higher_collective_increases_thrust(): def test_thrust_monotone_with_collective(): - """Thrust must increase monotonically across the collective range.""" - cols = [-0.20, -0.10, 0.0, 0.10, 0.20] + """Thrust must increase monotonically across the collective range. + + Kept within the steep attached-flow region: thrust rises steeply from + col=-0.10 to col=+0.05, then plateaus/rolls off gently as blade AoA + approaches alpha_stall_deg=13 deg (beaupoil_2026.yaml) at higher + collective -- that roll-off is a legitimate BEM/stall nonlinearity, not + a sign-convention bug, so it's out of scope for this check. + """ + cols = [-0.10, -0.05, 0.0, 0.02, 0.05] thrusts = [_thrust(c) for c in cols] for i in range(len(thrusts) - 1): assert thrusts[i] < thrusts[i + 1], ( diff --git a/tests/envelope/test_tension_continuation.py b/tests/envelope/test_tension_continuation.py index cebdf84..9feb3f7 100644 --- a/tests/envelope/test_tension_continuation.py +++ b/tests/envelope/test_tension_continuation.py @@ -14,10 +14,11 @@ import pytest -from dynbem import rotor_definition as rd -from dynbem import create_aero +from dynbem import create_aero, RotorInputs, step_omega from simulation.frames import build_orb_frame +from simulation.physics_core import BEARING_FRICTION_NM from envelope.point_mass import tether_hat, balance_bz +from envelope.rotor_helpers import load_default_rotor EL = 30.0 @@ -44,11 +45,12 @@ def _run_continuation(el=EL, t_start=T_START, t_max=T_MAX, Run settle phase then ramp phase. Returns list of dicts with per-second snapshots. """ - rotor = rd.default() - aero = create_aero(rotor, model="peters_he") - dk = rotor.dynamics_kwargs() - mass = dk["mass"] - I_spin = dk["I_spin"] + rotor = load_default_rotor() + aero = create_aero(rotor, model="quasi_static") + rotor_state = aero.initial_rotor_state() + mass = float(rotor.inertia.mass_kg) + I_ode = float(rotor.autorotation.I_ode_kgm2 or 10.0) + omega_min = float(rotor.autorotation.omega_min_rad_s or 0.5) gravity = 9.81 weight = mass * gravity @@ -57,6 +59,7 @@ def _run_continuation(el=EL, t_start=T_START, t_max=T_MAX, clamp = math.radians(cyc_clamp_deg) omega = float(omega_init) + spin_angle = 0.0 vel = np.zeros(3) col_now = 0.0 c_lon = c_lat = 0.0 @@ -88,18 +91,30 @@ def _run_continuation(el=EL, t_start=T_START, t_max=T_MAX, F_teth = tension * t_hat try: - f = aero.compute_forces(col_now, c_lon, c_lat, R, vel.copy(), - omega, wind_v) + inputs = RotorInputs( + collective_rad = col_now, tilt_lon = c_lon, tilt_lat = c_lat, + R_hub = R, v_hub_world = vel.copy(), + wind_world = wind_v, omega_rad_s = omega, + rho_kg_m3 = 1.225, + ) + f, rotor_state = aero.step(inputs, rotor_state, dt) except (OverflowError, ValueError, FloatingPointError): if diverged_at is None: diverged_at = (t_sim, tension) break + if not np.all(np.isfinite(f.F_world)) or not math.isfinite(f.Q_spin): + if diverged_at is None: + diverged_at = (t_sim, tension) + break thrust = float(np.dot(f.F_world, bz)) - Q_net = float(np.dot(aero.last_M_spin, bz)) F_net = f.F_world + F_teth + np.array([0.0, 0.0, weight]) - omega = max(0.0, omega + (Q_net / I_spin) * dt) + omega, spin_angle = step_omega( + omega, spin_angle, float(f.Q_spin), 0.0, I_ode, dt, + bearing_friction_Nm=BEARING_FRICTION_NM, + ) + omega = max(omega_min, omega) vel = vel + (F_net / mass) * dt if not np.all(np.isfinite(vel)): diff --git a/tests/envelope/test_tension_ramp_30deg.py b/tests/envelope/test_tension_ramp_30deg.py index 13210ba..03737df 100644 --- a/tests/envelope/test_tension_ramp_30deg.py +++ b/tests/envelope/test_tension_ramp_30deg.py @@ -15,10 +15,11 @@ import numpy as np -from dynbem import rotor_definition as rd -from dynbem import create_aero +from dynbem import create_aero, RotorInputs, step_omega from simulation.frames import build_orb_frame +from simulation.physics_core import BEARING_FRICTION_NM from envelope.point_mass import tether_hat, balance_bz, simulate_point +from envelope.rotor_helpers import load_default_rotor EL = 30.0 WIND = 10.0 @@ -35,17 +36,19 @@ def _run_standalone_ramp(): """Own ODE loop — omega=28, vel=0, 40 s settle at T=25 N, then ramp to 1000 N.""" - rotor = rd.default() - aero = create_aero(rotor, model="peters_he") - dk = rotor.dynamics_kwargs() - mass = dk["mass"] - I_spin = dk["I_spin"] + rotor = load_default_rotor() + aero = create_aero(rotor, model="quasi_static") + rotor_state = aero.initial_rotor_state() + mass = float(rotor.inertia.mass_kg) + I_ode = float(rotor.autorotation.I_ode_kgm2 or 10.0) + omega_min = float(rotor.autorotation.omega_min_rad_s or 0.5) weight = mass * 9.81 clamp = math.radians(8.6) wind_v = np.array([0.0, -float(WIND), 0.0]) t_hat = tether_hat(EL) omega = float(OMEGA_INIT) + spin_angle = 0.0 vel = np.zeros(3) col_now = 0.0 c_lon = c_lat = 0.0 @@ -78,17 +81,28 @@ def _run_standalone_ramp(): F_teth = tension * t_hat try: - f = aero.compute_forces(col_now, c_lon, c_lat, R, vel.copy(), - omega, wind_v) + inputs = RotorInputs( + collective_rad = col_now, tilt_lon = c_lon, tilt_lat = c_lat, + R_hub = R, v_hub_world = vel.copy(), + wind_world = wind_v, omega_rad_s = omega, + rho_kg_m3 = 1.225, + ) + f, rotor_state = aero.step(inputs, rotor_state, DT) except (OverflowError, ValueError, FloatingPointError): diverged_at = (t_sim, tension) break + if not np.all(np.isfinite(f.F_world)) or not math.isfinite(f.Q_spin): + diverged_at = (t_sim, tension) + break thrust = float(np.dot(f.F_world, bz)) - Q_net = float(np.dot(aero.last_M_spin, bz)) F_net = f.F_world + F_teth + np.array([0.0, 0.0, weight]) - omega = max(0.0, omega + (Q_net / I_spin) * DT) + omega, spin_angle = step_omega( + omega, spin_angle, float(f.Q_spin), 0.0, I_ode, DT, + bearing_friction_Nm=BEARING_FRICTION_NM, + ) + omega = max(omega_min, omega) vel = vel + (F_net / mass) * DT if not np.all(np.isfinite(vel)): @@ -122,7 +136,6 @@ def _run_standalone_ramp(): c_lon = c_lon, c_lat = c_lat, int_vx = int_vx, int_vy = int_vy, int_vcol = int_vcol, - aero_state = aero.to_dict(), ) samples.append((ti, v_along, col_now, ic_out)) next_grid += float(TENSION_LIST[1] - TENSION_LIST[0]) diff --git a/tests/simtests/test_generate_ic.py b/tests/simtests/test_generate_ic.py index b61b262..da0aa34 100644 --- a/tests/simtests/test_generate_ic.py +++ b/tests/simtests/test_generate_ic.py @@ -33,7 +33,7 @@ import simulation import simulation.mediator as _mediator_module -from dynbem import create_aero, RotorInputs, relax_inflow, solve_trim_cyclic, euler_step_omega +from dynbem import create_aero, RotorInputs, relax_inflow, solve_trim_cyclic, step_omega from simulation.frames import build_orb_frame from tests.simtests.simtest_runner import PhysicsRunner from tests.common.mock_ardupilot import MockArdupilot @@ -179,7 +179,7 @@ def _spin_equilibrium(R_hub, col_c, omega_seed): st = relax_inflow(aero, st, inp, n_steps=250, dt=dt_eq) for _ in range(200): res, st = aero.step(inp, st, dt_eq) - om, ang = euler_step_omega(om, ang, float(res.Q_spin), 0.0, I_ode, dt_eq) + om, ang = step_omega(om, ang, float(res.Q_spin), 0.0, I_ode, dt_eq) om = max(omega_min, min(80.0, om)) return om diff --git a/tests/simtests/test_ground_liftoff.py b/tests/simtests/test_ground_liftoff.py index e67ff5f..06b4bcb 100644 --- a/tests/simtests/test_ground_liftoff.py +++ b/tests/simtests/test_ground_liftoff.py @@ -41,9 +41,14 @@ DT = 2.5e-3 # 400 Hz LUA_PERIOD = 0.020 # 50 Hz Lua tick LUA_EVERY = round(LUA_PERIOD / DT) -WIND_NED = np.array([0.0, 0.0, -10.0]) # 10 m/s upward (NED Z down) -TETHER_FORCE_N = 200.0 # constant downward load [N] +WIND_NED = np.array([0.0, 0.0, 0.0]) # no wind +OMEGA_START_RAD_S = 40.0 # fixed rotor start speed [rad/s] +TETHER_FORCE_N = 10.0 # minimal constant downward load [N] TARGET_ALT_M = 5.0 # RAWES_ALT command sent each tick +TARGET_THRUST = 1.0 # RAWES_THR command sent each tick +TARGET_ROLL_RAD = 0.0 # RAWES_RIC command sent each tick +TARGET_PITCH_RAD = 0.0 # RAWES_PIC command sent each tick +AERO_MODEL = "quasi_static" # use default quasi-static aero model LIFTOFF_ALT_M = 2.0 # altitude threshold that counts as liftoff [m] LIFTOFF_TIMEOUT = 60.0 # must lift off within this many sim-seconds [s] @@ -78,17 +83,15 @@ def _build_ic() -> SimpleNamespace: """ Hub at altitude 1 m, horizontal disk. Anchor at NED origin. - omega_spin / eq_thrust from IC file if available. + eq_thrust from IC file if available; omega_spin is forced to OMEGA_START_RAD_S. """ try: from tests.simtests.simtest_ic import load_ic as _load_ic _ic = _load_ic() - omega_spin = float(_ic.omega_spin) eq_thrust = float(_ic.eq_thrust) except FileNotFoundError: from simulation.param_defaults import load_collective_phys_range as _lr col_min, col_max = _lr() - omega_spin = 39.42 eq_thrust = (-0.18 - col_min) / (col_max - col_min) return SimpleNamespace( @@ -97,17 +100,17 @@ def _build_ic() -> SimpleNamespace: R0 = np.eye(3), # horizontal disk; body_z = [0,0,1] rest_length = 1.0, eq_thrust = eq_thrust, - omega_spin = omega_spin, + omega_spin = OMEGA_START_RAD_S, ) # ── Test ────────────────────────────────────────────────────────────────────── -def test_ground_liftoff(): +def test_ground_liftoff(simtest_log): """ - Hub at altitude 1 m, horizontal disk, 10 m/s upward wind, 200 N downforce. - rawes.lua GUIDED mode, RAWES_ALT = 5.0. Lua VZ-PI must command enough - collective to climb above 2 m within 60 s. + Hub at altitude 1 m, horizontal disk, no wind, 10 N downforce. + rawes.lua GUIDED mode with RAWES_ALT=5.0, RAWES_THR=1.0, and fixed + RAWES_RIC/RAWES_PIC targets to hold level attitude while climbing. """ ic = _build_ic() @@ -122,12 +125,14 @@ def test_ground_liftoff(): runner = PhysicsRunner( _ROTOR, ic, WIND_NED, + aero_model=AERO_MODEL, z_floor = 0.0, ) - # Swap the elastic tether for a constant 200 N downward load. + # Swap the elastic tether for a constant 10 N downward load. runner._core._tether = _ConstantDownforce(TETHER_FORCE_N) lua = MockArdupilot.for_lua(sim, initial_thrust=ic.eq_thrust, wind=WIND_NED, dt=DT) + lua.tel_fn = lambda r, sr: dict(body_z_eq=None) total_steps = int(LIFTOFF_TIMEOUT / DT) liftoff_t = None max_alt = ic.pos[2] * -1.0 # start altitude @@ -137,12 +142,16 @@ def test_ground_liftoff(): def _inject(s, r): s.send_message(NamedValueFloat("RAWES_TEN", TETHER_FORCE_N)) s.send_message(NamedValueFloat("RAWES_ALT", TARGET_ALT_M)) + s.send_message(NamedValueFloat("RAWES_THR", TARGET_THRUST)) + s.send_message(NamedValueFloat("RAWES_RIC", TARGET_ROLL_RAD)) + s.send_message(NamedValueFloat("RAWES_PIC", TARGET_PITCH_RAD)) for i in range(total_steps): t = i * DT if i % LUA_EVERY == 0: lua.tick(t, runner, inject=_inject) sr = lua.step(runner, DT) + lua.log(runner, sr) alt = runner.altitude if alt > max_alt: @@ -161,6 +170,22 @@ def _inject(s, r): else: print(f" liftoff_t : did not reach {LIFTOFF_ALT_M} m [FAIL]") + lines = [ + f"t_final : {t_final:.1f} s", + f"max_altitude : {max_alt:.3f} m", + f"omega_spin : {omega_final:.2f} rad/s (started {ic.omega_spin:.2f})", + ( + f"liftoff_t : {liftoff_t:.2f} s [PASS]" + if liftoff_t is not None + else f"liftoff_t : did not reach {LIFTOFF_ALT_M} m [FAIL]" + ), + ] + for level, msg in sim.messages[:10]: + lines.append(f" [GCS {level}] {msg}") + + lua.write_telemetry(simtest_log.log_dir / "telemetry.csv") + simtest_log.write(lines, "ground_liftoff") + assert liftoff_t is not None, ( f"Hub did not reach {LIFTOFF_ALT_M} m altitude within {LIFTOFF_TIMEOUT} s " f"(max altitude reached: {max_alt:.4f} m)" diff --git a/tests/unit/test_controller.py b/tests/unit/test_controller.py index d0c3bea..21a967a 100644 --- a/tests/unit/test_controller.py +++ b/tests/unit/test_controller.py @@ -1,13 +1,8 @@ """ test_controller.py — Unit tests for simulation/controller.py. -Covers both: - compute_rc_rates() — NED truth-state based controller - compute_rc_from_attitude() — ArduPilot ATTITUDE message based controller - - -All tests use a hub at a realistic RAWES tether position (50 m tether, -~30° elevation, hub East of anchor at origin). +Covers compute_bz_tether, slerp_body_z, compute_rate_cmd, and related +geometry helpers. """ import sys from pathlib import Path @@ -17,11 +12,6 @@ from simulation.controller import ( - compute_rc_rates, - compute_rc_from_attitude, - compute_rc_from_physical_attitude, - PhysicalHoldController, - make_hold_controller, compute_bz_tether, slerp_body_z, compute_rate_cmd, @@ -31,394 +21,12 @@ # ── Helpers ────────────────────────────────────────────────────────────────── -def _Ry(angle_rad): - """Rotation matrix about Y axis (NED: tilts from Z toward X).""" - c, s = np.cos(angle_rad), np.sin(angle_rad) - return np.array([[c, 0., s], [0., 1., 0.], [-s, 0., c]]) - - -def _Rx(angle_rad): - """Rotation matrix about X axis.""" - c, s = np.cos(angle_rad), np.sin(angle_rad) - return np.array([[1., 0., 0.], [0., c, -s], [0., s, c]]) - - def _Rz(angle_rad): """Rotation matrix about Z axis.""" c, s = np.cos(angle_rad), np.sin(angle_rad) return np.array([[c, -s, 0.], [s, c, 0.], [0., 0., 1.]]) -def _state(pos, R, omega): - return { - "pos": np.array(pos, dtype=float), - "R": np.array(R, dtype=float), - "omega": np.array(omega, dtype=float), - } - - -# Reference configuration: hub 50 m from origin at ~30° elevation, hub East (NED Y) -# tether direction NED: [0, cos(30°), -sin(30°)] = [0, 0.866, -0.5] -_ANCHOR = np.zeros(3) -_TETHER_ELEV = np.radians(30.0) -_POS = 50.0 * np.array([0., np.cos(_TETHER_ELEV), -np.sin(_TETHER_ELEV)]) -_BODY_Z_EQ = _POS / np.linalg.norm(_POS) # tether direction = desired body_z - -# Rotation matrix whose body_z = tether direction (NED). -# body_z = [0, cos30, -sin30]; choose body_x = [1,0,0] (North), body_y = cross(body_z, body_x) -_body_x = np.array([1., 0., 0.]) -_body_z = _BODY_Z_EQ -_body_y = np.cross(_body_z, _body_x) -_body_x_orth = _body_x - np.dot(_body_x, _body_z) * _body_z -_body_x_orth /= np.linalg.norm(_body_x_orth) -_body_y = np.cross(_body_z, _body_x_orth) -_R_EQ = np.column_stack([_body_x_orth, _body_y, _body_z]) # columns = body axes in world - -# A representative NED velocity (hub drifting slowly North-East) -_VEL_NED = np.array([1.0, 0.5, 0.0]) - - -# ── Tests ───────────────────────────────────────────────────────────────────── - -def test_body_z_on_tether_no_rate(): - """Hub aligned with tether, zero omega → neutral sticks.""" - state = _state(_POS, _R_EQ, [0., 0., 0.]) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED) - assert rc[1] == 1500 - assert rc[2] == 1500 - assert rc[4] == 1500 - - -def test_body_z_on_tether_with_spin_only(): - """Hub aligned with tether, only spin omega (along body_z) → neutral sticks. - - Spin is along the tether direction; stripping it should leave zero orbital - rate and therefore no correction command. - """ - spin_world = 25.0 * _BODY_Z_EQ # 25 rad/s along tether = pure rotor spin - state = _state(_POS, _R_EQ, spin_world) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED) - assert rc[1] == 1500 - assert rc[2] == 1500 - assert rc[4] == 1500 - - -def test_tilt_error_gives_non_neutral_command(): - """A small tilt away from tether direction produces non-neutral RC.""" - # Rotate hub 5° away from equilibrium around world Y - R_tilted = _Ry(np.radians(5.0)) @ _R_EQ - state = _state(_POS, R_tilted, [0., 0., 0.]) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED) - # At least one channel must differ from 1500 - assert rc[1] != 1500 or rc[2] != 1500 or rc[4] != 1500 - - -def test_tilt_direction_corrects_toward_tether(): - """Opposite tilts produce opposite RC commands (sign symmetry).""" - R_pos = _Ry(+np.radians(10.0)) @ _R_EQ - R_neg = _Ry(-np.radians(10.0)) @ _R_EQ - state_pos = _state(_POS, R_pos, [0., 0., 0.]) - state_neg = _state(_POS, R_neg, [0., 0., 0.]) - rc_pos = compute_rc_rates(state_pos, _ANCHOR, _VEL_NED, kd=0.0) - rc_neg = compute_rc_rates(state_neg, _ANCHOR, _VEL_NED, kd=0.0) - # Commands should be mirrored around 1500 for at least one axis - # (the tilted axis will have |cmd - 1500| equal and opposite sign) - dev_pos = [rc_pos[ch] - 1500 for ch in (1, 2, 4)] - dev_neg = [rc_neg[ch] - 1500 for ch in (1, 2, 4)] - # Sum of each pair should be near zero (opposite deviations) - for dp, dn in zip(dev_pos, dev_neg): - assert abs(dp + dn) <= 2, f"Not mirrored: {dp} vs {dn}" - - -def test_larger_error_gives_larger_command(): - """Larger tilt angle → larger magnitude RC deviation.""" - R_small = _Ry(np.radians(5.0)) @ _R_EQ - R_large = _Ry(np.radians(20.0)) @ _R_EQ - rc_small = compute_rc_rates(_state(_POS, R_small, [0., 0., 0.]), _ANCHOR, _VEL_NED, kd=0.0) - rc_large = compute_rc_rates(_state(_POS, R_large, [0., 0., 0.]), _ANCHOR, _VEL_NED, kd=0.0) - # Max deviation over all channels - dev_small = max(abs(rc_small[ch] - 1500) for ch in (1, 2, 4)) - dev_large = max(abs(rc_large[ch] - 1500) for ch in (1, 2, 4)) - assert dev_large > dev_small - - -def test_orbital_rate_gives_damping_command(): - """A non-zero orbital rate at equilibrium attitude produces a damping RC command.""" - # Hub is aligned but has a small orbital angular velocity - omega_orbital = np.array([0., 0.3, 0.]) # world NED rad/s - state = _state(_POS, _R_EQ, omega_orbital) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED, kp=0.0) # P off; only D - assert rc[1] != 1500 or rc[2] != 1500 or rc[4] != 1500 - - -def test_opposing_orbital_rates_give_opposing_commands(): - """Opposite orbital rates produce opposite damping commands.""" - om_pos = np.array([0., +0.5, 0.]) - om_neg = np.array([0., -0.5, 0.]) - rc_pos = compute_rc_rates(_state(_POS, _R_EQ, om_pos), _ANCHOR, _VEL_NED, kp=0.0) - rc_neg = compute_rc_rates(_state(_POS, _R_EQ, om_neg), _ANCHOR, _VEL_NED, kp=0.0) - dev_pos = [rc_pos[ch] - 1500 for ch in (1, 2, 4)] - dev_neg = [rc_neg[ch] - 1500 for ch in (1, 2, 4)] - for dp, dn in zip(dev_pos, dev_neg): - assert abs(dp + dn) <= 2, f"Damping not mirrored: {dp} vs {dn}" - - -def test_spin_does_not_affect_command(): - """Pure rotor spin (omega along body_z) must be stripped: neutral output.""" - # Large spin along actual body_z (column 2 of R_EQ) - body_z_actual = _R_EQ[:, 2] - omega_spin = 28.0 * body_z_actual - state = _state(_POS, _R_EQ, omega_spin) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED) - assert rc[1] == 1500 - assert rc[2] == 1500 - assert rc[4] == 1500 - - -def test_rc_values_always_in_range(): - """All RC values must stay in [1000, 2000] regardless of inputs.""" - # Very large tilt error - R_huge = _Ry(np.radians(80.0)) @ _R_EQ - omega_huge = np.array([5., 5., 5.]) - state = _state(_POS, R_huge, omega_huge) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED) - for ch in (1, 2, 4): - assert 1000 <= rc[ch] <= 2000, f"Channel {ch} out of range: {rc[ch]}" - - -def test_large_error_saturates(): - """A large tilt error must saturate RC to 1000 or 2000.""" - R_sat = _Ry(np.radians(89.0)) @ _R_EQ - state = _state(_POS, R_sat, [0., 0., 0.]) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED, kp=10.0, kd=0.0) - devs = [abs(rc[ch] - 1500) for ch in (1, 2, 4)] - assert max(devs) == 500, f"Expected saturation but got max dev {max(devs)}" - - -def test_hub_too_close_to_anchor_gives_neutral(): - """Hub at/near anchor (tether length < 0.1 m) → safe neutral output.""" - state = _state([0.0, 0.0, 0.05], _R_EQ, [1., 2., 3.]) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED) - assert rc[1] == 1500 - assert rc[2] == 1500 - assert rc[4] == 1500 - - -def test_kp_zero_no_proportional_correction(): - """With kp=0, a pure attitude error (no rate) → neutral sticks.""" - R_tilted = _Ry(np.radians(15.0)) @ _R_EQ - state = _state(_POS, R_tilted, [0., 0., 0.]) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED, kp=0.0, kd=0.0) - assert rc[1] == 1500 - assert rc[2] == 1500 - assert rc[4] == 1500 - - -def test_kd_zero_no_damping(): - """With kd=0, a pure rate at equilibrium attitude → neutral sticks.""" - omega = np.array([0.3, 0.3, 0.0]) - state = _state(_POS, _R_EQ, omega) - rc = compute_rc_rates(state, _ANCHOR, _VEL_NED, kp=0.0, kd=0.0) - assert rc[1] == 1500 - assert rc[2] == 1500 - assert rc[4] == 1500 - - -# ── compute_rc_from_attitude tests ──────────────────────────────────────────── - -def test_att_zero_attitude_zero_rates_neutral(): - """Zero attitude error and zero rates → neutral sticks.""" - rc = compute_rc_from_attitude(0.0, 0.0, 0.0, 0.0, 0.0) - assert rc[1] == 1500 - assert rc[2] == 1500 - assert rc[4] == 1500 - - -def test_att_positive_roll_gives_negative_roll_command(): - """Positive roll error → negative roll rate command (corrects toward zero).""" - rc = compute_rc_from_attitude(roll=0.1, pitch=0.0, rollspeed=0.0, - pitchspeed=0.0, yawspeed=0.0, kd=0.0) - assert rc[1] < 1500 # negative roll rate → below neutral - - -def test_att_positive_pitch_gives_negative_pitch_command(): - """Positive pitch error → negative pitch rate command.""" - rc = compute_rc_from_attitude(roll=0.0, pitch=0.1, rollspeed=0.0, - pitchspeed=0.0, yawspeed=0.0, kd=0.0) - assert rc[2] < 1500 - - -def test_att_positive_rollspeed_gives_damping_command(): - """Positive rollspeed → negative roll rate command (damps the rate).""" - rc = compute_rc_from_attitude(roll=0.0, pitch=0.0, rollspeed=0.2, - pitchspeed=0.0, yawspeed=0.0, kp=0.0) - assert rc[1] < 1500 - - -def test_att_positive_yawspeed_gives_damping_command(): - """Positive yawspeed → negative yaw rate command.""" - rc = compute_rc_from_attitude(roll=0.0, pitch=0.0, rollspeed=0.0, - pitchspeed=0.0, yawspeed=0.3, kp=0.0) - assert rc[4] < 1500 - - -def test_att_opposite_errors_give_opposite_commands(): - """Opposite roll errors → opposite RC deviations.""" - rc_pos = compute_rc_from_attitude(+0.2, 0.0, 0.0, 0.0, 0.0, kd=0.0) - rc_neg = compute_rc_from_attitude(-0.2, 0.0, 0.0, 0.0, 0.0, kd=0.0) - assert rc_pos[1] + rc_neg[1] == 3000 # symmetric around 1500 - - -def test_att_larger_error_larger_command(): - """Larger attitude error → larger RC deviation.""" - rc_small = compute_rc_from_attitude(0.05, 0.0, 0.0, 0.0, 0.0, kd=0.0) - rc_large = compute_rc_from_attitude(0.30, 0.0, 0.0, 0.0, 0.0, kd=0.0) - assert abs(rc_large[1] - 1500) > abs(rc_small[1] - 1500) - - -def test_att_output_always_in_range(): - """PWM output always stays in [1000, 2000].""" - for roll in (-2.0, -0.5, 0.0, 0.5, 2.0): - for pitch in (-2.0, 0.0, 2.0): - for speed in (-5.0, 0.0, 5.0): - rc = compute_rc_from_attitude(roll, pitch, speed, speed, speed) - for ch in (1, 2, 4): - assert 1000 <= rc[ch] <= 2000 - - -def test_att_large_error_saturates(): - """Very large error saturates to 1000 or 2000.""" - rc = compute_rc_from_attitude(100.0, 0.0, 0.0, 0.0, 0.0, kp=10.0, kd=0.0) - assert rc[1] == 1000 # max negative roll rate - -# --------------------------------------------------------------------------- -# compute_rc_from_physical_attitude tests -# --------------------------------------------------------------------------- - -def _att_aligned(): - """Attitude dict for hub exactly aligned with tether (equilibrium).""" - import math - elev = math.radians(30.0) - L = 50.0 - pos = np.array([0.0, L * math.cos(elev), -L * math.sin(elev)]) # NED - # body_z along tether, build orbital frame - tether_dir = pos / np.linalg.norm(pos) - east = np.array([0., 1., 0.]) # NED: East = Y axis - bx = east - np.dot(east, tether_dir) * tether_dir - bx /= np.linalg.norm(bx) - R_hub = np.column_stack([bx, np.cross(tether_dir, bx), tether_dir]) - # NED attitude from R_hub - T = np.array([[0,1,0],[1,0,0],[0,0,-1]], dtype=float) - R_ned = T @ R_hub @ T - sy = R_ned[1, 0]; cy = R_ned[0, 0] - sp = -R_ned[2, 0] - cp = math.sqrt(R_ned[2,1]**2 + R_ned[2,2]**2) - sr = R_ned[2, 1] / cp; cr = R_ned[2, 2] / cp - roll = math.atan2(sr, cr) - pitch = math.atan2(sp, cp) - yaw = math.atan2(sy, cy) - # pos_ned for tether equilibrium hub - pos_ned = np.array([pos[1], pos[0], -pos[2]]) - anchor_ned = np.zeros(3) - return roll, pitch, yaw, pos_ned, anchor_ned - - -def test_physical_att_aligned_gives_neutral(): - """Hub exactly on tether → correction is near zero → neutral RC.""" - roll, pitch, yaw, pos_ned, anchor_ned = _att_aligned() - rc = compute_rc_from_physical_attitude( - roll, pitch, yaw, 0.0, 0.0, 0.0, - pos_ned=pos_ned, anchor_ned=anchor_ned, - kp=1.0, kd=0.3, - ) - # At perfect alignment cross(body_z, body_z_eq) = 0 → all neutral - assert rc[1] == 1500 - assert rc[2] == 1500 - - -def test_physical_att_output_in_range(): - """RC values from physical attitude controller always in [1000, 2000].""" - roll, pitch, yaw, pos_ned, anchor_ned = _att_aligned() - for delta in (-0.5, 0.0, 0.5): - rc = compute_rc_from_physical_attitude( - roll + delta, pitch + delta, yaw, 0.3, 0.3, 0.3, - pos_ned=pos_ned, anchor_ned=anchor_ned, - ) - for ch in (1, 2, 4): - assert 1000 <= rc[ch] <= 2000, f"ch{ch}={rc[ch]} out of range" - - -# --------------------------------------------------------------------------- -# PhysicalHoldController tests -# --------------------------------------------------------------------------- - -class _FakeGCS: - """Minimal GCS stub: records send_rc_override calls.""" - def __init__(self): - self.sent = [] - def send_rc_override(self, rc): - self.sent.append(dict(rc)) - - -def test_physical_hold_extra_params_compass_enabled(): - """PhysicalHoldController keeps compass enabled (COMPASS_USE at default).""" - ctrl = PhysicalHoldController(anchor_ned=np.zeros(3)) - assert "COMPASS_USE" not in ctrl.extra_params - assert "COMPASS_ENABLE" not in ctrl.extra_params - assert "EK3_SRC1_YAW" not in ctrl.extra_params - # Rate PID limits are left at ArduPilot/.parm defaults (not force-overridden). - assert "ATC_RAT_RLL_IMAX" not in ctrl.extra_params - - -# --------------------------------------------------------------------------- -# PhysicalHoldController tests -# --------------------------------------------------------------------------- - -def test_physical_hold_neutral_when_no_pos(): - ctrl = PhysicalHoldController(anchor_ned=np.zeros(3)) - gcs = _FakeGCS() - att = {"roll": 0.1, "pitch": 0.2, "yaw": 0.0, - "rollspeed": 0.0, "pitchspeed": 0.0, "yawspeed": 0.0} - rc = ctrl.send_correction(att, pos_ned=None, gcs=gcs) - assert rc[1] == 1500 - assert rc[2] == 1500 - - -def test_physical_hold_neutral_when_no_equilibrium(): - """send_correction returns neutral sticks until set_equilibrium is called.""" - import math - ctrl = PhysicalHoldController(anchor_ned=np.zeros(3)) - gcs = _FakeGCS() - pos_ned = (0.0, 0.0, -10.0) - att = {"roll": 0.5, "pitch": 0.3, "yaw": 1.0, - "rollspeed": 0.0, "pitchspeed": 0.0, "yawspeed": 0.0} - rc = ctrl.send_correction(att, pos_ned=pos_ned, gcs=gcs) - assert rc[1] == 1500 - assert rc[2] == 1500 - - -def test_physical_hold_sends_correction_near_equilibrium(): - """Near-equilibrium hub: controller sends a non-trivial correction. - - set_equilibrium must be called first (normally done in conftest step 4 - from the first ATTITUDE message during the 45 s kinematic startup). - """ - import math - roll, pitch, yaw, pos_ned, anchor_ned = _att_aligned() - # Tilt slightly to create a small error - ctrl = PhysicalHoldController(anchor_ned=anchor_ned) - ctrl.set_equilibrium(roll, pitch) # capture equilibrium before use - gcs = _FakeGCS() - small_tilt = math.radians(5.0) - att = {"roll": roll + small_tilt, "pitch": pitch + small_tilt, "yaw": yaw, - "rollspeed": 0.0, "pitchspeed": 0.0, "yawspeed": 0.0} - rc = ctrl.send_correction(att, pos_ned=tuple(pos_ned), gcs=gcs) - # RC must be in valid range - for ch in (1, 2, 3, 4): - assert 1000 <= rc[ch] <= 2000 - assert len(gcs.sent) == 1 - - # --------------------------------------------------------------------------- # compute_bz_tether tests # --------------------------------------------------------------------------- @@ -601,12 +209,3 @@ def test_rate_cmd_sqrt_transforms_into_body_frame(): r_rot90 = compute_rate_cmd_sqrt(bz_now, bz_eq, _Rz(np.radians(90.0)), kp=1.0, accel_max=10.0, dt=0.02) np.testing.assert_allclose(np.linalg.norm(r_world), np.linalg.norm(r_rot90), atol=1e-10) assert not np.allclose(r_world, r_rot90, atol=1e-6) - - -# --------------------------------------------------------------------------- -# make_hold_controller factory tests -# --------------------------------------------------------------------------- - -def test_make_hold_controller_returns_physical(): - ctrl = make_hold_controller(anchor_ned=np.zeros(3)) - assert isinstance(ctrl, PhysicalHoldController) diff --git a/tests/unit/test_force_balance.py b/tests/unit/test_force_balance.py index 842c155..d087115 100644 --- a/tests/unit/test_force_balance.py +++ b/tests/unit/test_force_balance.py @@ -121,7 +121,7 @@ def _d_omega(omega: float, col: float, R_hub: np.ndarray) -> float: ) # step() settles the inflow state on the first call. _r, _ = _AERO.step(inputs, state, 0.02) - return float(omega_derivative(_r.Q_spin, 0.0, I_ode)) + return float(omega_derivative(float(omega), _r.Q_spin, 0.0, I_ode)) def test_autorotation_d_omega_brackets_zero(): @@ -139,7 +139,7 @@ def test_autorotation_omega_equilibrium_in_range(): Settled omega in [8, 60] rad/s and |d_omega| < 0.01 rad/s^2. """ - from dynbem import RotorInputs, euler_step_omega, omega_derivative + from dynbem import RotorInputs, step_omega, omega_derivative R30 = _R_tilt(ELEV_DEG) col = math.radians(-10.0) state = _AERO.initial_rotor_state() @@ -156,9 +156,9 @@ def test_autorotation_omega_equilibrium_in_range(): wind_world=WIND_EAST, omega_rad_s=omega_now, rho_kg_m3=1.225, ) _r, state = _AERO.step(inputs, state, dt) - new_omega, spin_angle = euler_step_omega(omega_now, spin_angle, float(_r.Q_spin), 0.0, I_ode, dt) + new_omega, spin_angle = step_omega(omega_now, spin_angle, float(_r.Q_spin), 0.0, I_ode, dt) omega_now = max(omega_min, new_omega) - d_omega = float(omega_derivative(_r.Q_spin, 0.0, I_ode)) + d_omega = float(omega_derivative(omega_now, _r.Q_spin, 0.0, I_ode)) assert 8.0 <= omega_now <= 60.0, ( f"equilibrium omega={omega_now:.1f} rad/s outside [8, 60] rad/s" ) diff --git a/tests/unit/test_telemetry_aero_columns.py b/tests/unit/test_telemetry_aero_columns.py new file mode 100644 index 0000000..39ee89b --- /dev/null +++ b/tests/unit/test_telemetry_aero_columns.py @@ -0,0 +1,52 @@ +from types import SimpleNamespace + +import numpy as np + +from simulation.telemetry_csv import TelRow +from tests.simtests._rotor_helpers import load_default_rotor +from tests.simtests.simtest_runner import PhysicsRunner + + +def _build_runner(*, aero_model: str) -> PhysicsRunner: + rotor = load_default_rotor() + ic = SimpleNamespace( + pos=np.array([0.0, 0.0, -1.0]), + vel=np.zeros(3), + R0=np.eye(3), + rest_length=1.0, + eq_thrust=0.5, + omega_spin=40.0, + ) + return PhysicsRunner(rotor, ic, np.zeros(3), aero_model=aero_model, z_floor=0.0) + + +def test_aero_columns_populated_from_physics_step() -> None: + runner = _build_runner(aero_model="quasi_static") + step_result = runner._core.step(0.02, collective_rad=0.08, tilt_lon=0.01, tilt_lat=-0.02) + row = TelRow.from_physics( + runner, + step_result, + collective_rad=0.08, + wind_ned=np.zeros(3), + phase="unit_audit", + ) + + aero_result = step_result["aero_result"] + F = np.asarray(aero_result.F_world, dtype=float) + M = np.asarray(aero_result.m_hub_world, dtype=float) + + assert np.isclose(row.aero_fx, float(F[0])) + assert np.isclose(row.aero_fy, float(F[1])) + assert np.isclose(row.aero_fz, float(F[2])) + assert np.isclose(row.aero_mx, float(M[0])) + assert np.isclose(row.aero_my, float(M[1])) + assert np.isclose(row.aero_mz, float(M[2])) + assert np.isclose(row.aero_T, float(np.linalg.norm(F))) + assert np.isclose(row.aero_Q_spin, float(aero_result.Q_spin)) + + # AeroResult from aero only exposes F_world, m_hub_world, Q_spin, M_spin. + # Non-source decomposition diagnostics must remain zero. + assert np.isclose(row.aero_v_axial, 0.0) + assert np.isclose(row.aero_v_inplane, 0.0) + assert np.isclose(row.aero_v_i, 0.0) + diff --git a/viz3d/visualize_3d.py b/viz3d/visualize_3d.py index 8807480..f063cfd 100644 --- a/viz3d/visualize_3d.py +++ b/viz3d/visualize_3d.py @@ -1463,9 +1463,15 @@ def main(argv: Optional[list] = None) -> None: else: with _suppress_vtk_stderr(): viz.play() - # Exit cleanly — VTK/PyVista registers an atexit handler that exits with - # code 1 on Windows when the window closes. Override it here. - sys.exit(0) + + # The visualization started and ran successfully at this point, so this + # is always a success exit — pin it to 0. Use os._exit() rather than + # sys.exit(): PyVista/VTK registers an atexit handler that exits with + # code 1 on Windows when the render window closes, and only os._exit() + # (which skips atexit callbacks entirely) reliably bypasses it. + sys.stdout.flush() + sys.stderr.flush() + os._exit(0) def _suppress_vtk_stderr():