From 6aae00149bfd2e26324a7fec12e760fc7f477da2 Mon Sep 17 00:00:00 2001 From: Kristof Date: Sat, 18 Jul 2026 09:13:10 +0200 Subject: [PATCH] Fix SITL vertical anchor altitude bug; add EK3 baro/GPS blending The rawes.lua steady-flight controller crashed with "bad argument #2 to 'set_target_angle_and_rate_and_throttle' (out of range)" because the anchor altitude sent via NAMED_VALUE_INT (RAWES_AAL) did not account for ArduPilot's baro-zero-at-boot behavior: whatever altitude the vehicle physically occupies when the EKF's local origin is established becomes its vertical "zero", so a SITL test that boots kinematically-locked above the anchor must offset the sent anchor altitude by -home_alt_m. This drove the elevation-angle computation (asin(-rz/tlen)) drastically wrong, saturating the cyclic pitch command until it exceeded ArduPilot's valid range. Changes: - simulation/tests/sitl/flight/conftest.py: fix _send_anchor_location() to send anchor_alt_m = HOME_ALT_M - ctx.home_alt_m instead of the raw home altitude; general cleanup (remove unused numpy import, fix stale log message, blank-line/indentation nits). - simulation/scripts/rawes.lua: add throttled (~1Hz) pre-capture diagnostic logging of pos_ned/anchor geometry, and expand the "captured" STATUSTEXT with explicit geometry values, as a permanent diagnostic tool for future geometry-capture bugs. - simulation/tests/sitl/rawes_common_defaults.parm: set EK3_OGN_HGT_MASK=5 so GPS altitude progressively corrects baro-drift bias in the EKF's local vertical position over long flights (bits 0+2: correct when Baro is the active height source, apply to local position) -- mitigates weather-driven QNH drift over long-duration real-world pumping operation. - Other files (gcs.py, mediator.py, mock_ardupilot.lua, rawes_lua_harness.py, rawes_modes.py, stack_infra.py, stack_utils.py, analysis/*.py, various tests, design docs, AGENTS.md): carry the broader anchor-location-over-GPS migration this fix depends on (NAMED_VALUE_INT-based RAWES_LAT/LON/AAL anchor transmission, replacing the earlier fixed-NED-offset anchor assumption). Validated: bash test.sh stack -n 1 -k test_lua_flight_steady_sitl PASSED (stable=128s, max_activity=423 PWM), both before and after the EK3_OGN_HGT_MASK addition. --- AGENTS.md | 19 ++- design/flight_stack.md | 29 +++- design/sitl_flight_timeline.md | 87 +++++++--- simulation/analysis/analyse_run.py | 30 ++-- simulation/analysis/compare_runs.py | 35 ++-- simulation/analysis/flight_log.py | 8 +- simulation/gcs.py | 25 +++ simulation/mediator.py | 28 +++- simulation/mock_ardupilot.lua | 62 ++++++- simulation/rawes_lua_harness.py | 53 ++++-- simulation/rawes_modes.py | 73 +++++++-- simulation/scripts/rawes.lua | 151 +++++++++++++----- .../tests/simtests/test_ground_liftoff.py | 6 +- .../tests/simtests/test_pump_cycle_lua.py | 11 +- .../tests/simtests/test_steady_flight_lua.py | 11 +- simulation/tests/sitl/flight/conftest.py | 145 ++++++++++------- .../flight/test_lua_flight_steady_sitl.py | 2 +- .../tests/sitl/rawes_common_defaults.parm | 11 ++ simulation/tests/sitl/stack_infra.py | 25 ++- simulation/tests/sitl/stack_utils.py | 25 ++- simulation/tests/unit/test_math_lua.py | 9 +- 21 files changed, 637 insertions(+), 208 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 357cdfa..f63db95 100644 --- a/AGENTS.md +++ b/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 | diff --git a/design/flight_stack.md b/design/flight_stack.md index d01278e..5a262a9 100644 --- a/design/flight_stack.md +++ b/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("/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//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 diff --git a/simulation/analysis/analyse_run.py b/simulation/analysis/analyse_run.py index 0d63053..6f2b9da 100644 --- a/simulation/analysis/analyse_run.py +++ b/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 diff --git a/simulation/analysis/compare_runs.py b/simulation/analysis/compare_runs.py index 7fc5e24..431b37d 100644 --- a/simulation/analysis/compare_runs.py +++ b/simulation/analysis/compare_runs.py @@ -15,7 +15,7 @@ pumping_lua_test_pumping_cycle_lua \\ --plot -The script aligns both runs on kinematic exit time (first row with damp_alpha==0), +The script aligns both runs on kinematic exit time (first row after startup phases end), then prints a side-by-side comparison of key metrics in 5 s windows. Metrics compared: @@ -58,6 +58,11 @@ _COL_DIFF_WARN = 0.02 # rad _REACTION_PRE_S = 1.0 _REACTION_HORIZON_S = 3.0 +_KINEMATIC_PHASES = frozenset({"waiting_ekf", "positioning"}) + + +def _is_kinematic_row(r: TelRow) -> bool: + return str(r.phase or "") in _KINEMATIC_PHASES # --------------------------------------------------------------------------- @@ -65,10 +70,10 @@ # --------------------------------------------------------------------------- def _find_kin_exit_t(rows: list[TelRow]) -> Optional[float]: - """Return t_sim of first row with damp_alpha == 0 (kinematic exit).""" + """Return t_sim of first row where startup phase transitions to free-flight.""" prev = None for r in rows: - if r.damp_alpha == 0.0 and (prev is None or prev.damp_alpha > 0.0): + if (not _is_kinematic_row(r)) and (prev is None or _is_kinematic_row(prev)): return r.t_sim prev = r return None @@ -146,7 +151,7 @@ def _bucket_control(rows: list[TelRow], t_ref: float, window_s: float) -> dict[i out: dict[int, dict] = {} by_key: dict[int, list[TelRow]] = {} for r in rows: - if r.damp_alpha != 0.0: + if _is_kinematic_row(r): continue dt = r.t_sim - t_ref if dt < 0.0: @@ -293,7 +298,7 @@ def _pct(num: int, den: int) -> float: def _rows_in_rel_window(rows: list[TelRow], t_kin: float, t0: float, t1: float) -> list[TelRow]: - return [r for r in rows if r.damp_alpha == 0.0 and t0 <= (r.t_sim - t_kin) < t1] + return [r for r in rows if (not _is_kinematic_row(r)) and t0 <= (r.t_sim - t_kin) < t1] def _mean_attr(rows: list[TelRow], attr: str) -> Optional[float]: @@ -540,7 +545,7 @@ def _v(m: dict, k: str, fmt: str = ".2f") -> str: # ── Per-run summary stats ────────────────────────────────────────────────── def _run_summary(rows: list[TelRow], label: str, t_kin: float) -> None: - free_rows = [r for r in rows if r.damp_alpha == 0.0] + free_rows = [r for r in rows if not _is_kinematic_row(r)] if not free_rows: print(f"\n{label}: no free-flight rows") return @@ -593,20 +598,20 @@ def _plot(rows_a: list[TelRow], rows_b: list[TelRow], return def _ts(rows: list[TelRow], t_kin: float) -> list[float]: - return [r.t_sim - t_kin for r in rows if r.damp_alpha == 0.0] + return [r.t_sim - t_kin for r in rows if not _is_kinematic_row(r)] ta = _ts(rows_a, t_kin_a) tb = _ts(rows_b, t_kin_b) def _get(rows: list[TelRow], attr: str) -> list[float]: - return [getattr(r, attr) for r in rows if r.damp_alpha == 0.0] - - alt_a = [-r.pos_z for r in rows_a if r.damp_alpha == 0.0] - alt_b = [-r.pos_z for r in rows_b if r.damp_alpha == 0.0] - orb_a = [math.sqrt(r.pos_x**2 + r.pos_y**2) for r in rows_a if r.damp_alpha == 0.0] - orb_b = [math.sqrt(r.pos_x**2 + r.pos_y**2) for r in rows_b if r.damp_alpha == 0.0] - yaw_a = [math.degrees(r.orb_yaw_rad - r.rpy_yaw) for r in rows_a if r.damp_alpha == 0.0] - yaw_b = [math.degrees(r.orb_yaw_rad - r.rpy_yaw) for r in rows_b if r.damp_alpha == 0.0] + return [getattr(r, attr) for r in rows if not _is_kinematic_row(r)] + + alt_a = [-r.pos_z for r in rows_a if not _is_kinematic_row(r)] + alt_b = [-r.pos_z for r in rows_b if not _is_kinematic_row(r)] + orb_a = [math.sqrt(r.pos_x**2 + r.pos_y**2) for r in rows_a if not _is_kinematic_row(r)] + orb_b = [math.sqrt(r.pos_x**2 + r.pos_y**2) for r in rows_b if not _is_kinematic_row(r)] + yaw_a = [math.degrees(r.orb_yaw_rad - r.rpy_yaw) for r in rows_a if not _is_kinematic_row(r)] + yaw_b = [math.degrees(r.orb_yaw_rad - r.rpy_yaw) for r in rows_b if not _is_kinematic_row(r)] col_a = _get(rows_a, "collective_rad") col_b = _get(rows_b, "collective_rad") ten_a = _get(rows_a, "tether_tension") diff --git a/simulation/analysis/flight_log.py b/simulation/analysis/flight_log.py index 89e0db1..d50e969 100644 --- a/simulation/analysis/flight_log.py +++ b/simulation/analysis/flight_log.py @@ -442,12 +442,6 @@ def buckets(self, bucket_s: float = 1.0) -> list: kin_exit_rows = [r for r in self.tel_rows if r.note == "kinematic_exit"] if kin_exit_rows: _forced.append(kin_exit_rows[0].t_sim) - elif self.tel_rows: - # Fall back: first row where damp_alpha drops to 0 - for i in range(1, len(self.tel_rows)): - if self.tel_rows[i - 1].damp_alpha > 0 and self.tel_rows[i].damp_alpha == 0: - _forced.append(self.tel_rows[i].t_sim) - break for ft in _forced: if t_start < ft < t_end: @@ -462,7 +456,7 @@ def buckets(self, bucket_s: float = 1.0) -> list: brows = [r for r in self.tel_rows if bs <= r.t_sim < be] if brows: - is_kin = any(r.damp_alpha > 0 for r in brows) + is_kin = any((r.phase in ("waiting_ekf", "positioning")) for r in brows) if is_kin: phase = "kinematic" else: diff --git a/simulation/gcs.py b/simulation/gcs.py index d077ccc..f2e5d84 100644 --- a/simulation/gcs.py +++ b/simulation/gcs.py @@ -1034,6 +1034,31 @@ def send_named_float(self, name: str, value: float) -> None: ) log.debug("NAMED_VALUE_FLOAT sent: %s=%.4g", name, value) + def send_named_int(self, name: str, value: int) -> None: + """Send a NAMED_VALUE_INT MAVLink message to the vehicle. + + ArduPilot Lua scripts that have called mavlink.register_rx_msgid(252) + will receive this message via mavlink.receive_chan() on their next tick. + + Use this instead of send_named_float() whenever the value must keep + exact int32 precision (e.g. lat/lon in degrees*1e7) -- a float32 NVF + loses precision for large-magnitude values like absolute latitude. + + Parameters + ---------- + name : str + Message name, up to 10 ASCII characters (truncated + null-padded). + value : int + Signed 32-bit integer value carried by the message. + """ + name_b = name.encode("ascii")[:10].ljust(10, b"\x00") + self._mav.mav.named_value_int_send( + 0, # time_boot_ms — not meaningful for GCS-to-vehicle messages + name_b, + int(value), + ) + log.debug("NAMED_VALUE_INT sent: %s=%d", name, value) + # ------------------------------------------------------------------ # Telemetry receive # ------------------------------------------------------------------ diff --git a/simulation/mediator.py b/simulation/mediator.py index bc98310..329a2dd 100644 --- a/simulation/mediator.py +++ b/simulation/mediator.py @@ -78,6 +78,7 @@ # --------------------------------------------------------------------------- _ASYNC_MAV_LOCK = threading.Lock() _ASYNC_MAV: dict[str, float] = {} +_ASYNC_ARMED = False def update_async_mavlink(fields: dict[str, float]) -> None: @@ -98,6 +99,19 @@ def get_async_mavlink_snapshot() -> dict[str, float]: with _ASYNC_MAV_LOCK: return dict(_ASYNC_MAV) + +def update_async_armed(armed: bool) -> None: + """Update the latest armed state from HEARTBEAT.base_mode.""" + global _ASYNC_ARMED + with _ASYNC_MAV_LOCK: + _ASYNC_ARMED = bool(armed) + + +def get_async_armed() -> bool: + """Return the latest armed state from HEARTBEAT.base_mode.""" + with _ASYNC_MAV_LOCK: + return bool(_ASYNC_ARMED) + # GB4008 yaw-damper gain [N·m·s/rad]. Large value → near-perfect yaw lock. # Reduce to model imperfect GB4008 damping / yaw drift. _K_YAW_DEFAULT = 100.0 @@ -372,6 +386,11 @@ def _request_interval(message_name: str, message_id: int, rate_hz: float) -> Non if _tu: fields["mav_time_usec"] = float(_tu) + if mtype == "HEARTBEAT": + _base_mode = int(getattr(msg, "base_mode", 0)) + _armed = bool(_base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + update_async_armed(_armed) + if mtype not in ("ATTITUDE", "ATTITUDE_TARGET", "SERVO_OUTPUT_RAW", "NAMED_VALUE_FLOAT", "LOCAL_POSITION_NED", "PID_TUNING"): if fields: update_async_mavlink(fields) @@ -842,6 +861,13 @@ def step_fn(servos, t_sim): result = core.step(_dt, collective_rad, _tilt_lon, _tilt_lat, yaw_throttle=None) _damp_alpha = float(result.get("damp_alpha", 0.0)) + # Unified phase label for telemetry CSV. + # During kinematic hold, keep startup phases in the same `phase` field. + if _is_kinematic: + _phase_label = "positioning" if get_async_armed() else "waiting_ekf" + else: + _phase_label = str(_traj_cmd.get("phase", "") or "") + # ── Unpack physics results ──────────────────────────────────────── hub_state = result["hub_state"] tether_force = result["tether_force"] @@ -892,7 +918,7 @@ def step_fn(servos, t_sim): _mav_async = get_async_mavlink_snapshot() _rpy = sensor_data["rpy"] _ti = core.tether._last_info - _cur_phase = _traj_cmd.get("phase", "") + _cur_phase = _phase_label if _cur_phase and _cur_phase != _prev_phase and not _tel_note: _tel_note = f"phase_{_cur_phase.replace('-', '_')}_start" _prev_phase = _cur_phase diff --git a/simulation/mock_ardupilot.lua b/simulation/mock_ardupilot.lua index db3964d..5927af5 100644 --- a/simulation/mock_ardupilot.lua +++ b/simulation/mock_ardupilot.lua @@ -41,7 +41,8 @@ _mock = { R = {1,0,0, 0,1,0, 0,0,1}, params = { -- Script-generated RAWES_* parameters (mirrors rawes.lua param:add_param defaults). - -- Slew + anchor inputs are delivered via NAMED_VALUE_FLOAT. + -- Slew is delivered via NAMED_VALUE_FLOAT; anchor lat/lon/alt via NAMED_VALUE_INT + -- (see Location mock below). RAWES_MODE = 0, -- flight mode (0=disabled) RAWES_YAW_SLP = 0, -- yaw motor slope override (0 = use bench default) RAWES_KP_ALT = 0.0263, -- altitude P gain (thrust/m) @@ -109,6 +110,65 @@ function Vector3f:dot(o) return self._x * o:x() + self._y * o:y() + self._z * o:z() end +-- ── Location ───────────────────────────────────────────────────────────────── +-- Minimal stub for rawes.lua's onboard anchor GPS->NED conversion: +-- Location():lat()/:lng()/:alt() + :get_vector_from_origin_NEU_m(). +-- There is no real EKF in this harness, so the "EKF origin" is a FIXED +-- constant and get_vector_from_origin_NEU_m() never returns nil (unlike on +-- real ArduPilot before the EKF origin is set). These three numbers MUST +-- match rawes_modes.MOCK_ORIGIN_LAT_DEG / MOCK_ORIGIN_LON_DEG / +-- MOCK_ORIGIN_ALT_M -- rawes_modes.send_anchor_ned() computes anchor +-- lat/lon/alt as an offset from that same origin so it round-trips (up to +-- int32 quantization, ~1 cm) back through this exact flat-earth math. +-- Formula mirrors ArduPilot's AP_Common/Location.cpp +-- get_vector_from_origin_NEU_m()/longitude_scale() (LATLON_TO_M flat-earth +-- scaling), NOT a full geodesic calculation -- correct for the metre-scale +-- local offsets used here. + +local _EKF_ORIGIN_LAT_E7 = 515074000 -- 51.5074 deg * 1e7 +local _EKF_ORIGIN_LON_E7 = -1278000 -- -0.1278 deg * 1e7 +local _EKF_ORIGIN_ALT_CM = 5000 -- 50.0 m * 100 + +local _LATLON_TO_M = 0.011131884502145034 -- AP_Math/definitions.h LATLON_TO_M + +local function _longitude_scale(lat_e7) + local s = math.cos(lat_e7 * 1.0e-7 * math.pi / 180.0) + if s < 0.01 then s = 0.01 end + return s +end + +Location_ud = {} +Location_ud.__index = Location_ud + +function Location() + return setmetatable({_lat = 0, _lng = 0, _alt_cm = 0}, Location_ud) +end + +function Location_ud:lat(value) + if value ~= nil then self._lat = value; return end + return self._lat +end + +function Location_ud:lng(value) + if value ~= nil then self._lng = value; return end + return self._lng +end + +function Location_ud:alt(value) + if value ~= nil then self._alt_cm = value; return end + return self._alt_cm +end + +function Location_ud:get_vector_from_origin_NEU_m() + local mid_lat = (self._lat + _EKF_ORIGIN_LAT_E7) / 2.0 + local north_m = (self._lat - _EKF_ORIGIN_LAT_E7) * _LATLON_TO_M + local east_m = (self._lng - _EKF_ORIGIN_LON_E7) * _LATLON_TO_M * _longitude_scale(mid_lat) + local up_m = (self._alt_cm - _EKF_ORIGIN_ALT_CM) * 0.01 + local v = Vector3f() + v:x(north_m); v:y(east_m); v:z(up_m) + return v +end + -- ── uint32_t ───────────────────────────────────────────────────────────────── -- Replicates the ArduPilot Lua uint32_t userdata returned by millis()/micros(). -- The real binding stores an unsigned 32-bit integer and coerces the *other* diff --git a/simulation/rawes_lua_harness.py b/simulation/rawes_lua_harness.py index c968e06..c86d71f 100644 --- a/simulation/rawes_lua_harness.py +++ b/simulation/rawes_lua_harness.py @@ -119,7 +119,8 @@ class RawesLua: kp_el -> RAWES_KP_EL (elevation crosswind gain) kp_az -> RAWES_KP_AZ (azimuth crosswind gain) ...etc. Full names (e.g. "RAWES_KP_ALT") are also accepted. - Anchor/slew use sim.send_named_float ("RAWES_SLW"/"RAWES_ANN"/etc.). + Anchor uses sim.send_named_int ("RAWES_LAT"/"RAWES_LON"/"RAWES_AAL"); + slew uses sim.send_named_float ("RAWES_SLW"). """ # Base tick rate: rawes.lua BASE_PERIOD_MS = 10 ms (100 Hz) @@ -258,8 +259,9 @@ def R(self, mat): def set_param(self, name: str, value: float): """Set a parameter by ArduPilot name or the ``mode`` alias. - RAWES_MODE is a script-generated parameter; slew/anchor now flow via - send_named_float (RAWES_SLW / RAWES_ANN / RAWES_ANE / RAWES_AND). + RAWES_MODE is a script-generated parameter; slew flows via + send_named_float (RAWES_SLW); anchor flows via send_named_int + (RAWES_LAT / RAWES_LON / RAWES_AAL — see rawes_modes.send_anchor_ned()). Example: sim.set_param("mode", 1) # RAWES_MODE = 1 (steady) @@ -401,30 +403,51 @@ def clear_messages(self): """Discard all accumulated gcs:send_text messages.""" self._lua.execute("_mock.gcs_msgs = {}") - # ── MAVLink named-float inject ───────────────────────────────────────── + # ── MAVLink named-value inject (float + int) ──────────────────────────── - def send_named_float(self, name: str, value: float) -> None: - """Inject a NAMED_VALUE_FLOAT into the Lua mavlink inbox. + def _send_named(self, name: str, value, msg_cls, msgid: int) -> None: + """Inject a NAMED_VALUE_FLOAT/INT message into the Lua mavlink inbox. Builds the message with pymavlink (same serialiser used by the real GCS) - and extracts the wire payload (bytes 6..6+len), then prepends 12 null - bytes to match the mavlink_message_t internal-struct layout that - ArduPilot's mavlink.receive_chan() returns. Lua unpacks the payload - starting at byte 13 (1-indexed) with string.unpack(" None: + """Inject a NAMED_VALUE_FLOAT (msgid 251) into the Lua mavlink inbox.""" + from pymavlink import mavutil as _mu + + self._send_named( + name, float(value), _mu.mavlink.MAVLink_named_value_float_message, 251 + ) + + def send_named_int(self, name: str, value: int) -> None: + """Inject a NAMED_VALUE_INT (msgid 252) into the Lua mavlink inbox. + + Used for the anchor lat/lon/alt (RAWES_LAT/LON/AAL), which are sent as + int32 to preserve ArduPilot's own Location precision end-to-end. + """ + from pymavlink import mavutil as _mu + + self._send_named( + name, int(value), _mu.mavlink.MAVLink_named_value_int_message, 252 + ) diff --git a/simulation/rawes_modes.py b/simulation/rawes_modes.py index e5d8779..7fb9ed2 100644 --- a/simulation/rawes_modes.py +++ b/simulation/rawes_modes.py @@ -3,22 +3,26 @@ RAWES_MODE is a script-generated parameter (registered by rawes.lua via param:add_table). Valid values: 0=none, 1=steady, 3=passive, 4=landing. -Every other input (substate, tuning, anchor) is delivered via NAMED_VALUE_FLOAT -- -never encoded in a parameter. Substate is delivered via NAMED_VALUE_FLOAT("RAWES_SUB", N); -the slew rate and anchor position use RAWES_SLW (param) / RAWES_ANN / RAWES_ANE / RAWES_AND. +Every other dynamic input (substate, tuning) is delivered via NAMED_VALUE_FLOAT -- +never encoded in a parameter. Substate is delivered via NAMED_VALUE_FLOAT("RAWES_SUB", N). +The anchor location (RAWES_LAT/LON/AAL) is delivered via NAMED_VALUE_INT as an +absolute lat/lon/alt; rawes.lua resolves it on board to an EKF-local NED offset +via Location:get_vector_from_origin_NEU_m(). Keep this file in sync with the constant definitions in rawes.lua. Used by simtests, SITL stack tests, and calibrate.py. Usage ----- - from rawes_modes import MODE_STEADY, PUMP_REEL_OUT, NV_ANCHOR_N_KEY + from rawes_modes import MODE_STEADY, PUMP_REEL_OUT, send_anchor_ned gcs.set_param("RAWES_MODE", MODE_STEADY) # set mode (pumping runs in steady) gcs.send_named_float("RAWES_SUB", PUMP_REEL_OUT) # set substate - gcs.send_named_float(NV_ANCHOR_N_KEY, -pos0[0]) # anchor North (EKF frame) + send_anchor_ned(gcs, 0.0, 0.0, 0.0) # anchor at the mock/SITL origin """ +import math + # ── 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) @@ -34,13 +38,62 @@ NV_ARMON_KEY = "RAWES_ARM" # named-float key: arm vehicle and start disarm countdown # value = countdown milliseconds; re-send to refresh -# ── Named-float tuning + anchor keys ───────────────────────────────────────────────────── -# rawes.lua gates altitude-hold capture on all three anchor floats arriving. +# ── Named-float tuning key ──────────────────────────────────────────────────── NV_SLEW_KEY = "RAWES_SLW" # body_z / elevation slew rate limit [rad/s] — also a RAWES_* param (override via NVF for runtime changes) -NV_ANCHOR_N_KEY = "RAWES_ANN" # anchor North from EKF origin [m] -NV_ANCHOR_E_KEY = "RAWES_ANE" # anchor East from EKF origin [m] -NV_ANCHOR_D_KEY = "RAWES_AND" # anchor Down from EKF origin [m] + +# ── Named-int anchor keys ───────────────────────────────────────────────────── +# rawes.lua gates altitude-hold capture on all three anchor ints arriving AND +# the onboard lat/lon/alt -> NED conversion succeeding (see _try_resolve_anchor() +# in rawes.lua). Sent as NAMED_VALUE_INT (not FLOAT) to preserve ArduPilot's +# own Location int32 precision (~1 cm) end-to-end. + +NV_ANCHOR_LAT_KEY = "RAWES_LAT" # anchor latitude [deg * 1e7] +NV_ANCHOR_LON_KEY = "RAWES_LON" # anchor longitude [deg * 1e7] +NV_ANCHOR_ALT_KEY = "RAWES_AAL" # anchor altitude [cm, AMSL] + +# Fixed EKF/GPS origin used by the mock Lua harness (mock_ardupilot.lua) and +# the SITL stack tests (stack_utils.py re-exports these as HOME_LAT_DEG/ +# HOME_LON_DEG/HOME_ALT_M -- this module is the single source of truth). +MOCK_ORIGIN_LAT_DEG = 51.5074 +MOCK_ORIGIN_LON_DEG = -0.1278 +MOCK_ORIGIN_ALT_M = 50.0 + +# ArduPilot's flat-earth degrees<->metres scaling (AP_Math/definitions.h LATLON_TO_M). +_LATLON_TO_M = 0.011131884502145034 + + +def anchor_ned_to_gps(dn_m: float, de_m: float, dd_m: float) -> tuple[int, int, int]: + """Convert a desired anchor NED offset (from MOCK_ORIGIN_*) to (lat_e7, lon_e7, alt_cm). + + Inverse of ArduPilot's Location:get_vector_from_origin_NEU_m() flat-earth + math (AP_Common/Location.cpp), so a value produced here and sent via + RAWES_LAT/LON/AAL round-trips (up to int32 quantization, ~1 cm) back to + the SAME (dn_m, de_m, dd_m) once rawes.lua resolves it on board -- as long + as the receiving side's EKF/SITL origin is MOCK_ORIGIN_LAT_DEG/LON_DEG/ALT_M. + """ + origin_lat_e7 = round(MOCK_ORIGIN_LAT_DEG * 1e7) + origin_lon_e7 = round(MOCK_ORIGIN_LON_DEG * 1e7) + origin_alt_cm = round(MOCK_ORIGIN_ALT_M * 100) + + lat_e7 = origin_lat_e7 + round(dn_m / _LATLON_TO_M) + lon_scale = max(math.cos(math.radians(origin_lat_e7 * 1e-7)), 0.01) + lon_e7 = origin_lon_e7 + round(de_m / (_LATLON_TO_M * lon_scale)) + alt_cm = origin_alt_cm - round(dd_m * 100) # NED down -> AMSL alt (up) + return lat_e7, lon_e7, alt_cm + + +def send_anchor_ned(sim, dn_m: float, de_m: float, dd_m: float) -> None: + """Send the anchor at NED offset (dn_m, de_m, dd_m) from MOCK_ORIGIN_* via NAMED_VALUE_INT. + + `sim` must expose `send_named_int(name, value)` (RawesLua harness / gcs.py). + """ + lat_e7, lon_e7, alt_cm = anchor_ned_to_gps(dn_m, de_m, dd_m) + sim.send_named_int(NV_ANCHOR_LAT_KEY, lat_e7) + sim.send_named_int(NV_ANCHOR_LON_KEY, lon_e7) + sim.send_named_int(NV_ANCHOR_ALT_KEY, alt_cm) + + # ── Landing substates (sent as NAMED_VALUE_FLOAT "RAWES_SUB" when mode=MODE_LANDING) ─ diff --git a/simulation/scripts/rawes.lua b/simulation/scripts/rawes.lua index a3361c2..f35dbd2 100644 --- a/simulation/scripts/rawes.lua +++ b/simulation/scripts/rawes.lua @@ -26,13 +26,22 @@ Ground planner signals via NAMED_VALUE_FLOAT (dynamic in-flight values only): RAWES_ALT: target altitude [m] above anchor; Lua rate-limits elevation toward it RAWES_TEN: target/feed-forward tether tension [N]; used for gravity compensation RAWES_ARM: optional disarm timer (value = ms until forced disarm) - RAWES_ANN: anchor North from EKF origin [m] default 0.0 - RAWES_ANE: anchor East from EKF origin [m] default 0.0 - RAWES_AND: anchor Down from EKF origin [m] default 0.0 - The anchor is safety-critical: MODE_STEADY does NOT initialise - 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. + +Ground planner signals via NAMED_VALUE_INT (static anchor location, sent once): + RAWES_LAT: anchor latitude [deg * 1e7] (int32, no default) + RAWES_LON: anchor longitude [deg * 1e7] (int32, no default) + RAWES_AAL: anchor altitude [cm, AMSL] (int32, no default) + Sent as NAMED_VALUE_INT (not FLOAT) so the value keeps 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 the anchor is safety-critical and + gates altitude-hold init: MODE_STEADY does NOT initialise altitude + hold (and therefore commands no cyclic/collective) until all + three values have arrived AND the onboard NED conversion has + succeeded 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. @@ -72,14 +81,17 @@ POST_RELEASE_BLEND_S = 2.5 -- blend current->steady body_z after capture POST_RELEASE_RECOVERY_S = 2.0 -- ramp-in for altitude corrections _NVF_MSG_ID = 251 +_NVI_MSG_ID = 252 -- mavlink:init(queue_size, num_msgs). queue_size = max messages buffered -- between Lua ticks; with queue=1 (the old default) back-to-back NVFs from -- the ground get dropped because only the first survives until update() -- drains it. 20 is plenty for our ~5 named-floats / tick max rate. mavlink.init(20, 10) mavlink.register_rx_msgid(_NVF_MSG_ID) +mavlink.register_rx_msgid(_NVI_MSG_ID) _nv_floats = {} +_nv_ints = {} -- ── Mode numbers ────────────────────────────────────────────────────────────── @@ -202,15 +214,16 @@ _tension_cmd_n = 200.0 -- step-change tension commanded by ground via RAWES_T _az_ref = 0.0 -- plane-keeping azimuth estimate [rad] (low-pass of position azimuth) _az_initialized = false -- true once _az_ref seeded from first GPS fix --- Anchor delivered via NAMED_VALUE_FLOAT (must arrive before altitude-hold activates). +-- Anchor delivered via NAMED_VALUE_INT as an absolute lat/lon/alt (must arrive +-- and resolve to a NED offset before altitude-hold activates). _bz_slew = 0.40 -- RAWES_SLW param: elevation/body_z slew rate [rad/s] -_anchor_n = 0.0 -- RAWES_ANN: anchor North from EKF origin [m] -_anchor_e = 0.0 -- RAWES_ANE: anchor East from EKF origin [m] -_anchor_d = 0.0 -- RAWES_AND: anchor Down from EKF origin [m] -_got_anchor_n = false -- true once RAWES_ANN has been received at least once -_got_anchor_e = false -- true once RAWES_ANE has been received at least once -_got_anchor_d = false -- true once RAWES_AND has been received at least once -_anchor_received = false -- true once all three anchor floats have arrived +_anchor_n = 0.0 -- anchor North from EKF origin [m] (resolved on board) +_anchor_e = 0.0 -- anchor East from EKF origin [m] (resolved on board) +_anchor_d = 0.0 -- anchor Down from EKF origin [m] (resolved on board) +_anchor_lat_e7 = nil -- RAWES_LAT: anchor latitude [deg * 1e7] +_anchor_lon_e7 = nil -- RAWES_LON: anchor longitude [deg * 1e7] +_anchor_alt_cm = nil -- RAWES_AAL: anchor altitude [cm, AMSL] +_anchor_received = false -- true once the onboard lat/lon/alt -> NED conversion has succeeded -- Crosswind rate damping (all initialized from RAWES_* params at load time) _cw_rate_kp = 0.0 -- RAWES_KP_EL: in-plane (elevation) position rate-P [rad/s per m] @@ -236,6 +249,7 @@ _dbg_cap_bz_x = 0.0 _dbg_cap_bz_y = 0.0 _dbg_cap_bz_z = 1.0 _capture_ms = nil +_dbg_precap_last_ms = -100000 -- throttle for pre-capture pos_ned/anchor diagnostic _passive_hold_yaw_rad = nil _passive_yaw_fixed_rad = nil -- optional ground-provided fixed yaw target [rad] (RAWES_YIC) _first_nonzero_rate_logged = false @@ -487,9 +501,10 @@ local function _apply_tension_ramp(dt_s) end local function anchor_ned() - -- Anchor (EKF frame) is delivered via NAMED_VALUE_FLOAT RAWES_ANN/ANE/AND, - -- latched into _anchor_n/e/d. MODE_STEADY gates altitude-hold init on - -- _anchor_received so this is only consulted once all three have arrived. + -- Anchor (EKF frame), resolved on board from the absolute lat/lon/alt + -- delivered via NAMED_VALUE_INT (see _try_resolve_anchor()). MODE_STEADY + -- gates altitude-hold init on _anchor_received so this is only consulted + -- once the conversion has succeeded. local a = Vector3f() a:x(_anchor_n) a:y(_anchor_e) @@ -497,6 +512,31 @@ local function anchor_ned() return a end +-- Resolve the anchor's absolute lat/lon/alt (RAWES_LAT/LON/AAL) into an +-- EKF-local NED offset. Location:get_vector_from_origin_NEU_m() returns nil +-- until the EKF origin has been set, so this retries every tick until it +-- succeeds, then latches the result (the EKF origin is fixed for the rest of +-- the flight once set, so one successful resolution is sufficient). +local function _try_resolve_anchor() + if _anchor_received then return end + if _anchor_lat_e7 == nil or _anchor_lon_e7 == nil or _anchor_alt_cm == nil then return end + + local loc = Location() + loc:lat(_anchor_lat_e7) + loc:lng(_anchor_lon_e7) + loc:alt(_anchor_alt_cm) + local neu = loc:get_vector_from_origin_NEU_m() + if neu == nil then return end -- EKF origin not set yet; retry next tick + + _anchor_n = neu:x() + _anchor_e = neu:y() + _anchor_d = -neu:z() -- NEU up -> NED down + _anchor_received = true + gcs:send_text(6, string.format( + "RAWES: anchor resolved N=%.2f E=%.2f D=%.2f", + _anchor_n, _anchor_e, _anchor_d)) +end + -- Convert a desired body_z direction (NED Vector3f) to ZYX Euler roll/pitch (degrees) -- given the current heading yaw_rad. At RAWES tether elevation 65 deg the resulting -- pitch is ~25 deg from level -- well clear of the 90 deg gimbal-lock singularity. @@ -565,12 +605,31 @@ local function run_flight() "pre_capture_hold") -- Check for GPS position; initialize altitude hold on first valid fix. - -- Gate on _anchor_received: the anchor arrives via NAMED_VALUE_FLOAT - -- (RAWES_ANN/ANE/AND), so do NOT capture geometry (and start commanding - -- cyclic/collective) until all three have been received -- otherwise a - -- zero/stale anchor would put the anchor directly below the hub and - -- drive a saturated cyclic. Until then we keep holding current attitude. + -- Gate on _anchor_received: the anchor's absolute lat/lon/alt arrives + -- via NAMED_VALUE_INT (RAWES_LAT/LON/AAL) and must be resolved to a + -- NED offset on board (_try_resolve_anchor()), so do NOT capture + -- geometry (and start commanding cyclic/collective) until that has + -- succeeded -- otherwise a zero/stale anchor would put the anchor + -- directly below the hub and drive a saturated cyclic. Until then we + -- keep holding current attitude. local pos_ned = ahrs:get_relative_position_NED_origin() + + -- Throttled diagnostic (~1 Hz): show exactly what pos_ned/anchor look + -- like on the way to capture, so a bad geometry capture can be traced + -- back to raw EKF/anchor values instead of only the derived el/az/tlen. + if now_ms - _dbg_precap_last_ms >= 1000 then + _dbg_precap_last_ms = now_ms + local pos_str = pos_ned and string.format("(%.2f,%.2f,%.2f)", pos_ned:x(), pos_ned:y(), pos_ned:z()) or "nil" + local anch_str = "nil" + if _anchor_received then + local anch_dbg = anchor_ned() + anch_str = string.format("(%.2f,%.2f,%.2f)", anch_dbg:x(), anch_dbg:y(), anch_dbg:z()) + end + gcs:send_text(6, string.format( + "RAWES DBG precap: t=%.1fs anchor_rcvd=%s pos_ned=%s anch=%s", + ms_to_s(now_ms), tostring(_anchor_received), pos_str, anch_str)) + end + if _anchor_received and pos_ned then local anch = anchor_ned() local rx = pos_ned:x() - anch:x() @@ -607,8 +666,10 @@ local function run_flight() d_cap, math.deg(_el_rad), math.deg(_az_ref), _tension_n)) end gcs:send_text(6, string.format( - "RAWES steady: captured el=%.1f deg alt=%.1f m tlen=%.1f m", - math.deg(_el_rad), _target_alt, tlen)) + "RAWES steady: captured el=%.1f deg alt=%.1f m tlen=%.1f m rel=(%.2f,%.2f,%.2f) pos_ned=(%.2f,%.2f,%.2f) anch=(%.2f,%.2f,%.2f)", + math.deg(_el_rad), _target_alt, tlen, rx, ry, rz, + pos_ned:x(), pos_ned:y(), pos_ned:z(), + anch:x(), anch:y(), anch:z())) end end return @@ -1005,14 +1066,22 @@ end local function update() _diag = _diag + 1 - -- Drain MAVLink named-float inbox (all modes including 0) - local nvf_raw = mavlink.receive_chan() - while nvf_raw do - local _, nv_val, nv_name = string.unpack(" dict: sim.vel_ned = _IC.vel.tolist() sim.R = _IC.R0 sim.gyro = [0.0, 0.0, 0.0] - # rawes.lua now gates steady capture on anchor NVFs. In this simtest path - # the anchor is at NED origin, so publish zeros once at startup. - sim.send_named_float("RAWES_ANN", 0.0) - sim.send_named_float("RAWES_ANE", 0.0) - sim.send_named_float("RAWES_AND", 0.0) + # rawes.lua now gates steady capture on the anchor location arriving and + # resolving on board. In this simtest path the anchor is at the mock EKF + # origin (NED offset zero), so publish it once at startup. + send_anchor_ned(sim, 0.0, 0.0, 0.0) # ── Physics ─────────────────────────────────────────────────────────────── runner = PhysicsRunner(_ROTOR, _IC, WIND, aero_model=aero_model) diff --git a/simulation/tests/simtests/test_steady_flight_lua.py b/simulation/tests/simtests/test_steady_flight_lua.py index 17fe994..b167808 100644 --- a/simulation/tests/simtests/test_steady_flight_lua.py +++ b/simulation/tests/simtests/test_steady_flight_lua.py @@ -31,7 +31,7 @@ from simtest_runner import PhysicsRunner from tests.common.mock_ardupilot import MockArdupilot from rawes_lua_harness import RawesLua -from rawes_modes import MODE_STEADY +from rawes_modes import MODE_STEADY, send_anchor_ned from tests.simtests._rotor_helpers import load_default_rotor _IC = load_ic() @@ -56,11 +56,10 @@ def _run_steady(log) -> dict: sim.vel_ned = _IC.vel.tolist() sim.R = _IC.R0 sim.gyro = [0.0, 0.0, 0.0] - # rawes.lua now requires anchor NVFs before steady capture can initialise. - # In this pure simtest path the anchor is at NED origin. - sim.send_named_float("RAWES_ANN", 0.0) - sim.send_named_float("RAWES_ANE", 0.0) - sim.send_named_float("RAWES_AND", 0.0) + # rawes.lua now requires the anchor location to arrive and resolve on + # board before steady capture can initialise. In this pure simtest path + # the anchor is at the mock EKF origin (NED offset zero). + send_anchor_ned(sim, 0.0, 0.0, 0.0) runner = PhysicsRunner(_ROTOR, _IC, WIND) lua = MockArdupilot.for_lua(sim, initial_thrust=_IC.eq_thrust, wind=WIND, dt=DT) diff --git a/simulation/tests/sitl/flight/conftest.py b/simulation/tests/sitl/flight/conftest.py index a4365c6..2e42658 100644 --- a/simulation/tests/sitl/flight/conftest.py +++ b/simulation/tests/sitl/flight/conftest.py @@ -10,11 +10,7 @@ All IC-start fixtures share the _ic_trapezoid_stack initialization helper. """ import contextlib -import json import math -import os - -import numpy as np import pytest @@ -25,11 +21,66 @@ _install_lua_scripts, _STARTING_STATE, _STARTUP_DAMP_S, + HOME_LAT_DEG, + HOME_LON_DEG, + HOME_ALT_M, ) from ic import load_ic from torque_model import HubParams, equilibrium_throttle +def _send_anchor_location(ctx) -> None: + """Send the tether anchor's absolute GPS location to rawes.lua via NAMED_VALUE_INT. + + The anchor is fixed at the mediator's world-frame origin (config.py + anchor_ned=[0, 0, 0]). Horizontally, the bare-NED JSON physics backend + maps this directly onto SITL's fixed launch location (stack_utils. + HOME_LAT_DEG / HOME_LON_DEG, matching the --custom-location SITL launch + arg) -- GPS lat/lon is an absolute measurement, so rawes.lua's + Location:get_vector_from_origin_NEU_m() resolves the correct N/E offset + regardless of where the EKF origin actually lands. + + Altitude is different: ArduPilot's vertical estimate is baro-anchored, + so whatever altitude the vehicle physically occupies when the EKF's + local origin is established becomes its "zero" -- a real sensor + behavior (baro is zeroed at boot), not a simulation artifact. This + stack test boots the vehicle already kinematically locked ctx.home_alt_m + metres above the anchor, so the EKF's local origin altitude ends up that + far above the anchor's true altitude. The anchor's sent altitude must + therefore be offset by -ctx.home_alt_m so it resolves to the correct + (positive-down) D offset once EKF-local. + + Sent as NAMED_VALUE_INT (not NAMED_VALUE_FLOAT): lat/lon in degrees lose + ~0.5-1 m of precision when round-tripped through a float32 NVF wire + field, whereas NAMED_VALUE_INT carries the same int32 degrees*1e7 / cm + representation ArduPilot's own Location object uses natively. + """ + anchor_alt_m = HOME_ALT_M - ctx.home_alt_m + lat_e7 = round(HOME_LAT_DEG * 1e7) + lon_e7 = round(HOME_LON_DEG * 1e7) + alt_cm = round(anchor_alt_m * 100) + ctx.gcs.send_named_int("RAWES_LAT", lat_e7) + ctx.gcs.send_named_int("RAWES_LON", lon_e7) + ctx.gcs.send_named_int("RAWES_AAL", alt_cm) + ctx.log.info( + " anchor location sent via NVI (lat=%.7f lon=%.7f alt=%.1fm, " + "home_alt_m=%.2f)", + HOME_LAT_DEG, HOME_LON_DEG, anchor_alt_m, ctx.home_alt_m, + ) + + +def _wait_and_configure_mode_anchor( + ctx, + *, + mode: int, + mode_label: str, +) -> None: + """Send the anchor location once, then set RAWES_MODE.""" + _send_anchor_location(ctx) + ctx.gcs.set_param("RAWES_MODE", int(mode), timeout=5.0) + ctx.log.info(" mode=%s set", mode_label) + + # --------------------------------------------------------------------------- # Fixtures — thin wrappers around _acro_stack # --------------------------------------------------------------------------- @@ -76,7 +127,6 @@ def guided_nogps_armed_pumping_lua(tmp_path, request): yield ctx - @pytest.fixture def guided_nogps_armed_landing_lua(tmp_path, request): """ @@ -115,7 +165,7 @@ def guided_nogps_armed_landing_lua(tmp_path, request): t=45..65 s kinematic ramp phase: vel ramps 0.96->0 m/s (vel_ramp_s=20) t~51 s ahrs:healthy() True; Lua enters KINEMATIC_SETTLE_MS wait t~62 s Lua KINEMATIC_SETTLE_MS (62 s) expires; captures body_z - t~62..65 s Lua sends GUIDED setpoints; kinematic still ramps vel to 0 + t~62..65 s Lua sends GUIDED setpoints; kinematic still ramps vel to 0 t=65 s kinematic exits; hub at pos0 with vel=0, tension~0 t~65..102 s Lua VZ controller descends hub; WinchController reels in tether t~102 s Lua triggers final_drop STATUSTEXT (alt_est <= 2 m) @@ -157,26 +207,11 @@ def guided_nogps_armed_landing_lua(tmp_path, request): } with _acro_stack(tmp_path, extra_config=extra, test_name=request.node.name) as ctx: - # Post-arm: configure rawes.lua. Mode via RAWES_MODE; slew + anchor via - # NAMED_VALUE_FLOAT. - # RAWES_AND = -pos0[2] = altitude of pos0 above anchor ~ 19.696 m. - # vel0[2]=0 => altitude constant during kinematic => EKF_ORIGIN.z = pos0[2]. - # anch_EKF.z = RAWES_AND = -pos0[2] = altitude above EKF origin. - # Lua: alt_est = anch.z - hub_ned.z (hub_ned.z from LOCAL_POSITION_NED). - # - # RAWES_MODE=4 set here; rawes.lua DELAYS body_z capture until - # millis() >= KINEMATIC_SETTLE_MS (62 s) to ensure EKF has converged. - ctx.log.info("Setting RAWES_MODE + slew/anchor NVFs for rawes.lua (landing mode) ...") - ctx.gcs.set_param("SCR_ENABLE", 1, timeout=5.0) # persist scripting in EEPROM - ctx.gcs.set_param("RAWES_MODE", 4, timeout=5.0) # landing mode - # Slew + anchor are NAMED_VALUE_FLOAT. The anchor is at world origin; - # here anchor North/East = 0 and anchor Down (EKF frame) = -pos0[2]. - # rawes.lua gates altitude-hold capture on all three anchor floats. - ctx.gcs.send_named_float("RAWES_ANN", 0.0) # anchor North [m] - ctx.gcs.send_named_float("RAWES_ANE", 0.0) # anchor East [m] - ctx.gcs.send_named_float("RAWES_AND", -float(extra["pos0"][2])) # anchor Down [m] - ctx.log.info(" mode=4 (landing); slew+anchor sent via NVF (anchor D=%.2f)", - -float(extra["pos0"][2])) + # Post-arm: configure rawes.lua for landing mode. + # rawes.lua delays body_z capture until KINEMATIC_SETTLE_MS, so this can + # be applied before kinematic exit without premature guidance capture. + ctx.log.info("Setting RAWES_MODE + anchor NVI for rawes.lua (landing mode) ...") + _wait_and_configure_mode_anchor(ctx, mode=4, mode_label="4 (landing)") ctx.wait_drain(timeout=1.0, label="post-param") yield ctx @@ -306,36 +341,22 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch test_name=test_name, arm_at_sim_s=_arm_at_sim_s, ) as ctx: - ctx.log.info("Setting mode param + slew/anchor NVFs for rawes.lua ...") - # The anchor is at world origin; the EKF origin is the hub's first - # GPS fix = launch position pos0. So anchor-in-EKF-frame = -pos0. - # RAWES_ANN/ANE must encode the horizontal offset (negated launch x/y) - # or the Lua sees the anchor directly below the hub and computes - # elevation = 90 deg, producing a saturated cyclic that kicks the - # body at kinematic_exit. - _pos0 = ctx.initial_state["pos"] if ctx.initial_state else [0.0, 0.0, -ctx.home_alt_m] - # Mode is the only RAWES_* script-generated param set here; slew + anchor are NAMED_VALUE_FLOAT. - ctx.gcs.set_param("SCR_ENABLE", 1, timeout=5.0) # persist scripting in EEPROM - # MODE_PASSIVE (3): vehicle stays armed (motor interlock kept high) but - # Lua emits no rate commands, so ArduPilot's rate PID has no setpoint to - # wind up against while the body is kinematically locked. The test must - # promote to MODE_STEADY (1) immediately after kinematic_exit. - ctx.gcs.set_param("RAWES_MODE", 3, timeout=5.0) # MODE_PASSIVE - # rawes.lua gates altitude-hold capture on all three anchor floats arriving. - ctx.gcs.send_named_float("RAWES_ANN", -float(_pos0[0])) # anchor North (EKF) [m] - ctx.gcs.send_named_float("RAWES_ANE", -float(_pos0[1])) # anchor East (EKF) [m] - ctx.gcs.send_named_float("RAWES_AND", float(ctx.home_alt_m)) # anchor Down (EKF) [m] - ctx.log.info(" mode=3 (PASSIVE); slew+anchor via NVF (anchor EKF=%.2f,%.2f,%.2f)", - -float(_pos0[0]), -float(_pos0[1]), float(ctx.home_alt_m)) - ctx.wait_drain(timeout=1.0, label="post-param") + # MODE_PASSIVE (3) is set immediately after arm, decoupled from anchor + # calibration below (which needs telemetry/EKF data that can take + # longer to become ready). Lua emits no rate commands in this mode, + # so ArduPilot's rate PID has no setpoint to wind up against while the + # body is kinematically locked -- this must happen right after arm, + # not after waiting on anchor telemetry. + ctx.log.info("Setting RAWES_MODE=3 (PASSIVE) immediately after arm ...") + ctx.gcs.set_param("RAWES_MODE", 3, timeout=5.0) # Stream IC collective to Lua so MODE_PASSIVE holds the IC collective # through GUIDED throttle and omega_spin doesn't droop while the body is kinematically # locked. _ic = ctx.initial_state if _ic is not None: - # Seed the IC immediately, together with MODE_PASSIVE, right after - # arm. MODE_PASSIVE only commands the IC attitude/collective once + # Seed the IC immediately, right after MODE_PASSIVE is set. + # MODE_PASSIVE only commands the IC attitude/collective once # the full atomic seed (RAWES_RIC + RAWES_PIC + RAWES_THR) has # arrived, so sending it now gives the nul-aero the entire kinematic # hold window to slew the disk to the IC tilt before release. @@ -368,13 +389,26 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch ) # Stream the IC equilibrium tension and target altitude from the IC. - # Tension feeds the orientation force balance; altitude overrides the - # EKF-reported altitude (which has a ~2.5 m bias at capture time due - # to EKF vertical convergence lag) so Lua targets the physics IC - # altitude, not the biased EKF altitude. + # Tension feeds the orientation force balance; altitude is the + # physics-truth IC altitude (ctx.home_alt_m), not a live EKF + # reading, avoiding the ~2.5 m EKF vertical convergence-lag bias + # present at capture time. _tension_eq = float(_ic["tension_eq_n"]) - _alt_ic = float(-_ic["pos"][2]) # physics altitude above anchor ctx.gcs.send_named_float("RAWES_TEN", _tension_eq) + + # Anchor location is a static constant, sent EXACTLY ONCE here + # (after the IC seed values are queued) -- it does not depend on + # telemetry/EKF availability, so it can be sent right away without + # gating the MODE_PASSIVE assignment above. + _send_anchor_location(ctx) + + # Altitude above anchor at IC = ctx.home_alt_m (precomputed in + # stack_infra._acro_stack as -initial_state["pos"][2]; the anchor + # sits at the mediator's world-frame origin so this equals the + # physics-truth altitude directly -- no telemetry/EKF calibration + # needed). + _alt_ic = float(ctx.home_alt_m) + ctx.gcs.send_named_float("RAWES_ALT", _alt_ic) ctx.log.info("IC equilibrium tension: %.0f N target altitude: %.1f m", _tension_eq, _alt_ic) @@ -391,6 +425,7 @@ def _ic_trapezoid_stack(tmp_path, *, test_name, winch_cmd_port, run_ground_winch _yff_seed = equilibrium_throttle(float(_ic["omega_spin"]), HubParams()) ctx.gcs.send_named_float("RAWES_YFF", _yff_seed) ctx.log.info("IC yaw-trim equilibrium seed: %.3f", _yff_seed) + ctx.wait_drain(timeout=1.0, label="post-param") ctx.wait_drain(timeout=0.5, label="post-col") # Wait for GPS fusion before yielding. diff --git a/simulation/tests/sitl/flight/test_lua_flight_steady_sitl.py b/simulation/tests/sitl/flight/test_lua_flight_steady_sitl.py index b20fce1..d6995e4 100644 --- a/simulation/tests/sitl/flight/test_lua_flight_steady_sitl.py +++ b/simulation/tests/sitl/flight/test_lua_flight_steady_sitl.py @@ -246,7 +246,7 @@ def _handle(msg, t_rel): "Checklist:\n" " * SCR_ENABLE=1 baked into eeprom (prime_eeprom=True in fixture)\n" " * rawes.lua installed to /ardupilot/scripts/\n" - " * RAWES_AND (anchor Down) = home_alt_m correct\n" + " * RAWES_LAT/LON/AAL (anchor location) sent and resolved to a NED offset\n" " * SITL log for Lua load errors" ) if now - state["t_last_log"] >= _POS_LOG_INTERVAL: diff --git a/simulation/tests/sitl/rawes_common_defaults.parm b/simulation/tests/sitl/rawes_common_defaults.parm index 744eaa2..cd7e638 100644 --- a/simulation/tests/sitl/rawes_common_defaults.parm +++ b/simulation/tests/sitl/rawes_common_defaults.parm @@ -36,6 +36,17 @@ ATC_ANGLE_BOOST 0 # disable auto-leveling (physical tilt ~65 deg) ACRO_TRAINER 0 +# EKF3 baro/GPS altitude blending: baro is the primary vertical position source +# (EK3_SRC1_POSZ default = Baro), but baro has no absolute reference and drifts +# with weather-driven QNH changes over long flights (~8-9 m per hPa). GPS +# altitude has no drift (only noise), so bits 0+2 let a Bayes filter slowly +# correct the baro-derived local position toward GPS altitude over time without +# giving up baro's short-term smoothness. See AP_NavEKF3_PosVelFusion.cpp +# correctEkfOriginHeight() / OGN_HGT_MASK doc for the mechanism. +# bit 0 (1): correct when Baro is the active height source +# bit 2 (4): apply the correction to local position (not just origin height) +EK3_OGN_HGT_MASK 5 + # Collective servo range: full 1000-2000 us. H_COL_MIN 1000 H_COL_MAX 2000 diff --git a/simulation/tests/sitl/stack_infra.py b/simulation/tests/sitl/stack_infra.py index 13ec9fa..6a431d4 100644 --- a/simulation/tests/sitl/stack_infra.py +++ b/simulation/tests/sitl/stack_infra.py @@ -18,6 +18,7 @@ _TORQUE_STARTUP_HOLD_S _STARTUP_TIMEOUT, _ARM_TIMEOUT, _MODE_TIMEOUT, _STARTUP_DAMP_S _static_stack + HOME_LAT_DEG, HOME_LON_DEG, HOME_ALT_M """ import contextlib import dataclasses @@ -61,6 +62,9 @@ def _project_default_rotor(): ARDUPILOT_ENV, STACK_ENV_FLAG, SIM_VEHICLE_ENV, + HOME_LAT_DEG, + HOME_LON_DEG, + HOME_ALT_M, ParamSetup, SITL_UNSUPPORTED_PARAMS, _check_ardupilot_version, @@ -309,6 +313,8 @@ class StackContext: 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 Torque tests only (default 0.0 for flight tests) ------------------------------------------------- @@ -334,6 +340,7 @@ class StackContext: sim_dir: Path | None = None controller: object = None test_log_dir: Path | None = None + last_local_position_ned: tuple | None = None # ── pumping socket (default 0 = disabled) ──────────────────────────────── winch_cmd_port: int = 0 # ── torque tests (default 0.0 for flight) ───────────────────────────────── @@ -382,13 +389,25 @@ def _recv_one(recv_timeout: float) -> None: type=["STATUSTEXT", "ATTITUDE", "LOCAL_POSITION_NED", "EKF_STATUS_REPORT", "SERVO_OUTPUT_RAW"], blocking=True, timeout=recv_timeout) - if msg is not None and msg.get_type() == "STATUSTEXT": + if msg is None: + last_text[0] = None + return + mtype = msg.get_type() + if mtype == "STATUSTEXT": text = msg.text.rstrip("\x00").strip() self.all_statustext.append(text) self.log.info("STATUSTEXT [%s]: %s", label, text) last_text[0] = text - else: - last_text[0] = None + return + if mtype == "LOCAL_POSITION_NED": + # Only the most recent sample is kept -- callers needing a + # gated/finite sample poll self.last_local_position_ned via + # `until` rather than accumulating history here. + self.last_local_position_ned = ( + float(msg.x), float(msg.y), float(msg.z), + float(msg.vx), float(msg.vy), float(msg.vz), + ) + last_text[0] = None def _check_liveness() -> None: if not check_procs: diff --git a/simulation/tests/sitl/stack_utils.py b/simulation/tests/sitl/stack_utils.py index ee3d3e8..c885be6 100644 --- a/simulation/tests/sitl/stack_utils.py +++ b/simulation/tests/sitl/stack_utils.py @@ -16,6 +16,11 @@ import numpy as np from frames import build_gps_yaw_frame +from rawes_modes import ( + MOCK_ORIGIN_LAT_DEG as HOME_LAT_DEG, + MOCK_ORIGIN_LON_DEG as HOME_LON_DEG, + MOCK_ORIGIN_ALT_M as HOME_ALT_M, +) # --------------------------------------------------------------------------- # Environment variable names @@ -26,6 +31,24 @@ SIM_VEHICLE_ENV = "RAWES_SIM_VEHICLE" +# --------------------------------------------------------------------------- +# Fixed SITL home location +# --------------------------------------------------------------------------- +# All stack tests launch SITL at this fixed absolute location (see +# --custom-location in _launch_sitl below). The mediator's tether anchor +# sits at the mediator's own world-frame origin (config.py +# anchor_ned=[0, 0, 0]), and the bare-NED JSON physics backend maps that +# origin directly onto SITL's home location (ArduPilot's +# Aircraft::update_home()/set_start_location() in SIM_Aircraft.cpp) -- so +# this location IS the anchor's absolute GPS position, sent to rawes.lua by +# _send_anchor_location() in flight/conftest.py. +# +# HOME_LAT_DEG/HOME_LON_DEG/HOME_ALT_M are re-exported aliases of +# rawes_modes.MOCK_ORIGIN_LAT_DEG/LON_DEG/ALT_M -- that module is the single +# source of truth (also used by the mock Lua harness's Location EKF-origin +# stub) so the SITL and mock-harness "world origin" never drift apart. + + # --------------------------------------------------------------------------- # Params present in the RAWES hardware param set but NOT compiled into the # ArduCopter-heli SITL build. These configure BLHeli / bidirectional-DShot on @@ -303,7 +326,7 @@ def _launch_sitl( str(sim_vehicle), "--vehicle", "ArduCopter", "--frame", "heli", - "--custom-location=51.5074,-0.1278,50,0", + f"--custom-location={HOME_LAT_DEG},{HOME_LON_DEG},{HOME_ALT_M},0", "--model", "JSON", "--sim-address", "127.0.0.1", "--no-rebuild", diff --git a/simulation/tests/unit/test_math_lua.py b/simulation/tests/unit/test_math_lua.py index 492ba4a..afdfa65 100644 --- a/simulation/tests/unit/test_math_lua.py +++ b/simulation/tests/unit/test_math_lua.py @@ -25,6 +25,7 @@ compute_bz_altitude_hold, ) from rawes_lua_harness import RawesLua +from rawes_modes import send_anchor_ned # ── Module-level harness ────────────────────────────────────────────────────── @@ -309,11 +310,11 @@ def test_first_angle_command_is_close_to_ic_body_z(self, sim): sim.vel_ned = [0.0, 0.0, 0.0] sim.clear_messages() - # Match the stack path: anchor encoded in EKF frame, GPS position at home. + # Match the stack path: anchor encoded as a NED offset from the mock's + # fixed EKF/Location origin (see rawes_modes.send_anchor_ned), GPS + # position at home. sim.set_param("mode", 1) - sim.send_named_float("RAWES_ANN", -float(pos0[0])) - sim.send_named_float("RAWES_ANE", -float(pos0[1])) - sim.send_named_float("RAWES_AND", -float(pos0[2])) + send_anchor_ned(sim, -float(pos0[0]), -float(pos0[1]), -float(pos0[2])) sim.pos_ned = [0.0, 0.0, 0.0] sim.send_named_float("RAWES_THR", float(ic["eq_thrust"]))