Reintroduce GUIDED-only flight-logic feature set with CH4-override fix

AGENTS.md

@@ -238,6 +238,20 @@ the tail of each failure, including the assertion error). The full output is als
 saved — read it directly if needed:
     `Get-Content simulation/logs/<test_name>/worker.log | Select-String "ERROR|CRITICAL|Traceback|assert|FAIL" | Select-Object -First 30`
 
+Long-running test commands (agent-critical, efficiency):
+- Unit and simtest runs can take 1-5+ minutes. Do NOT pipe them through `| tail -N` when
+  running in a mode that may background the command — a slow/idle command piped through
+  `tail` produces no output until the pipeline's stdout closes, so a background poll via
+  `get_terminal_output` just returns the same stale snapshot every time (wastes calls,
+  looks like a hang). Run the bare command first; only pipe through `tail`/`grep` after
+  confirming the run is fast enough to complete synchronously, or redirect to a file
+  (`... > /tmp/out.log 2>&1`) and read/grep the file instead.
+- Once a command has been moved to background, do not repeatedly call `get_terminal_output`
+  in a tight loop — it will not return new content until the process actually produces more
+  output or exits. Wait for the automatic completion notification instead of polling.
+- Prefer `bash test.sh stack -n 1 -k <test_name>` (single test) while iterating; only widen
+  to `-n 4`/full suite once the targeted test is confirmed passing, to keep turnaround short.
+
 ## Visualization
 
 Flight telemetry (pumping, steady, passive SITL runs):

simulation/controller.py

@@ -1,26 +1,19 @@
 """
-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
+controller.py — RAWES guidance/control helpers.
+
+Legacy RC-override helpers are retained for historical tests only; runtime
+flight stack control is GUIDED-only.
 """
 
 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, INTERLOCK_PWM_HIGH
+from servo_pwm  import SWASH_PWM_NEUTRAL, SWASH_PWM_RANGE
 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
 
 
@@ -57,8 +50,8 @@ def compute_rc_rates(
     Returns
     -------
     dict
-        RC channel overrides: {1: pwm, 2: pwm, 4: pwm, 8: 2000}.
-        Channel 1 = roll rate, 2 = pitch rate, 4 = yaw rate, 8 = motor interlock.
+        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)
@@ -70,7 +63,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, 8: INTERLOCK_PWM_HIGH}
+        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
@@ -106,7 +99,6 @@ 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
     }
 
 
@@ -236,7 +228,7 @@ def compute_rc_from_attitude(
 
     Returns
     -------
-    dict : {1: pwm, 2: pwm, 4: pwm, 8: 2000}
+    dict : {1: pwm, 2: pwm, 4: pwm}
     """
     max_rate = np.radians(rate_max_deg)
 
@@ -251,7 +243,6 @@ def _pwm(w: float) -> int:
         1: _pwm(cmd_roll),
         2: _pwm(cmd_pitch),
         4: _pwm(cmd_yaw),
-        8: INTERLOCK_PWM_HIGH,
     }
 
 
@@ -292,7 +283,7 @@ def compute_rc_from_physical_attitude(
 
     Returns
     -------
-    dict : {1: pwm, 2: pwm, 4: pwm, 8: 2000}
+    dict : {1: pwm, 2: pwm, 4: pwm}
     """
     max_rate = np.radians(rate_max_deg)
 
@@ -317,7 +308,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, 8: INTERLOCK_PWM_HIGH}
+        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.
@@ -341,7 +332,6 @@ 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,
     }
 
 
@@ -353,7 +343,7 @@ def _pwm(w: float) -> int:
 
 class PhysicalHoldController:
     """
-    ACRO hold controller for physical sensor mode.
+    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)
@@ -395,18 +385,17 @@ def send_correction(
         gcs,
     ) -> dict:
         """
-        Compute and send RC override.  Returns the rc dict sent (for logging).
+        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.
 
-        Sends neutral sticks when equilibrium has not been set yet.
+        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, 8: INTERLOCK_PWM_HIGH}
+                3: SWASH_PWM_NEUTRAL, 4: SWASH_PWM_NEUTRAL}
 
         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
@@ -420,7 +409,6 @@ 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
 
@@ -798,6 +786,49 @@ def compute_bz_altitude_hold(
     return raw / np.linalg.norm(raw)
 
 
+class YawTrimObserver:
+    """Python port of rawes.lua's yaw trim observer (anti-rotation feedforward).
+
+    Mirrors ``yaw_trim_step(dt, u, psi_dot)`` in ``simulation/scripts/rawes.lua``
+    exactly (see ``test_yaw_trim_lua.py`` for the Lua-side unit tests and
+    ``test_yaw_trim_parity.py`` for the cross-check). Keep these two in sync —
+    per AGENTS.md, any change to the Lua formula must be mirrored here in the
+    same commit.
+
+    Reads back the total applied Motor4/SERVO9 throttle ``u`` [0, 1] and the
+    measured body yaw rate ``psi_dot`` [rad/s], computes the equilibrium trim
+    that would hold psi_dot=0 (``u - psi_dot / YFF_A``), and low-passes it
+    into a running trim estimate (clamped to [0, YFF_MAX]).
+    """
+
+    #: Motor throttle→yaw-rate slope, bench-calibrated: 0.504 RPM/µs over the
+    #: SERVO9 PWM span (1000 µs) → rad/s per unit throttle. Matches rawes.lua's
+    #: default (RAWES_YAW_SLP=0 → this bench default).
+    YFF_A_DEFAULT: float = 0.504 * 1000.0 * (2.0 * math.pi / 60.0)
+    YFF_MAX_DEFAULT: float = 0.7
+    YFF_TAU_DEFAULT: float = 0.3
+
+    def __init__(self, yaw_slope_rpm_per_us: float = 0.0,
+                 yff_max: float = YFF_MAX_DEFAULT,
+                 yff_tau: float = YFF_TAU_DEFAULT):
+        slope = float(yaw_slope_rpm_per_us) if yaw_slope_rpm_per_us > 0 else 0.504
+        self.yff_a = slope * 1000.0 * (2.0 * math.pi / 60.0)
+        self.yff_max = float(yff_max)
+        self.yff_tau = float(yff_tau)
+        self.trim = 0.0
+
+    def reset(self) -> None:
+        self.trim = 0.0
+
+    def step(self, dt: float, u: float, psi_dot: float) -> float:
+        trim_target = u - psi_dot / self.yff_a
+        trim_target = max(0.0, min(self.yff_max, trim_target))
+        alpha = dt / (self.yff_tau + dt)
+        self.trim += alpha * (trim_target - self.trim)
+        self.trim = max(0.0, min(self.yff_max, self.trim))
+        return self.trim
+
+
 class AltitudeHoldController:
     """
     Elevation-holding cyclic controller.
@@ -876,8 +907,8 @@ class ElevationHoldController:
         ElevationHoldController  →  (rate_roll_sp, rate_pitch_sp)
         HeliCyclicController       →  (tilt_lon, tilt_lat)
 
-    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.
+    This mirrors the rawes.lua outer-loop structure, but runtime stack control
+    now uses GUIDED setpoints rather than RC override transport.
 
     Usage
     -----
@@ -1101,11 +1132,16 @@ 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(
-            collective_rad, rate_roll_sp, rate_pitch_sp, omega_body, dt,
+        tilt_lon, tilt_lat, col_actual = acro.step_from_thrust(
+            thrust_cmd,
+            rate_roll_sp, rate_pitch_sp, omega_body, dt,
         )
 
     Parameters mapped to ArduPilot:
@@ -1152,6 +1188,7 @@ 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
 
@@ -1167,7 +1204,28 @@ 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 step(
+    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))
+
+    @property
+    def yaw_cmd(self) -> float:
+        """Last tail-rotor/yaw-axis command from the rate PID (arduloop
+        HeliRateOutput.yaw_cmd, [-1, 1]).  Callers wanting a GB4008 motor
+        throttle should clamp to [0, 1] (the physical motor cannot reverse;
+        this mirrors ArduPilot's SERVO_TRIM=SERVO_MIN convention for the
+        one-directional Motor4 output)."""
+        return float(self._ctrl.out.yaw_cmd)
+
+    def _step_collective(
         self,
         collective_cmd: float,
         rate_roll_sp  : float,
@@ -1176,7 +1234,7 @@ def step(
         dt            : float,
         collective_norm: float = 0.0,
     ) -> "tuple[float, float, float]":
-        """Advance one timestep.
+        """Advance one timestep from physical collective [rad].
 
         Returns ``(tilt_lon, tilt_lat, col_actual)``.  All three channels
         pass through the SwashplateServoModel so collective and cyclic
@@ -1205,6 +1263,34 @@ def step(
             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
 # ---------------------------------------------------------------------------