Fix SITL vertical anchor altitude bug; add EK3 baro/GPS altitude blending

AGENTS.md

@@ -146,18 +146,31 @@ Ground→Lua NAMED_VALUE_FLOAT interface (not AP params):
 | RAWES_TEN  | Target/feed-forward tether tension [N]                           |
 | RAWES_SUB  | Substate (0=hold,1=reel_out,2=transition,3=reel_in,4=transition_back) |
 | RAWES_ARM  | Optional disarm timer [ms until forced disarm]                   |
-| RAWES_ANN  | Anchor North from EKF origin [m]                                 |
-| RAWES_ANE  | Anchor East from EKF origin [m]                                  |
-| RAWES_AND  | Anchor Down from EKF origin [m]                                  |
 | RAWES_THR  | IC thrust [0..1] (passive seed; committed atomically with RIC/PIC) |
 | RAWES_RIC  | IC roll [rad]                                                    |
 | RAWES_PIC  | IC pitch [rad]                                                   |
 | RAWES_YIC  | Optional fixed yaw target [rad] for MODE_PASSIVE                 |
 
+Ground→Lua NAMED_VALUE_INT interface (one-shot anchor location, sent once post-arm):
+
+| NVI key    | Purpose                                                          |
+|---|---|
+| RAWES_LAT  | Anchor latitude  [deg * 1e7]                                     |
+| RAWES_LON  | Anchor longitude [deg * 1e7]                                     |
+| RAWES_AAL  | Anchor altitude  [cm, AMSL]                                      |
+
+Sent as NAMED_VALUE_INT (not FLOAT) for ArduPilot's Location int32 precision
+(~1 cm) end-to-end. Lua resolves this to an EKF-local NED anchor offset on
+board via `Location:get_vector_from_origin_NEU_m()` (see `_try_resolve_anchor()`
+in `rawes.lua`); MODE_STEADY does not initialise altitude hold until all three
+ints have arrived AND that resolution has succeeded. Python helper:
+`rawes_modes.send_anchor_ned(sim_or_gcs, dn_m, de_m, dd_m)`.
+
 Set RAWES_MODE per-test; other RAWES_* are in rawes_common_defaults.parm.
 
 RAWES mode -> vehicle control API (current `rawes.lua` behavior):
 
+
 | RAWES_MODE | Mode | Vehicle API used |
 |---|---|---|
 | 0 | none | none |

design/flight_stack.md

@@ -344,21 +344,39 @@ All other flight tunables (anchor position, slew rate, cyclic gains) are deliver
 | RAWES_ARM | ms | Arm vehicle + start disarm countdown of `ms` milliseconds. Re-send refreshes timer. |
 | RAWES_SUB | 0–4 | Pumping substate or landing trigger (LAND_FINAL_DROP=1) |
 | RAWES_ALT | m | Target altitude above anchor. Lua rate-limits elevation at RAWES_SLW rad/s. |
-| RAWES_ANN | m | Anchor North from EKF origin. MODE_STEADY does not initialise altitude hold until ANN/ANE/AND have all been received. |
-| RAWES_ANE | m | Anchor East from EKF origin. |
-| RAWES_AND | m | Anchor Down from EKF origin (positive downward in NED). Set to `−initial_state["pos"][2]` (NED Z negated). |
 | RAWES_TEN | N | **Commanded** tether tension (the winch setpoint, broadcast to the AP). Feedforward into the orientation force balance in mode 1 (incl. the pumping schedule). Never the measured/load-cell tension. Ramped by RAWES_TRP. |
 | RAWES_RIC | rad | IC roll — part of the atomic passive IC seed (`RAWES_RIC`/`RAWES_PIC`/`RAWES_THR`). MODE_PASSIVE commands it as the GUIDED roll angle target. |
 | RAWES_PIC | rad | IC pitch — part of the atomic IC seed. MODE_PASSIVE commands it as the GUIDED pitch angle target. |
 | RAWES_THR | [0..1] | IC thrust — part of the atomic IC seed. MODE_PASSIVE maps it directly to GUIDED throttle to preserve rotor RPM during kinematic. |
 
+**Named int inputs (ground → Lua, via `gcs.send_named_int`, one-shot anchor location):**
+
+| Name | Value | Purpose |
+|---|---|---|
+| RAWES_LAT | deg × 1e7 | Anchor latitude. |
+| RAWES_LON | deg × 1e7 | Anchor longitude. |
+| RAWES_AAL | cm, AMSL | Anchor altitude. |
+
+Sent as NAMED_VALUE_INT (not FLOAT) to preserve ArduPilot's own Location int32
+precision (~1 cm) end-to-end — a float32 NVF would quantize latitude to
+~0.5–1 m. Lua converts this absolute location into the EKF-local NED anchor
+offset on board via `Location:get_vector_from_origin_NEU_m()`, which returns
+`nil` until the EKF origin is set, so all three ints must arrive AND the
+onboard conversion must succeed at least once before MODE_STEADY initialises
+altitude hold (see `_try_resolve_anchor()` in rawes.lua).
+
 **MAVLink rx queue:** `mavlink:init(queue_size, num_msgs)` is called as
 `(20, 10)` at module load.  The first arg is the per-tick rx buffer depth;
 with `1` (the prior default) multiple back-to-back NAMED_VALUE_FLOATs sent
 by the ground get dropped — only the first survives until the next
 update() drains it.  20 is safe for the typical ~5 NVFs/tick burst.
+Both NAMED_VALUE_FLOAT (msgid 251) and NAMED_VALUE_INT (msgid 252) are
+registered and share this one queue; the drain loop peeks the 3-byte msgid
+at byte offset 10 (`string.unpack("<I3", raw, 10)`) to dispatch each message.
 
-`_nv_floats` dict resets to `{}` on every mode change.
+`_nv_floats` dict resets to `{}` on every mode change. `_nv_ints` (anchor) is
+NOT cleared on mode change — the anchor is a static, one-shot location that
+persists for the whole flight once resolved.
 
 **Key physical constants:**
 
@@ -664,7 +682,8 @@ bulk DC trim so AP's rate loop mainly acts as a fast disturbance-rejection assis
 | RAWES_MODE | 0 | Mode selector (script-generated param registered by rawes.lua). Set via GCS or parm file. |
 | RAWES_YAW_SLP | 0 | Yaw motor slope override [RPM/µs]. 0 → bench default 0.504. Set from bench calibration. |
 
-Anchor position (RAWES_ANN/ANE/AND) and slew rate (RAWES_SLW) are NVFs sent post-arm by the ground station, not boot-time params.
+Anchor location (RAWES_LAT/LON/AAL, NAMED_VALUE_INT) and slew rate (RAWES_SLW,
+NAMED_VALUE_FLOAT) are sent post-arm by the ground station, not boot-time params.
 
 **SCR_ENABLE bootstrap:** After EEPROM wipe, Lua only starts if SCR_ENABLE=1 is already in
 EEPROM. The Lua flight fixture sets it via MAVLink post-arm (persists for future boots).

design/sitl_flight_timeline.md

@@ -33,6 +33,26 @@ Rules:
 - If `kinematic_exit` is missing, treat the run as invalid for flight-phase
   root-cause analysis until telemetry/event logging is fixed.
 
+## Field naming contract (docs vs code)
+
+Use the exact field names below when writing analysis notes/scripts.
+
+Telemetry CSV (`simulation/logs/<test>/telemetry.csv`):
+- `t_sim`, `note`, `phase`, `omega_rotor`
+- `mav_att_roll_deg`, `mav_att_pitch_deg`, `mav_att_yaw_deg`
+- `mav_att_target_roll_deg`, `mav_att_target_pitch_deg`, `mav_att_target_yaw_deg`
+- `ekf_pos_x`, `ekf_pos_y`, `ekf_pos_z`
+
+MAVLink JSONL (`simulation/logs/<test>/mavlink.jsonl`):
+- `GPS_RAW_INT.fix_type`, `GPS_RAW_INT.satellites_visible`
+- `EKF_STATUS_REPORT.flags`
+- `HEARTBEAT.custom_mode`, `HEARTBEAT.base_mode` (armed bit lives in `base_mode`)
+- `STATUSTEXT.text`
+
+Not valid in current telemetry CSV schema:
+- `mav_fix_type`, `mav_satellites_visible`, `mav_ekf_flags`, `mav_mode`, `mav_armed`
+- `roll_rad`, `pitch_rad`
+
 ## Standard phase timeline (IC-start stacks)
 
 This is the expected sequence for IC-start SITL flight stacks.
@@ -48,25 +68,54 @@ Notes:
 - Phase duration differences do not change the anchor rule: always compare using
   `t_rel` based on `kinematic_exit`.
 
-## Pre-release event matrix (major columns)
-
-Use this table format for steady SITL timeline analysis up to kinematic exit.
-Each row is a milestone, and each major subsystem has its own column.
-
-| Time marker | Kinematic / trajectory | GPS | EKF | Vehicle mode / arm | RAWES / Lua | Analysis note |
-|---|---|---|---|---|---|---|
-| `t_sim = 0`, `t_rel < 0` | Kinematic startup begins | No guaranteed fix yet | Startup alignment state | Disarmed | `RAWES_MODE=0` expected | Run initialization |
-| Early hold (`t_rel << 0`) | Trapezoid accel/cruise/decel progressing | `GPS_RAW_INT` transitions 0 -> 1 -> 6 | `EKF_STATUS` transitions toward aiding-ready flags | GUIDED_NOGPS setup starts, then arm pending | Passive pipeline preparing IC seed | Confirm bring-up order |
-| Mid hold (`t_rel < 0`) | Kinematic still active | Fix 6 expected to be stable | Origin set, then GPS aiding active | Armed, GUIDED_NOGPS stable | `RAWES_MODE=3` (passive), IC ready status | Verify pre-release stability |
-| Late hold (`t_rel -> 0-`) | Near release target state (at IC pose/vel) | Fix remains stable | Aiding-ready flags stable | Armed/mode unchanged | Passive hold commands continue | Ensure no last-second state jumps |
-| `note == kinematic_exit`, `t_rel = 0` | Kinematic handoff ends | Should still be locked | Should still be aiding | Armed/mode continuous across handoff | Steady logic eligible to take control | Start post-release comparisons |
-
-Minimum fields to populate per row:
-- Time: `t_sim`, `t_rel`.
-- GPS: `fix_type`, `satellites_visible`.
-- EKF: `EKF_STATUS_REPORT.flags` and key STATUSTEXT milestones.
-- Mode/arm: HEARTBEAT `custom_mode` and armed bit.
-- RAWES/Lua: mode transitions and passive/steady status text.
+## Canonical event matrix (reference run)
+
+Reference artifacts:
+- `simulation/logs/test_lua_flight_steady_sitl/telemetry.csv`
+- `simulation/logs/test_lua_flight_steady_sitl/mavlink.jsonl`
+- `simulation/logs/test_lua_flight_steady_sitl/events.jsonl`
+
+Anchor:
+- `t_kin_exit = 60.003 s` from telemetry/event marker `note == "kinematic_exit"`
+
+Use this as the compact reference table for this run.
+
+| `t_start` (s) | Kind | Milestone | Evidence |
+|---:|---|---|---|
+| 0.000 | observed | Mediator startup and 60 s kinematic profile start | `events.jsonl: startup`, `events.jsonl: kinematic_config.total_s=60` |
+| 8.860-8.960 | observed | EKF yaw aligned and GPS reaches 3D fix (`fix_type=6`) | `STATUSTEXT "EKF3 IMU0 yaw aligned"`, `GPS_RAW_INT.fix_type=6` |
+| 14.000 (nominal) | scheduled | Arm gate reached in fixture | `conftest.py _arm_at_sim_s=14.0` |
+| 14.000+ (nominal) | scheduled | PASSIVE IC seeds scheduled, including tilt targets (`RAWES_RIC/PIC`) | `conftest.py send_named_float RAWES_THR/RIC/PIC`, `RAWES_MODE=3` |
+| 24.077 | observed | EKF starts GPS aiding | `STATUSTEXT "EKF3 IMU0 is using GPS"` |
+| 26.748 | observed | Tilt target first visible at AP interface | `telemetry.csv: |mav_att_target_pitch_deg| >= 1` |
+| 26.848 | observed | Physical tilt response begins (real attitude moves) | `telemetry.csv: |mav_att_pitch_deg| >= 5` |
+| 27.178 | observed | Large tilt achieved | `telemetry.csv: |mav_att_pitch_deg| >= 40` |
+| 60.003 | observed | Kinematic handoff (`t_kin_exit`) | `telemetry.csv/events.jsonl: note=="kinematic_exit"` |
+| 60.503 (nominal) | scheduled | Test schedules steady takeover (`RAWES_MODE=1` then `RAWES_ALT`) | `test_lua_flight_steady_sitl.py`, `_PASSIVE_SETTLE_S=0.5` |
+| 61.060 | observed | Steady capture confirmed | `STATUSTEXT "RAWES steady: captured"` |
+
+Quick summary:
+- Horizontal start at `t=0`; no IC tilt yet.
+- IC tilt targets are scheduled right after arm (~`t=14`).
+- Targets appear on AP side around `t=26.748`, and physical tilt follows shortly (`t=26.848`).
+- Handoff is at `t=60.003`; steady capture follows near `t=61.060`.
+
+Rotor state reference for this run:
+- `omega_rotor` is already ~`38.10 rad/s` from the first telemetry sample and at
+  `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.
+- 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.
+
+Tilt command-to-response flow (IC-start path):
+1. Fixture schedules IC tilt targets (`RAWES_RIC/PIC`) right after arm (`t_start~14s`).
+2. Lua `MODE_PASSIVE` emits GUIDED angle targets once IC seed is complete and `guided_ok` is true.
+3. During kinematic hold with `kinematic_aero_mode="nul"`, the physics side realizes tilt by
+   integrating body rates (`omega_body = gain * tilt`) until measured attitude matches target.
 
 ## Required analysis convention
 

simulation/analysis/analyse_run.py

@@ -52,6 +52,13 @@
 from flight_log import FlightLog  # noqa: E402
 
 
+_KINEMATIC_PHASES = frozenset({"waiting_ekf", "positioning"})
+
+
+def _is_kinematic_row(row: TelRow) -> bool:
+    return str(getattr(row, "phase", "") or "") in _KINEMATIC_PHASES
+
+
 # ---------------------------------------------------------------------------
 # Public API — importable from tests
 # ---------------------------------------------------------------------------
@@ -77,7 +84,7 @@ def compute_steady_metrics(
 ) -> SteadyMetrics:
     """Compute stability metrics from TelRow list (physics ground truth).
 
-    Considers only free-flight rows (damp_alpha == 0).  The stable window is
+    Considers only free-flight rows (non-startup phase). The stable window is
     measured in simulation seconds so it is independent of wall-clock timing.
 
     Args:
@@ -87,7 +94,7 @@ def compute_steady_metrics(
     Returns:
         SteadyMetrics with physics-based pass/fail values.
     """
-    free_rows = [r for r in tel_rows if r.damp_alpha == 0.0]
+    free_rows = [r for r in tel_rows if not _is_kinematic_row(r)]
     if not free_rows:
         return SteadyMetrics(
             min_phys_alt=0.0, max_stable_s=0.0, floor_hits=0,
@@ -448,8 +455,8 @@ def _print_mediator(r: RunReport, fl: "FlightLog | None" = None) -> None:
         k_ang_str = f"  k_ang={r.k_ang:.1f}" if r.k_ang is not None else ""
         print(f"  damping     : T={r.damp_T:.0f}s{k_ang_str}")
 
-    damp_rows = [row for row in rows if row.damp_alpha > 0.0]
-    free_rows  = [row for row in rows if row.damp_alpha == 0.0]
+    damp_rows = [row for row in rows if _is_kinematic_row(row)]
+    free_rows  = [row for row in rows if not _is_kinematic_row(row)]
 
     # Damping phase
     if damp_rows:
@@ -666,8 +673,7 @@ def _print_mediator(r: RunReport, fl: "FlightLog | None" = None) -> None:
                     print("    diagnosis              : spin decay occurs primarily BEFORE arm/NVF hold capture,")
                     print("                               then stabilizes post-arm. Investigate pre-arm collective/throttle state.")
 
-    # Kinematic exit (transition) — row stamped note="kinematic_exit" in CSV;
-    # fall back to first free-flight row (damp_alpha==0) for older logs.
+    # Kinematic exit (transition) — row stamped note="kinematic_exit" in CSV.
     _tr_rows = [r for r in rows if r.note == "kinematic_exit"]
     tr = _tr_rows[0] if _tr_rows else (free_rows[0] if free_rows else None)
     if tr is not None:
@@ -1145,9 +1151,9 @@ def _yaw_gap_deg(row: TelRow) -> float:
 
     # Group into three logical segments: kinematic, free-flight, pumping phases
     segments = [
-        ("kinematic (damp_alpha>0)",  [r for r in rows if r.damp_alpha > 0.0]),
-        ("free-flight (damp_alpha=0, no pump phase)",
-         [r for r in rows if r.damp_alpha == 0.0
+        ("kinematic (phase is waiting_ekf/positioning)",  [r for r in rows if _is_kinematic_row(r)]),
+        ("free-flight (phase not startup, no pump phase)",
+         [r for r in rows if not _is_kinematic_row(r)
                           and r.phase not in ("reel-out", "reel-in", "descent", "final_drop")]),
         ("pumping (reel-out/reel-in)", [r for r in rows if r.phase in ("reel-out", "reel-in")]),
         ("landing (descent/final_drop)", [r for r in rows if r.phase in ("descent", "final_drop")]),
@@ -1246,9 +1252,9 @@ def _print_sensor_consistency(r: RunReport) -> None:
 
     segments = [
         ("kinematic",
-         [row for row in rows if row.damp_alpha > 0.0]),
+         [row for row in rows if _is_kinematic_row(row)]),
         ("free-flight (no pump/landing)",
-         [row for row in rows if row.damp_alpha == 0.0
+         [row for row in rows if not _is_kinematic_row(row)
           and row.phase not in ("reel-out", "reel-in", "descent", "final_drop")]),
         ("pumping (reel-out/reel-in)",
          [row for row in rows if row.phase in ("reel-out", "reel-in")]),
@@ -1575,7 +1581,7 @@ def _print_release_window(
     if tr_rows:
         t_exit = tr_rows[0].t_sim
     else:
-        free_rows = [row for row in rows if row.damp_alpha == 0.0]
+        free_rows = [row for row in rows if not _is_kinematic_row(row)]
         if not free_rows:
             return
         t_exit = free_rows[0].t_sim