From 45eec0b2529ba548943d90ea725f2826d50463cd Mon Sep 17 00:00:00 2001 From: Kristof Roomp Date: Thu, 16 Jul 2026 07:34:10 +0200 Subject: [PATCH 1/2] SITL infra: fix dead-code observe() bug in torque_test_utils.py; test.sh WSL auto-reinvoke Only files still missing from origin/main relative to the full squash target (pr/rewind-ba85919). Everything else in that squash was already merged into origin/main via d44027a..219a9ac. --- simulation/tests/sitl/torque/torque_test_utils.py | 4 ++-- test.sh | 14 ++++++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/simulation/tests/sitl/torque/torque_test_utils.py b/simulation/tests/sitl/torque/torque_test_utils.py index de7b80d..25a2fed 100644 --- a/simulation/tests/sitl/torque/torque_test_utils.py +++ b/simulation/tests/sitl/torque/torque_test_utils.py @@ -110,8 +110,8 @@ def handle(msg, t_rel): return True return None - observe(ctx, settle_s + observe_s + timeout_margin_s, handle, - msg_types=["ATTITUDE", "STATUSTEXT", "SERVO_OUTPUT_RAW", "NAMED_VALUE_FLOAT"]) + observe(ctx, settle_s + observe_s + timeout_margin_s, handle, + msg_types=["ATTITUDE", "STATUSTEXT", "SERVO_OUTPUT_RAW", "NAMED_VALUE_FLOAT"]) return obs, rows diff --git a/test.sh b/test.sh index 6a1f016..747e1be 100644 --- a/test.sh +++ b/test.sh @@ -21,6 +21,20 @@ # Suppress path mangling in MSYS/Git-for-Windows; harmless elsewhere. [[ -n "${MSYSTEM:-}" ]] && export MSYS_NO_PATHCONV=1 +# Docker Desktop's Windows named-pipe backend is not reachable from an +# MSYS/Git-Bash shell on this box (fails with +# "failed to connect to the docker API at npipe:////./pipe/dockerDesktopLinuxEngine"). +# Docker works natively inside WSL2 instead, so when we detect we're running +# under Git-for-Windows bash (MSYSTEM set, not already inside WSL), reinvoke +# ourselves inside WSL with the repo path translated from /c/... (mingw) to +# /mnt/c/... (WSL), forwarding all original args. +if [[ -n "${MSYSTEM:-}" && -z "${WSL_DISTRO_NAME:-}" ]] && command -v wsl.exe >/dev/null 2>&1; then + _script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + _drive="${_script_dir:1:1}" + _wsl_dir="/mnt/${_drive,,}${_script_dir:2}" + exec wsl.exe -e bash -lc "cd '$_wsl_dir' && bash test.sh $(printf '%q ' "$@")" +fi + REPO_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" SIM_DIR="$REPO_DIR/simulation" From 5ff09c167cd1f90830be2bc7a97f67fedd13fa43 Mon Sep 17 00:00:00 2001 From: Kristof Roomp Date: Thu, 16 Jul 2026 07:57:15 +0200 Subject: [PATCH 2/2] Revert broken flight-logic changes from d44027a..219a9ac (fixes SITL steady-flight regression) test_lua_flight_steady_sitl was failing on current main (wild rpy oscillation, thrust saturation, floor collisions) since d44027a et al. removed the RC/CH4 override keepalive and disabled RC_CHANNELS_OVERRIDE (gcs.py send_rc_override now raises RuntimeError). Reverts the flight-logic surface (rawes.lua, controller.py, gcs.py, rawes_modes.py, physics_core.py, mock_ardupilot.py, rawes_params.json, plus matching unit/simtest test files) back to their last-known-good (ba85919, pre-regression) versions -- the exact combination already validated as SITL-passing. Does not touch any of the unrelated docs/tooling changes that came in via the same commit range (d44027a, 6adeab5, 347876b, 226db0e, 615be28) -- those remain intact. Validated on this branch: - unit: 462 passed - simtest: 14 passed, 5 skipped - SITL test_lua_flight_steady_sitl: PASSED (stable=128s, max_activity=418 PWM) --- simulation/controller.py | 102 +++++--------- simulation/gcs.py | 53 ++++++-- simulation/physics_core.py | 9 +- simulation/rawes_modes.py | 2 +- simulation/scripts/rawes.lua | 126 ++++-------------- simulation/scripts/rawes_params.json | 10 +- simulation/tests/common/mock_ardupilot.py | 18 ++- simulation/tests/simtests/simtest_runner.py | 54 ++++---- simulation/tests/simtests/test_generate_ic.py | 7 +- simulation/tests/simtests/test_landing.py | 2 +- .../tests/simtests/test_pump_cycle_lua.py | 3 +- .../tests/simtests/test_pump_cycle_unified.py | 4 +- .../tests/simtests/test_sensor_closed_loop.py | 4 +- .../tests/simtests/test_steady_flight.py | 5 +- .../test_steady_flight_ic_angle_only.py | 10 +- .../tests/simtests/test_steady_flight_lua.py | 6 +- simulation/tests/unit/_controller_rig.py | 19 +-- simulation/tests/unit/test_armon_lua.py | 29 ++-- .../tests/unit/test_attitude_convergence.py | 11 +- simulation/tests/unit/test_controller.py | 33 ++++- .../tests/unit/test_full_loop_stability.py | 11 +- simulation/tests/unit/test_roll_sign_chain.py | 10 +- 22 files changed, 236 insertions(+), 292 deletions(-) diff --git a/simulation/controller.py b/simulation/controller.py index 6173249..4ae4372 100644 --- a/simulation/controller.py +++ b/simulation/controller.py @@ -1,19 +1,26 @@ """ -controller.py — RAWES guidance/control helpers. - -Legacy RC-override helpers are retained for historical tests only; runtime -flight stack control is GUIDED-only. +controller.py — RAWES attitude rate controllers for ArduPilot ACRO mode. + +The mediator reports actual orbital-frame orientation (~65° from NED vertical +at tether equilibrium). PhysicalHoldController derives the tether-alignment +error from the equilibrium captured during kinematic startup and uses +compute_rc_from_attitude to send corrective ACRO RC overrides. +GPS + compass are fused normally. + +Usage +----- +controller = make_hold_controller(anchor_ned=anchor_ned) +controller.send_correction(att, pos_ned, gcs) # in the hold loop """ import math import numpy as np from frames import build_orb_frame, cross3 # noqa: F401 — build_orb_frame re-exported for callers -from servo_pwm import SWASH_PWM_NEUTRAL, SWASH_PWM_RANGE +from servo_pwm import SWASH_PWM_NEUTRAL, SWASH_PWM_RANGE, INTERLOCK_PWM_HIGH from swashplate import SwashplateServoModel from dynbem import RotorInputs from param_defaults import load_ap_params as _load_ap_params -from param_defaults import load_collective_phys_range as _load_col_range from arduloop import HeliRateController, HeliParams, RateAxisParams @@ -50,8 +57,8 @@ def compute_rc_rates( Returns ------- dict - RC channel overrides: {1: pwm, 2: pwm, 4: pwm}. - Channel 1 = roll rate, 2 = pitch rate, 4 = yaw rate. + RC channel overrides: {1: pwm, 2: pwm, 4: pwm, 8: 2000}. + Channel 1 = roll rate, 2 = pitch rate, 4 = yaw rate, 8 = motor interlock. Neutral = 1500, min = 1000, max = 2000. """ pos = np.asarray(hub_state["pos"], dtype=float) @@ -63,7 +70,7 @@ def compute_rc_rates( 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} + return {1: SWASH_PWM_NEUTRAL, 2: SWASH_PWM_NEUTRAL, 4: SWASH_PWM_NEUTRAL, 8: INTERLOCK_PWM_HIGH} # FRD body_z points DOWN through the disk → hub→anchor in tethered hover. body_z_eq = -tether / t_len @@ -99,6 +106,7 @@ def _pwm(w: float) -> int: 1: _pwm(omega_body[0]), # roll rate 2: _pwm(omega_body[1]), # pitch rate 4: _pwm(omega_body[2]), # yaw rate + 8: INTERLOCK_PWM_HIGH, # motor interlock always on } @@ -228,7 +236,7 @@ def compute_rc_from_attitude( Returns ------- - dict : {1: pwm, 2: pwm, 4: pwm} + dict : {1: pwm, 2: pwm, 4: pwm, 8: 2000} """ max_rate = np.radians(rate_max_deg) @@ -243,6 +251,7 @@ def _pwm(w: float) -> int: 1: _pwm(cmd_roll), 2: _pwm(cmd_pitch), 4: _pwm(cmd_yaw), + 8: INTERLOCK_PWM_HIGH, } @@ -283,7 +292,7 @@ def compute_rc_from_physical_attitude( Returns ------- - dict : {1: pwm, 2: pwm, 4: pwm} + dict : {1: pwm, 2: pwm, 4: pwm, 8: 2000} """ max_rate = np.radians(rate_max_deg) @@ -308,7 +317,7 @@ def _pwm(w: float) -> int: 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} + return {1: SWASH_PWM_NEUTRAL, 2: SWASH_PWM_NEUTRAL, 4: SWASH_PWM_NEUTRAL, 8: INTERLOCK_PWM_HIGH} body_z_eq = tether_ned / t_len # Attitude error: rotation axis needed to align body_z with body_z_eq. @@ -332,6 +341,7 @@ def _pwm(w: float) -> int: 1: _pwm(omega_corr[0]), # roll rate 2: _pwm(omega_corr[1]), # pitch rate 4: _pwm(omega_corr[2]), # yaw rate + 8: INTERLOCK_PWM_HIGH, } @@ -343,7 +353,7 @@ def _pwm(w: float) -> int: class PhysicalHoldController: """ - Legacy hold controller retained for compatibility tests. + ACRO hold controller for physical sensor mode. ATTITUDE.roll/pitch are actual NED Euler angles. Error is computed as deviation from the tether equilibrium orientation (roll_eq, pitch_eq) @@ -385,17 +395,18 @@ def send_correction( gcs, ) -> dict: """ - Compute and send a legacy RC correction dict. + Compute and send RC override. Returns the rc dict sent (for logging). 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. + Sends 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} + 3: SWASH_PWM_NEUTRAL, 4: SWASH_PWM_NEUTRAL, 8: INTERLOCK_PWM_HIGH} if self._roll_eq is None or self._pitch_eq is None: + gcs.send_rc_override(_neutral) return _neutral roll_dev = att["roll"] - self._roll_eq @@ -409,6 +420,7 @@ def send_correction( yawspeed = att["yawspeed"], ) rc[3] = SWASH_PWM_NEUTRAL # neutral collective + rc[8] = INTERLOCK_PWM_HIGH # motor interlock on gcs.send_rc_override(rc) return rc @@ -864,8 +876,8 @@ class ElevationHoldController: ElevationHoldController → (rate_roll_sp, rate_pitch_sp) HeliCyclicController → (tilt_lon, tilt_lat) - This mirrors the rawes.lua outer-loop structure, but runtime stack control - now uses GUIDED setpoints rather than RC override transport. + This mirrors the rawes.lua architecture where the outer loop sends rate + commands via RC override and the firmware ACRO PIDs close the inner loop. Usage ----- @@ -1089,16 +1101,11 @@ class HeliCyclicController: is applied to the controller output so the closed-loop bandwidth in sim matches the bandwidth limited by the actual servos. - This class is thrust-first. Callers provide thrust in ``[0..1]`` via - ``step_from_thrust()`` and conversion to physical collective radians is - applied once at the swashplate/physics boundary. - Usage:: acro = HeliCyclicController(rotor) - tilt_lon, tilt_lat, col_actual = acro.step_from_thrust( - thrust_cmd, - rate_roll_sp, rate_pitch_sp, omega_body, dt, + tilt_lon, tilt_lat, col_actual = acro.step( + collective_rad, rate_roll_sp, rate_pitch_sp, omega_body, dt, ) Parameters mapped to ArduPilot: @@ -1145,7 +1152,6 @@ def __init__( ) self._ctrl = HeliRateController(params) self._servo = SwashplateServoModel.from_rotor(rotor) - self._col_min, self._col_max = _load_col_range() self._tilt_lon_trim = 0.0 self._tilt_lat_trim = 0.0 @@ -1161,19 +1167,7 @@ def set_trim(self, tilt_lon: float, tilt_lat: float) -> None: self._tilt_lon_trim = float(tilt_lon) self._tilt_lat_trim = float(tilt_lat) - def reset_from_thrust( - self, - thrust_cmd: float, - *, - tilt_lon: float = 0.0, - tilt_lat: float = 0.0, - ) -> None: - """Reset servo state using thrust [0..1] at the physics boundary.""" - thrust = float(np.clip(thrust_cmd, 0.0, 1.0)) - col_rad = self._col_min + thrust * (self._col_max - self._col_min) - self._servo.reset(col_rad, tilt_lon=float(tilt_lon), tilt_lat=float(tilt_lat)) - - def _step_collective( + def step( self, collective_cmd: float, rate_roll_sp : float, @@ -1182,7 +1176,7 @@ def _step_collective( dt : float, collective_norm: float = 0.0, ) -> "tuple[float, float, float]": - """Advance one timestep from physical collective [rad]. + """Advance one timestep. Returns ``(tilt_lon, tilt_lat, col_actual)``. All three channels pass through the SwashplateServoModel so collective and cyclic @@ -1211,34 +1205,6 @@ def _step_collective( float(collective_cmd), tilt_lon_cmd, tilt_lat_cmd, dt) return float(tlon), float(tlat), float(col_act) - def step_from_thrust( - self, - thrust_cmd : float, - rate_roll_sp : float, - rate_pitch_sp : float, - omega_body : np.ndarray, - dt : float, - collective_norm: float | None = None, - ) -> "tuple[float, float, float]": - """Advance one timestep using thrust [0..1] as input. - - This is the preferred API for guided-stack integration because it matches - ArduPilot's throttle/thrust-facing interface. Conversion to physical - collective radians is done exactly once at this boundary. - """ - thrust = float(np.clip(thrust_cmd, 0.0, 1.0)) - col_rad = self._col_min + thrust * (self._col_max - self._col_min) - c_norm = float(2.0 * thrust - 1.0) if collective_norm is None else float(collective_norm) - tlon, tlat, col_act = self._step_collective( - collective_cmd=col_rad, - rate_roll_sp=rate_roll_sp, - rate_pitch_sp=rate_pitch_sp, - omega_body=omega_body, - dt=dt, - collective_norm=c_norm, - ) - return float(tlon), float(tlat), float(col_act) - # --------------------------------------------------------------------------- # Aero-model utilities # --------------------------------------------------------------------------- diff --git a/simulation/gcs.py b/simulation/gcs.py index d077ccc..b690f80 100644 --- a/simulation/gcs.py +++ b/simulation/gcs.py @@ -719,6 +719,7 @@ def arm( self, timeout: float = 15.0, force: bool = False, + rc_override: dict[int, int] | None = None, ) -> None: """Send arm command and confirm via HEARTBEAT armed flag. @@ -729,8 +730,10 @@ def arm( bypass all remaining pre-arm safety checks. Use in simulation when hardware-specific interlocks (motor interlock, RC failsafe) are irrelevant. - The stack uses GUIDED setpoint APIs for control. RC channel overrides - are intentionally not part of arm sequencing. + rc_override : dict[int, int] | None + If provided, RC_CHANNELS_OVERRIDE is re-sent every 0.5 s while + waiting for arm confirmation, preventing ArduPilot from expiring + the override (default expiry ~1 s). Format: {channel: pwm, ...} """ param2 = 21196.0 if force else 0.0 log.info("Sending arm command (force=%s) …", force) @@ -743,9 +746,15 @@ def arm( param2, 0, 0, 0, 0, 0, ) deadline = self.sim_now() + timeout + t_last_override = self.sim_now() t_last_arm_send = self.sim_now() _poll = 0.5 while self.sim_now() < deadline: + # Refresh RC override every 0.5 s sim-time (ArduPilot expiry is ~1 s). + if rc_override and (self.sim_now() - t_last_override) >= _poll: + self.send_rc_override(rc_override) + t_last_override = self.sim_now() + msg = self._recv( type=["HEARTBEAT", "COMMAND_ACK", "STATUSTEXT", "ATTITUDE"], blocking=True, timeout=max(_poll, 0.005), @@ -848,13 +857,19 @@ def set_mode( self, mode_id: int, timeout: float = 10.0, + rc_override: dict[int, int] | None = None, ) -> None: """Set ArduCopter flight mode by custom mode number. - The stack uses GUIDED setpoint APIs for control. RC channel overrides - are intentionally not part of mode switching. + Parameters + ---------- + rc_override : dict[int, int] | None + If provided, RC_CHANNELS_OVERRIDE is re-sent every 0.5 s while + waiting for mode confirmation (prevents ArduPilot from expiring + the override, which would disengage the motor interlock). """ log.info("Setting mode %d …", mode_id) + t_last_override = self.sim_now() t_last_send = self.sim_now() self._mav.mav.command_long_send( self._target_system, @@ -868,6 +883,11 @@ def set_mode( deadline = self.sim_now() + timeout _poll = 0.5 while self.sim_now() < deadline: + # Refresh RC override every 0.5 s sim-time (ArduPilot expiry is ~1 s). + if rc_override and (self.sim_now() - t_last_override) >= _poll: + self.send_rc_override(rc_override) + t_last_override = self.sim_now() + msg = self._recv( type=["HEARTBEAT", "COMMAND_ACK", "STATUSTEXT", "ATTITUDE"], blocking=True, @@ -1003,15 +1023,28 @@ def set_message_interval(self, msg_id: int, interval_us: int) -> None: def send_rc_override(self, channels: dict[int, int]) -> None: """ - RC overrides are disabled in the stack. + Send RC_CHANNELS_OVERRIDE to the vehicle. - Policy: flight control is GUIDED-only, and the only permitted override - is Lua-managed CH8 interlock in rawes.lua. + Parameters + ---------- + channels : dict[int, int] + Mapping of channel number (1-indexed) to PWM value. + Channels not listed are set to 0 (= no override). + + Example + ------- + gcs.send_rc_override({8: 2000}) # CH8 = full high (motor interlock release) """ - raise RuntimeError( - "RC_CHANNELS_OVERRIDE is disabled. Use GUIDED setpoints; only " - "rawes.lua may hold CH8 override." + pwm = [0] * 18 + for ch, val in channels.items(): + if 1 <= ch <= 18: + pwm[ch - 1] = val + self._mav.mav.rc_channels_override_send( + self._target_system, + self._target_component, + *pwm[:18], ) + log.debug("RC override sent: %s", channels) def send_named_float(self, name: str, value: float) -> None: """Send a NAMED_VALUE_FLOAT MAVLink message to the vehicle. diff --git a/simulation/physics_core.py b/simulation/physics_core.py index 9d310be..6390cd6 100644 --- a/simulation/physics_core.py +++ b/simulation/physics_core.py @@ -42,7 +42,6 @@ from dynbem import create_aero, RotorInputs, euler_step_omega from tether import TetherModel from rotor_physics import resolve_i_spin_kgm2 -from param_defaults import thrust_to_coll_rad as _t2c @dataclass @@ -318,7 +317,7 @@ def warm_inflow(self, collective_rad: float, n_steps: int = 500, Parameters ---------- - collective_rad : equilibrium collective [rad] + collective_rad : equilibrium collective [rad] — use thrust_to_coll_rad(ic.eq_thrust) n_steps : number of ODE steps (default 500 @ 1 ms = 0.5 s) dt : inflow ODE time step [s] (default 1e-3) """ @@ -336,12 +335,6 @@ def warm_inflow(self, collective_rad: float, n_steps: int = 500, for _ in range(n_steps): _, self._rotor_state = self._aero.step(inputs, self._rotor_state, dt) - def warm_inflow_from_thrust(self, thrust_cmd: float, n_steps: int = 500, - dt: float = 1e-3) -> None: - """Thrust-domain wrapper around warm_inflow().""" - thrust = float(np.clip(thrust_cmd, 0.0, 1.0)) - self.warm_inflow(_t2c(thrust), n_steps=n_steps, dt=dt) - # ── Internal integration ────────────────────────────────────────────────── def _integrate(self, dt: float, collective_rad: float, diff --git a/simulation/rawes_modes.py b/simulation/rawes_modes.py index e5d8779..be18aba 100644 --- a/simulation/rawes_modes.py +++ b/simulation/rawes_modes.py @@ -21,7 +21,7 @@ # ── Mode numbers (RAWES_MODE script-generated param; 0=none 1=steady 3=passive 4=landing) ── -MODE_NONE = 0 # script passive: no control-channel overrides (CH8 interlock hold still applies while armed) +MODE_NONE = 0 # script passive: no RC overrides; logs every 5 s + any NV message MODE_STEADY = 1 # bz_altitude_hold cyclic (commanded tension) + altitude-PID collective # mode 2 reserved (unused) MODE_PASSIVE = 3 # kinematic capture helper: hold IC attitude during release diff --git a/simulation/scripts/rawes.lua b/simulation/scripts/rawes.lua index 8169416..7b16cf1 100644 --- a/simulation/scripts/rawes.lua +++ b/simulation/scripts/rawes.lua @@ -3,7 +3,7 @@ rawes.lua -- Unified RAWES flight controller Works in both ArduPilot SITL (mcroomp fork) and on the Pixhawk 6C. Mode is selected at runtime via RAWES_MODE (script-generated parameter): - 0 none -- script passive: no control-channel overrides; CH8 interlock hold still applies while armed + 0 none -- script passive: no RC overrides; logs every 5 s + any NV message 1 steady -- primary guided flight path (set_target_rate_and_throttle) 2 reserved -- unused 3 passive -- kinematic capture helper; keeps the IC attitude stable during release @@ -33,16 +33,11 @@ Ground planner signals via NAMED_VALUE_FLOAT (dynamic in-flight values only): altitude hold (and therefore commands no cyclic/collective) until all three anchor floats (ANN/ANE/AND) have been received at least once. Until then run_flight only holds the current attitude. - -- IC seed (MODE_PASSIVE): - -- thrust-only seed (RAWES_THR) is valid and drives zero-rate hold. - -- full attitude hold commits once RAWES_THR+RAWES_RIC+RAWES_PIC all arrive. + -- IC seed (MODE_PASSIVE; committed atomically once all three arrive): RAWES_THR: IC thrust [0..1] (calibrate: --trim thr=) RAWES_RIC: IC roll [rad] (calibrate: --roll ) RAWES_PIC: IC pitch [rad] (calibrate: --pitch ) RAWES_YIC: optional fixed yaw target [rad] for MODE_PASSIVE (calibrate: --yaw ) - Sending -1000 (RAWES_YIC_CAPTURE_SENTINEL) instead of a yaw angle - captures the current AHRS roll/pitch/yaw as the IC roll, pitch, - and fixed yaw target all at once. Parameters (script-generated; visible in GCS as RAWES_* params): RAWES_MODE Mode selector (0=none,1=steady,3=passive,4=landing) default 0 @@ -88,25 +83,13 @@ MODE_STEADY = 1 MODE_PASSIVE = 3 -- kinematic capture helper: hold the IC attitude stable during release. MODE_LANDING = 4 -- reserved; not implemented here --- Sentinel value for RAWES_YIC: sending this instead of a yaw angle tells --- MODE_PASSIVE to capture the CURRENT roll, pitch, AND yaw from AHRS as the --- IC seed (roll/pitch pending IC + fixed yaw target), instead of treating the --- value as a fixed yaw target in radians. -1000 rad is far outside any valid --- yaw angle so it cannot collide with a real command. -RAWES_YIC_CAPTURE_SENTINEL = -1000.0 - -- MODE_PASSIVE IC seeds are provided over NVF (10-char names max): -- RAWES_THR : IC thrust [0..1] -- RAWES_RIC : IC roll [rad] -- RAWES_PIC : IC pitch [rad] --- RAWES_THR alone is accepted for thrust-only zero-rate hold while waiting for --- attitude seed. Full IC attitude hold commits atomically after all three. +-- They start nil and are committed atomically only after all three arrive. -- RAWES_YIC : fixed yaw target [rad] (optional). When present, PASSIVE holds -- this absolute yaw instead of capturing the (spinning) AHRS yaw. --- Sending RAWES_YIC_CAPTURE_SENTINEL (-1000) instead captures the --- the current AHRS roll/pitch if RAWES_RIC/RAWES_PIC have not --- already been received, so a yaw-only IC command doesn't block --- the atomic thrust+roll+pitch seed. -- RAWES_SUB carries a generic substate index (delivered via NAMED_VALUE_FLOAT). -- The ground pumping schedule runs in MODE_STEADY and uses RAWES_SUB only for @@ -182,6 +165,7 @@ _mode_ms = 0 _submode_ms = 0 -- Cached RC channel objects +_rc_ch4 = rc:get_channel(4) _rc_ch8 = rc:get_channel(8) -- Thrust state [0..1] @@ -226,7 +210,6 @@ _ic_seeded = false _ic_pending_thrust = nil _ic_pending_roll_deg = nil _ic_pending_pitch_deg = nil -_ic_thrust_only_reported = false -- One-shot debug state for capture/first-command handoff diagnostics. _dbg_cap_logged = false @@ -898,28 +881,15 @@ local function run_passive_mode(now) -- release transitions smoothly. Hold zero body-rate demand and -- pass IC collective through GUIDED throttle (no H_FLYBAR_MODE -- dependency). - local ic_full_ready = (_ic_thrust ~= nil and _ic_roll_deg ~= nil and _ic_pitch_deg ~= nil) - local ic_thrust_only = (_ic_thrust ~= nil and _ic_roll_deg == nil and _ic_pitch_deg == nil) + local ic_ready = (_ic_thrust ~= nil and _ic_roll_deg ~= nil and _ic_pitch_deg ~= nil) local mode_now = vehicle:get_mode() local guided_ok = (mode_now == GUIDED_MODE_NUM or mode_now == 20) and ahrs:healthy() - local gyro = ahrs:get_gyro() - local gx, gy, gz = 0.0, 0.0, 0.0 - if gyro then - gx = gyro:x() - gy = gyro:y() - gz = gyro:z() - end if now - _passive_status_ms >= 1000 then _passive_status_ms = now local armed_s = arming:is_armed() and "ARMED" or "disarmed" - local ic_s = "waiting" - if ic_full_ready then - ic_s = "ready" - elseif ic_thrust_only then - ic_s = "thr_only" - end + local ic_s = ic_ready and "ready" or "waiting" local g_s = guided_ok and "ok" or "no" local y_s = (_passive_hold_yaw_rad ~= nil) and string.format("%.1f", math.deg(_passive_hold_yaw_rad)) or "n/a" gcs:send_text(6, string.format( @@ -927,21 +897,15 @@ local function run_passive_mode(now) armed_s, ic_s, g_s, y_s, ic_thrust_or_default())) end - if not ic_full_ready then - -- Without full attitude seed, run thrust-only zero-rate hold when - -- RAWES_THR is available. This damps rotation while preserving rotor - -- RPM and avoids blocking PASSIVE on RAWES_RIC/PIC. - _diag_set("OL_RSP", 0.0) - _diag_set("OL_PSP", 0.0) - _diag_set("OL_YSP", 0.0) - _diag_set("OL_RER", -gx) - _diag_set("OL_PER", -gy) - _diag_set("OL_YER", -gz) - _diag_set("OL_AP", 0.0) - _diag_set("OL_AI", 0.0) - _diag_set("OL_AD", 0.0) - _diag_set("OL_COL", ic_thrust_or_default()) - if guided_ok and arming:is_armed() and _ic_thrust ~= nil then + if not ic_ready then + -- No IC seed yet: instead of sitting idle, actively damp any rotation + -- to a standstill using the rate-only GUIDED API. Commanding zero body + -- rates lets ArduPilot's rate controller null the rotor-reaction / + -- disturbance spin while we wait for the ground to send the IC seed. + -- Gated on armed so we never emit control-API traffic during the pre-arm + -- kinematic phase (motors are off when disarmed anyway). Uses a default + -- collective so rotor RPM is preserved. + if guided_ok and arming:is_armed() then vehicle:set_target_rate_and_throttle(0.0, 0.0, 0.0, ic_thrust_or_default()) end return @@ -964,16 +928,6 @@ local function run_passive_mode(now) end local col_thrust_p = ic_thrust_or_default() - _diag_set("OL_RSP", 0.0) - _diag_set("OL_PSP", 0.0) - _diag_set("OL_YSP", 0.0) - _diag_set("OL_RER", -gx) - _diag_set("OL_PER", -gy) - _diag_set("OL_YER", -gz) - _diag_set("OL_AP", 0.0) - _diag_set("OL_AI", 0.0) - _diag_set("OL_AD", 0.0) - _diag_set("OL_COL", col_thrust_p) if guided_ok then send_guided_angle_rate_throttle( _ic_roll_deg, _ic_pitch_deg, math.deg(_passive_hold_yaw_rad or 0.0), @@ -1018,56 +972,21 @@ local function update() "RAWES anchor set: N=%.1f E=%.1f D=%.1f", _anchor_n, _anchor_e, _anchor_d)) end -- Crosswind rate damping gains now come from RAWES_* params; no NVF override needed. - -- IC seed handling: - -- RAWES_THR alone is accepted for thrust-only zero-rate hold. - -- full attitude hold commits once RAWES_THR+RAWES_RIC+RAWES_PIC are all present. + -- IC seed handling: do not set active IC commands until all three + -- (collective, roll, pitch) have been observed at least once. if _nv_floats["RAWES_THR"] then _ic_pending_thrust = _nv_floats["RAWES_THR"] end if _nv_floats["RAWES_RIC"] then _ic_pending_roll_deg = math.deg(_nv_floats["RAWES_RIC"]) end if _nv_floats["RAWES_PIC"] then _ic_pending_pitch_deg = math.deg(_nv_floats["RAWES_PIC"]) end -- Optional fixed yaw target for MODE_PASSIVE. When present, PASSIVE holds -- this absolute yaw instead of capturing (and holding) the spinning AHRS yaw. - -- Sending RAWES_YIC_CAPTURE_SENTINEL instead captures roll/pitch/yaw all - -- at once from the current AHRS attitude. - -- One-shot: RAWES_YIC persists in _nv_floats once received, so it must be - -- cleared after processing -- otherwise every tick re-enters this block, - -- continuously re-capturing the (possibly still-moving) AHRS attitude and - -- re-emitting the capture statustext (mirrors the RAWES_ARM one-shot idiom - -- in run_armon()). - if _nv_floats["RAWES_YIC"] then - local yic = _nv_floats["RAWES_YIC"] - if yic <= RAWES_YIC_CAPTURE_SENTINEL + 0.5 then - if ahrs:healthy() then - _nv_floats["RAWES_YIC"] = nil - _ic_pending_roll_deg = math.deg(ahrs:get_roll_rad()) - _ic_pending_pitch_deg = math.deg(ahrs:get_pitch_rad()) - _passive_yaw_fixed_rad = ahrs:get_yaw_rad() - gcs:send_text(6, string.format( - "RAWES YIC capture: r=%.1f p=%.1f y=%.1f", - _ic_pending_roll_deg, _ic_pending_pitch_deg, math.deg(_passive_yaw_fixed_rad))) - end - -- else: AHRS not yet healthy -- leave RAWES_YIC in the inbox and - -- retry the capture next tick. - else - _nv_floats["RAWES_YIC"] = nil - _passive_yaw_fixed_rad = yic - end - end + if _nv_floats["RAWES_YIC"] then _passive_yaw_fixed_rad = _nv_floats["RAWES_YIC"] end if not _ic_seeded then - if _ic_pending_thrust ~= nil then + if _ic_pending_thrust ~= nil and _ic_pending_roll_deg ~= nil and _ic_pending_pitch_deg ~= nil then _ic_thrust = _ic_pending_thrust - if (not _ic_thrust_only_reported) - and _ic_pending_roll_deg == nil and _ic_pending_pitch_deg == nil then - _ic_thrust_only_reported = true - gcs:send_text(6, string.format( - "RAWES IC thrust set: thr=%.3f (attitude pending)", _ic_thrust)) - end - end - if _ic_thrust ~= nil and _ic_pending_roll_deg ~= nil and _ic_pending_pitch_deg ~= nil then _ic_roll_deg = _ic_pending_roll_deg _ic_pitch_deg = _ic_pending_pitch_deg _ic_seeded = true - _ic_thrust_only_reported = false gcs:send_text(6, string.format( "RAWES IC seed set: r=%.1f p=%.1f thr=%.3f", _ic_roll_deg, _ic_pitch_deg, _ic_thrust)) @@ -1090,6 +1009,12 @@ local function update() -- smoothly when the ground transitions between phase tension targets. _apply_tension_ramp(BASE_PERIOD_MS * 0.001) + -- Ch4 yaw always neutral (no RC receiver; prevents yaw integrator wind-up). + -- In PASSIVE before full IC seed, avoid all non-essential control API calls. + if mode ~= MODE_PASSIVE or _ic_seeded then + if _rc_ch4 then _rc_ch4:set_override(1500) end + end + -- Latch the motor interlock (CH8) high while armed so the heli rotor stays -- runup-complete and ArduPilot clears land_complete (otherwise GUIDED angle -- commands are silently dropped by angle_control_run's takeoff/relax branch). @@ -1157,4 +1082,3 @@ gcs:send_text(6, string.format( -- @@UNIT_TEST_HOOK return update, BASE_PERIOD_MS - diff --git a/simulation/scripts/rawes_params.json b/simulation/scripts/rawes_params.json index 0839eed..bd4bfe4 100644 --- a/simulation/scripts/rawes_params.json +++ b/simulation/scripts/rawes_params.json @@ -16,9 +16,9 @@ "note": "active Lua flight mode (script-generated param): 0=none (default) 1=steady 3=passive 4=landing (pumping runs in steady)" }, { - "name": "ARMING_SKIPCHK", - "expected": 65535, - "note": "0xFFFF=skip all prearm checks (ArduPilot 4.7+); ARMING_CHECK is legacy" + "name": "ARMING_CHECK", + "expected": 0, + "note": "0=prearm checks disabled; required for Lua arming:arm() to succeed" }, { "name": "BRD_SAFETY_DEFLT", @@ -44,8 +44,8 @@ }, { "name": "ATC_RAT_YAW_I", - "expected": 0.0015, - "note": "yaw rate I gain; kept small (~P/10) so ArduPilot can remove residual drift without fighting the Lua-owned trim observer." + "expected": 0.0, + "note": "yaw rate I gain; disabled while characterizing P alone (one-sided actuator + asymmetric integrator makes I aggressive). After P is settled, re-enable at ~P/10 (~0.0015) to remove steady-state drift." }, { "name": "ATC_RAT_YAW_D", diff --git a/simulation/tests/common/mock_ardupilot.py b/simulation/tests/common/mock_ardupilot.py index 3240b42..a11955a 100644 --- a/simulation/tests/common/mock_ardupilot.py +++ b/simulation/tests/common/mock_ardupilot.py @@ -23,7 +23,7 @@ from landing_planner import LandingCommand from physics_core import HubObservation from pumping_planner import TensionCommand -from param_defaults import get_ap_param, load_ap_params, load_rawes_lua_constants, load_collective_phys_range +from param_defaults import get_ap_param, load_ap_params, load_rawes_lua_constants, load_collective_phys_range, thrust_to_coll_rad from telemetry_csv import TelRow, write_csv @@ -96,9 +96,9 @@ class _MockArdupilotBase: def __init__(self, *, wind: "np.ndarray", dt: float, initial_thrust: float = 0.263) -> None: self._last_thrust = float(initial_thrust) + self.col_rad = thrust_to_coll_rad(self._last_thrust) self.roll_sp = 0.0 self.pitch_sp = 0.0 - self._col_min, self._col_max = load_collective_phys_range() self._wind = wind self._tel_every = _tel_every_from_env(dt) self.tel_fn: "Callable[..., dict] | None" = None @@ -129,7 +129,8 @@ def step_physics(self, runner, dt: float, *, rest_length: "float | None" = None) if heli_out.collective_norm_cmd is not None and abs(float(self._guided_ctrl.climbrate_ms)) > 1e-6: c_norm = float(np.clip(heli_out.collective_norm_cmd, -1.0, 1.0)) self._last_thrust = 0.5 * (c_norm + 1.0) - return runner.step_guided(dt, self._last_thrust, heli_out, rest_length=rest_length) + self.col_rad = thrust_to_coll_rad(self._last_thrust) + return runner.step_guided(dt, self.col_rad, heli_out, rest_length=rest_length) def log(self, runner, sr: dict) -> None: if self.tel_fn is None or self._tel_every is None: @@ -217,9 +218,8 @@ def log(self, runner, sr: dict) -> None: ) extra_kwargs.update(rate_terms) - collective_rad = self._col_min + self._last_thrust * (self._col_max - self._col_min) self._telemetry.append( - TelRow.from_physics(runner, sr, collective_rad, self._wind, **extra_kwargs) + TelRow.from_physics(runner, sr, self.col_rad, self._wind, **extra_kwargs) ) self._log_step += 1 @@ -231,10 +231,6 @@ def write_telemetry(self, path) -> None: def telemetry(self) -> list: return self._telemetry - @property - def thrust(self) -> float: - return float(self._last_thrust) - class _LuaBackend(_MockArdupilotBase): def __init__(self, sim, *, initial_thrust: float = 0.263, wind: "np.ndarray", dt: float) -> None: @@ -263,10 +259,12 @@ def tick(self, t_sim: float, runner, *, inject=None, accel_ned: "np.ndarray | No gt_throttle = self._sim._mock.guided_throttle if gt_throttle is not None: self._last_thrust = float(gt_throttle) + self.col_rad = thrust_to_coll_rad(self._last_thrust) else: ch3 = self._sim.ch_out[3] if ch3 is not None: self._last_thrust = max(0.0, min(1.0, (ch3 - 1000) / 1000.0)) + self.col_rad = thrust_to_coll_rad(self._last_thrust) gt_rate = self._sim._mock.guided_rate_target if gt_rate is not None: @@ -696,7 +694,7 @@ def controller_step( accel_ned: "np.ndarray | None" = None, ) -> "tuple[float, float, float]": thrust, roll_sp, pitch_sp = self._mode.step(obs, dt, accel_ned=accel_ned) - self._last_thrust = float(thrust) + self.col_rad = thrust_to_coll_rad(float(thrust)) self.roll_sp = roll_sp self.pitch_sp = pitch_sp return thrust, roll_sp, pitch_sp diff --git a/simulation/tests/simtests/simtest_runner.py b/simulation/tests/simtests/simtest_runner.py index d2892b6..e5dd010 100644 --- a/simulation/tests/simtests/simtest_runner.py +++ b/simulation/tests/simtests/simtest_runner.py @@ -3,8 +3,8 @@ PhysicsRunner is a thin wrapper around PhysicsCore (simulation/physics_core.py). - step_from_thrust(dt, thrust, rate_roll, rate_pitch, omega_body) - → runs HeliCyclicController.step_from_thrust(), then core.step() + step(dt, collective, rate_roll, rate_pitch, omega_body) + → runs HeliCyclicController (baked in) then core.step() PhysicsCore owns all physics constants (k_yaw, T_AERO_OFFSET) and the integration loop (dynamics, aero, tether, spin ODE, angular damping). @@ -22,11 +22,10 @@ Lua backend wraps the 50-100 Hz tick pattern that every Lua test repeats: millis update → feed_obs → update_fn → PWM decode. -In GUIDED mode rawes.lua drives cyclic/collective via -vehicle:set_target_angle_and_rate_and_throttle and -vehicle:set_target_rate_and_throttle. The Lua backend feeds those targets into -GuidedAttitudeController (400 Hz inner loop) and calls runner.step_guided() so -physics sees the correct swashplate tilt. +In GUIDED mode rawes.lua drives cyclic via vehicle:set_target_angle_and_climbrate +(stored in _mock.guided_target) rather than ch1/ch2 RC overrides. The Lua backend +feeds guided_target into a GuidedAttitudeController (400 Hz inner loop) and +calls runner.step_guided() so physics sees the correct swashplate tilt. Collective is sourced from the guided vertical channel when available, with ch3 kept as a legacy fallback. """ @@ -40,7 +39,7 @@ from physics_core import PhysicsCore, HubObservation from controller import HeliCyclicController -from param_defaults import load_collective_phys_range as _load_col_range +from param_defaults import thrust_to_coll_rad as _t2c, load_collective_phys_range as _load_col_range class PhysicsRunner: @@ -54,7 +53,7 @@ class PhysicsRunner: ----- runner = PhysicsRunner(rotor, ic, wind) for i in range(steps): - sr = runner.step_from_thrust(DT, thrust_cmd, rate_roll, rate_pitch, omega_body, rest_length=winch.rest_length) + sr = runner.step(DT, collective_rad, rate_roll, rate_pitch, omega_body, rest_length=winch.rest_length) tension_now = runner.tension_now hub = runner.hub_state """ @@ -79,13 +78,15 @@ def __init__(self, rotor, ic, wind, *, z_floor: float = -1.0, """ self._core = PhysicsCore(rotor, ic, wind, z_floor=z_floor, aero_model=aero_model, aero_override=aero_override) - self._col_min, self._col_max = _load_col_range() self._tilt_lon_trim = float(getattr(ic, "trim_tilt_lon", 0.0)) self._tilt_lat_trim = float(getattr(ic, "trim_tilt_lat", 0.0)) self._acro = HeliCyclicController(rotor) self._acro.set_trim(self._tilt_lon_trim, self._tilt_lat_trim) - self._acro.reset_from_thrust( - float(ic.eq_thrust), + # coll_eq_rad is the physics equilibrium collective; eq_thrust is its + # normalized form. Use coll_eq_rad if present, else convert eq_thrust. + servo_col = getattr(ic, 'coll_eq_rad', None) or _t2c(ic.eq_thrust) + self._acro._servo.reset( + servo_col, tilt_lon=self._tilt_lon_trim, tilt_lat=self._tilt_lat_trim, ) @@ -94,6 +95,7 @@ def __init__(self, rotor, ic, wind, *, z_floor: float = -1.0, @classmethod def for_warmup(cls, rotor, pos, R0, rest_length, eq_thrust, omega_spin, wind): + col_min, col_max = _load_col_range() ic = SimpleNamespace( pos = np.asarray(pos, dtype=float), vel = np.zeros(3), @@ -102,7 +104,7 @@ def for_warmup(cls, rotor, pos, R0, rest_length, eq_thrust, omega_spin, wind): eq_thrust = float(eq_thrust), omega_spin = float(omega_spin), ) - return cls(rotor, ic, wind) + return cls(rotor, ic, wind, col_min_rad=col_min, col_max_rad=col_max) # ── Read-only state properties ──────────────────────────────────────────── @@ -143,13 +145,18 @@ def altitude(self) -> float: # ── Physics steps ───────────────────────────────────────────────────────── - def step_from_thrust(self, dt: float, thrust_cmd: float, - rate_roll: float, rate_pitch: float, - omega_body: np.ndarray, - *, rest_length: "float | None" = None) -> dict: - """400 Hz step for Python-AP tests using thrust [0..1].""" - tlon, tlat, col_act = self._acro.step_from_thrust( - thrust_cmd, rate_roll, rate_pitch, omega_body, dt) + def step(self, dt: float, collective_rad: float, + rate_roll: float, rate_pitch: float, + omega_body: np.ndarray, + *, rest_length: "float | None" = None) -> dict: + """ + 400 Hz step for Python-AP tests. + + Runs HeliCyclicController (arduloop rate PID + servo model) then physics. + Use when a Python AP controller produces (collective, rate_roll, rate_pitch). + """ + tlon, tlat, col_act = self._acro.step( + collective_rad, rate_roll, rate_pitch, omega_body, dt) return self._core.step(dt, col_act, tlon, tlat, rest_length) def observe(self) -> HubObservation: @@ -159,7 +166,7 @@ def observe(self) -> HubObservation: def step_guided( self, dt: float, - thrust_cmd: float, + collective_cmd: float, heli_out, *, rest_length: "float | None" = None, @@ -167,19 +174,16 @@ def step_guided( """400 Hz step for GUIDED tests. Takes a HeliRateOutput from GuidedAttitudeController.update() directly, - converts thrust [0..1] to collective radians at the boundary, applies the SwashplateServoModel for servo lag, then calls physics. Bypasses the rate PIDs in _acro (those are inside GuidedAttitudeController). """ - # Sign mapping matches HeliCyclicController._step_collective(): + # Sign mapping matches HeliCyclicController.step(): # roll_cyclic -> tilt_lat (no sign flip) # pitch_cyclic -> -tilt_lon (sign flip) tilt_lat_cmd = heli_out.roll_cyclic tilt_lon_cmd = -heli_out.pitch_cyclic tilt_lat_cmd += self._tilt_lat_trim tilt_lon_cmd += self._tilt_lon_trim - thrust = float(np.clip(thrust_cmd, 0.0, 1.0)) - collective_cmd = self._col_min + thrust * (self._col_max - self._col_min) col_act, tlon, tlat = self._acro._servo.step( collective_cmd, tilt_lon_cmd, tilt_lat_cmd, dt) return self._core.step(dt, col_act, tlon, tlat, rest_length) diff --git a/simulation/tests/simtests/test_generate_ic.py b/simulation/tests/simtests/test_generate_ic.py index 388bbae..6e7987b 100644 --- a/simulation/tests/simtests/test_generate_ic.py +++ b/simulation/tests/simtests/test_generate_ic.py @@ -515,7 +515,7 @@ def _run_steady(pos0: np.ndarray, vel0: np.ndarray, R0: np.ndarray, ), _DT_CMD) if step % _AP_EVERY == 0: ap.tick(step * _DT, runner) - sr = runner.step_from_thrust(_DT, ap.thrust, ap.roll_sp, ap.pitch_sp, + sr = runner.step(_DT, ap.col_rad, ap.roll_sp, ap.pitch_sp, runner.omega_body, rest_length=rest_now) ap.log(runner, sr) @@ -562,6 +562,7 @@ def _print_steady(log, m: dict, label: str) -> None: def test_ic_force_balance(simtest_log): """ The IC must be a genuine physics fixed point at coll_eq_rad. + At static equilibrium (vel=0, coll=coll_eq_rad, R0=force-balanced), the net force on the hub should be near zero. We verify this by running 1 second of open-loop physics (no controller — fixed collective = coll_eq_rad, @@ -600,8 +601,8 @@ def test_ic_force_balance(simtest_log): # Run 1 second open-loop at the IC collective + trim cyclic (no rate commands). N_STEPS = int(1.0 / _DT) for _ in range(N_STEPS): - runner.step_from_thrust(_DT, float(ic_static.eq_thrust), 0.0, 0.0, - runner.omega_body, rest_length=ic_static.rest_length) + runner.step(_DT, ic_static.coll_eq_rad, 0.0, 0.0, + runner.omega_body, rest_length=ic_static.rest_length) final_speed = float(np.linalg.norm(runner.hub_state["vel"])) final_tension = runner.tension_now diff --git a/simulation/tests/simtests/test_landing.py b/simulation/tests/simtests/test_landing.py index 1b33f7f..ce9d520 100644 --- a/simulation/tests/simtests/test_landing.py +++ b/simulation/tests/simtests/test_landing.py @@ -167,7 +167,7 @@ def _run_landing(log) -> "tuple[dict, object]": # ── Physics 400 Hz ──────────────────────────────────────────────── omega_body = runner.omega_body omega_body[2] = 0.0 - sr = runner.step_from_thrust(DT, ap.thrust, ap.roll_sp, ap.pitch_sp, omega_body, + sr = runner.step(DT, ap.col_rad, ap.roll_sp, ap.pitch_sp, omega_body, rest_length=winch.rest_length) phase = cmd.phase diff --git a/simulation/tests/simtests/test_pump_cycle_lua.py b/simulation/tests/simtests/test_pump_cycle_lua.py index 64c26ae..dc85b0d 100644 --- a/simulation/tests/simtests/test_pump_cycle_lua.py +++ b/simulation/tests/simtests/test_pump_cycle_lua.py @@ -178,7 +178,8 @@ def _run_pumping(log, aero_model: "str | None" = None) -> dict: comms.send(planner.step(0.0, tel.tension_n, tel.rest_length), DT_PLANNER) # ── Inflow warm-up ──────────────────────────────────────────────────── - runner._core.warm_inflow_from_thrust(float(_IC.eq_thrust), n_steps=500) + from param_defaults import thrust_to_coll_rad as _t2c + runner._core.warm_inflow(collective_rad=_t2c(float(_IC.eq_thrust)), n_steps=500) t_sim = 0.0 for i in range(max_steps): diff --git a/simulation/tests/simtests/test_pump_cycle_unified.py b/simulation/tests/simtests/test_pump_cycle_unified.py index 2196bcd..27ce7be 100644 --- a/simulation/tests/simtests/test_pump_cycle_unified.py +++ b/simulation/tests/simtests/test_pump_cycle_unified.py @@ -167,7 +167,7 @@ def _tel_fn(r, sr): comms.send_command(0.0, cmd) # ── Inflow warm-up ──────────────────────────────────────────────────── - runner._core.warm_inflow_from_thrust(float(_IC.eq_thrust), n_steps=500) + runner._core.warm_inflow(collective_rad=thrust_to_coll_rad(float(_IC.eq_thrust)), n_steps=500) t_sim = 0.0 for i in range(max_steps): @@ -227,7 +227,7 @@ def _tel_fn(r, sr): # ── Physics 400 Hz ──────────────────────────────────────────────── omega_body = runner.omega_body omega_body[2] = 0.0 - sr = runner.step_from_thrust(DT, ap.thrust, ap.roll_sp, ap.pitch_sp, omega_body, + sr = runner.step(DT, ap.col_rad, ap.roll_sp, ap.pitch_sp, omega_body, rest_length=winch.rest_length) prev_accel_ned = sr.get("accel_specific_world") diff --git a/simulation/tests/simtests/test_sensor_closed_loop.py b/simulation/tests/simtests/test_sensor_closed_loop.py index 5e75d23..d6ea1db 100644 --- a/simulation/tests/simtests/test_sensor_closed_loop.py +++ b/simulation/tests/simtests/test_sensor_closed_loop.py @@ -100,7 +100,7 @@ def _run(t_sim: float = T_SIM): """ runner = PhysicsRunner(_ROTOR_S, _IC, WIND) runner._acro = HeliCyclicController(_ROTOR_S) - runner._acro.reset_from_thrust(float(_IC.eq_thrust)) + runner._acro._servo.reset(_IC.coll_eq_rad) # Ground-side tension-regulating winch. tension_target = 300.0 @@ -156,7 +156,7 @@ def _run(t_sim: float = T_SIM): dT = runner.tension_now - tension_target v_winch = max(-_WINCH_VMAX, min(_WINCH_VMAX, _WINCH_KP * dT)) rest_now += v_winch * DT - runner.step_from_thrust(DT, float(_IC.eq_thrust), rate_roll, rate_pitch, omega_body, + runner.step(DT, _IC.coll_eq_rad, rate_roll, rate_pitch, omega_body, rest_length=rest_now) events.check_floor(runner.hub_state["pos"][2], t, "flight") diff --git a/simulation/tests/simtests/test_steady_flight.py b/simulation/tests/simtests/test_steady_flight.py index 451e77e..c1200c1 100644 --- a/simulation/tests/simtests/test_steady_flight.py +++ b/simulation/tests/simtests/test_steady_flight.py @@ -36,6 +36,7 @@ from simtest_runner import PhysicsRunner from tests.common.mock_ardupilot import MockArdupilot from pumping_planner import TensionCommand +from param_defaults import thrust_to_coll_rad from simtest_ic import load_ic from tests.simtests._rotor_helpers import ( load_default_rotor, dynamics_kwargs, BODY_Z_SLEW_RATE_RAD_S, @@ -88,7 +89,7 @@ def _run_simulation(log, steps: int = 4000, *, ic=None): _ROTOR, # All AP gains/limits come from the merged central .parm chain. ) - runner._acro.reset_from_thrust(float(ic.eq_thrust)) + runner._acro._servo.reset(thrust_to_coll_rad(ic.eq_thrust)) ap = MockArdupilot.for_pumping( ic_pos=ic.pos, mass_kg=MASS, @@ -152,7 +153,7 @@ def _run_simulation(log, steps: int = 4000, *, ic=None): ap.write_telemetry(log.log_dir / "telemetry.csv") return { - "coll_deg": math.degrees(float(getattr(ic, "coll_eq_rad", 0.0))), + "coll_deg": math.degrees(thrust_to_coll_rad(ic.eq_thrust)), "omega_spin_eq": runner.omega_spin, "pos0": ic.pos, "vel0": ic.vel, diff --git a/simulation/tests/simtests/test_steady_flight_ic_angle_only.py b/simulation/tests/simtests/test_steady_flight_ic_angle_only.py index 9384a57..ca1050f 100644 --- a/simulation/tests/simtests/test_steady_flight_ic_angle_only.py +++ b/simulation/tests/simtests/test_steady_flight_ic_angle_only.py @@ -43,6 +43,7 @@ from frames import build_gps_yaw_frame from telemetry_csv import TelRow, write_csv from simtest_ic import load_ic +from param_defaults import thrust_to_coll_rad from simtest_runner import PhysicsRunner from tests.simtests._rotor_helpers import load_default_rotor @@ -73,7 +74,8 @@ def _run_angle_ic_only(steps: int = 4000): ) runner = PhysicsRunner(_ROTOR, ic_run, WIND) - runner._acro.reset_from_thrust(float(_IC.eq_thrust), tilt_lon=0.0, tilt_lat=0.0) + from param_defaults import thrust_to_coll_rad as _t2c + runner._acro._servo.reset(_t2c(_IC.eq_thrust), tilt_lon=0.0, tilt_lat=0.0) _ap = _lp() rp = RateAxisParams.from_ap_dict(_ap, "RLL") @@ -91,7 +93,7 @@ def _run_angle_ic_only(steps: int = 4000): from param_defaults import load_collective_phys_range as _lr col_min, col_max = _lr() - col_norm = (_IC.coll_eq_rad - col_min) / (col_max - col_min) * 2.0 - 1.0 + col_norm = (_t2c(_IC.eq_thrust) - col_min) / (col_max - col_min) * 2.0 - 1.0 pos_hist = np.zeros((steps, 3)) ten_hist = np.zeros(steps) @@ -140,7 +142,7 @@ def _run_angle_ic_only(steps: int = 4000): heli_out=heli_out, ) ) - sr = runner.step_guided(DT, float(_IC.eq_thrust), heli_out, rest_length=float(_IC.rest_length)) + sr = runner.step_guided(DT, thrust_to_coll_rad(_IC.eq_thrust), heli_out, rest_length=float(_IC.rest_length)) # Extract guided controller state for telemetry tgt_euler = guided.attitude_target_euler_deg @@ -154,7 +156,7 @@ def _run_angle_ic_only(steps: int = 4000): TelRow.from_physics( runner, sr, - float(getattr(_IC, "coll_eq_rad", 0.0)), + thrust_to_coll_rad(_IC.eq_thrust), WIND, body_z_eq=np.asarray(ic_run.R0[:, 2], dtype=float), phase="steady", diff --git a/simulation/tests/simtests/test_steady_flight_lua.py b/simulation/tests/simtests/test_steady_flight_lua.py index 17fe994..50a7ac7 100644 --- a/simulation/tests/simtests/test_steady_flight_lua.py +++ b/simulation/tests/simtests/test_steady_flight_lua.py @@ -9,9 +9,9 @@ VZ-PI collective immediately. RAWES_TEN is fed from the 300 N target tension each Lua tick for gravity compensation. -In GUIDED mode rawes.lua calls vehicle:set_target_angle_and_rate_and_throttle -and vehicle:set_target_rate_and_throttle. MockArdupilot Lua backend feeds that -into GuidedAttitudeController and guided throttle is used for collective. +In GUIDED mode rawes.lua calls vehicle:set_target_angle_and_climbrate for +cyclic (not ch1/ch2). MockArdupilot Lua backend feeds that into GuidedAttitudeController; ch3 +is still decoded for collective. Non-Lua reference: test_steady_flight.py """ diff --git a/simulation/tests/unit/_controller_rig.py b/simulation/tests/unit/_controller_rig.py index 8fb2fbe..8c50dac 100644 --- a/simulation/tests/unit/_controller_rig.py +++ b/simulation/tests/unit/_controller_rig.py @@ -46,7 +46,7 @@ # Step response with candidate gains metrics = probe_step_response(rotor, R_hub, omega_spin=28.0, kp_inner=0.1, kd_inner=0.05, - thrust_cmd=0.6, setpoint=1.0) + setpoint=1.0) """ from __future__ import annotations @@ -59,7 +59,6 @@ from dynbem import RotorInputs, OyeBEMModel from controller import HeliCyclicController from dynamics import RigidBodyDynamics -from param_defaults import thrust_to_coll_rad _G_M_S2 = 9.81 @@ -271,17 +270,14 @@ def probe_step_response( channel: str = "roll", # "roll" | "pitch" duration_s: float = 2.0, dt: float = 0.0025, - thrust_cmd: float, + collective_rad: float = -0.05, mass_kg: float = 5.0, I_body: tuple = (5.0, 5.0, 10.0), I_spin: float = 4.0, ) -> StepMetrics: """Apply a step rate setpoint through ``HeliCyclicController`` and return time-response metrics. Use as the cost function for an - automated gain search. - - Uses ``thrust_cmd`` in [0..1] as the collective command input. - """ + automated gain search.""" aero = OyeBEMModel(defn=rotor) state = _settle_inflow(aero, omega_spin, R_hub) acro = HeliCyclicController( @@ -295,9 +291,6 @@ def probe_step_response( ) F_grav_cancel = np.array([0.0, 0.0, -mass_kg * _G_M_S2]) - thrust = float(np.clip(thrust_cmd, 0.0, 1.0)) - collective_for_aero = float(thrust_to_coll_rad(thrust)) - n_steps = int(round(duration_s / dt)) t_log = np.arange(n_steps, dtype=float) * dt omega_x_log = np.empty(n_steps) @@ -321,15 +314,15 @@ def probe_step_response( omega_y_log[i] = omega_b[1] rate_roll = setpoint if channel == "roll" else 0.0 rate_pitch = setpoint if channel == "pitch" else 0.0 - tlon, tlat, _ = acro.step_from_thrust( - thrust_cmd=thrust, + tlon, tlat, _ = acro.step( + collective_cmd=collective_rad, rate_roll_sp=rate_roll, rate_pitch_sp=rate_pitch, omega_body=omega_b, dt=dt, ) tlon_log[i] = tlon tlat_log[i] = tlat inputs = RotorInputs( - collective_rad=collective_for_aero, tilt_lon=tlon, tilt_lat=tlat, + collective_rad=collective_rad, tilt_lon=tlon, tilt_lat=tlat, R_hub=R, v_hub_world=s["vel"], wind_world=np.zeros(3), omega_rad_s=float(omega_spin), rho_kg_m3=1.225, ) diff --git a/simulation/tests/unit/test_armon_lua.py b/simulation/tests/unit/test_armon_lua.py index 5fd0727..72df441 100644 --- a/simulation/tests/unit/test_armon_lua.py +++ b/simulation/tests/unit/test_armon_lua.py @@ -126,25 +126,32 @@ def test_expiry_is_only_enforced_when_armed(self): # --------------------------------------------------------------------------- -# 8. CH4 is not script-driven +# 8. CH4 yaw always neutral # --------------------------------------------------------------------------- -class TestCh4NotOverridden: - def test_ch4_unset_in_mode0_before_arm(self): - """rawes.lua must not write CH4 in mode 0 before arming.""" +class TestCh4AlwaysNeutral: + def test_ch4_neutral_in_mode0_before_arm(self): + """CH4 must be overridden to 1500 even in passive mode 0 before arming.""" sim = _sim() sim.tick() - assert sim.ch_out[4] is None + assert sim.ch_out[4] == 1500, f"CH4={sim.ch_out[4]}, expected 1500" - def test_ch4_unset_in_mode1(self): - """rawes.lua must not write CH4 in mode 1 either.""" + def test_ch4_neutral_in_mode1(self): + """CH4 stays neutral during steady flight (mode 1).""" sim = RawesLua(mode=1) sim.tick() - assert sim.ch_out[4] is None + assert sim.ch_out[4] == 1500 - def test_ch4_unset_while_armed(self): - """CH4 remains untouched throughout RAWES_ARM armed window.""" + def test_ch4_neutral_while_armed(self): + """CH4 stays neutral throughout RAWES_ARM armed window.""" sim = _sim() _send_arm(sim, 60_000.0) sim.run(0.5) # reach armed state - assert sim.ch_out[4] is None + assert sim.ch_out[4] == 1500 + + def test_ch4_set_every_tick(self): + """CH4=1500 must be written on every tick, not just once.""" + sim = _sim() + for _ in range(20): + sim.tick() + assert sim.ch_out[4] == 1500 diff --git a/simulation/tests/unit/test_attitude_convergence.py b/simulation/tests/unit/test_attitude_convergence.py index 00d1f01..548a76e 100644 --- a/simulation/tests/unit/test_attitude_convergence.py +++ b/simulation/tests/unit/test_attitude_convergence.py @@ -26,7 +26,6 @@ from controller import HeliCyclicController, compute_rate_cmd from dynamics import RigidBodyDynamics from frames import build_orb_frame -from param_defaults import coll_rad_to_thrust from rotor_physics import resolve_i_spin_kgm2 from tests.unit._aero_probe import load_rotor, make_probe @@ -77,7 +76,6 @@ def _run_attitude_loop( F_grav_cancel = np.array([0.0, 0.0, -_MASS * 9.81]) history = [] - thrust_cmd = coll_rad_to_thrust(0.0) n = int(round(t_total / DT)) for i in range(n): s = dyn.state @@ -86,8 +84,8 @@ def _run_attitude_loop( omega_b = R.T @ omega_w bz_now = R[:, 2] rate_body = compute_rate_cmd(bz_now, body_z_eq, R, kp=KP_OUTER, kd=0.0) - tlon, tlat, _col = acro.step_from_thrust( - thrust_cmd=thrust_cmd, + tlon, tlat, _col = acro.step( + collective_cmd=0.0, rate_roll_sp =rate_body[0], rate_pitch_sp=rate_body[1], omega_body =omega_b, @@ -219,10 +217,9 @@ def test_acro_trim_feedforward_cancels_baseline_disturbance(): acro.set_trim(trim.tilt_lon, trim.tilt_lat) # Run a few steps with zero rate command so the servo settles to trim. omega_body_zero = np.zeros(3) - trim_thrust = coll_rad_to_thrust(-0.05) for _ in range(200): - tlon, tlat, _ = acro.step_from_thrust( - thrust_cmd=trim_thrust, + tlon, tlat, _ = acro.step( + collective_cmd=-0.05, rate_roll_sp =0.0, rate_pitch_sp=0.0, omega_body =omega_body_zero, diff --git a/simulation/tests/unit/test_controller.py b/simulation/tests/unit/test_controller.py index 2eb0f9d..6a26f73 100644 --- a/simulation/tests/unit/test_controller.py +++ b/simulation/tests/unit/test_controller.py @@ -87,6 +87,7 @@ def test_body_z_on_tether_no_rate(): assert rc[1] == 1500 assert rc[2] == 1500 assert rc[4] == 1500 + assert rc[8] == 2000 def test_body_z_on_tether_with_spin_only(): @@ -101,6 +102,7 @@ def test_body_z_on_tether_with_spin_only(): assert rc[1] == 1500 assert rc[2] == 1500 assert rc[4] == 1500 + assert rc[8] == 2000 def test_tilt_error_gives_non_neutral_command(): @@ -182,7 +184,7 @@ def test_rc_values_always_in_range(): 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): + for ch in (1, 2, 4, 8): assert 1000 <= rc[ch] <= 2000, f"Channel {ch} out of range: {rc[ch]}" @@ -202,6 +204,18 @@ def test_hub_too_close_to_anchor_gives_neutral(): assert rc[1] == 1500 assert rc[2] == 1500 assert rc[4] == 1500 + assert rc[8] == 2000 + + +def test_motor_interlock_always_2000(): + """Channel 8 (motor interlock) must always be 2000.""" + # Several different states + for omega in ([0., 0., 0.], [5., 5., 5.], [0., 0., 25.]): + for tilt_deg in (0, 10, 45): + R = _Ry(np.radians(tilt_deg)) @ _R_EQ + state = _state(_POS, R, omega) + rc = compute_rc_rates(state, _ANCHOR, _VEL_NED) + assert rc[8] == 2000 def test_kp_zero_no_proportional_correction(): @@ -232,6 +246,7 @@ def test_att_zero_attitude_zero_rates_neutral(): assert rc[1] == 1500 assert rc[2] == 1500 assert rc[4] == 1500 + assert rc[8] == 2000 def test_att_positive_roll_gives_negative_roll_command(): @@ -282,7 +297,7 @@ def test_att_output_always_in_range(): 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): + for ch in (1, 2, 4, 8): assert 1000 <= rc[ch] <= 2000 @@ -291,6 +306,14 @@ def test_att_large_error_saturates(): 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 + +def test_att_motor_interlock_always_2000(): + """Channel 8 is always 2000 regardless of inputs.""" + for roll in (-1.0, 0.0, 1.0): + rc = compute_rc_from_attitude(roll, 0.0, 0.0, 0.0, 0.0) + assert rc[8] == 2000 + + # --------------------------------------------------------------------------- # compute_rc_from_physical_attitude tests # --------------------------------------------------------------------------- @@ -334,6 +357,7 @@ def test_physical_att_aligned_gives_neutral(): # At perfect alignment cross(body_z, body_z_eq) = 0 → all neutral assert rc[1] == 1500 assert rc[2] == 1500 + assert rc[8] == 2000 def test_physical_att_output_in_range(): @@ -382,6 +406,7 @@ def test_physical_hold_neutral_when_no_pos(): rc = ctrl.send_correction(att, pos_ned=None, gcs=gcs) assert rc[1] == 1500 assert rc[2] == 1500 + assert rc[8] == 2000 def test_physical_hold_neutral_when_no_equilibrium(): @@ -395,6 +420,7 @@ def test_physical_hold_neutral_when_no_equilibrium(): rc = ctrl.send_correction(att, pos_ned=pos_ned, gcs=gcs) assert rc[1] == 1500 assert rc[2] == 1500 + assert rc[8] == 2000 def test_physical_hold_sends_correction_near_equilibrium(): @@ -413,9 +439,10 @@ def test_physical_hold_sends_correction_near_equilibrium(): 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 + # RC must be in valid range; motor interlock must be 2000 for ch in (1, 2, 3, 4): assert 1000 <= rc[ch] <= 2000 + assert rc[8] == 2000 assert len(gcs.sent) == 1 diff --git a/simulation/tests/unit/test_full_loop_stability.py b/simulation/tests/unit/test_full_loop_stability.py index 5034778..10e2c47 100644 --- a/simulation/tests/unit/test_full_loop_stability.py +++ b/simulation/tests/unit/test_full_loop_stability.py @@ -19,7 +19,6 @@ from controller import HeliCyclicController, compute_rate_cmd, compute_bz_tether from frames import build_orb_frame from physics_core import PhysicsCore -from param_defaults import coll_rad_to_thrust from rotor_physics import resolve_i_spin_kgm2 from tests.unit._aero_probe import load_rotor @@ -153,15 +152,14 @@ def _skew(w): [-w[1], w[0], 0 ]]) angle_hist = [] - thrust_cmd = coll_rad_to_thrust(col_fixed) n = int(round(t_total / DT)) for i in range(n): bz_now = R[:, 2] omega_b = R.T @ omega rate_sp = compute_rate_cmd(bz_now, bz_eq_fixed, R, kp=KP_OUTER, kd=0.0) - tlon, tlat, _ = acro.step_from_thrust( - thrust_cmd=thrust_cmd, + tlon, tlat, _ = acro.step( + collective_cmd=col_fixed, rate_roll_sp =rate_sp[0], rate_pitch_sp=rate_sp[1], omega_body =omega_b, @@ -341,7 +339,6 @@ def _run_with_constant_tether_force( alt_hist, v_hist, angle_hist, dist_hist = [], [], [], [] target_angle_hist, north_hist, tilt_hist, rate_hist, omega_hist = [], [], [], [], [] - thrust_cmd = coll_rad_to_thrust(COL_FIXED) n = int(round(t_total / DT)) for i in range(n): t_now = i * DT @@ -385,8 +382,8 @@ def _run_with_constant_tether_force( rate[0] += float(omega_body_corr[0]) rate[1] += float(omega_body_corr[1]) - tlon, tlat, _ = acro.step_from_thrust( - thrust_cmd=thrust_cmd, + tlon, tlat, _ = acro.step( + collective_cmd=COL_FIXED, rate_roll_sp=rate[0], rate_pitch_sp=rate[1], omega_body=omega_b, dt=DT, ) diff --git a/simulation/tests/unit/test_roll_sign_chain.py b/simulation/tests/unit/test_roll_sign_chain.py index ea874d7..4b91afe 100644 --- a/simulation/tests/unit/test_roll_sign_chain.py +++ b/simulation/tests/unit/test_roll_sign_chain.py @@ -80,7 +80,7 @@ def test_rate_command_sign_from_error(self): def test_cyclic_output_sign_from_rate_error(self): """Link 2: negative rate error should produce negative cyclic via PID. - HeliCyclicController.step_from_thrust() takes rate setpoint and body rate, + HeliCyclicController.step() takes rate setpoint and body rate, computes error, and outputs cyclic command. """ heli = HeliCyclicController(_ROTOR) @@ -100,8 +100,8 @@ def test_cyclic_output_sign_from_rate_error(self): tilt_lat_out = None for step in range(100): - tilt_lon_out, tilt_lat_out, _ = heli.step_from_thrust( - float(_IC.eq_thrust), rate_sp_roll, rate_sp_pitch, omega_body, dt, + tilt_lon_out, tilt_lat_out, _ = heli.step( + _IC.coll_eq_rad, rate_sp_roll, rate_sp_pitch, omega_body, dt, collective_norm=0.5 ) @@ -217,8 +217,8 @@ def test_positive_roll_error_corrects_to_left(self): # Run controller to generate cyclic (should be negative to roll left) dt = 1.0 / 400.0 for _ in range(50): - tilt_lon_cmd, tilt_lat_cmd, _ = heli.step_from_thrust( - float(_IC.eq_thrust), rate_sp, 0.0, omega_body_right_roll, dt, + tilt_lon_cmd, tilt_lat_cmd, _ = heli.step( + _IC.coll_eq_rad, rate_sp, 0.0, omega_body_right_roll, dt, collective_norm=0.5 )